diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 9bc33e1..d0cdc9f 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/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/mechanisms/Elevator.java b/src/main/java/com/team766/robot/mechanisms/Elevator.java index 0e047d4..6aebb00 100644 --- a/src/main/java/com/team766/robot/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/mechanisms/Elevator.java @@ -1,4 +1,6 @@ package com.team766.robot.mechanisms; + +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; @@ -25,15 +27,15 @@ public enum Position { /** 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.5), + HUMAN_CUBES(39), /** Elevator is at appropriate height to grab cones from the human player. */ - HUMAN_CONES(40.5), + HUMAN_CONES(40), /** Elevator is fully extended. */ EXTENDED(40); @@ -48,7 +50,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; @@ -78,6 +80,9 @@ public Elevator() { throw new IllegalStateException("Motor are not CANSparkMaxes!"); } + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + leftMotor = (CANSparkMax) halLeftMotor; rightMotor = (CANSparkMax) halRightMotor; diff --git a/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java index 1525fa0..e37ff50 100644 --- a/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java +++ b/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java @@ -48,16 +48,16 @@ public static double elevatorRotationsToHeight(double rotations) { * Converts a target rotation (in degrees) to encoder units for the shoulder motor. */ public static double shoulderDegreesToRotations(double angle) { - // angle * net gear ratio * (rotations / degrees) - return angle * (4. / 1.) * (3. / 1.) * (1. / 360.); + // 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 * net gear ratio * (degrees / rotations) - return rotations * (1. / 4.) * (1. / 3.) * (360. / 1.); + // rotations * sprocket ratio * net gear ratio * (degrees / rotations) + return rotations * (12.0 / 52.0) * (30.0 / 64.0) * (1. / 4.) * (1. / 3.) * (360. / 1.); } /** diff --git a/src/main/java/com/team766/robot/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/mechanisms/Shoulder.java index 6378390..9983ac6 100644 --- a/src/main/java/com/team766/robot/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/mechanisms/Shoulder.java @@ -1,7 +1,9 @@ package com.team766.robot.mechanisms; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.ExternalFollower; import com.revrobotics.SparkMaxPIDController; import com.team766.config.ConfigFileReader; import com.team766.framework.Mechanism; @@ -28,14 +30,17 @@ public enum Position { // TODO: adjust these! - /** Shoulder is in top position. */ + /** Shoulder is at the highest achievable position. */ TOP(45), - /** Shoulder is in position to intake and outtake pieces from/to the floor. Starting position. */ - FLOOR(20), + /** Shoulder is in position to intake from the substation or score in the upper nodes. */ + RAISED(40), - /** Shoulder is fully down. **/ - BOTTOM(15); + /** 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; @@ -43,7 +48,7 @@ public enum Position { this.angle = angle; } - private double getAngle() { + public double getAngle() { return angle; } } @@ -78,12 +83,15 @@ public Shoulder() { 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.elevatorHeightToRotations(Position.BOTTOM.getAngle())); + leftMotor.getEncoder().setPosition(EncoderUtils.shoulderDegreesToRotations(Position.BOTTOM.getAngle())); pidController = leftMotor.getPIDController(); pidController.setFeedbackDevice(leftMotor.getEncoder()); @@ -168,10 +176,11 @@ public void rotate(double angle) { SmartDashboard.putNumber("[SHOULDER] ff", ff); SmartDashboard.putNumber("[SHOULDER] reference", angle); - pidController.setOutputRange(-1, 1); + 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); @@ -182,6 +191,8 @@ 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/mechanisms/Wrist.java b/src/main/java/com/team766/robot/mechanisms/Wrist.java index ac6a258..78522ca 100644 --- a/src/main/java/com/team766/robot/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/mechanisms/Wrist.java @@ -29,15 +29,15 @@ public class Wrist extends Mechanism { public enum Position { /** 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), - HUMAN_CONES(-17.5), - HUMAN_CUBES(-17.5), + HUMAN_CONES(-4), + HUMAN_CUBES(-8), /** Wrist is fully down. **/ BOTTOM(60); diff --git a/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHigh.java b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHigh.java index 228d5df..c8df305 100644 --- a/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHigh.java +++ b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHigh.java @@ -1,11 +1,12 @@ package com.team766.robot.procedures; import com.team766.robot.mechanisms.Elevator; +import com.team766.robot.mechanisms.Shoulder; import com.team766.robot.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/procedures/ExtendWristvatorToHuman.java b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHuman.java index cc5942c..80a0cab 100644 --- a/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHuman.java +++ b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToHuman.java @@ -1,13 +1,15 @@ package com.team766.robot.procedures; import com.team766.robot.mechanisms.Elevator; +import com.team766.robot.mechanisms.Shoulder; import com.team766.robot.mechanisms.Wrist; import com.team766.robot.mechanisms.Intake.GamePieceType; public class ExtendWristvatorToHuman extends MoveWristvator { public ExtendWristvatorToHuman(GamePieceType gamePieceType) { - super(gamePieceType == GamePieceType.CONE + super(Shoulder.Position.RAISED, + gamePieceType == GamePieceType.CONE ? Elevator.Position.HUMAN_CONES : Elevator.Position.HUMAN_CUBES, gamePieceType == GamePieceType.CONE diff --git a/src/main/java/com/team766/robot/procedures/ExtendWristvatorToLow.java b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToLow.java index 5b2ba6c..d8f073d 100644 --- a/src/main/java/com/team766/robot/procedures/ExtendWristvatorToLow.java +++ b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToLow.java @@ -1,11 +1,12 @@ package com.team766.robot.procedures; import com.team766.robot.mechanisms.Elevator; +import com.team766.robot.mechanisms.Shoulder; import com.team766.robot.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/procedures/ExtendWristvatorToMid.java b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToMid.java index d740b61..b69f8e0 100644 --- a/src/main/java/com/team766/robot/procedures/ExtendWristvatorToMid.java +++ b/src/main/java/com/team766/robot/procedures/ExtendWristvatorToMid.java @@ -1,11 +1,12 @@ package com.team766.robot.procedures; import com.team766.robot.mechanisms.Elevator; +import com.team766.robot.mechanisms.Shoulder; import com.team766.robot.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/procedures/MoveWristvator.java b/src/main/java/com/team766/robot/procedures/MoveWristvator.java index 5bde9ca..a1437b4 100644 --- a/src/main/java/com/team766/robot/procedures/MoveWristvator.java +++ b/src/main/java/com/team766/robot/procedures/MoveWristvator.java @@ -4,13 +4,16 @@ import com.team766.framework.Procedure; import com.team766.robot.Robot; import com.team766.robot.mechanisms.Elevator; +import com.team766.robot.mechanisms.Shoulder; import com.team766.robot.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 +22,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/procedures/RetractWristvator.java b/src/main/java/com/team766/robot/procedures/RetractWristvator.java index 3659ef2..e9f1723 100644 --- a/src/main/java/com/team766/robot/procedures/RetractWristvator.java +++ b/src/main/java/com/team766/robot/procedures/RetractWristvator.java @@ -1,11 +1,12 @@ package com.team766.robot.procedures; import com.team766.robot.mechanisms.Elevator; +import com.team766.robot.mechanisms.Shoulder; import com.team766.robot.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); } }