diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/OI.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/OI.java index c09f19c..a4b738d 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/OI.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/OI.java @@ -5,8 +5,8 @@ import edu.wpi.first.wpilibj.buttons.JoystickButton; import org.techvalleyhigh.frc5881.deepspace.robot.commands.climber.ClimberLegsBackToggle; import org.techvalleyhigh.frc5881.deepspace.robot.commands.climber.ClimberLegsFrontToggle; -import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.ElevatorDown; -import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.ElevatorUp; +import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.LiftDown; +import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.LiftUp; import org.techvalleyhigh.frc5881.deepspace.robot.commands.groups.SwitchRobotMode; /** @@ -86,9 +86,9 @@ Right thumb stick drives climber (up and down) // When the "Y" button is pressed change the arm mode driveControllerButtonY.whenPressed(new SwitchRobotMode()); - // When the left/ right bumper is pressed lower or raise the elevator - driveControllerLeftBumper.whenPressed(new ElevatorDown()); - driveControllerRightBumper.whenPressed(new ElevatorUp()); + // When the left/ right bumper is pressed lower or raise the lift + driveControllerLeftBumper.whenPressed(new LiftDown()); + driveControllerRightBumper.whenPressed(new LiftUp()); // When the "Back" button is pressed toggle front climber driveControllerBackButton.whenPressed(new ClimberLegsFrontToggle()); diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/Robot.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/Robot.java index 3c904c6..119d987 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/Robot.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/Robot.java @@ -3,9 +3,10 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.ElevatorSave; +import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.LiftSave; import org.techvalleyhigh.frc5881.deepspace.robot.commands.drive.ArcadeDrive; import org.techvalleyhigh.frc5881.deepspace.robot.commands.drive.DriveSave; import org.techvalleyhigh.frc5881.deepspace.robot.subsystem.*; @@ -20,6 +21,7 @@ public class Robot extends TimedRobot { private static final String kDefaultAuto = "Default"; private static final String kCustomAuto = "My Auto"; + private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); @@ -30,7 +32,6 @@ public class Robot extends TimedRobot { public static DriveControl driveControl; public static Elevator elevator; public static Intake intake; - public static Arm arm; public static UpsideDown upsideDown; public static LED led; @@ -52,7 +53,6 @@ public void robotInit() { elevator = new Elevator(); intake = new Intake(); upsideDown = new UpsideDown(); - arm = new Arm(); /* OI must be constructed after subsystems. If the OI creates Commands @@ -64,8 +64,13 @@ constructed yet. Thus, their requires() statements may grab null driveCommand = new ArcadeDrive(); - SPI.Port port = SPI.Port.kOnboardCS0; - navX = new AHRS(port); + try { + SPI.Port port = SPI.Port.kMXP; + navX = new AHRS(port); + } catch (RuntimeException ex) { + System.err.println(ex); + } + m_chooser.setDefaultOption("Default Auto", kDefaultAuto); m_chooser.addOption("My Auto", kCustomAuto); @@ -82,13 +87,103 @@ constructed yet. Thus, their requires() statements may grab null */ @Override public void robotPeriodic() { + Scheduler.getInstance().run(); + SmartDashboard.putNumber("X accel", navX.getRawAccelX()); SmartDashboard.putNumber("Y accel", navX.getRawAccelY()); SmartDashboard.putNumber("Z accel", navX.getRawAccelZ()); - SmartDashboard.putNumber("X gyro", navX.getRawGyroX()); - SmartDashboard.putNumber("Y gyro", navX.getRawGyroY()); - SmartDashboard.putNumber("Z gyro", navX.getRawGyroZ()); + /* Display 6-axis Processed Angle Data */ + SmartDashboard.putBoolean("IMU_Connected", navX.isConnected()); + SmartDashboard.putBoolean("IMU_IsCalibrating", navX.isCalibrating()); + SmartDashboard.putNumber("IMU_Yaw", navX.getYaw()); + SmartDashboard.putNumber("IMU_Pitch", navX.getPitch()); + SmartDashboard.putNumber("IMU_Roll", navX.getRoll()); + + /* Display tilt-corrected, Magnetometer-based heading (requires */ + /* magnetometer calibration to be useful) */ + + SmartDashboard.putNumber("IMU_CompassHeading", navX.getCompassHeading()); + + /* Display 9-axis Heading (requires magnetometer calibration to be useful) */ + SmartDashboard.putNumber("IMU_FusedHeading", navX.getFusedHeading()); + + /* These functions are compatible w/the WPI Gyro Class, providing a simple */ + /* path for upgrading from the Kit-of-Parts gyro to the navx-MXP */ + + SmartDashboard.putNumber("IMU_TotalYaw", navX.getAngle()); + SmartDashboard.putNumber("IMU_YawRateDPS", navX.getRate()); + + /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) */ + + SmartDashboard.putNumber("IMU_Accel_X", navX.getWorldLinearAccelX()); + SmartDashboard.putNumber("IMU_Accel_Y", navX.getWorldLinearAccelY()); + SmartDashboard.putBoolean("IMU_IsMoving", navX.isMoving()); + SmartDashboard.putBoolean("IMU_IsRotating", navX.isRotating()); + + /* Display estimates of velocity/displacement. Note that these values are */ + /* not expected to be accurate enough for estimating robot position on a */ + /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding */ + /* of these errors due to single (velocity) integration and especially */ + /* double (displacement) integration. */ + + SmartDashboard.putNumber("Velocity_X", navX.getVelocityX()); + SmartDashboard.putNumber("Velocity_Y", navX.getVelocityY()); + SmartDashboard.putNumber("Displacement_X", navX.getDisplacementX()); + SmartDashboard.putNumber("Displacement_Y", navX.getDisplacementY()); + + /* Display Raw Gyro/Accelerometer/Magnetometer Values */ + /* NOTE: These values are not normally necessary, but are made available */ + /* for advanced users. Before using this data, please consider whether */ + /* the processed data (see above) will suit your needs. */ + + SmartDashboard.putNumber("RawGyro_X", navX.getRawGyroX()); + SmartDashboard.putNumber("RawGyro_Y", navX.getRawGyroY()); + SmartDashboard.putNumber("RawGyro_Z", navX.getRawGyroZ()); + SmartDashboard.putNumber("RawAccel_X", navX.getRawAccelX()); + SmartDashboard.putNumber("RawAccel_Y", navX.getRawAccelY()); + SmartDashboard.putNumber("RawAccel_Z", navX.getRawAccelZ()); + SmartDashboard.putNumber("RawMag_X", navX.getRawMagX()); + SmartDashboard.putNumber("RawMag_Y", navX.getRawMagY()); + SmartDashboard.putNumber("RawMag_Z", navX.getRawMagZ()); + SmartDashboard.putNumber("IMU_Temp_C", navX.getTempC()); + + /* Omnimount Yaw Axis Information */ + /* For more info, see http://navx-mxp.kauailabs.com/installation/omnimount */ + AHRS.BoardYawAxis yaw_axis = navX.getBoardYawAxis(); + SmartDashboard.putString("YawAxisDirection", yaw_axis.up ? "Up" : "Down"); + SmartDashboard.putNumber("YawAxis", yaw_axis.board_axis.getValue()); + + /* Sensor Board Information */ + SmartDashboard.putString("FirmwareVersion", navX.getFirmwareVersion()); + + /* Quaternion Data */ + /* Quaternions are fascinating, and are the most compact representation of */ + /* orientation data. All of the Yaw, Pitch and Roll Values can be derived */ + /* from the Quaternions. If interested in motion processing, knowledge of */ + /* Quaternions is highly recommended. */ + SmartDashboard.putNumber("QuaternionW", navX.getQuaternionW()); + SmartDashboard.putNumber("QuaternionX", navX.getQuaternionX()); + SmartDashboard.putNumber("QuaternionY", navX.getQuaternionY()); + SmartDashboard.putNumber("QuaternionZ", navX.getQuaternionZ()); + + /* Connectivity Debugging Support */ + SmartDashboard.putNumber("IMU_Byte_Count", navX.getByteCount()); + SmartDashboard.putNumber("IMU_Update_Count", navX.getUpdateCount()); + + // Puts the Elevator encoder position into Smart Dashboard + SmartDashboard.putNumber("Elevator Encoder", elevator.getElevatorEncoderPosition()); + // Puts the Elevator error value into the Smart Dashboard + SmartDashboard.putNumber("Elevator Error", elevator.getElevatorError()); + // Puts the Elevator set point value into the Smart Dashboard + SmartDashboard.putNumber("Elevator Set Point", elevator.getElevatorSetpoint()); + + // Puts the Bar encoder position into Smart Dashboard + SmartDashboard.putNumber("Bar Encoder", elevator.getBarEncoderPosition()); + // Puts the Bar error value into the Smart Dashboard + SmartDashboard.putNumber("Bar Error", elevator.getBarError()); + // Puts the Bar set point value into the Smart Dashboard + SmartDashboard.putNumber("Bar Set Point", elevator.getBarSetpoint()); } /** @@ -130,6 +225,8 @@ public void autonomousPeriodic() { */ @Override public void teleopInit() { + elevator.init(); + // Start the drive command driveCommand.start(); } @@ -146,8 +243,8 @@ public void teleopPeriodic() { } // If the bot is at an angle of greater than 30 degrees then do elevator save. if (navX.getRawGyroY() > 30) { - ElevatorSave elevatorSave = new ElevatorSave(); - elevatorSave.start(); + LiftSave liftSave = new LiftSave(); + liftSave.start(); } } diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmFlip.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmFlip.java deleted file mode 100644 index bc1f679..0000000 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmFlip.java +++ /dev/null @@ -1,54 +0,0 @@ -package org.techvalleyhigh.frc5881.deepspace.robot.commands.arm; - -import edu.wpi.first.wpilibj.command.Command; -import org.techvalleyhigh.frc5881.deepspace.robot.Robot; -import org.techvalleyhigh.frc5881.deepspace.robot.subsystem.Arm; - -/** - * Flips arm from cargo to hatch position or hatch to cargo - */ -public class ArmFlip extends Command { - public ArmFlip() { - requires(Robot.arm); - } - - /** - * Called just before this Command runs the first time - */ - @Override - protected void initialize() { - Robot.arm.flip(); - System.out.println("ArmFlip Command initialized"); - } - - /** - * Called repeatedly when this Command is scheduled to run - */ - @Override - protected void execute() { } - - /** - * Make this return true when this Command no longer needs to run execute() - */ - @Override - protected boolean isFinished() { return Math.abs(Robot.arm.getError()) < Arm.MAX_ERROR; } - - /** - * Called once after isFinished returns true OR the command is interrupted - */ - @Override - protected void end() { - System.out.println("ArmFlip ended"); - } - - /** - * Called when another command which requires one or more of the same - * subsystems is scheduled to run - */ - @Override - protected void interrupted() { - end(); - } -} - - diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmSetCargo.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmSetCargo.java deleted file mode 100644 index f0f1c1b..0000000 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmSetCargo.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.techvalleyhigh.frc5881.deepspace.robot.commands.arm; - -import edu.wpi.first.wpilibj.command.Command; -import org.techvalleyhigh.frc5881.deepspace.robot.Robot; -import org.techvalleyhigh.frc5881.deepspace.robot.subsystem.Arm; - -/** - * Sets arm to be positioned to pick up cargo - */ -public class ArmSetCargo extends Command { - public ArmSetCargo() { - requires(Robot.arm); - } - - /** - * Called just before this Command runs the first time - */ - @Override - protected void initialize() { - Robot.arm.setToCargoTicks(); - System.out.println("ArmSetCargo Command initialized"); - } - - /** - * Called repeatedly when this Command is scheduled to run - */ - @Override - protected void execute() { } - - /** - * Make this return true when this Command no longer needs to run execute() - */ - @Override - protected boolean isFinished() { return Math.abs(Robot.arm.getError()) < Arm.MAX_ERROR; } - - /** - * Called once after isFinished returns true OR the command is interrupted - */ - @Override - protected void end() { - System.out.println("ArmSetCargo ended"); - } - - /** - * Called when another command which requires one or more of the same - * subsystems is scheduled to run - */ - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmSetHatch.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmSetHatch.java deleted file mode 100644 index 9db0768..0000000 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/arm/ArmSetHatch.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.techvalleyhigh.frc5881.deepspace.robot.commands.arm; - -import edu.wpi.first.wpilibj.command.Command; -import org.techvalleyhigh.frc5881.deepspace.robot.Robot; -import org.techvalleyhigh.frc5881.deepspace.robot.subsystem.Arm; - -/** - * Sets arm to be positioned to pick up hatches - */ -public class ArmSetHatch extends Command { - public ArmSetHatch() { - requires(Robot.arm); - } - - /** - * Called just before this Command runs the first time - */ - @Override - protected void initialize() { - Robot.arm.setToHatchTicks(); - System.out.println("ArmSetHatch Command initialized"); - } - - /** - * Called repeatedly when this Command is scheduled to run - */ - @Override - protected void execute() { } - - /** - * Make this return true when this Command no longer needs to run execute() - */ - @Override - protected boolean isFinished() { return Math.abs(Robot.arm.getError()) < Arm.MAX_ERROR; } - - /** - * Called once after isFinished returns true OR the command is interrupted - */ - @Override - protected void end() { - System.out.println("ArmSetHatch ended"); - } - - /** - * Called when another command which requires one or more of the same - * subsystems is scheduled to run - */ - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/Dock.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/Dock.java new file mode 100644 index 0000000..cd4536a --- /dev/null +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/Dock.java @@ -0,0 +1,18 @@ +package org.techvalleyhigh.frc5881.deepspace.robot.commands.auto; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import org.techvalleyhigh.frc5881.deepspace.robot.utils.VisionUtil; + +public class Dock extends CommandGroup { + /** + * Creates a new {@link CommandGroup CommandGroup}. The name of this command will be set to its + * class name. + */ + public Dock() { + double turnAngle = VisionUtil.getSeparationAngle(); + addSequential(new Turn(turnAngle)); + // Turn to face tape + + // Run motion profile + } +} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/DockCargo.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/DockCargo.java deleted file mode 100644 index f432e2b..0000000 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/DockCargo.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.techvalleyhigh.frc5881.deepspace.robot.commands.auto; - -public class DockCargo { -} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/DockHatch.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/DockHatch.java deleted file mode 100644 index 77cb26a..0000000 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/DockHatch.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.techvalleyhigh.frc5881.deepspace.robot.commands.auto; - -public class DockHatch { -} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/Turn.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/Turn.java new file mode 100644 index 0000000..f214402 --- /dev/null +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/auto/Turn.java @@ -0,0 +1,76 @@ +package org.techvalleyhigh.frc5881.deepspace.robot.commands.auto; + +import edu.wpi.first.wpilibj.command.Command; +import org.techvalleyhigh.frc5881.deepspace.robot.Robot; + +public class Turn extends Command { + private double absoluteAngle; + + /** + * Creates a new command. The name of this command will be set to its class name. + */ + public Turn(double relativeAngle) { + requires(Robot.driveControl); + absoluteAngle = Robot.navX.getYaw() + relativeAngle; + } + + /** + * The initialize method is called the first time this Command is run after being started. + */ + @Override + protected void initialize() { + Robot.driveControl.setGyroSetpoint(absoluteAngle); + } + + /** + * The execute method is called repeatedly until this Command either finishes or is canceled. + */ + @Override + protected void execute() { + Robot.driveControl.rawArcadeDrive(0, Robot.driveControl.getGyroPIDoutput()); + } + + /** + * Called when the command ends because somebody called {@link Command#cancel() cancel()} or + * another command shared the same requirements as this one, and booted it out. + * + *

This is where you may want to wrap up loose ends, like shutting off a motor that was being + * used in the command. + * + *

Generally, it is useful to simply call the {@link Command#end() end()} method within this + * method, as done here. + */ + @Override + protected void interrupted() { + System.out.println("Turn to " + absoluteAngle + "command interrupted... That shouldn't happen"); + } + + /** + * Called when the command ended peacefully. This is where you may want to wrap up loose ends, + * like shutting off a motor that was being used in the command. + */ + @Override + protected void end() { + Robot.driveControl.stop(); + } + + /** + * Returns whether this command is finished. If it is, then the command will be removed and {@link + * Command#end() end()} will be called. + * + *

It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()} + * method for time-sensitive commands. + * + *

Returning false will result in the command never ending automatically. It may still be + * cancelled manually or interrupted by another command. Returning true will result in the + * command executing once and finishing immediately. We recommend using {@link InstantCommand} + * for this. + * + * @return whether this command is finished. + * @see Command#isTimedOut() isTimedOut() + */ + @Override + protected boolean isFinished() { + return Robot.driveControl.getGyroOnTarget(); + } +} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorFlip.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorFlip.java index e9903ce..f287ff9 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorFlip.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorFlip.java @@ -4,7 +4,7 @@ import org.techvalleyhigh.frc5881.deepspace.robot.Robot; /** - * Changes the elevator height + * Changes the lifts mode accordingly to the expected game piece to be receiving */ public class ElevatorFlip extends Command { @@ -18,18 +18,16 @@ public ElevatorFlip() { @Override protected void initialize() { System.out.println("Elevator flip initialized"); + Robot.elevator.elevatorFlip(); } @Override protected void execute() { - if(!Robot.elevator.isFired){ - Robot.elevator.elevatorFlip(); - } } @Override protected boolean isFinished() { - return Robot.elevator.isSetpointReached(); + return Robot.elevator.isElevatorSetpointReached(); } /** diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorDown.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftDown.java similarity index 90% rename from src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorDown.java rename to src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftDown.java index 6ab22e7..abd195a 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorDown.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftDown.java @@ -7,9 +7,8 @@ * When executed moves the elevator down to the next lowest level * If it is at the lowest possible level it will do nothing */ -public class ElevatorDown extends Command { - - public ElevatorDown() { +public class LiftDown extends Command { + public LiftDown() { requires(Robot.elevator); } @@ -19,6 +18,7 @@ public ElevatorDown() { @Override protected void initialize() { System.out.println("Elevator down initialized"); + Robot.elevator.elevatorDown(); } /** @@ -26,12 +26,11 @@ protected void initialize() { */ @Override protected void execute() { - Robot.elevator.elevatorDown(); } @Override protected boolean isFinished() { - return Robot.elevator.isSetpointReached(); + return Robot.elevator.isElevatorSetpointReached(); } /** diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorSave.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftSave.java similarity index 91% rename from src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorSave.java rename to src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftSave.java index c8f9ab0..e6ab26a 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorSave.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftSave.java @@ -6,8 +6,8 @@ /** * "Saves" the bot from tipping by lowering the elevator when we are tipping and not climbing */ -public class ElevatorSave extends Command { - public ElevatorSave() { +public class LiftSave extends Command { + public LiftSave() { requires(Robot.elevator); } @@ -33,7 +33,7 @@ protected void execute() { */ @Override protected boolean isFinished() { - return Robot.elevator.isSetpointReached(); + return Robot.navX.getRawGyroY() < 30; } /** diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorUp.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftUp.java similarity index 88% rename from src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorUp.java rename to src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftUp.java index 27e4c90..0b9cac2 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/ElevatorUp.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/LiftUp.java @@ -6,8 +6,8 @@ /** * Makes the elevator go up */ -public class ElevatorUp extends Command { - public ElevatorUp() { +public class LiftUp extends Command { + public LiftUp() { requires(Robot.elevator); } @@ -17,6 +17,7 @@ public ElevatorUp() { @Override protected void initialize() { System.out.println("Elevator up initialized"); + Robot.elevator.elevatorUp(); } /** @@ -24,7 +25,6 @@ protected void initialize() { */ @Override protected void execute() { - Robot.elevator.elevatorUp(); } /** @@ -33,7 +33,7 @@ protected void execute() { */ @Override protected boolean isFinished() { - return Robot.elevator.isSetpointReached(); + return Robot.elevator.isElevatorSetpointReached() == Robot.elevator.isBarSetpointReached(); } /** diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/SetElevator.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/SetLift.java similarity index 86% rename from src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/SetElevator.java rename to src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/SetLift.java index 5f7a57d..f8818b8 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/SetElevator.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/elevator/SetLift.java @@ -7,10 +7,10 @@ /** * Sets the height of the elevator */ -public class SetElevator extends Command { +public class SetLift extends Command { private Elevator.ElevatorState target; - public SetElevator(Elevator.ElevatorState state) { + public SetLift(Elevator.ElevatorState state) { target = state; requires(Robot.elevator); @@ -22,6 +22,7 @@ public SetElevator(Elevator.ElevatorState state) { @Override protected void initialize() { System.out.println("Set elevator initialized"); + Robot.elevator.setLiftSetpoint(target); } /** @@ -29,7 +30,6 @@ protected void initialize() { */ @Override protected void execute() { - Robot.elevator.setElevator(target); } /** @@ -38,7 +38,7 @@ protected void execute() { */ @Override protected boolean isFinished() { - return Robot.elevator.isSetpointReached(); + return Robot.elevator.isElevatorSetpointReached(); } /** diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/groups/SwitchRobotMode.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/groups/SwitchRobotMode.java index c2cf7f3..dd11c0b 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/groups/SwitchRobotMode.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/commands/groups/SwitchRobotMode.java @@ -1,15 +1,13 @@ package org.techvalleyhigh.frc5881.deepspace.robot.commands.groups; import edu.wpi.first.wpilibj.command.CommandGroup; -import org.techvalleyhigh.frc5881.deepspace.robot.commands.arm.ArmFlip; import org.techvalleyhigh.frc5881.deepspace.robot.commands.elevator.ElevatorFlip; /** - * Will change the mode of the arm. + * Will change the mode of the lift. */ public class SwitchRobotMode extends CommandGroup { public SwitchRobotMode() { - addParallel(new ArmFlip()); addParallel(new ElevatorFlip()); } } \ No newline at end of file diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Arm.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Arm.java deleted file mode 100644 index 4eddf31..0000000 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Arm.java +++ /dev/null @@ -1,134 +0,0 @@ -package org.techvalleyhigh.frc5881.deepspace.robot.subsystem; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; - - -/** - * Subsystem that has everything to do with the arm - */ -public class Arm extends Subsystem { - - //Define motor - private static WPI_TalonSRX armMotor = new WPI_TalonSRX(5); - - //The maximum and minimum numbers of ticks for how far the arm can move - private static final int MAX_TICKS = 0; - private static final int MIN_TICKS = 200; - - //TODO: Find cargo and hatch ticks - public static final int CARGO_TICKS = 10; - public static final int HATCH_TICKS = 15; - - public static final int MAX_ERROR = 50; - - - @Override - protected void initDefaultCommand() { - } - - /**Create the subsystem with a default name*/ - public Arm (){ - super (); - init (); - } - -/** - * Initialize SmartDashboard and other local variables - */ - public void init() { - armMotor.setName("Arm", "Motor"); - armMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); - armMotor.setNeutralMode(NeutralMode.Brake); - LiveWindow.add(armMotor); - } - - /** - * Set the setPoint to stay in between the minimum and maximum amount of ticks - * If setPoint is less than minTicks, it will be equal to minTicks - * If setPoint is more than maxTicks, it will be equal to maxTicks - */ - public void setArmMotor (double setPoint){ - if (setPoint < MIN_TICKS) { - setPoint = MIN_TICKS; - } - if (setPoint > MAX_TICKS) { - setPoint = MAX_TICKS; - } - - armMotor.set(ControlMode.Position, setPoint); - } - - /** - * set arm to target cargo - */ - public void setToCargoTicks(){ - setArmMotor(CARGO_TICKS); - } - - /** - * set arm to target hatch - */ - public void setToHatchTicks(){ - setArmMotor(HATCH_TICKS); - } - - /** - * flip arm setpoint from cargo to hatch and vice versa - */ - public void flip() { - if(getSetPoint() == HATCH_TICKS) { - setToCargoTicks(); - } else if(getSetPoint() == CARGO_TICKS){ - setToHatchTicks(); - } else { - // Default to cargo - setToCargoTicks(); - } - } - - /** - * identifies if arm is in cargo mode - * @return true if cargo, false otherwise - */ - public boolean isCargo() { - return getSetPoint() == CARGO_TICKS; - } - - /** - * identifies if arm is in hatch mode - * @return true if cargo, false otherwise - */ - public boolean isHatch() { - return getSetPoint() == HATCH_TICKS; - } - - /** - * Gets current setpoint of the arm motor - * @return double setpoint - */ - public double getSetPoint() { - return armMotor.getClosedLoopTarget(); - } - - /** - * Gets how far the arm is from the setpoint - * @return double error - */ - public double getError() { - return armMotor.getClosedLoopError(); - } - - /** - * Gets the position of the arm - * @return double position - */ - public double getPosition() { - return armMotor.getSelectedSensorPosition(); - } - -} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Climber.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Climber.java index 0032091..b9646c4 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Climber.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Climber.java @@ -18,8 +18,8 @@ public class Climber extends Subsystem { public static DoubleSolenoid frontSolenoid = new DoubleSolenoid(20, 2, 3); public static DoubleSolenoid backSolenoid = new DoubleSolenoid(20, 4, 5); - public static WPI_TalonSRX leftMotor = new WPI_TalonSRX(5); - public static WPI_TalonSRX rightMotor = new WPI_TalonSRX(6); + //public static WPI_TalonSRX leftMotor = new WPI_TalonSRX(16); + //public static WPI_TalonSRX rightMotor = new WPI_TalonSRX(17); // Differential drive to handle arcade drive private DifferentialDrive climberDriveBase; @@ -56,13 +56,13 @@ public Climber(){ } public void init() { - leftMotor.setName("Climber", "Left Motor"); - leftMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - LiveWindow.add(leftMotor); + //leftMotor.setName("Climber", "Left Motor"); + //leftMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + //LiveWindow.add(leftMotor); - rightMotor.setName("Climber", "Right Motor"); - rightMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - LiveWindow.add(rightMotor); + //rightMotor.setName("Climber", "Right Motor"); + //rightMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + //LiveWindow.add(rightMotor); frontSolenoid.setName ("Climber", "Front Piston"); LiveWindow.add(frontSolenoid); @@ -70,9 +70,9 @@ public void init() { backSolenoid.setName("Climber", "Back Piston"); LiveWindow.add(backSolenoid); - SpeedControllerGroup m_left = new SpeedControllerGroup(leftMotor); - SpeedControllerGroup m_right = new SpeedControllerGroup(rightMotor); - climberDriveBase = new DifferentialDrive(m_right, m_left); + //SpeedControllerGroup m_left = new SpeedControllerGroup(leftMotor); + //SpeedControllerGroup m_right = new SpeedControllerGroup(rightMotor); + //climberDriveBase = new DifferentialDrive(m_right, m_left); } /** diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/DriveControl.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/DriveControl.java index ea8cf21..fad4e14 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/DriveControl.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/DriveControl.java @@ -3,12 +3,15 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.command.PIDCommand; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.techvalleyhigh.frc5881.deepspace.robot.OI; import org.techvalleyhigh.frc5881.deepspace.robot.Robot; +import org.techvalleyhigh.frc5881.deepspace.robot.utils.GyroPIDOutput; /** * Subsystem to control everything that has to do with driving, except motion profiling @@ -20,14 +23,22 @@ public class DriveControl extends Subsystem { public static WPI_TalonSRX backLeftMotor = new WPI_TalonSRX(3); public static WPI_TalonSRX backRightMotor = new WPI_TalonSRX(4); + // Speed to target when we start tipping + public static final double TIPPING_SPEED = 0.5; + // Define robot drive for controls private DifferentialDrive robotDrive; + // Gyro PID for turning + private PIDController gyroPID; + private GyroPIDOutput gyroPIDoutput = new GyroPIDOutput(); + /** * Starts a command on init of subsystem, defining commands in robot and OI is preferred */ @Override - protected void initDefaultCommand() {} + protected void initDefaultCommand() { + } /** * Create the subsystem with a default name @@ -35,6 +46,20 @@ protected void initDefaultCommand() {} public DriveControl() { super(); + // Put numbers on SmartDashboard + SmartDashboard.putNumber("left kP", 2); + SmartDashboard.putNumber("left kI", 0); + SmartDashboard.putNumber("left kD", 20); + SmartDashboard.putNumber("left kF", 0.076); + SmartDashboard.putNumber("right kP", 2); + SmartDashboard.putNumber("right kI", 0); + SmartDashboard.putNumber("right kD", 20); + SmartDashboard.putNumber("right kF", 0.076); + + SmartDashboard.putNumber("gyro kP", 0); + SmartDashboard.putNumber("gyro kI", 0); + SmartDashboard.putNumber("gyro kD", 0); + init(); } @@ -44,10 +69,18 @@ public DriveControl() { public void init() { frontLeftMotor.setName("Drive", "Front Left"); frontLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + frontLeftMotor.config_kP(0, getLeft_kP(), 10); + frontLeftMotor.config_kI(0, getLeft_kI(), 10); + frontLeftMotor.config_kD(0, getLeft_kD(), 10); + frontLeftMotor.config_kF(0, getLeft_kF(), 10); LiveWindow.add(frontLeftMotor); frontRightMotor.setName("Drive", "Front Right"); frontRightMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + frontRightMotor.config_kP(0, getRight_kP(), 10); + frontRightMotor.config_kI(0, getRight_kI(), 10); + frontRightMotor.config_kD(0, getRight_kD(), 10); + frontRightMotor.config_kF(0, getRight_kF(), 10); LiveWindow.add(frontRightMotor); backLeftMotor.setName("Drive", "Back Left"); @@ -61,34 +94,99 @@ public void init() { SpeedControllerGroup m_left = new SpeedControllerGroup(frontLeftMotor); SpeedControllerGroup m_right = new SpeedControllerGroup(frontRightMotor); robotDrive = new DifferentialDrive(m_right, m_left); + + robotDrive.setName("Drive"); + LiveWindow.add(robotDrive); + + gyroPID = new PIDController(getGyro_kP(), getGyro_kI(), getGyro_kD(), Robot.navX, gyroPIDoutput); + gyroPID.setOutputRange(-1, 1); } /** * Pass raw values to arcade drive, don't pass joystick inputs directly + * * @param speed Drive speed -1 backwards -> 1 forward - * @param turn Turn rate -1 left -> 1 right + * @param turn Turn rate -1 left -> 1 right */ - public void rawArcadeDrive(double speed, double turn){ + public void rawArcadeDrive(double speed, double turn) { robotDrive.arcadeDrive(speed, turn, true); } /** * Implements arcade drive with joystick inputs */ - public void arcadeJoystickInputs (){ + @SuppressWarnings("Duplicates") + public void arcadeJoystickInputs() { double speed = Robot.oi.driverController.getRawAxis(OI.XBOX_LEFT_Y_AXIS); double turn = Robot.oi.driverController.getRawAxis(OI.XBOX_RIGHT_X_AXIS); - if(Math.abs(turn) < 0.1) { + if (Math.abs(turn) < 0.1) { turn = 0; } - if(Math.abs(speed) < 0.1) { + if (Math.abs(speed) < 0.1) { speed = 0; } rawArcadeDrive(turn, speed); } - public static final double TIPPING_SPEED = 0.5; + public void stop() { + robotDrive.stopMotor(); + } + + public void setGyroSetpoint(double setpoint) { + gyroPID.setSetpoint(setpoint); + } + + public double getGyroSetpoint() { + return gyroPID.getSetpoint(); + } + + public double getGyroError() { + return gyroPID.getError(); + } + public boolean getGyroOnTarget() { + return gyroPID.onTarget(); + } + + public double getGyroPIDoutput() { + return gyroPIDoutput.getOutput(); + } + + public double getLeft_kP() { + return SmartDashboard.getNumber("left kP", 2.0); + } + public double getLeft_kI(){ + return SmartDashboard.getNumber("left kI", 2.0); + } + public double getLeft_kD(){ + return SmartDashboard.getNumber("left kD",2.0); + } + public double getLeft_kF(){ + return SmartDashboard.getNumber("left kF",2.0); + } + public double getRight_kP() { + return SmartDashboard.getNumber("right kP", 2.0); + } + public double getRight_kI(){ + return SmartDashboard.getNumber("right kI", 2.0); + } + public double getRight_kD(){ + return SmartDashboard.getNumber("right kD",2.0); + } + public double getRight_kF(){ + return SmartDashboard.getNumber("right kF",2.0); + } + + public double getGyro_kP() { + return SmartDashboard.getNumber("gyro kP", 0.0); + } + public double getGyro_kI() { + return SmartDashboard.getNumber("gyro kI", 0.0); + } + public double getGyro_kD() { + return SmartDashboard.getNumber("gyro kD", 0.0); + } } + diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Elevator.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Elevator.java index 7ae5373..c7b87fc 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Elevator.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/subsystem/Elevator.java @@ -2,393 +2,393 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.techvalleyhigh.frc5881.deepspace.robot.Robot; + +import javax.naming.ldap.Control; +import java.util.Arrays; /** - * Contains methods to get the error of the motors, set the PIDs and even move the elevator and lift! + * Contains methods to get the error of the motors, set the PIDs and even move the elevator and bar! */ public class Elevator extends Subsystem { + private ElevatorState elevatorState = ElevatorState.FLOOR; + private LiftMode liftMode = LiftMode.HATCH; - public boolean isFired = false; - - public ElevatorState elevatorState = ElevatorState.HIGH_HATCH; - - // TODO: Change the "deviceNumber" to whatever the actual number on the talon is. - private WPI_TalonSRX elevatorMasterMotor = new WPI_TalonSRX(2); - // || || - // \/ is the talon for the four bar lift motor \/ - private WPI_TalonSRX liftMasterMotor = new WPI_TalonSRX(3); - - /* - The order of heights is: (greatest to least) - - 1. High cargo - - 2. High hatch - - 3. Middle cargo - - 4. Middle hatch + // TODO: Change the "deviceNumber" to whatever the actual number on the talon is. + private WPI_TalonSRX elevatorMasterMotor = new WPI_TalonSRX(5); + // \/ is the talon for the four bar lift motor \/ + private WPI_TalonSRX barMasterMotor = new WPI_TalonSRX(3); - 5. Low cargo - - 6. Low hatch - */ + /** + * Is the mode of the lift + */ + public enum LiftMode { + HATCH, + CARGO + } /** - * Is the state of the elevator + * Is the state of the elevator and bar */ public enum ElevatorState { - FLOOR, - - LOW_HATCH, - - LOW_CARGO, - - MIDDLE_HATCH, - - MIDDLE_CARGO, + FLOOR(0, 0), + LOW_HATCH(125, 50), + START(100, 100), + LOW_CARGO( 20, 150), + MIDDLE_HATCH(125, 1000), + MIDDLE_CARGO(20, 1000), + HIGH_HATCH(125, 8000), + HIGH_CARGO(20, 10000), + TOP(17640, 11000); + + private double barPos, elevatorPos; + + ElevatorState(double bar, double elevator) { + barPos = bar; + elevatorPos = elevator; + } - HIGH_HATCH, + public double getElevatorPosition() { + return elevatorPos; + } - HIGH_CARGO + public double getBarPosition() { + return barPos; + } } // TODO: We need to figure out what the actual heights of these things will be. /** * For all of the following double[]'s the first double value is the required height for the elevator to move - * and the second value is the height that the lift is required to move. + * and the second value is the ticks that the bar is required turn. */ - public static final double[] Floor = {0, 0}; - - public static final double[] Low_Hatch = {5, 5}; - - public static final double[] Low_Cargo = {5 , 5}; - - public static final double[] Mid_Hatch = {5, 5}; - - public static final double[] Mid_Cargo = {5, 5}; - - public static final double[] High_Hatch = {5, 5}; - - public static final double[] High_Cargo = {5, 5}; - - public static final double[] Top = {5, 5}; +// private static final double[] FLOOR = {0, 0}; +// // TODO: Figure out how we want the elevator and bar to start +// private static final double[] START = {1000, 0}; +// private static final double[] LOW_HATCH = {2000, 1000}; +// private static final double[] LOW_CARGO = {4000 , 2000}; +// private static final double[] MIDDLE_HATCH = {6000, 3000}; +// private static final double[] MIDDLE_CARGO = {8000, 4000}; +// private static final double[] HIGH_HATCH = {10000, 5000}; +// private static final double[] HIGH_CARGO = {12000, 6000}; +// private static final double[] TOP = {14000, 7000}; + + private static final double BAR_ALLOWED_ERROR = 50; + private static final double ELEVATOR_ALLOWED_ERROR = 50; /** * Does the normal stuff but also adds the PID values to Smart Dashboard. */ public Elevator() { - super(); - // Put numbers on SmartDashboard - SmartDashboard.putNumber("elevator kP", 2); - SmartDashboard.putNumber("elevator kI", 0); - SmartDashboard.putNumber("elevator kD", 20); - SmartDashboard.putNumber("elevator kF", 0.076); - SmartDashboard.putNumber("lift kP", 2); - SmartDashboard.putNumber("lift kI", 0); - SmartDashboard.putNumber("lift kD", 20); - SmartDashboard.putNumber("lift kF", 0.076); - init(); - } + super(); + // Puts Elevator PID values into Smart Dashboard + SmartDashboard.putNumber("Elevator kP", 2); + SmartDashboard.putNumber("Elevator kI", 0); + SmartDashboard.putNumber("Elevator kD", 20); + SmartDashboard.putNumber("Elevator kF", 0.076); + + // Puts Bar PID values into Smart Dashboard + SmartDashboard.putNumber("Bar kP", 2); + SmartDashboard.putNumber("Bar kI", 0); + SmartDashboard.putNumber("Bar kD", 20); + SmartDashboard.putNumber("Bar kF", 0.076); + + init(); + } /** * Adds the encoder to the motor/ Talon * Also sets the PID values */ - private void init(){ - liftMasterMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - elevatorMasterMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - - - elevatorMasterMotor.config_kP(0, getElevator_kP(), 10); - elevatorMasterMotor.config_kI(0, getElevator_kI(), 10); - elevatorMasterMotor.config_kD(0, getElevator_kD(), 10); - elevatorMasterMotor.config_kF(0, getElevator_kF(), 10); - - liftMasterMotor.config_kP(0, getLift_kP(), 10); - liftMasterMotor.config_kI(0, getLift_kI(), 10); - liftMasterMotor.config_kD(0, getLift_kD(), 10); - liftMasterMotor.config_kF(0, getLift_kF(), 10); - } - - @Override - protected void initDefaultCommand() { + public void init(){ + barMasterMotor.set(ControlMode.Position, 0); + elevatorMasterMotor.set(ControlMode.Position, 0); + barMasterMotor.setSelectedSensorPosition(0); + elevatorMasterMotor.setSelectedSensorPosition(0); + + barMasterMotor.setInverted(true); + barMasterMotor.setSensorPhase(true); + elevatorMasterMotor.setInverted(false); + + elevatorMasterMotor.setNeutralMode(NeutralMode.Coast); + barMasterMotor.setNeutralMode(NeutralMode.Coast); + + // Configures an encoder for the bar motor and elevator motor + elevatorMasterMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + barMasterMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + + // Configures the elevators PID values + elevatorMasterMotor.config_kP(0, getElevator_kP(), 10); + elevatorMasterMotor.config_kI(0, getElevator_kI(), 10); + elevatorMasterMotor.config_kD(0, getElevator_kD(), 10); + elevatorMasterMotor.config_kF(0, getElevator_kF(), 10); + + // TODO: Find out what the max amperage should be + elevatorMasterMotor.configPeakCurrentLimit(24); + elevatorMasterMotor.configPeakCurrentDuration(2000); + + // Configures the bar's PID values + barMasterMotor.config_kP(0, getBar_kP(), 10); + barMasterMotor.config_kI(0, getBar_kI(), 10); + barMasterMotor.config_kD(0, getBar_kD(), 10); + barMasterMotor.config_kF(0, getBar_kF(), 10); + + barMasterMotor.configPeakCurrentLimit(24); + barMasterMotor.configPeakCurrentDuration(2000); + } - } + @Override + protected void initDefaultCommand() { + } /** * Moves the elevator up to the next possible level. */ - public void elevatorUp(){ - // Checks if the arm mode is "hatch" - if(Robot.arm.isHatch()){ - // Checks to see if the elevator state is "Floor" - if(ElevatorState.FLOOR.equals(elevatorState)){ - setElevator(Low_Hatch); - } else if (ElevatorState.LOW_HATCH.equals(elevatorState)) { - setElevator(Mid_Hatch); - // Checks if the elevator state is "middle hatch" - } else if(ElevatorState.MIDDLE_HATCH.equals(elevatorState)){ - setElevator(High_Hatch); - } - // Checks if the arm mode is "cargo" - } else if(Robot.arm.isCargo()){ - // If the elevator state is "low cargo" then proceed - if(ElevatorState.LOW_CARGO.equals(elevatorState)){ - setElevator(Mid_Cargo); - // Checks if the elevator state is "middle cargo" - } else if(ElevatorState.MIDDLE_CARGO.equals(elevatorState)){ - setElevator(High_Cargo); - } - } + public void elevatorUp() { + switch(elevatorState) { + case FLOOR: + if (isHatch()) { + System.out.println("Going to low hatch (up)"); + setLiftSetpoint(ElevatorState.LOW_HATCH); + } else { + System.out.println("Going to low cargo (up)"); + setLiftSetpoint(ElevatorState.LOW_CARGO); + } + break; + case LOW_HATCH: + System.out.println("Going to middle hatch (up)"); + setLiftSetpoint(ElevatorState.MIDDLE_HATCH); + break; + case LOW_CARGO: + System.out.println("Going to middle cargo (up)"); + setLiftSetpoint(ElevatorState.MIDDLE_CARGO); + break; + case MIDDLE_HATCH: + System.out.println("Going to high hatch (up)"); + setLiftSetpoint(ElevatorState.HIGH_HATCH); + break; + case MIDDLE_CARGO: + System.out.println("Going to high cargo (up)"); + setLiftSetpoint(ElevatorState.HIGH_CARGO); + break; } + } /** * Moves the elevator down to the next possible state. */ - public void elevatorDown(){ - // Check if the arm mode is "hatch" - if (Robot.arm.isHatch()) { - // If the elevator's height is "high hatch" then proceed - if (ElevatorState.HIGH_HATCH.equals(elevatorState)) { - setElevator(Mid_Hatch); - // Check if the elevator state is middle hatch - } else if (ElevatorState.MIDDLE_HATCH.equals(elevatorState)) { - setElevator(Low_Hatch); - // Checks if the elevator's state is "low hatch" - } else if (ElevatorState.LOW_HATCH.equals(elevatorState)) { - setElevator(Floor); - } - // Checks if the arm mode is "cargo" - } else if (Robot.arm.isCargo()){ - // Checks if the elevator state is "high cargo" - if(ElevatorState.HIGH_CARGO.equals(elevatorState)) { - setElevator(Mid_Cargo); - // Checks if the elevator state is "middle cargo" - } else if(ElevatorState.MIDDLE_CARGO.equals(elevatorState)){ - setElevator(Low_Cargo); - // Checks if elevator state is "low cargo" - } else if(ElevatorState.LOW_CARGO.equals(elevatorState)){ - setElevator(Floor); - } + @SuppressWarnings("Duplicates") + public void elevatorDown() { + switch (elevatorState) { + case LOW_HATCH: + System.out.println("Going to floor (down)"); + setLiftSetpoint(ElevatorState.FLOOR); + break; + case LOW_CARGO: + System.out.println("Going to floor (down)"); + setLiftSetpoint(ElevatorState.FLOOR); + break; + case MIDDLE_HATCH: + System.out.println("Going to low hatch (down)"); + setLiftSetpoint(ElevatorState.LOW_HATCH); + break; + case MIDDLE_CARGO: + System.out.println("Going to low cargo (down)"); + setLiftSetpoint(ElevatorState.LOW_CARGO); + break; + case HIGH_HATCH: + System.out.println("Going to middle hatch (down)"); + setLiftSetpoint(ElevatorState.MIDDLE_HATCH); + break; + case HIGH_CARGO: + System.out.println("Going to middle cargo (down)"); + setLiftSetpoint(ElevatorState.MIDDLE_CARGO); + break; } } /** - * Sets the elevator and lift height to the lowest setting as to prevent a high center of mass - */ - public void saveElevator(){ - setElevator(Floor); - } - - /** - * Gets the total target height of the elevator and lift together - * @return Returns the sum of the elevator target height and the lift target height to get the total desired height - */ - public double overallTarget(){ - return elevatorTarget() + liftTarget(); - } - - /** - * Checks to see if the elevator and lift are at the desired location - * @return Returns true if it has reached destination, returns false if it has not reached destination + * If elevatorFlip is equal to one thing than set elevator to that height */ - public boolean isSetpointReached(){ - double goal = getSetpoint(); - double goalError = getSetpoint() + 5; - double goalError2 = getSetpoint() - 5; - if(overallTarget() == goal && overallTarget() <= goalError && overallTarget() >= goalError2){ - return true; + @SuppressWarnings("Duplicates") + public void elevatorFlip(){ + if (isHatch()) { + liftMode = LiftMode.CARGO; } else { - return false; + liftMode = LiftMode.HATCH; } - } - /** - * If elevatorFlip is equal to one thing than set elevator to that height - */ - public void elevatorFlip(){ switch (elevatorState) { - case FLOOR: - setElevator(ElevatorState.FLOOR); - isFired = true; - break; case LOW_HATCH: - setElevator(ElevatorState.LOW_HATCH); - isFired = true; + setLiftSetpoint(ElevatorState.LOW_HATCH); break; case LOW_CARGO: - setElevator(ElevatorState.LOW_CARGO); - isFired = true; + setLiftSetpoint(ElevatorState.LOW_HATCH); break; case MIDDLE_HATCH: - setElevator(ElevatorState.MIDDLE_HATCH); - isFired = true; + setLiftSetpoint(ElevatorState.MIDDLE_CARGO); break; case MIDDLE_CARGO: - setElevator(ElevatorState.MIDDLE_CARGO); - isFired = true; + setLiftSetpoint(ElevatorState.MIDDLE_HATCH); break; case HIGH_HATCH: - setElevator(ElevatorState.HIGH_HATCH); - isFired = true; + setLiftSetpoint(ElevatorState.HIGH_CARGO); break; case HIGH_CARGO: - setElevator(ElevatorState.HIGH_CARGO); - isFired = true; + setLiftSetpoint(ElevatorState.HIGH_HATCH); break; } } /** - * Checks to see if the amount of error in the elevator motors is acceptable - * @return Returns true if it is within the allowed range, returns false if it is not close enough + * Sets the elevator and bar height to the lowest setting as to prevent a high center of mass */ - public boolean isElevatorAllowableError(){ - double error = 5; - if(getElevatorError() <= error && getElevatorError() >= -error){ - return true; - } else { - return false; - } + public void saveElevator() { + setLiftSetpoint(ElevatorState.START); + System.out.println("Saved Elevator"); } /** - * Gets the target height of the elevator - * @return Returns the desired height of the elevator + * Checks to see if the elevator is within the margin of error of it's target + * @return true if it's close enough to the desired target */ - private double elevatorTarget(){ - return elevatorMasterMotor.getClosedLoopTarget(2); + public boolean isElevatorSetpointReached(){ + return isElevatorAllowableError(); } /** - * Gets the target height of the lift - * @return Returns the desired height of the lift + * Checks to see if the amount of error in the elevator motors is acceptable + * @return Returns true if it is within the allowed range, returns false if it is not close enough */ - private double liftTarget(){ - return liftMasterMotor.getClosedLoopTarget(3); + private boolean isElevatorAllowableError() { + return Math.abs(getElevatorError()) <= ELEVATOR_ALLOWED_ERROR; } /** - * Checks to see if te amount of error in the lift motors is acceptable + * Checks to see if the bar is within the margin of error of it's target + * @return true if it's close enough to the desired target + */ + public boolean isBarSetpointReached(){ + return isBarAllowableError(); + } + + /** + * Checks to see if te amount of error in the bar motors is acceptable * @return Returns true if it is within the allowed range, returns false if it is not close enough */ - public boolean isLiftAllowableError(){ - double error = 5; - return getLiftError() <= error && getLiftError() >= -error; + private boolean isBarAllowableError(){ + return Math.abs(getBarError()) <= BAR_ALLOWED_ERROR; } /** - * Tells the elevator to go to the specified height, also sets elevatorState + * Tells the bar and elevator to go to the specified height, also sets elevatorState * @param state Is the state of which the elevator is wanted to go to */ - public void setElevator(ElevatorState state) { + public void setLiftSetpoint(ElevatorState state) { + System.out.println("---------------"); + System.out.println(isHatch()); switch (state) { case FLOOR: - setElevator(Floor); + System.out.println("FLOOR"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.FLOOR; break; case LOW_HATCH: - setElevator(Low_Hatch); + System.out.println("LOW_HATCH"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.LOW_HATCH; break; case LOW_CARGO: - setElevator(Low_Cargo); + System.out.println("LOW_CARGO"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.LOW_CARGO; break; case MIDDLE_HATCH: - setElevator(Mid_Hatch); + System.out.println("MIDDLE_HATCH"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.MIDDLE_HATCH; break; case MIDDLE_CARGO: - setElevator(Mid_Cargo); + System.out.println("MIDDLE_CARGO"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.MIDDLE_CARGO; break; case HIGH_HATCH: - setElevator(High_Hatch); + System.out.println("HIGH_HATCH"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.HIGH_HATCH; break; case HIGH_CARGO: - setElevator(High_Cargo); + System.out.println("HIGH_CARGO"); + setLiftSetpoint(state.getBarPosition(), state.getElevatorPosition()); elevatorState = ElevatorState.HIGH_CARGO; break; } } /** - * "Runs" setSetpointElevator and setSetpointLift with the appropriate parameters. - * @param setpoints double[2] both + * "Runs" setLiftSetpoint and setBarSetpoint with the appropriate parameters. + * @param barSetpoint is the desired setpoint for the bar to go to + * @param elevatorSetpoint is the desired setpoint for the elevator to go to */ - public void setElevator(double[] setpoints) { - setSetpointLift(setpoints[0]); - setSetpointElevator(setpoints[1]); + + public void setLiftSetpoint(double barSetpoint, double elevatorSetpoint) { + System.out.println(barSetpoint + " <-- is bar setpoint"); + System.out.println(elevatorSetpoint + " <-- is elevator setpoint"); + setElevatorSetpoint(elevatorSetpoint); + setBarSetpoint(barSetpoint); } /** * Sets the height of the elevator - * Will not move the elevator if you want to move it below 0 ticks or above the Top ticks number * @param setpoint is the height (in ticks) of which you want the elevator to go to */ - private void setSetpointElevator(double setpoint) { - // Checks to see if the elevator is within safe operating heights - if(getSetpointElevator() >= Floor[1] && getSetpointElevator() <= Top[1]) { - elevatorMasterMotor.set(ControlMode.Position, setpoint); - } - } + private void setElevatorSetpoint(double setpoint) { + elevatorMasterMotor.set(ControlMode.Position, setpoint); + } /** - * Sets the height of the lift - * @param setpoint Is the height to which is need to get to + * Sets the setpoint of the bar + * @param setpoint Is the setpoint to which the bar needs to get to */ - private void setSetpointLift(double setpoint){ - // Checks to see if the lift is within safe operation heights - if(getSetpointLift() >= Floor[2] && getSetpointLift() <= Top[2]) { - liftMasterMotor.set(ControlMode.Position, setpoint); - } + private void setBarSetpoint(double setpoint){ + System.out.println(setpoint); + barMasterMotor.set(ControlMode.Position, setpoint); } /** * Gets the error of the elevator motor * @return Returns the error of the elevator motor */ - public double getElevatorError(){ - return elevatorMasterMotor.getClosedLoopError(2); + public double getElevatorError() { + return elevatorMasterMotor.getClosedLoopError(0); } /** - * Gets the error of the lift motor - * @return Returns the error of the lift motor + * Gets the error of the bar motor + * @return Returns the error of the bar motor */ - public double getLiftError(){ - return liftMasterMotor.getClosedLoopError(3); + public double getBarError() { + return barMasterMotor.getClosedLoopError(0); } /** - * Gets the total height of the elevator and lift together - * @return /\ - * || + * Gets the value of elevator setpoint + * @return returns the value of elevator setpoint */ - public double getSetpoint(){ - return getSetpointElevator() + getSetpointLift(); + public double getElevatorSetpoint(){ + return elevatorMasterMotor.getClosedLoopTarget(0); } /** - * Gets the value of Setpoint - * @return returns the value of Setpoint + * Gets the value of bar setpoint + * @return returns the value of bar setpoint */ - public double getSetpointElevator(){ - return elevatorMasterMotor.getClosedLoopTarget(0); - } - /** - * Gets the value of Setpoint - * @return returns the value of Setpoint - */ - public double getSetpointLift(){ - return liftMasterMotor.getClosedLoopTarget(0); + public double getBarSetpoint(){ + return barMasterMotor.getClosedLoopTarget(0); } /** @@ -396,80 +396,96 @@ public double getSetpointLift(){ * @return Returns the value of Elevator_kP */ private double getElevator_kP() { - return SmartDashboard.getNumber("elevator kP", 2.0); - } + return SmartDashboard.getNumber("Elevator kP", 2.0); + } /** * Gets the value of Elevator_kI from Smart Dashboard * @return Returns the value of Elevator_kI */ private double getElevator_kI() { - return SmartDashboard.getNumber("elevator kI", 0); - } + return SmartDashboard.getNumber("Elevator kI", 0); + } /** * Gets the value of Elevator_kD from Smart Dashboard * @return Returns the value of Elevator_kD */ private double getElevator_kD(){ - return SmartDashboard.getNumber("elevator kD", 20); - } + return SmartDashboard.getNumber("Elevator kD", 20); + } /** * Gets the value of Elevator_kF from Smart Dashboard * @return Returns the value of Elevator_kF */ private double getElevator_kF() { - return SmartDashboard.getNumber("elevator kF", 0.076); - } + return SmartDashboard.getNumber("Elevator kF", 0.076); + } + + public double getElevatorEncoderPosition(){ + return elevatorMasterMotor.getSelectedSensorPosition(0); + } /** - * Gets the value of Lift_kP from Smart Dashboard - * @return Returns the value of Lift_kP + * Gets the value of Bar_kP from Smart Dashboard + * @return Returns the value of Bar_kP */ - private double getLift_kP() { - return SmartDashboard.getNumber("lift kP", 2.0); - } + private double getBar_kP() { + return SmartDashboard.getNumber("Bar kP", 2.0); + } /** - * Gets the value of Lift_kI from Smart Dashboard - * @return Returns the value of Lift_kI + * Gets the value of Bar_kI from Smart Dashboard + * @return Returns the value of Bar_kI */ - private double getLift_kI() { - return SmartDashboard.getNumber("lift kI", 0); - } + private double getBar_kI() { + return SmartDashboard.getNumber("Bar kI", 0); + } /** - * Gets the value of Lift_kD from Smart Dashboard - * @return Returns the value of Lift_kD + * Gets the value of Bar_kD from Smart Dashboard + * @return Returns the value of Bar_kD */ - private double getLift_kD(){ - return SmartDashboard.getNumber("lift kD", 20); - } + private double getBar_kD(){ + return SmartDashboard.getNumber("Bar kD", 20); + } /** - * Gets the value of Lift_kF from Smart Dashboard - * @return Returns the value of Lift_kF + * Gets the value of Bar_kF from Smart Dashboard + * @return Returns the value of Bar_kF */ - private double getLift_kF() { - return SmartDashboard.getNumber("lift kF", 0.076); - } + private double getBar_kF() { + return SmartDashboard.getNumber("Bar kF", 0.076); + } + + public double getBarEncoderPosition(){ + return barMasterMotor.getSelectedSensorPosition(0); + } + + public boolean isHatch() { + return liftMode == LiftMode.HATCH; + } + + public boolean isCargo() { + return liftMode == LiftMode.CARGO; + } + /** * Tells you how many ticks you need to turn a motor to go a certain amount of inches. * @param inches is the amount of inches you want to convert to ticks. * @return Returns the amount of ticks it takes to go the certain amount of inches. */ private double getInchesToTicks(double inches){ - // The amount of ticks that the given amount of inches will be - double ticks; - // Is the amount of inches per amount of ticks per rotation - double ticksPerRotation = 1000; - double inchesMovedPerRotation = 0.5; - - // The part in parenthesis (hopefully) calculates how many rotations you would need to get to the next "closest" number - // Then it is multiplied by the amount of ticks per rotation as to make it into the amount of ticks. - ticks = (inches / inchesMovedPerRotation) * ticksPerRotation; - - return ticks; + // The amount of ticks that the given amount of inches will be + double ticks; + // Is the amount of inches per amount of ticks per rotation + double ticksPerRotation = 1000; + double inchesMovedPerRotation = 0.5; + // The part in parenthesis (hopefully) calculates how many rotations you would need to get to the next "closest" number + // Then it is multiplied by the amount of ticks per rotation as to make it into the amount of ticks. + ticks = (inches / inchesMovedPerRotation) * ticksPerRotation; + + return ticks; } } diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/GyroPIDOutput.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/GyroPIDOutput.java new file mode 100644 index 0000000..10f5276 --- /dev/null +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/GyroPIDOutput.java @@ -0,0 +1,20 @@ +package org.techvalleyhigh.frc5881.deepspace.robot.utils; + +import edu.wpi.first.wpilibj.PIDOutput; + +public class GyroPIDOutput implements PIDOutput { + private double output = 0; + /** + * Set the output to the value calculated by PIDController. + * + * @param output the value calculated by PIDController + */ + @Override + public void pidWrite(double output) { + this.output = output; + } + + public double getOutput() { + return output; + } +} diff --git a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/VisionUtil.java b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/VisionUtil.java index 5d92238..a8afbb4 100644 --- a/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/VisionUtil.java +++ b/src/main/java/org/techvalleyhigh/frc5881/deepspace/robot/utils/VisionUtil.java @@ -1,4 +1,21 @@ package org.techvalleyhigh.frc5881.deepspace.robot.utils; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class VisionUtil { + /** + * Angle to turn to face the tape and take measurement + * @return double relative angle + */ + public static double getSeparationAngle() { + return SmartDashboard.getNumber("Separation Angle", 0); + } + + /** + * Angle robot must finish trajectory facing to be co-linear with tape + * @return double relative angle + */ + public static double getFinalAngle() { + return SmartDashboard.getNumber("Final Angle", 0); + } }