diff --git a/src/com/team3925/utils/SendablePIDTuner.java b/SendablePIDTuner.java similarity index 96% rename from src/com/team3925/utils/SendablePIDTuner.java rename to SendablePIDTuner.java index 2ba4406..ab23e7d 100644 --- a/src/com/team3925/utils/SendablePIDTuner.java +++ b/SendablePIDTuner.java @@ -1,54 +1,54 @@ -package com.team3925.utils; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class SendablePIDTuner implements Sendable { - - String name = "PIDTuner"; - String subsystem; - PIDTunable pid_values; - - public SendablePIDTuner(Subsystem subsystem, PIDTunable pid_values) { - this.subsystem = subsystem.getName() + "Tuner"; - this.pid_values = pid_values; - } - - @Override - public String getName() { - return name; - } - - @Override - public void setName(String name) { - this.name = name; - - } - - @Override - public String getSubsystem() { - return subsystem; - } - - @Override - public void setSubsystem(String subsystem) { - this.subsystem = subsystem; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("PIDController"); - builder.addDoubleProperty("p", pid_values::getkP, pid_values::setkP); - builder.addDoubleProperty("i", pid_values::getkI, pid_values::setkI); - builder.addDoubleProperty("d", pid_values::getkD, pid_values::setkD); - builder.addDoubleProperty("f", pid_values::getkF, pid_values::setkF); - } - - public void updateDashboard() { - // TODO: should this be name or subsystem? The subsystem will allow us to - // differentiate it. - SmartDashboard.putData(subsystem, this); - } +package com.team3925.utils; + +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SendablePIDTuner implements Sendable { + + String name = "PIDTuner"; + String subsystem; + PIDTunable pid_values; + + public SendablePIDTuner(Subsystem subsystem, PIDTunable pid_values) { + this.subsystem = subsystem.getName() + "Tuner"; + this.pid_values = pid_values; + } + + @Override + public String getName() { + return name; + } + + @Override + public void setName(String name) { + this.name = name; + + } + + @Override + public String getSubsystem() { + return subsystem; + } + + @Override + public void setSubsystem(String subsystem) { + this.subsystem = subsystem; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PIDController"); + builder.addDoubleProperty("p", pid_values::getkP, pid_values::setkP); + builder.addDoubleProperty("i", pid_values::getkI, pid_values::setkI); + builder.addDoubleProperty("d", pid_values::getkD, pid_values::setkD); + builder.addDoubleProperty("f", pid_values::getkF, pid_values::setkF); + } + + public void updateDashboard() { + // TODO: should this be name or subsystem? The subsystem will allow us to + // differentiate it. + SmartDashboard.putData(subsystem, this); + } } \ No newline at end of file diff --git a/src/com/team3925/frc2018/Logger.java b/src/com/team3925/frc2018/Logger.java index fbefc82..4ab1008 100644 --- a/src/com/team3925/frc2018/Logger.java +++ b/src/com/team3925/frc2018/Logger.java @@ -1,22 +1,19 @@ -package com.team3925.frc2018; - -import com.team3925.frc2018.subsystems.Drivetrain; -import com.team3925.utils.SendablePIDTuner; - -public class Logger { - SendablePIDTuner drivetrainTuner; - - private static Logger instance; - public static Logger getInstance() { - if (instance == null) - instance = new Logger(); - return instance; - } - - private Logger() { - drivetrainTuner = new SendablePIDTuner(Drivetrain.getInstance(), Drivetrain.getInstance()); - } - public void update() { - drivetrainTuner.updateDashboard(); - } -} +package com.team3925.frc2018; + +public class Logger { +// SendablePIDTuner drivetrainTuner; + + private static Logger instance; + public static Logger getInstance() { + if (instance == null) + instance = new Logger(); + return instance; + } + + private Logger() { +// drivetrainTuner = new SendablePIDTuner(Drivetrain.getInstance(), Drivetrain.getInstance()); + } + public void update() { +// drivetrainTuner.updateDashboard(); + } +} diff --git a/src/com/team3925/frc2018/OI.java b/src/com/team3925/frc2018/OI.java index 6437d94..fb89b24 100644 --- a/src/com/team3925/frc2018/OI.java +++ b/src/com/team3925/frc2018/OI.java @@ -142,7 +142,11 @@ protected void end() { @Override protected boolean isFinished() { - return Intake.getInstance().isAtSetpoint(); + if(Elevator.state == ElevatorState.BOTTOM) { + return Intake.getInstance().isAtSetpoint(); + }else { + return true; + } } }); shootCube.whenInactive(new Command() { @@ -150,7 +154,9 @@ protected boolean isFinished() { @Override protected void initialize() { Intake.getInstance().setIntakeRollers(0); - Intake.getInstance().setAngle(85); + if (Elevator.state == ElevatorState.BOTTOM) { + Intake.getInstance().setAngle(85); + } } @Override protected boolean isFinished() { diff --git a/src/com/team3925/frc2018/Robot.java b/src/com/team3925/frc2018/Robot.java index cb005e3..e22efee 100644 --- a/src/com/team3925/frc2018/Robot.java +++ b/src/com/team3925/frc2018/Robot.java @@ -8,6 +8,7 @@ import com.team3925.frc2018.commands.autos.CenterSwitchAuto; import com.team3925.frc2018.commands.autos.CenterSwitchAuto.AutoSide; import com.team3925.frc2018.commands.autos.DriveForwardAuto; +import com.team3925.frc2018.commands.autos.LeftScaleAuto; import com.team3925.frc2018.subsystems.Drivetrain; import com.team3925.frc2018.subsystems.Elevator; import com.team3925.frc2018.subsystems.Intake; @@ -31,7 +32,7 @@ public class Robot extends IterativeRobot { Command zeroIntake; UsbCamera camera; SendableChooser autoSelector; - boolean isElevatorZeroed = false; + private static boolean isElevatorZeroed; @Override public void robotInit() { @@ -45,27 +46,30 @@ public void robotInit() { camera.setFPS(12); autoSelector.addDefault("Drive Forward", "DriveForward"); autoSelector.addObject("CenterSwitch", "Center"); + autoSelector.addObject("Left Scale", "LeftScale"); SmartDashboard.putData("Auto", autoSelector); isElevatorZeroed = false; } @Override public void autonomousInit() { - isElevatorZeroed = true; + Intake.getInstance().setIntakeRollers(0.25); Elevator.getInstance().zero(); Intake.getInstance().zeroLift(); testAuto = new DriveForwardAuto(); + System.out.println(DriverStation.getInstance().getGameSpecificMessage().charAt(0)); if (autoSelector.getSelected().equals("DriveForward")) { testAuto = new DriveForwardAuto(); }else if(autoSelector.getSelected().equals("Center")){ if (DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'L') { testAuto = new CenterSwitchAuto(AutoSide.LEFT); - System.out.println("RUNnningLEFt"); }else if(DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'R') { testAuto = new CenterSwitchAuto(AutoSide.RIGHT); } } + testAuto = new LeftScaleAuto(); testAuto.start(); + isElevatorZeroed = true; } @Override @@ -73,12 +77,13 @@ public void autonomousPeriodic() { } public void teleopInit() { - if (!isElevatorZeroed) { + if (isElevatorZeroed == false) { Elevator.getInstance().zero(); Intake.getInstance().zeroLift(); + System.out.println("ZeroCalled"); } drive.start(); - Intake.getInstance().setIntakeRollers(0.3); + Intake.getInstance().setIntakeRollers(0.25); // Elevator.getInstance().setPosition(ElevatorState.BOTTOM); // Intake.getInstance().zeroLift(); } @@ -98,6 +103,5 @@ public void robotPeriodic() { @Override public void disabledInit() { - isElevatorZeroed = false; } } diff --git a/src/com/team3925/frc2018/commands/DriveManual.java b/src/com/team3925/frc2018/commands/DriveManual.java index 1efda70..a487d7e 100644 --- a/src/com/team3925/frc2018/commands/DriveManual.java +++ b/src/com/team3925/frc2018/commands/DriveManual.java @@ -1,75 +1,75 @@ -package com.team3925.frc2018.commands; - -import javax.print.attribute.standard.PrinterResolution; - -import com.team3925.frc2018.subsystems.Drivetrain; -import com.team3925.frc2018.subsystems.Elevator; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveManual extends Command { - - public interface DriveManualInput { - public abstract double getFwd(); - - public abstract double getLeft(); - } - - private final boolean doReverseWhenReversing; - - private DriveManualInput input; - private double prelimLeft, prelimRight; - private double fwd, turn; - private double scale; - private static final double K_HEIGHT_SUBTRACTION = 0.9; - - public DriveManual(DriveManualInput input) { - this.input = input; - doReverseWhenReversing = false; - requires(Drivetrain.getInstance()); - } - - @Override - protected void initialize() { - Drivetrain.getInstance().setRaw(0, 0); - } - - @Override - protected void execute() { - fwd = input.getFwd(); - turn = input.getLeft(); - if (Math.abs(fwd) < 0.1) - fwd = 0; - if (doReverseWhenReversing && fwd != 0) - turn *= Math.signum(fwd); - prelimLeft = fwd + turn; - prelimRight = fwd - turn; - if (prelimLeft != 0 || prelimRight != 0) { - scale = Math.max(Math.abs(fwd), Math.abs(turn)) / Math.max(Math.abs(prelimLeft), Math.abs(prelimRight)); - prelimLeft *= scale; - prelimRight *= scale; - } - - Drivetrain.getInstance().setRaw( - prelimLeft * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)), - prelimRight * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)) - ); - } - - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - Drivetrain.getInstance().setRaw(0, 0); - } - - @Override - protected void interrupted() { - Drivetrain.getInstance().setRaw(0, 0); - } - -} +package com.team3925.frc2018.commands; + +import javax.print.attribute.standard.PrinterResolution; + +import com.team3925.frc2018.subsystems.Drivetrain; +import com.team3925.frc2018.subsystems.Elevator; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveManual extends Command { + + public interface DriveManualInput { + public abstract double getFwd(); + + public abstract double getLeft(); + } + + private final boolean doReverseWhenReversing; + + private DriveManualInput input; + private double prelimLeft, prelimRight; + private double fwd, turn; + private double scale; + private static final double K_HEIGHT_SUBTRACTION = 0.8; + + public DriveManual(DriveManualInput input) { + this.input = input; + doReverseWhenReversing = false; + requires(Drivetrain.getInstance()); + } + + @Override + protected void initialize() { + Drivetrain.getInstance().setRaw(0, 0); + } + + @Override + protected void execute() { + fwd = input.getFwd(); + turn = input.getLeft(); + if (Math.abs(fwd) < 0.1) + fwd = 0; + if (doReverseWhenReversing && fwd != 0) + turn *= Math.signum(fwd); + prelimLeft = fwd + turn; + prelimRight = fwd - turn; + if (prelimLeft != 0 || prelimRight != 0) { + scale = Math.max(Math.abs(fwd), Math.abs(turn)) / Math.max(Math.abs(prelimLeft), Math.abs(prelimRight)); + prelimLeft *= scale; + prelimRight *= scale; + } + + Drivetrain.getInstance().setRaw( + prelimLeft * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)), + prelimRight * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)) + ); + } + + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + Drivetrain.getInstance().setRaw(0, 0); + } + + @Override + protected void interrupted() { + Drivetrain.getInstance().setRaw(0, 0); + } + +} diff --git a/src/com/team3925/frc2018/commands/RunElevator.java b/src/com/team3925/frc2018/commands/RunElevator.java index 1243c01..a9c502c 100644 --- a/src/com/team3925/frc2018/commands/RunElevator.java +++ b/src/com/team3925/frc2018/commands/RunElevator.java @@ -1,33 +1,36 @@ -package com.team3925.frc2018.commands; - -import com.team3925.frc2018.OI; -import com.team3925.frc2018.subsystems.Elevator; -import com.team3925.frc2018.subsystems.Intake; -import com.team3925.frc2018.subsystems.Elevator.ElevatorState; - -import edu.wpi.first.wpilibj.command.Command; - -public class RunElevator extends Command { - - private ElevatorState state; - - public RunElevator(ElevatorState state) { - this.state = state; - requires(Elevator.getInstance()); - } - - @Override - protected void initialize() { - Elevator.getInstance().setPosition(state); - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Intake.getInstance().setAngle(0); - } +package com.team3925.frc2018.commands; + +import com.team3925.frc2018.subsystems.Elevator; +import com.team3925.frc2018.subsystems.Elevator.ElevatorState; +import com.team3925.frc2018.subsystems.Intake; + +import edu.wpi.first.wpilibj.command.Command; + +public class RunElevator extends Command { + + private ElevatorState state; + + public RunElevator(ElevatorState state) { + this.state = state; + requires(Elevator.getInstance()); + } + + @Override + protected void initialize() { + Elevator.getInstance().setPosition(state); + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + if (Elevator.state == (ElevatorState.TOP)) { + Intake.getInstance().setAngle(45); + }else { + Intake.getInstance().setAngle(0); + } + } } \ No newline at end of file diff --git a/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java b/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java index 7146492..6a6fbd0 100644 --- a/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java +++ b/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java @@ -5,9 +5,11 @@ import com.team3925.frc2018.commands.RunElevator; import com.team3925.frc2018.commands.RunIntakeWheels; import com.team3925.frc2018.commands.ZeroIntake; +import com.team3925.frc2018.subsystems.Intake; import com.team3925.frc2018.subsystems.Elevator.ElevatorState; import com.team3925.utils.MotionProfileCommand; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.WaitCommand; @@ -18,16 +20,25 @@ public enum AutoSide{ } public CenterSwitchAuto(AutoSide side) { + addParallel(new Command() { + + @Override + protected boolean isFinished() { + // TODO Auto-generated method stub + Intake.getInstance().setIntakeRollers(0.25); + return true; + } + }); addParallel(new CloseGrabbers()); - addParallel(new RunElevator(ElevatorState.SWITCH)); - if (side == AutoSide.LEFT) { - addParallel(new MotionProfileCommand("CENTER_LEFTSWITCH")); - }else { - addParallel(new MotionProfileCommand("CENTER_RIGHTSWITCH")); + if (side.equals(AutoSide.LEFT)) { + addSequential(new MotionProfileCommand("CENTER_LEFTSWITCH")); + }else if (side.equals(AutoSide.RIGHT)){ + addSequential(new MotionProfileCommand("CENTER_RIGHTSWITCH")); } - addSequential(new WaitCommand(2)); + addSequential(new WaitCommand(4)); + addParallel(new RunElevator(ElevatorState.SWITCH)); addSequential(new OpenGrabbers()); - addSequential(new RunIntakeWheels(-0.5)); + addSequential(new RunIntakeWheels(-0.25)); } } diff --git a/src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java b/src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java new file mode 100644 index 0000000..4d38a0c --- /dev/null +++ b/src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java @@ -0,0 +1,38 @@ +package com.team3925.frc2018.commands.autos; + +import com.team3925.frc2018.commands.CloseGrabbers; +import com.team3925.frc2018.commands.OpenGrabbers; +import com.team3925.frc2018.commands.RunElevator; +import com.team3925.frc2018.commands.RunIntakeWheels; +import com.team3925.frc2018.commands.ZeroIntake; +import com.team3925.frc2018.subsystems.Intake; +import com.team3925.frc2018.subsystems.Elevator.ElevatorState; +import com.team3925.utils.MotionProfileCommand; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class LeftScaleAuto extends CommandGroup { + + public enum AutoSide { + LEFT, RIGHT + } + + public LeftScaleAuto() { + addParallel(new Command() { + + @Override + protected boolean isFinished() { + // TODO Auto-generated method stub + Intake.getInstance().setIntakeRollers(0.25); + return true; + } + }); + addParallel(new CloseGrabbers()); + addParallel(new RunElevator(ElevatorState.SCALE_MAX)); + addSequential(new MotionProfileCommand("LEFT_LEFTSCALE")); + addSequential(new RunIntakeWheels(-1)); + } + +} diff --git a/src/com/team3925/frc2018/subsystems/Elevator.java b/src/com/team3925/frc2018/subsystems/Elevator.java index 11e6b30..12c0fd2 100644 --- a/src/com/team3925/frc2018/subsystems/Elevator.java +++ b/src/com/team3925/frc2018/subsystems/Elevator.java @@ -12,29 +12,31 @@ public class Elevator extends Subsystem { - public enum ElevatorState{ + public enum ElevatorState { TOP, SCALE_MAX, SCALE_MED, SCALE_LOW, SWITCH, BOTTOM, UNKNOWN, OTHER; } - + private final TalonSRX elevatorMaster = RobotMap.ElevatorMap.MASTER; private static final double K_ELEVATOR = 1176.48; - + public static final double TELEOP_ELEVATOR_INCREMENT = 8; + + public static ElevatorState state = ElevatorState.UNKNOWN; - private static final double kP = 0.2; //0.015 | 0.2 + private static final double kP = 0.2; // 0.015 | 0.2 private static final double kI = 0; private static final double kD = 0; - private static final double kF = 0.35; //0.35 + private static final double kF = 0.35; // 0.35 private static final int MOTION_MAGIC_ACCELERATION = 9001; private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; -// private static final int MOTION_MAGIC_ACCELERATION = 9001; -// private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; -// private static final int MOTION_MAGIC_ACCELERATION = 20594; -// private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; - - private static final double MAX_SCALE_HEIGHT = (12 * 5.5) + 4.6667; + // private static final int MOTION_MAGIC_ACCELERATION = 9001; + // private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; + // private static final int MOTION_MAGIC_ACCELERATION = 20594; + // private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; + + private static final double MAX_SCALE_HEIGHT = 77000;//78900 public static double elevatorHeight = 0; private static Elevator instance; @@ -57,63 +59,90 @@ public Elevator() { elevatorMaster.config_kI(0, kI, Constants.TIMEOUT_MS); elevatorMaster.config_kD(0, kD, Constants.TIMEOUT_MS); elevatorMaster.config_kF(0, kF, Constants.TIMEOUT_MS); + + elevatorMaster.configForwardSoftLimitThreshold((int)MAX_SCALE_HEIGHT, Constants.TIMEOUT_MS); + elevatorMaster.configForwardSoftLimitEnable(true, Constants.TIMEOUT_MS); + elevatorMaster.configReverseSoftLimitThreshold(0, Constants.TIMEOUT_MS); + elevatorMaster.configReverseSoftLimitEnable(true, Constants.TIMEOUT_MS); elevatorMaster.configMotionAcceleration(MOTION_MAGIC_ACCELERATION, Constants.TIMEOUT_MS); elevatorMaster.configMotionCruiseVelocity(MOTION_MAGIC_CRUISE_VELOCITY, Constants.TIMEOUT_MS); elevatorMaster.overrideLimitSwitchesEnable(false); elevatorMaster.configContinuousCurrentLimit(200, Constants.TIMEOUT_MS); - + elevatorMaster.configNominalOutputForward(0, Constants.TIMEOUT_MS); elevatorMaster.configNominalOutputReverse(0, Constants.TIMEOUT_MS); RobotMap.ElevatorMap.SLAVE.configNominalOutputForward(0, Constants.TIMEOUT_MS); RobotMap.ElevatorMap.SLAVE.configNominalOutputReverse(0, Constants.TIMEOUT_MS); } - public void setRaw(double speed) { elevatorMaster.set(ControlMode.PercentOutput, speed); } - + public boolean isElevatorAtSetpoint() { return elevatorMaster.getClosedLoopError(0) < 100; } - public void setPosition(double inches){ -// elevatorMaster.set(ControlMode.Position, (inches * K_ELEVATOR)); - elevatorMaster.set(ControlMode.MotionMagic, (Math.min(inches, MAX_SCALE_HEIGHT) * K_ELEVATOR)); + public void setPosition(double inches) { + // elevatorMaster.set(ControlMode.Position, (inches * K_ELEVATOR)); +// elevatorMaster.set(ControlMode.MotionMagic, (Math.min(inches, MAX_SCALE_HEIGHT) * K_ELEVATOR)); + elevatorMaster.set(ControlMode.MotionMagic, inches); elevatorHeight = inches; } - - public void setPosition(ElevatorState state){ - switch(state) { + + public void setPosition(ElevatorState state) { + this.state = state; + // switch(state) { + // case TOP: + // setPosition((12 * 4.5) + 4.667); + // break; + // case SCALE_MAX: + // setPosition(12 * 4.5); + // break; + // case SCALE_MED: + // setPosition(12 * 4); + // break; + // case SCALE_LOW: + // setPosition(12 * 3.5); + // break; + // case BOTTOM: + // setPosition(0); + // break; + // case SWITCH: + // setPosition(12 * 2); + // break; + // } + switch (state) { case TOP: - setPosition((12 * 4.5) + 4.667); + setPosition(MAX_SCALE_HEIGHT); break; case SCALE_MAX: - setPosition(12 * 4.5); + setPosition(77428); break; case SCALE_MED: - setPosition(12 * 4); + setPosition(70000); //66301 break; case SCALE_LOW: - setPosition(12 * 3.5); + setPosition(54297); break; case BOTTOM: setPosition(0); break; case SWITCH: - setPosition(12 * 2); + setPosition(40000); break; } } + public double getElevatorHeightPercentage() { return elevatorHeight / MAX_SCALE_HEIGHT; } public void zero() { elevatorMaster.setSelectedSensorPosition(0, Constants.PID_ID_X, Constants.TIMEOUT_MS); -// setPosition(ElevatorState.BOTTOM); + // setPosition(ElevatorState.BOTTOM); } public boolean getLimitSwitch() { diff --git a/src/com/team3925/frc2018/subsystems/Intake.java b/src/com/team3925/frc2018/subsystems/Intake.java index cc74fd5..a8f0cb2 100644 --- a/src/com/team3925/frc2018/subsystems/Intake.java +++ b/src/com/team3925/frc2018/subsystems/Intake.java @@ -45,12 +45,11 @@ private Intake() { liftMotor.config_kD(0, 0, Constants.TIMEOUT_MS); liftMotor.config_kF(0, 0.35, Constants.TIMEOUT_MS); - liftMotor.configMotionAcceleration(300, Constants.TIMEOUT_MS); - liftMotor.configMotionCruiseVelocity(500, Constants.TIMEOUT_MS); + liftMotor.configMotionAcceleration(400, Constants.TIMEOUT_MS); + liftMotor.configMotionCruiseVelocity(600, Constants.TIMEOUT_MS); liftMotor.setInverted(true); liftMotor.setSensorPhase(true); liftMotor.overrideLimitSwitchesEnable(true); - liftMotor.configContinuousCurrentLimit(10, Constants.TIMEOUT_MS) leftIntake.setInverted(true); } @@ -60,7 +59,7 @@ private void setPosition(double revolutions) { public void setIntakeRollers(double speed) { if (Math.abs(speed) <= 0) - speed = 0.3; + speed = 0.25; leftIntake.set(ControlMode.PercentOutput, speed); rightIntake.set(ControlMode.PercentOutput, speed); } diff --git a/src/com/team3925/utils/MotionProfileCommand.java b/src/com/team3925/utils/MotionProfileCommand.java index fe2f48f..6224e91 100644 --- a/src/com/team3925/utils/MotionProfileCommand.java +++ b/src/com/team3925/utils/MotionProfileCommand.java @@ -1,126 +1,126 @@ -package com.team3925.utils; - -import java.io.File; - -import com.team3925.frc2018.subsystems.Drivetrain; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import jaci.pathfinder.Pathfinder; -import jaci.pathfinder.Trajectory; -import jaci.pathfinder.Waypoint; -import jaci.pathfinder.modifiers.TankModifier; - -/** - * - */ -public class MotionProfileCommand extends Command { - - TankModifier modifier; - - Trajectory trajectory; - Trajectory leftTra; - Trajectory rightTra; - - GnarlyController left; - GnarlyController right; - - Waypoint[] points; - - // TUNE THESE VALUES - public static double kP = 0.3; // 0.5 - public static double kV = 0.5; - public static double kA = 0.2; // 0.07 - public static double kG = 0.3; // 0.5 - - public static final double MAX_VEL = 12; - public static final double MAX_ACL = 24; - public static final double MAX_JERK = 200; - - public static final double WHEEL_BASE_WIDTH = (8.07 / Math.PI); - public static final int ENC_TPR = 1024; - public static final double WHEEL_DI = 0.505; - - public MotionProfileCommand(Waypoint[] points) { - Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, - Trajectory.Config.SAMPLES_HIGH, 0.05, MAX_VEL, MAX_ACL, MAX_JERK); - this.points = points; - trajectory = Pathfinder.generate(this.points, config); - modifier = new TankModifier(trajectory).modify(WHEEL_BASE_WIDTH); - - leftTra = modifier.getLeftTrajectory(); - rightTra = modifier.getRightTrajectory(); - requires(Drivetrain.getInstance()); - - } - - public MotionProfileCommand(String fileName) { - leftTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_left_detailed.csv")); - rightTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_right_detailed.csv")); - requires(Drivetrain.getInstance()); - } - - // Called just before this Command runs the first time - protected void initialize() { - - left = new GnarlyController(leftTra); - right = new GnarlyController(rightTra); - left.config((int) Drivetrain.getInstance().getLeftEncoderPosition(), ENC_TPR, WHEEL_DI, true, - Drivetrain.getInstance().getGyroHeading()); - right.config((int) Drivetrain.getInstance().getRightEncoderPosition(), ENC_TPR, WHEEL_DI, false, - Drivetrain.getInstance().getGyroHeading()); - - left.configurePIDVAG(kP, 0, 0, kV, kA, kG); - right.configurePIDVAG(kP, 0, 0, kV, kA, kG); - - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Drivetrain.getInstance().setVelocity( - left.calculate((int) Drivetrain.getInstance().getLeftEncoderPosition(), - Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getLeftSpeed()), - right.calculate((int) Drivetrain.getInstance().getRightEncoderPosition(), - Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getRightSpeed())); - logData(); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - if (left.isFinished() && right.isFinished()) { - System.out.println("Done"); - return true; - } else { - return false; - } - } - - // Called once after isFinished returns true - protected void end() { - Drivetrain.getInstance().setRaw(0, 0); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - this.end(); - } - - public void logData() { - SmartDashboard.putNumber("Left Position Predicted", left.logPosition()[0]); - SmartDashboard.putNumber("Left Position Actual", left.logPosition()[1]); - - SmartDashboard.putNumber("Left Velocity Predicted", left.logVelocity()[0]); - SmartDashboard.putNumber("Left Velocity Actual", left.logVelocity()[1]); - - SmartDashboard.putNumber("Right Position Predicted", right.logPosition()[0]); - SmartDashboard.putNumber("Right Position Actual", right.logPosition()[1]); - - SmartDashboard.putNumber("Right Velocity Predicted", right.logVelocity()[0]); - SmartDashboard.putNumber("Right Velocity Actual", right.logVelocity()[1]); - - SmartDashboard.putNumber("Heading Predicted", right.logAngle()[0]); - SmartDashboard.putNumber("Heading Actual", right.logAngle()[1]); - } - -} +package com.team3925.utils; + +import java.io.File; + +import com.team3925.frc2018.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import jaci.pathfinder.Pathfinder; +import jaci.pathfinder.Trajectory; +import jaci.pathfinder.Waypoint; +import jaci.pathfinder.modifiers.TankModifier; + +/** + * + */ +public class MotionProfileCommand extends Command { + + TankModifier modifier; + + Trajectory trajectory; + Trajectory leftTra; + Trajectory rightTra; + + GnarlyController left; + GnarlyController right; + + Waypoint[] points; + + // TUNE THESE VALUES + public static double kP = 0.3; // 0.5 + public static double kV = 0.5; + public static double kA = 0.3; // 0.07 + public static double kG = 0.5; // 0.5 + + public static final double MAX_VEL = 12; + public static final double MAX_ACL = 24; + public static final double MAX_JERK = 200; + + public static final double WHEEL_BASE_WIDTH = (8.07 / Math.PI); + public static final int ENC_TPR = 1024; + public static final double WHEEL_DI = 0.505; + + public MotionProfileCommand(Waypoint[] points) { + Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, + Trajectory.Config.SAMPLES_HIGH, 0.05, MAX_VEL, MAX_ACL, MAX_JERK); + this.points = points; + trajectory = Pathfinder.generate(this.points, config); + modifier = new TankModifier(trajectory).modify(WHEEL_BASE_WIDTH); + + leftTra = modifier.getLeftTrajectory(); + rightTra = modifier.getRightTrajectory(); + requires(Drivetrain.getInstance()); + + } + + public MotionProfileCommand(String fileName) { + leftTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_left_detailed.csv")); + rightTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_right_detailed.csv")); + requires(Drivetrain.getInstance()); + } + + // Called just before this Command runs the first time + protected void initialize() { + + left = new GnarlyController(leftTra); + right = new GnarlyController(rightTra); + left.config((int) Drivetrain.getInstance().getLeftEncoderPosition(), ENC_TPR, WHEEL_DI, true, + Drivetrain.getInstance().getGyroHeading()); + right.config((int) Drivetrain.getInstance().getRightEncoderPosition(), ENC_TPR, WHEEL_DI, false, + Drivetrain.getInstance().getGyroHeading()); + + left.configurePIDVAG(kP, 0, 0, kV, kA, kG); + right.configurePIDVAG(kP, 0, 0, kV, kA, kG); + + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Drivetrain.getInstance().setVelocity( + left.calculate((int) Drivetrain.getInstance().getLeftEncoderPosition(), + Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getLeftSpeed()), + right.calculate((int) Drivetrain.getInstance().getRightEncoderPosition(), + Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getRightSpeed())); + logData(); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + if (left.isFinished() && right.isFinished()) { + System.out.println("Done"); + return true; + } else { + return false; + } + } + + // Called once after isFinished returns true + protected void end() { + Drivetrain.getInstance().setRaw(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + this.end(); + } + + public void logData() { + SmartDashboard.putNumber("Left Position Predicted", left.logPosition()[0]); + SmartDashboard.putNumber("Left Position Actual", left.logPosition()[1]); + + SmartDashboard.putNumber("Left Velocity Predicted", left.logVelocity()[0]); + SmartDashboard.putNumber("Left Velocity Actual", left.logVelocity()[1]); + + SmartDashboard.putNumber("Right Position Predicted", right.logPosition()[0]); + SmartDashboard.putNumber("Right Position Actual", right.logPosition()[1]); + + SmartDashboard.putNumber("Right Velocity Predicted", right.logVelocity()[0]); + SmartDashboard.putNumber("Right Velocity Actual", right.logVelocity()[1]); + + SmartDashboard.putNumber("Heading Predicted", right.logAngle()[0]); + SmartDashboard.putNumber("Heading Actual", right.logAngle()[1]); + } + +}