diff --git a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java index 1e9da0972..2f84aa4d8 100644 --- a/src/main/java/com/team766/robot/gatorade/AutonomousModes.java +++ b/src/main/java/com/team766/robot/gatorade/AutonomousModes.java @@ -1,6 +1,7 @@ package com.team766.robot.gatorade; import com.team766.framework.AutonomousMode; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import com.team766.robot.gatorade.procedures.*; public class AutonomousModes { @@ -16,10 +17,13 @@ public class AutonomousModes { // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), // new AutonomousMode("FollowPoints", () -> new FollowPoints()), // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), - // new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), + // new AutonomousMode("ScoreHighCube", () -> new ScoreHighCube(GamePieceType.CUBE)), + // new AutonomousMode("OnePieceExitCommunity", () -> new + // OnePieceExitCommunity(GamePieceType.CUBE)), + // new AutonomousMode("OnePieceExitCommunityBalance", () -> new + // OnePieceExitCommunityBalance(GamePieceType.CUBE)), new AutonomousMode( - "OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), - // AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), + "OnePieceBalanceCube", () -> new OnePieceBalance(GamePieceType.CUBE)), // new AutonomousMode("FollowPointsFile", () -> new // FollowPoints("FollowPoints.json")), // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index 68e775a7a..5cc924c9e 100644 --- a/src/main/java/com/team766/robot/gatorade/OI.java +++ b/src/main/java/com/team766/robot/gatorade/OI.java @@ -109,7 +109,7 @@ public void run(Context context) { > 0) { context.takeOwnership(Robot.drive); // If a button is pressed, drive is just fine adjustment - if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { + if (rightJoystick.getButton(InputConstants.FINE_DRIVING)) { Robot.drive.controlFieldOriented( Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), @@ -133,24 +133,29 @@ public void run(Context context) { if (boxopGamepad.getPOV() == InputConstants.POV_UP) { new GoForCones().run(context); setLightsForGamePiece(); + SmartDashboard.putBoolean("Game Piece", true); } else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) { new GoForCubes().run(context); setLightsForGamePiece(); + SmartDashboard.putBoolean("Game Piece", false); } // look for button presses to queue placement of intake/wrist/elevator superstructure - if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { + if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) { + placementPosition = PlacementPosition.NONE; + // setLightsForPlacement(); + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { placementPosition = PlacementPosition.LOW_NODE; - setLightsForPlacement(); + // setLightsForPlacement(); } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) { placementPosition = PlacementPosition.MID_NODE; - setLightsForPlacement(); + // setLightsForPlacement(); } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) { placementPosition = PlacementPosition.HIGH_NODE; - setLightsForPlacement(); + // setLightsForPlacement(); } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) { placementPosition = PlacementPosition.HUMAN_PLAYER; - setLightsForPlacement(); + // setLightsForPlacement(); } // look for button hold to start intake, release to idle intake @@ -158,16 +163,8 @@ public void run(Context context) { new IntakeIn().run(context); } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) { new IntakeIdle().run(context); - } - - // see if we should reset the button states - if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) { - // stop the intake + } else if (boxopGamepad.getButton(InputConstants.BUTTON_INTAKE_STOP)) { new IntakeStop().run(context); - // reset the placement position - placementPosition = PlacementPosition.NONE; - // reset the cone/cube selection to cones - new GoForCones().run(context); } // look for button hold to extend intake/wrist/elevator superstructure, @@ -187,7 +184,7 @@ public void run(Context context) { break; case HUMAN_PLAYER: context.startAsync( - new ExtendWristvatorToHuman(Robot.intake.getGamePieceType())); + new ExtendToHumanWithIntake(Robot.intake.getGamePieceType())); break; default: // warn, ignore @@ -197,84 +194,59 @@ public void run(Context context) { break; } } else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - context.startAsync(new RetractWristvator()); + if (placementPosition == PlacementPosition.HUMAN_PLAYER) { + context.startAsync(new RetractWristvatorIdleIntake()); + } else { + context.startAsync(new RetractWristvator()); + } } - // TODO: refactor this code. it's getting gnarly. // look for manual nudges // we only allow these if the extend elevator trigger is extended if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { - // the y axis is flipped from what we expect. invert so up is positive, down is - // negative. + + // look for elevator nudges double elevatorNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT); - double wristNudgeAxis = - -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); - - if (boxopGamepad.getButtonPressed( - InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { - // bypass PID - if (Math.abs(elevatorNudgeAxis) > 0.05) { - elevatorManual = true; - context.takeOwnership(Robot.elevator); - Robot.elevator.nudgeNoPID(elevatorNudgeAxis); - context.releaseOwnership(Robot.elevator); - } else if (elevatorManual) { - context.takeOwnership(Robot.elevator); - Robot.elevator.stopElevator(); - context.releaseOwnership(Robot.elevator); - elevatorManual = false; - } - - if ((Math.abs(wristNudgeAxis) > 0.05)) { - wristManual = true; - context.takeOwnership(Robot.wrist); - Robot.wrist.nudgeNoPID(wristNudgeAxis); - context.releaseOwnership(Robot.wrist); - } else if (wristManual) { - context.takeOwnership(Robot.wrist); - Robot.wrist.stopWrist(); - context.releaseOwnership(Robot.wrist); - elevatorManual = false; - } - } else if (boxopGamepad.getButtonReleased( - InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) { - context.takeOwnership(Robot.wrist); + if (Math.abs(elevatorNudgeAxis) > 0.05) { + // elevatorManual = true; context.takeOwnership(Robot.elevator); - Robot.wrist.resetEncoder(); - Robot.elevator.resetEncoder(); - context.releaseOwnership(Robot.wrist); - context.releaseOwnership(Robot.elevator); - } else { - // look for elevator nudges - if (Math.abs(elevatorNudgeAxis) > 0.05) { - context.takeOwnership(Robot.elevator); - if (elevatorNudgeAxis > 0) { - Robot.elevator.nudgeUp(); - } else { - Robot.elevator.nudgeDown(); - } - context.releaseOwnership(Robot.elevator); + // Robot.elevator.nudgeNoPID(elevatorNudgeAxis); + if (elevatorNudgeAxis > 0) { + Robot.elevator.nudgeUp(); + } else { + Robot.elevator.nudgeDown(); } + context.releaseOwnership(Robot.elevator); + } else if (false && elevatorManual) { + Robot.elevator.stopElevator(); + elevatorManual = false; + } - // look for wrist nudges - if (Math.abs(wristNudgeAxis) > 0.05) { - context.takeOwnership(Robot.wrist); - if (wristNudgeAxis > 0) { - Robot.wrist.nudgeUp(); - } else { - Robot.wrist.nudgeDown(); - } - context.releaseOwnership(Robot.wrist); + // look for wrist nudges + double wristNudgeAxis = + -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); + if (Math.abs(wristNudgeAxis) > 0.05) { + // wristManual = true; + context.takeOwnership(Robot.wrist); + // Robot.wrist.nudgeNoPID(wristNudgeAxis); + if (wristNudgeAxis > 0) { + Robot.wrist.nudgeUp(); + } else { + Robot.wrist.nudgeDown(); } + context.releaseOwnership(Robot.wrist); + } else if (false && wristManual) { + Robot.wrist.stopWrist(); + wristManual = true; } } if (lightsRateLimit.next()) { - if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) { + if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 17) { Robot.lights.rainbow(); } else { - setLightsForPlacement(); + setLightsForGamePiece(); } } } @@ -282,18 +254,18 @@ public void run(Context context) { private void setLightsForPlacement() { switch (placementPosition) { - case NONE: - Robot.lights.white(); - break; - case LOW_NODE: - Robot.lights.green(); - break; - case MID_NODE: - Robot.lights.red(); - break; - case HIGH_NODE: - Robot.lights.orange(); - break; + // case NONE: + // Robot.lights.white(); + // break; + // case LOW_NODE: + // Robot.lights.green(); + // break; + // case MID_NODE: + // Robot.lights.red(); + // break; + // case HIGH_NODE: + // Robot.lights.orange(); + // break; case HUMAN_PLAYER: setLightsForGamePiece(); break; diff --git a/src/main/java/com/team766/robot/gatorade/Robot.java b/src/main/java/com/team766/robot/gatorade/Robot.java index e40383f33..5881f5b7f 100644 --- a/src/main/java/com/team766/robot/gatorade/Robot.java +++ b/src/main/java/com/team766/robot/gatorade/Robot.java @@ -7,6 +7,7 @@ public class Robot { public static Intake intake; public static Wrist wrist; public static Elevator elevator; + public static Shoulder shoulder; public static Drive drive; public static Gyro gyro; public static Lights lights; @@ -16,6 +17,7 @@ public static void robotInit() { intake = new Intake(); wrist = new Wrist(); elevator = new Elevator(); + shoulder = new Shoulder(); drive = new Drive(); gyro = new Gyro(); lights = new Lights(); diff --git a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java index 8d20e4935..e86573c84 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java @@ -36,4 +36,15 @@ private ConfigConstants() {} public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity"; public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity"; public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel"; + + // shoulder config values + public static final String SHOULDER_LEFT_MOTOR = "shoulder.leftMotor"; + public static final String SHOULDER_RIGHT_MOTOR = "shoulder.rightMotor"; + public static final String SHOULDER_PGAIN = "shoulder.sparkPID.pGain"; + public static final String SHOULDER_IGAIN = "shoulder.sparkPID.iGain"; + public static final String SHOULDER_DGAIN = "shoulder.sparkPID.dGain"; + public static final String SHOULDER_FFGAIN = "shoulder.sparkPID.ffGain"; + public static final String SHOULDER_MAX_VELOCITY = "shoulder.sparkPID.maxVelocity"; + public static final String SHOULDER_MIN_OUTPUT_VELOCITY = "shoulder.sparkPID.minOutputVelocity"; + public static final String SHOULDER_MAX_ACCEL = "shoulder.sparkPID.maxAccel"; } diff --git a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java index c5572ecbe..ede98047e 100644 --- a/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java +++ b/src/main/java/com/team766/robot/gatorade/constants/InputConstants.java @@ -24,24 +24,24 @@ public final class InputConstants { // Boxop Gamepad Buttons - // LT - public static final int BUTTON_INTAKE_IN = 7; // RT - public static final int BUTTON_EXTEND_WRISTVATOR = 8; + public static final int BUTTON_INTAKE_IN = 8; + // LT + public static final int BUTTON_EXTEND_WRISTVATOR = 7; // Start - public static final int BUTTON_RESET_STATE = 10; + public static final int BUTTON_INTAKE_STOP = 10; // used for development - // left axis - public static final int AXIS_WRIST_MOVEMENT = 1; // right axis - public static final int AXIS_ELEVATOR_MOVEMENT = 3; + public static final int AXIS_WRIST_MOVEMENT = 3; + // left axis + public static final int AXIS_ELEVATOR_MOVEMENT = 1; // pov public static final int POV_UP = 0; public static final int POV_DOWN = 180; - // LB - public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5; + // RB + public static final int BUTTON_PLACEMENT_NONE = 6; // X/A/B/Y public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X public static final int BUTTON_PLACEMENT_HIGH = 4; // Y diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java index d8c14b4ac..41e314675 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Drive.java @@ -5,7 +5,6 @@ import com.ctre.phoenix.sensors.CANCoder; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; -import com.team766.hal.MotorController.ControlMode; import com.team766.hal.RobotProvider; import com.team766.logging.Category; import com.team766.odometry.Odometry; @@ -18,40 +17,11 @@ public class Drive extends Mechanism { - // Drive motors - private MotorController m_DriveFR; - private MotorController m_DriveFL; - private MotorController m_DriveBR; - private MotorController m_DriveBL; - - // Motors to turn each wheel in place - private MotorController m_SteerFR; - private MotorController m_SteerFL; - private MotorController m_SteerBR; - private MotorController m_SteerBL; - - // Absolute encoders (cancoders) - private CANCoder e_FrontRight; - private CANCoder e_FrontLeft; - private CANCoder e_BackRight; - private CANCoder e_BackLeft; - - /* - * Specific offsets to align each wheel - * These are assigned upon startup with setEncoderOffset() - */ - private double offsetFR; - private double offsetFL; - private double offsetBR; - private double offsetBL; - - /* - * Factor that converts between motor units and degrees - * Multiply to convert from degrees to motor units - * Divide to convert from motor units to degrees - */ - private final double ENCODER_CONVERSION_FACTOR = - (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + // SwerveModules + private final SwerveModule swerveFR; + private final SwerveModule swerveFL; + private final SwerveModule swerveBR; + private final SwerveModule swerveBL; // TODO: rework odometry so it doesn't have to go through drive @@ -60,49 +30,38 @@ public class Drive extends Mechanism { // variable representing current position private static PointDir currentPosition; - // other variables to set up odometry - private MotorController[] motorList; - private CANCoder[] CANCoderList; - private Point[] wheelPositions; - public Drive() { - loggerCategory = Category.DRIVE; - // Initializations of motors - // Initialize the drive motors - m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); - m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); - m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); - m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); - - // Initialize the steering motors - m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); - m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); - m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); - m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); - - // Initialize the encoders - e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); - e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); - e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); - e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); - - // Current limit for motors to avoid breaker problems - m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveFL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_SteerFR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerFL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + // create the drive motors + MotorController driveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + MotorController driveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + MotorController driveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + MotorController driveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); + + // create the steering motors + MotorController steerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + MotorController steerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + MotorController steerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + MotorController steerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); + + // create the encoders + CANCoder encoderFR = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderFL = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBR = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBL = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); + + // initialize the swerve modules + swerveFR = new SwerveModule("FR", driveFR, steerFR, encoderFR); + swerveFL = new SwerveModule("FL", driveFL, steerFL, encoderFL); + swerveBR = new SwerveModule("BR", driveBR, steerBR, encoderBR); + swerveBL = new SwerveModule("BL", driveBL, steerBL, encoderBL); // Sets up odometry currentPosition = new PointDir(0, 0, 0); - motorList = new MotorController[] {m_DriveFR, m_DriveFL, m_DriveBL, m_DriveBR}; - CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; - wheelPositions = + MotorController[] motorList = new MotorController[] {driveFR, driveFL, driveBL, driveBR}; + CANCoder[] encoderList = new CANCoder[] {encoderFR, encoderFL, encoderBL, encoderBR}; + Point[] wheelPositions = new Point[] { new Point( OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, @@ -118,70 +77,16 @@ public Drive() { OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2) }; log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + CANCoderList.length); + log("CANCoderList Length: " + encoderList.length); swerveOdometry = new Odometry( motorList, - CANCoderList, + encoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - - // Sets the offset value for each steering motor so that each is aligned - setEncoderOffset(); - } - - /** - * Sets just the steer for a specific module - * Can be used to turn the wheels without moving - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteer(MotorController steerMotor, Vector2D vector, double offset) { - - // Calculates the angle of the vector from -180° to 180° - final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); - - // Add 360 * number of full rotations to vectorTheta, then add offset - final double angleDegrees = - vectorTheta - + 360 - * (Math.round( - (steerMotor.getSensorPosition() / ENCODER_CONVERSION_FACTOR - - offset - - vectorTheta) - / 360)) - + offset; - - // Sets the degree of the steer wheel - // Needs to multiply by encoderconversionfactor to translate into a unit the motor - // understands - steerMotor.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees); - - SmartDashboard.putNumber("Angle", angleDegrees); - } - - /** - * Sets the motion for a specific module - * @param driveMotor the drive motor of this module - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteerAndPower( - MotorController driveMotor, - MotorController steerMotor, - Vector2D vector, - double offset) { - checkContextOwnership(); - - controlModuleSteer(steerMotor, vector, offset); - - // Sets the power to the magnitude of the vector - driveMotor.set(vector.getNorm()); } /** @@ -191,44 +96,34 @@ public void controlModuleSteerAndPower( * @param turn the turn value from the rotation joystick */ public void controlRobotOriented(double x, double y, double turn) { + checkContextOwnership(); + // Finds the vectors for turning and for translation of each module, and adds them // Applies this for each module - controlModuleSteerAndPower( - m_DriveFL, - m_SteerFL, + swerveFL.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X) - .normalize()), - offsetFL); - controlModuleSteerAndPower( - m_DriveFR, - m_SteerFR, + .normalize())); + swerveFR.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X) - .normalize()), - offsetFR); - controlModuleSteerAndPower( - m_DriveBR, - m_SteerBR, + .normalize())); + swerveBR.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X) - .normalize()), - offsetBR); - controlModuleSteerAndPower( - m_DriveBL, - m_SteerBL, + .normalize())); + swerveBL.driveAndSteer( new Vector2D(x, y) .add( turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X) - .normalize()), - offsetBL); + .normalize())); } /** @@ -239,6 +134,8 @@ public void controlRobotOriented(double x, double y, double turn) { * @param turn the turn value from the rotation joystick */ public void controlFieldOriented(double yawRad, double x, double y, double turn) { + checkContextOwnership(); + // Applies a rotational translation to controlRobotOriented // Counteracts the forward direction changing when the robot turns // TODO: change to inverse rotation matrix (rather than negative angle) @@ -248,56 +145,27 @@ public void controlFieldOriented(double yawRad, double x, double y, double turn) turn); } - /* - * Compares the absolute encoder to the motor encoder to find each motor's offset - * This helps each wheel to always be aligned - */ - public void setEncoderOffset() { - offsetFR = - (m_SteerFR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_FrontRight.getAbsolutePosition(); - offsetFL = - (m_SteerFL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_FrontLeft.getAbsolutePosition(); - offsetBR = - (m_SteerBR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_BackRight.getAbsolutePosition(); - offsetBL = - (m_SteerBL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - e_BackLeft.getAbsolutePosition(); - } - /* * Stops each drive motor */ public void stopDrive() { checkContextOwnership(); - m_DriveFR.stopMotor(); - m_DriveFL.stopMotor(); - m_DriveBR.stopMotor(); - m_DriveBL.stopMotor(); + swerveFR.stopDrive(); + swerveFL.stopDrive(); + swerveBR.stopDrive(); + swerveBL.stopDrive(); } /* * Turns wheels in a cross formation to prevent robot from moving */ public void setCross() { - controlModuleSteer( - m_SteerFL, - new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X), - offsetFL); - controlModuleSteer( - m_SteerFR, - new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X), - offsetFR); - controlModuleSteer( - m_SteerBL, - new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X), - offsetBL); - controlModuleSteer( - m_SteerBR, - new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X), - offsetBR); + checkContextOwnership(); + + swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); + swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); + swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); + swerveBR.steer(new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X)); } // TODO: rework odometry so it doesn't have to go through drive diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java index bb5b2660c..47efa290b 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java @@ -2,6 +2,7 @@ import static com.team766.robot.gatorade.constants.ConfigConstants.*; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.SparkMaxPIDController; @@ -15,21 +16,25 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** + * Basic elevator mechanism. Used in conjunction with the {@link Intake} and {@link Wrist}. + * Can be moved up and down as part of teleop or autonomous control to move the {@link Wrist} + * and {@link Intake} closer to a game piece or game element (eg node in the + * field, human player station). + */ public class Elevator extends Mechanism { public enum Position { - // TODO: do we need separate heights for cones vs cubes? - - /** Elevator is fully retracted. */ + /** Elevator is fully retracted. Starting position. */ RETRACTED(0), /** Elevator is the appropriate height to place game pieces at the low node. */ - LOW(5), + LOW(0), /** Elevator is the appropriate height to place game pieces at the mid node. */ MID(18), /** Elevator is at appropriate height to place game pieces at the high node. */ HIGH(40), /** Elevator is at appropriate height to grab cubes from the human player. */ - HUMAN_CUBES(40), + HUMAN_CUBES(39), /** Elevator is at appropriate height to grab cones from the human player. */ HUMAN_CONES(40), /** Elevator is fully extended. */ @@ -46,7 +51,7 @@ private double getHeight() { } } - private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_INCREMENT = 2.0; private static final double NUDGE_DAMPENER = 0.25; private static final double NEAR_THRESHOLD = 2.0; @@ -76,12 +81,18 @@ public Elevator() { throw new IllegalStateException("Motor are not CANSparkMaxes!"); } + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + leftMotor = (CANSparkMax) halLeftMotor; rightMotor = (CANSparkMax) halRightMotor; - rightMotor.follow(leftMotor, true); + rightMotor.follow(leftMotor, true /* invert */); - resetEncoder(); + leftMotor + .getEncoder() + .setPosition( + EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); pidController = leftMotor.getPIDController(); pidController.setFeedbackDevice(leftMotor.getEncoder()); @@ -95,13 +106,6 @@ public Elevator() { maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL); } - public void resetEncoder() { - leftMotor - .getEncoder() - .setPosition( - EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight())); - } - public double getRotations() { return leftMotor.getEncoder().getPosition(); } @@ -139,7 +143,6 @@ public void nudgeUp() { double height = getHeight(); // NOTE: this could artificially limit nudge range double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight()); - System.err.println("Target: " + targetHeight); moveTo(targetHeight); } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java index febf753f4..731ce300b 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/EncoderUtils.java @@ -16,7 +16,6 @@ private EncoderUtils() {} */ public static double wristDegreesToRotations(double angle) { // angle * net gear ratio * (rotations / degrees) - // FIXME: replace 32 with actual # of teeth return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.); } @@ -25,7 +24,6 @@ public static double wristDegreesToRotations(double angle) { */ public static double wristRotationsToDegrees(double rotations) { // rotations * net gear ratio * (degrees / rotations) - // FIXME: replace 32 with actual # of teeth return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.); } @@ -42,10 +40,25 @@ public static double elevatorHeightToRotations(double height) { */ public static double elevatorRotationsToHeight(double rotations) { // rotations * net gear ratio * (height / rotations) - // FIXME: everything return rotations * (12. / 36.) * ((1.641 * Math.PI) / 1.); } + /** + * Converts a target rotation (in degrees) to encoder units for the shoulder motor. + */ + public static double shoulderDegreesToRotations(double angle) { + // angle * sprocket ratio * net gear ratio * (rotations / degrees) + return angle * (52.0 / 12.0) * (64.0 / 30.0) * (4. / 1.) * (3. / 1.) * (1. / 360.); + } + + /** + * Converts the shoulder motor's rotations to degrees. + */ + public static double shoulderRotationsToDegrees(double rotations) { + // rotations * sprocket ratio * net gear ratio * (degrees / rotations) + return rotations * (12.0 / 52.0) * (30.0 / 64.0) * (1. / 4.) * (1. / 3.) * (360. / 1.); + } + /** * Cosine law * @param side1 @@ -57,7 +70,7 @@ public static double lawOfCosines(double side1, double side2, double angle) { double side3Squared = (Math.pow(side1, 2.0) + Math.pow(side2, 2.0) - - 2 * side1 * side2 * Math.cos(Math.toRadians(angle))); + - (2 * side1 * side2 * Math.cos(Math.toRadians(angle)))); return Math.sqrt(side3Squared); } diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java index ff32fd344..7e42a32eb 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Intake.java @@ -23,7 +23,7 @@ public class Intake extends Mechanism { private static final double POWER_IN = 0.3; private static final double POWER_OUT = 0.25; - private static final double POWER_IDLE = 0.02; + private static final double POWER_IDLE = 0.05; /** * The current type of game piece the Intake is preparing to hold or is holding. diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java index 7a7afba0a..d4dc89e04 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Lights.java @@ -4,16 +4,17 @@ import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.RainbowAnimation; import com.team766.framework.Mechanism; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; public class Lights extends Mechanism { private CANdle candle; private static final int CANID = 5; private static final int LED_COUNT = 90; - private Animation rainbowAnimation = new RainbowAnimation(1, 0.1, LED_COUNT); + private Animation rainbowAnimation = new RainbowAnimation(1, 1.5, LED_COUNT); public Lights() { - candle = new CANdle(CANID); + candle = new CANdle(CANID, SwerveDriveConstants.SWERVE_CANBUS); } public void purple() { @@ -29,7 +30,7 @@ public void white() { public void yellow() { checkContextOwnership(); - candle.setLEDs(255, 255, 0); + candle.setLEDs(255, 150, 0); } public void red() { diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java new file mode 100644 index 000000000..e84a7d49d --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java @@ -0,0 +1,200 @@ +package com.team766.robot.gatorade.mechanisms; + +import static com.team766.robot.gatorade.constants.ConfigConstants.*; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.SparkMaxPIDController; +import com.team766.config.ConfigFileReader; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Basic shoulder mechanism. Rotates the {@link Elevator} to different angles, to allow it (and the + * attached {@link Wrist} and {@link Intake}) to reach different positions, from the floor to different + * heights of nodes. + */ +public class Shoulder extends Mechanism { + + /** + * Pre-set positions for the shoulder. + */ + public enum Position { + + // TODO: adjust these! + + /** Shoulder is at the highest achievable position. */ + TOP(45), + + /** Shoulder is in position to intake from the substation or score in the upper nodes. */ + RAISED(40), + + /** Shoulder is in position to intake and outtake pieces from/to the floor. */ + FLOOR(10), + + /** Shoulder is fully down. Starting position. **/ + BOTTOM(0); + + private final double angle; + + Position(double angle) { + this.angle = angle; + } + + public double getAngle() { + return angle; + } + } + + private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.15; + + private static final double NEAR_THRESHOLD = 5.0; + + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final ValueProvider maxVelocity; + private final ValueProvider minOutputVelocity; + private final ValueProvider maxAccel; + + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Constructs a new Shoulder. + */ + public Shoulder() { + MotorController halLeftMotor = RobotProvider.instance.getMotor(SHOULDER_LEFT_MOTOR); + MotorController halRightMotor = RobotProvider.instance.getMotor(SHOULDER_RIGHT_MOTOR); + + if (!((halLeftMotor instanceof CANSparkMax) && (halRightMotor instanceof CANSparkMax))) { + log(Severity.ERROR, "Motors are not CANSparkMaxes!"); + throw new IllegalStateException("Motor are not CANSparkMaxes!"); + } + + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + + leftMotor = (CANSparkMax) halLeftMotor; + rightMotor = (CANSparkMax) halRightMotor; + + rightMotor.follow(leftMotor, true /* invert */); + + leftMotor + .getEncoder() + .setPosition(EncoderUtils.shoulderDegreesToRotations(Position.BOTTOM.getAngle())); + + pidController = leftMotor.getPIDController(); + pidController.setFeedbackDevice(leftMotor.getEncoder()); + + pGain = ConfigFileReader.getInstance().getDouble(SHOULDER_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(SHOULDER_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(SHOULDER_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(SHOULDER_FFGAIN); + maxVelocity = ConfigFileReader.getInstance().getDouble(SHOULDER_MAX_VELOCITY); + minOutputVelocity = ConfigFileReader.getInstance().getDouble(SHOULDER_MIN_OUTPUT_VELOCITY); + maxAccel = ConfigFileReader.getInstance().getDouble(SHOULDER_MAX_ACCEL); + } + + public double getRotations() { + return leftMotor.getEncoder().getPosition(); + } + + /** + * Returns the current angle of the wrist. + */ + public double getAngle() { + return EncoderUtils.shoulderRotationsToDegrees(leftMotor.getEncoder().getPosition()); + } + + public boolean isNearTo(Position position) { + return isNearTo(position.getAngle()); + } + + public boolean isNearTo(double angle) { + return Math.abs(angle - getAngle()) < NEAR_THRESHOLD; + } + + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + leftMotor.set(clampedValue); + } + + public void stopShoulder() { + checkContextOwnership(); + leftMotor.set(0); + } + + public void nudgeUp() { + System.err.println("Nudging up."); + double angle = getAngle(); + double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.TOP.getAngle()); + + rotate(targetAngle); + } + + public void nudgeDown() { + System.err.println("Nudging down."); + double angle = getAngle(); + double targetAngle = Math.max(angle - NUDGE_INCREMENT, Position.BOTTOM.getAngle()); + rotate(targetAngle); + } + + /** + * Rotates the wrist to a pre-set {@link Position}. + */ + public void rotate(Position position) { + rotate(position.getAngle()); + } + + /** + * Starts rotating the wrist to the specified angle. + * NOTE: this method returns immediately. Check the current wrist position of the wrist + * with {@link #getAngle()}. + */ + public void rotate(double angle) { + checkContextOwnership(); + + System.err.println("Setting target angle to " + angle); + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + // pidController.setFF(ffGain.get()); + double ff = ffGain.get() * Math.cos(Math.toRadians(angle)); + SmartDashboard.putNumber("[SHOULDER] ff", ff); + SmartDashboard.putNumber("[SHOULDER] reference", angle); + + pidController.setOutputRange(-0.4, 0.4); + + // convert the desired target degrees to rotations + double rotations = EncoderUtils.shoulderDegreesToRotations(angle); + SmartDashboard.putNumber("[SHOULDER] Setpoint", rotations); + + // set the reference point for the wrist + pidController.setReference(rotations, ControlType.kPosition, 0, ff); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); + SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations()); + SmartDashboard.putNumber("[SHOULDER] Left Effort", leftMotor.getOutputCurrent()); + SmartDashboard.putNumber("[SHOULDER] Right Effort", rightMotor.getOutputCurrent()); + } + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java new file mode 100644 index 000000000..9b37dc763 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/SwerveModule.java @@ -0,0 +1,105 @@ +package com.team766.robot.gatorade.mechanisms; + +import com.ctre.phoenix.sensors.CANCoder; +import com.team766.hal.MotorController; +import com.team766.hal.MotorController.ControlMode; +import com.team766.robot.gatorade.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +/** + * Encapsulates the motors and encoders used for each physical swerve module and + * provides driving and steering controls for each module. + */ +public class SwerveModule { + private final String modulePlacement; + private final MotorController drive; + private final MotorController steer; + private final CANCoder encoder; + private final double offset; + + /* + * Factor that converts between motor units and degrees + * Multiply to convert from degrees to motor units + * Divide to convert from motor units to degrees + */ + private static final double ENCODER_CONVERSION_FACTOR = + (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + + /** + * Creates a new SwerveModule. + * + * @param modulePlacement String description of the placement for this module, eg "FL". + * @param drive Drive MotorController for this module. + * @param steer Steer MotorController for this module. + * @param encoder CANCoder for this module. + */ + public SwerveModule( + String modulePlacement, + MotorController drive, + MotorController steer, + CANCoder encoder) { + this.modulePlacement = modulePlacement; + this.drive = drive; + this.steer = steer; + this.encoder = encoder; + this.offset = computeEncoderOffset(); + + // Current limit for motors to avoid breaker problems + drive.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + steer.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + } + + private double computeEncoderOffset() { + return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - encoder.getAbsolutePosition(); + } + + /** + * Controls just the steer for this module. + * Can be used to turn the wheels without moving + * @param vector the vector specifying the module's motion + */ + public void steer(Vector2D vector) { + // Calculates the angle of the vector from -180° to 180° + final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); + + // Add 360 * number of full rotations to vectorTheta, then add offset + final double angleDegrees = + vectorTheta + + 360 + * (Math.round( + (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR + - offset + - vectorTheta) + / 360)) + + offset; + + // Sets the degree of the steer wheel + // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor + // understands + steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees); + + SmartDashboard.putNumber("[" + modulePlacement + "]" + "Angle", angleDegrees); + } + + /** + * Controls both steer and power (based on the target vector) for this module. + * @param vector the vector specifying the module's motion + */ + public void driveAndSteer(Vector2D vector) { + // apply the steer + steer(vector); + + // sets the power to the magnitude of the vector + // TODO: does this need to be clamped to a specific range, eg btn -1 and 1? + drive.set(vector.getNorm()); + } + + /** + * Stops the drive motor for this module. + */ + public void stopDrive() { + drive.stopMotor(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java index a6942f53e..ab740bad6 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Wrist.java @@ -29,17 +29,17 @@ public class Wrist extends Mechanism { */ public enum Position { - // TODO: adjust these values. - /** Wrist is in top position. Starting position. */ - TOP(-135), + TOP(-180), /** Wrist is in the position for moving around the field. */ - RETRACTED(-120.0), + RETRACTED(-175.0), /** Wrist is level with ground. */ - LEVEL(0.0), - HIGH_NODE(-30), + LEVEL(-65), + HIGH_NODE(-20), MID_NODE(-25.5), - /** Wrist is fully down. */ + HUMAN_CONES(-4), + HUMAN_CUBES(-8), + /** Wrist is fully down. **/ BOTTOM(60); private final double angle; @@ -77,7 +77,8 @@ public Wrist() { } motor = (CANSparkMax) halMotor; - resetEncoder(); + motor.getEncoder() + .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); // stash the PIDController for convenience. will update the PID values to the latest from // the config @@ -92,11 +93,6 @@ public Wrist() { ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN); } - public void resetEncoder() { - motor.getEncoder() - .setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle())); - } - public double getRotations() { return motor.getEncoder().getPosition(); } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java b/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java deleted file mode 100644 index 2e0f90625..000000000 --- a/src/main/java/com/team766/robot/gatorade/procedures/ChargeStationPathFinder.java +++ /dev/null @@ -1,104 +0,0 @@ -package com.team766.robot.gatorade.procedures; - -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.odometry.PointDir; -import com.team766.robot.gatorade.constants.ChargeConstants; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import java.util.ArrayList; -import java.util.List; - -// Helper class for AlignCharger, see AlignCharger.md for more details -public class ChargeStationPathFinder { - - // Since this class does not extend Procedure, we need to instantiate logger - private static final Logger logger = Logger.get(Category.PROCEDURES); - private final Alliance alliance; - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public ChargeStationPathFinder(Alliance alliance) { - this.alliance = alliance; - } - - /** - * Calculates points that the robot needs to follow based on alliance and current position - * @param curPos the current robot position, represented by a pointDir - * @return PointDir[]: list of the calculated points that can be entered into FollowPoints - */ - public PointDir[] calculatePoints(PointDir curPos) { - - // Sets curX and curY variables based on curPos param - double curX = curPos.getX(); - double curY = curPos.getY(); - - // Creates ArrayList for points - List points = new ArrayList(); - switch (alliance) { - case Red: - // Calls addPoints method for red alliance coordinates - addPoints( - points, - curX, - curY, - ChargeConstants.RED_BALANCE_TARGET_X, - ChargeConstants.RED_LEFT_PT, - ChargeConstants.RED_RIGHT_PT, - ChargeConstants.MIDDLE); - break; - - case Blue: - // Calls addPoints method for red alliance coordinates - addPoints( - points, - curX, - curY, - ChargeConstants.BLUE_BALANCE_TARGET_X, - ChargeConstants.BLUE_LEFT_PT, - ChargeConstants.BLUE_RIGHT_PT, - ChargeConstants.MIDDLE); - break; - default: - logger.logRaw(null, "Invalid Alliance"); - } - - // Converts pointDir arrayList to array and returns it - return points.toArray(new PointDir[points.size()]); - } - - /** - * Adds a set of points to an arrayList based on passed charge station coordinates - * @param points ArrayList to add points to - * @param curX Current robot X value - * @param curY Current robot Y value - * @param target X value of center of target charge station - * @param left X value of left side of target charge station - * @param right X value of right side of target charge station - * @param height Y value of center of target charge station - */ - private void addPoints( - List points, - double curX, - double curY, - double target, - double left, - double right, - double height) { - // If on the right side of the target, check if robot is blocked by the right side of the - // charge station, and add points accordingly - if (curX > target) { - if (curX < right) points.add(new PointDir(right, curY)); - - points.add(new PointDir(right, height)); - - // Otherwise, it will be on the left side of the target, so check if robot is blocked by - // the left side of the charge station, and add points accordingly - } else { - if (curX > left) points.add(new PointDir(left, curY)); - - points.add(new PointDir(left, height)); - } - } -} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java new file mode 100644 index 000000000..16d86a292 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExitCommunity.java @@ -0,0 +1,20 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.constants.FollowPointsInputConstants; + +public class ExitCommunity extends Procedure { + + private static final double DIST = 4; + + public void run(Context context) { + context.takeOwnership(Robot.drive); + double startY = Robot.drive.getCurrentPosition().getY(); + Robot.drive.controlFieldOriented( + Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0); + context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST); + Robot.drive.stopDrive(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java new file mode 100644 index 000000000..ff7644f00 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendToHumanWithIntake.java @@ -0,0 +1,23 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class ExtendToHumanWithIntake extends Procedure { + private final GamePieceType gamePieceType; + + public ExtendToHumanWithIntake(GamePieceType gamePieceType) { + this.gamePieceType = gamePieceType; + } + + public void run(Context context) { + context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.wrist); + context.takeOwnership(Robot.elevator); + + new IntakeIn().run(context); + new ExtendWristvatorToHuman(gamePieceType).run(context); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java index 23c2f172a..01db3df37 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHigh.java @@ -1,11 +1,12 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToHigh extends MoveWristvator { public ExtendWristvatorToHigh() { - super(Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); + super(Shoulder.Position.RAISED, Elevator.Position.HIGH, Wrist.Position.HIGH_NODE); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java index d01d999d6..cc970a5de 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToHuman.java @@ -2,15 +2,19 @@ import com.team766.robot.gatorade.mechanisms.Elevator; import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToHuman extends MoveWristvator { public ExtendWristvatorToHuman(GamePieceType gamePieceType) { super( + Shoulder.Position.RAISED, gamePieceType == GamePieceType.CONE ? Elevator.Position.HUMAN_CONES : Elevator.Position.HUMAN_CUBES, - Wrist.Position.LEVEL); + gamePieceType == GamePieceType.CONE + ? Wrist.Position.HUMAN_CONES + : Wrist.Position.HUMAN_CUBES); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java index 602f8497d..486455ab5 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToLow.java @@ -1,11 +1,13 @@ package com.team766.robot.gatorade.procedures; +import com.team766.robot.gatorade.constants.*; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToLow extends MoveWristvator { public ExtendWristvatorToLow() { - super(Elevator.Position.LOW, Wrist.Position.LEVEL); + super(Shoulder.Position.FLOOR, Elevator.Position.LOW, Wrist.Position.LEVEL); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java index e34b96930..33b47090e 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/ExtendWristvatorToMid.java @@ -1,11 +1,12 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class ExtendWristvatorToMid extends MoveWristvator { public ExtendWristvatorToMid() { - super(Elevator.Position.MID, Wrist.Position.MID_NODE); + super(Shoulder.Position.RAISED, Elevator.Position.MID, Wrist.Position.MID_NODE); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java index f61647aab..20e3ac1e0 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java @@ -17,7 +17,8 @@ private enum State { GROUND, RAMP_TRANSITION, RAMP_TILT, - RAMP_LEVEL + RAMP_LEVEL, + OVERSHOOT } // Direction determines which direction the robot moves @@ -45,7 +46,9 @@ private enum Direction { private final double CORRECTION_DELAY = 0.7; private final double SPEED_GROUND = .3; private final double SPEED_TRANSITION = .25; - private final double SPEED_TILT = .18; + private final double SPEED_TILT = .12; + private final double SPEED_OVERSHOOT = .08; + private final double OVERSHOOT_INCORRECT_MULT = 0.5; /** * Constructor to create a new GyroBalance instance @@ -65,6 +68,9 @@ public void run(Context context) { // driveSpeed is actual value of speed passed into the swerveDrive method double driveSpeed = 1; + // extend wristvator to put CG in a place where robot can climb ramp + new ExtendWristvatorToMid().run(context); + // Sets movement direction ground state if on ground setDir(curY); @@ -84,7 +90,7 @@ public void run(Context context) { tilt = Robot.gyro.getAbsoluteTilt(); // log("curX:" + curX); // log("direction: " + direction); - setState(); + setState(context); // Both being on Red alliance and needing to move right would make the movement // direction negative, so this expression corrects for that @@ -101,21 +107,10 @@ public void run(Context context) { } // Loops until robot is level or until a call to the abort() method while (!(curState == State.RAMP_LEVEL)); - - // After the robot is level, drives for correctionDelay seconds. - // Direction is opposite due to inversion of speed in setState() so it corrects for - // overshooting - context.waitForSeconds(CORRECTION_DELAY); - - // Locks wheels once balanced - context.startAsync(new SetCross()); - - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); } // Sets state in state machine, see more details in GyroBalance.md - private void setState() { + private void setState(Context context) { if (prevState == State.GROUND && tilt > LEVEL) { curState = State.RAMP_TRANSITION; absSpeed = SPEED_TRANSITION; @@ -123,12 +118,24 @@ private void setState() { } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { curState = State.RAMP_TILT; absSpeed = SPEED_TILT; + context.startAsync(new RetractWristvator()); log("Tilt, prevState: " + prevState + ", curState: " + curState); } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { - curState = State.RAMP_LEVEL; + curState = State.OVERSHOOT; // If level, sets speed to negative to correct for overshooting + absSpeed = SPEED_OVERSHOOT; absSpeed = -absSpeed; + log("Overshoot, prevState: " + prevState + ", curState: " + curState); + } else if (prevState == State.OVERSHOOT && tilt < LEVEL) { + context.startAsync(new SetCross()); log("Level, prevState: " + prevState + ", curState: " + curState); + context.waitForSeconds(1); + tilt = Robot.gyro.getAbsoluteTilt(); + if (tilt < LEVEL) { + curState = State.RAMP_LEVEL; + } else { + absSpeed *= -OVERSHOOT_INCORRECT_MULT; + } } if (curState == State.GROUND) { absSpeed = SPEED_GROUND; diff --git a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java index 25d45df6d..7053ed7c1 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/MoveWristvator.java @@ -4,13 +4,19 @@ import com.team766.framework.Procedure; import com.team766.robot.gatorade.Robot; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class MoveWristvator extends Procedure { + private final Shoulder.Position shoulderSetpoint; private final Elevator.Position elevatorSetpoint; private final Wrist.Position wristSetpoint; - public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) { + public MoveWristvator( + Shoulder.Position shoulderSetpoint_, + Elevator.Position elevatorSetpoint_, + Wrist.Position wristSetpoint_) { + this.shoulderSetpoint = shoulderSetpoint_; this.elevatorSetpoint = elevatorSetpoint_; this.wristSetpoint = wristSetpoint_; } @@ -19,18 +25,29 @@ public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristS public final void run(Context context) { context.takeOwnership(Robot.wrist); context.takeOwnership(Robot.elevator); + context.takeOwnership(Robot.shoulder); // Always retract the wrist before moving the elevator. // It might already be retracted, so it's possible that this step finishes instantaneously. Robot.wrist.rotate(Wrist.Position.RETRACTED); + // If raising the shoulder, do that before the elevator (else, lower it after the elevator). + if (shoulderSetpoint.getAngle() > Robot.shoulder.getAngle()) { + Robot.shoulder.rotate(shoulderSetpoint); + context.waitFor(() -> Robot.shoulder.isNearTo(shoulderSetpoint)); + } context.waitFor(() -> Robot.wrist.isNearTo(Wrist.Position.RETRACTED)); // Move the elevator. Wait until it gets near the target position. Robot.elevator.moveTo(elevatorSetpoint); context.waitFor(() -> Robot.elevator.isNearTo(elevatorSetpoint)); + // If lowering the shoulder, do that after the elevator. + if (shoulderSetpoint.getAngle() < Robot.shoulder.getAngle()) { + Robot.shoulder.rotate(shoulderSetpoint); + } // Lastly, move the wrist. Robot.wrist.rotate(wristSetpoint); context.waitFor(() -> Robot.wrist.isNearTo(wristSetpoint)); + context.waitFor(() -> Robot.shoulder.isNearTo(shoulderSetpoint)); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java index 28e7d0481..aa16e623a 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java @@ -4,16 +4,22 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; public class OnePieceBalance extends Procedure { + private final GamePieceType type; + + public OnePieceBalance(GamePieceType type) { + this.type = type; + } + public void run(Context context) { context.takeOwnership(Robot.drive); - // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); + Robot.gyro.resetGyro180(); Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { @@ -32,7 +38,7 @@ public void run(Context context) { log("invalid alliance"); return; } - // new IntakeRelease().run(context); + new ScoreHigh(type).run(context); new GyroBalance(alliance.get()).run(context); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java index 19d609ece..07234b7e3 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java @@ -4,25 +4,33 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; public class OnePieceExitCommunity extends Procedure { + private final GamePieceType type; + + public OnePieceExitCommunity(GamePieceType type) { + this.type = type; + } + public void run(Context context) { context.takeOwnership(Robot.drive); // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); + Robot.gyro.resetGyro180(); Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { switch (alliance.get()) { case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); + Robot.drive.setCurrentPosition(new PointDir(0.75, 2)); break; case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); + Robot.drive.setCurrentPosition(new PointDir(0.75, 14.5)); break; default: log("invalid alliance"); @@ -33,6 +41,8 @@ public void run(Context context) { return; } log("exiting"); - new OPECHelper().run(context); + new ScoreHigh(type).run(context); + new RetractWristvator().run(context); + new ExitCommunity().run(context); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java index 6baf0ae02..3264b8595 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java @@ -4,16 +4,24 @@ import com.team766.framework.Procedure; import com.team766.odometry.PointDir; import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; public class OnePieceExitCommunityBalance extends Procedure { + private final GamePieceType type; + + public OnePieceExitCommunityBalance(GamePieceType type) { + this.type = type; + } + public void run(Context context) { context.takeOwnership(Robot.drive); // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro(); + Robot.gyro.resetGyro180(); + Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { @@ -33,8 +41,9 @@ public void run(Context context) { return; } log("exiting"); - new OPECHelper().run(context); + new ScoreHigh(type).run(context); + new ExitCommunity().run(context); log("Transitioning"); - new GyroBalance(alliance.get()).run(context); // already checked, alliance should be set + new GyroBalance(alliance.get()).run(context); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java index 5323e13f2..eb0238312 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvator.java @@ -1,11 +1,12 @@ package com.team766.robot.gatorade.procedures; import com.team766.robot.gatorade.mechanisms.Elevator; +import com.team766.robot.gatorade.mechanisms.Shoulder; import com.team766.robot.gatorade.mechanisms.Wrist; public class RetractWristvator extends MoveWristvator { public RetractWristvator() { - super(Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); + super(Shoulder.Position.BOTTOM, Elevator.Position.RETRACTED, Wrist.Position.RETRACTED); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java new file mode 100644 index 000000000..a71250577 --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/RetractWristvatorIdleIntake.java @@ -0,0 +1,11 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; + +public class RetractWristvatorIdleIntake extends Procedure { + public void run(Context context) { + new RetractWristvator().run(context); + new IntakeIdle().run(context); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java b/src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java new file mode 100644 index 000000000..1133c16ac --- /dev/null +++ b/src/main/java/com/team766/robot/gatorade/procedures/ScoreHigh.java @@ -0,0 +1,24 @@ +package com.team766.robot.gatorade.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.gatorade.Robot; +import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; + +public class ScoreHigh extends Procedure { + private final GamePieceType type; + + public ScoreHigh(GamePieceType type) { + this.type = type; + } + + public void run(Context context) { + context.takeOwnership(Robot.intake); + Robot.intake.setGamePieceType(type); + context.releaseOwnership(Robot.intake); + new ExtendWristvatorToHigh().run(context); + new IntakeOut().run(context); + context.waitForSeconds(1); + new IntakeStop().run(context); + } +}