diff --git a/simgui.json b/simgui.json index 9137a86..b6870ba 100644 --- a/simgui.json +++ b/simgui.json @@ -41,15 +41,28 @@ "transitory": { "Shuffleboard": { "Arm": { + "Derivatives": { + "open": true + }, "Feedforward": { "open": true }, + "Goal": { + "open": true + }, "Position": { "open": true }, + "Setpoint": { + "open": true + }, "Velocities": { "open": true - } + }, + "Voltages": { + "open": true + }, + "open": true }, "Climber": { "Elevator": { @@ -57,14 +70,12 @@ }, "Linear Actuator": { "open": true - }, - "open": true + } }, "Intake": { "Roller": { "open": true - }, - "open": true + } }, "Odometry": { "Position": { diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 6237a00..0c0c556 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -43,6 +43,5 @@ public enum Subsystem { VISION } - public static final Set REAL_SUBSYSTEMS = - EnumSet.of(Subsystem.INTAKE, Subsystem.SHOOTER); + public static final Set REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM, Subsystem.INTAKE); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2be6e8..9fa4114 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,12 +2,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Telemetry; import frc.robot.arm.Arm; +import frc.robot.arm.ArmState; import frc.robot.auto.Auto; import frc.robot.climber.Climber; import frc.robot.intake.Intake; +import frc.robot.intake.IntakeConstants.PivotMotorConstants; import frc.robot.lights.Lights; import frc.robot.odometry.Odometry; import frc.robot.shooter.Shooter; @@ -68,25 +71,24 @@ private void configureBindings() { driverController.y().onTrue(odometry.tare()); driverController.x().whileTrue(swerve.cross()); - // operatorController - // .leftTrigger() - // .whileTrue( - // Commands.parallel(arm.to(ArmState.INTAKE), intake.out()) - // .andThen(Commands.parallel(intake.intake(), - // shooter.intake()))).onFalse(Commands.runOnce(() -> - // intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE))); + Command toIntake = Commands.parallel( + Commands.waitUntil(intake::isNotStowed).andThen(arm.to(ArmState.INTAKE)), + intake.out()); - // operatorController - // .rightTrigger() - // .whileTrue(Commands.parallel(arm.to(ArmState.SHOOT)).andThen(shooter.shoot())); + Command toStow = Commands.runOnce(() -> { + arm.setGoal(ArmState.STOW); + intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE); + }); - // operatorController.rightBumper().whileTrue(shooter.shoot()); + operatorController + .leftTrigger() + .whileTrue( + toIntake + .andThen(Commands.parallel(intake.intake(), shooter.intake()))).onFalse(toStow); - // operatorController.a().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.AMP))); - // operatorController.b().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.STOW))); - - operatorController.a().whileTrue(intake.in()); - operatorController.b().whileTrue(intake.out()); + operatorController + .rightTrigger() + .whileTrue(arm.to(ArmState.SHOOT).andThen(shooter.shoot())).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW))); } /** diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index f7c22b9..887a4d6 100644 --- a/src/main/java/frc/robot/RobotMechanisms.java +++ b/src/main/java/frc/robot/RobotMechanisms.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.arm.ArmConstants.ElbowMotorConstants; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; +import frc.robot.arm.ArmConstants.WristMotorConstants; import frc.robot.arm.ArmState; import frc.robot.intake.IntakeConstants.PivotMotorConstants; @@ -20,7 +20,7 @@ public class RobotMechanisms { private final Mechanism2d mechanism; - private MechanismLigament2d shoulder, elbow, intake; + private MechanismLigament2d shoulder, wrist, intake; private final double WIDTH = 2 @@ -55,11 +55,11 @@ private void initializeArmMechanism() { 0, armThickness, new Color8Bit(Color.kOrange))); - elbow = + wrist = shoulder.append( new MechanismLigament2d( - "elbow", - ElbowMotorConstants.JOINT_CONSTANTS.lengthMeters(), + "wrist", + WristMotorConstants.JOINT_CONSTANTS.lengthMeters(), 0, armThickness, new Color8Bit(Color.kGreen))); @@ -127,11 +127,10 @@ public Mechanism2d getMechanism() { public void setArmState(ArmState state) { Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position); - Rotation2d elbowRotation = Rotation2d.fromRotations(state.elbow().position); - // Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position); + Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position); shoulder.setAngle(shoulderRotation); - elbow.setAngle(elbowRotation.minus(shoulderRotation)); + wrist.setAngle(wristRotation.plus(Rotation2d.fromDegrees(90))); } public void setIntakeAngle(Rotation2d angle) { diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index d832993..d6bf103 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -8,12 +8,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.TwoJointedArmFeedforward; -import frc.lib.TwoJointedArmFeedforward.TwoJointedArmFeedforwardResult; import frc.robot.RobotMechanisms; -import frc.robot.arm.ArmConstants.ElbowMotorConstants; -import frc.robot.arm.ArmConstants.ShoulderMotorConstants; -import frc.robot.arm.ElbowMotorIO.ElbowMotorIOValues; +import frc.robot.arm.WristMotorIO.WristMotorIOValues; import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues; /** Subsystem class for the arm subsystem. */ @@ -28,29 +24,28 @@ public class Arm extends Subsystem { /** Shoulder motor values. */ private final ShoulderMotorIOValues shoulderMotorValues = new ShoulderMotorIOValues(); - /** Elbow motor. */ - private final ElbowMotorIO elbowMotor; + /** Wrist motor. */ + private final WristMotorIO wristMotor; - /** Elbow motor values. */ - private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues(); + /** Wrist motor values. */ + private final WristMotorIOValues wristMotorValues = new WristMotorIOValues(); - private ArmState goal, setpoint; + private final ArmState initialState = ArmState.STOW; - private final TwoJointedArmFeedforward feedforward; + private ArmState goal, setpoint; /** Creates a new instance of the arm subsystem. */ private Arm() { shoulderMotor = ArmFactory.createShoulderMotor(); - elbowMotor = ArmFactory.createElbowMotor(); + wristMotor = ArmFactory.createWristMotor(); - setPosition(ArmState.STOW.withElbow(Rotation2d.fromDegrees(0))); + shoulderMotor.configure(); + wristMotor.configure(); - goal = getPosition(); - setpoint = getPosition(); + setPosition(initialState); - feedforward = - new TwoJointedArmFeedforward( - ShoulderMotorConstants.JOINT_CONSTANTS, ElbowMotorConstants.JOINT_CONSTANTS); + goal = initialState.withWrist(ArmState.STOW.wrist()); + setpoint = getPosition(); } /** @@ -69,8 +64,9 @@ public static Arm getInstance() { @Override public void periodic() { shoulderMotor.update(shoulderMotorValues); + wristMotor.update(wristMotorValues); - setSetpoint(getSetpoint().nextSetpoint(goal)); + setSetpoint(setpoint.nextSetpoint(goal)); RobotMechanisms.getInstance().setArmState(getPosition()); } @@ -83,15 +79,13 @@ public void addToShuffleboard(ShuffleboardTab tab) { "Shoulder Position (deg)", () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations)); position.addDouble( - "Elbow Position (deg)", () -> Units.rotationsToDegrees(elbowMotorValues.positionRotations)); + "Wrist Position (deg)", () -> Units.rotationsToDegrees(wristMotorValues.positionRotations)); ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint"); setpoint.addDouble( "Shoulder Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().shoulder().position)); - setpoint.addDouble( - "Elbow Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().elbow().position)); setpoint.addDouble( "Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().wrist().position)); setpoint.addBoolean("At Setpoint?", this::atSetpoint); @@ -100,8 +94,6 @@ public void addToShuffleboard(ShuffleboardTab tab) { goal.addDouble( "Shoulder Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().shoulder().position)); - goal.addDouble( - "Elbow Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().elbow().position)); goal.addDouble( "Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().wrist().position)); goal.addBoolean("At Goal?", this::atGoal); @@ -109,7 +101,7 @@ public void addToShuffleboard(ShuffleboardTab tab) { ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages"); voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.appliedVolts); - voltages.addDouble("Elbow Voltage (V)", () -> elbowMotorValues.appliedVolts); + voltages.addDouble("Wrist Voltage (V)", () -> wristMotorValues.appliedVolts); ShuffleboardLayout derivatives = Telemetry.addColumn(tab, "Derivatives"); @@ -119,21 +111,15 @@ public void addToShuffleboard(ShuffleboardTab tab) { "Shoulder Acceleration (rpsps)", () -> shoulderMotorValues.accelerationRotationsPerSecondPerSecond); derivatives.addDouble( - "Elbow Velocity (rps)", () -> elbowMotorValues.velocityRotationsPerSecond); + "Wrist Velocity (rps)", () -> wristMotorValues.velocityRotationsPerSecond); derivatives.addDouble( - "Elbow Acceleration (rpsps)", - () -> elbowMotorValues.accelerationRotationsPerSecondPerSecond); - - ShuffleboardLayout feedforward = Telemetry.addColumn(tab, "Feedforward"); - - feedforward.addDouble("Shoulder Feedforward (V)", () -> calculateFeedforward().shoulderVolts()); - feedforward.addDouble("Elbow Feedforward (V)", () -> calculateFeedforward().elbowVolts()); + "Wrist Acceleration (rpsps)", + () -> wristMotorValues.accelerationRotationsPerSecondPerSecond); } public void setPosition(ArmState state) { shoulderMotor.setPosition(state.shoulder().position); - elbowMotor.setPosition(state.elbow().position); - // wristMotor.setPosition(state.wrist().getRotations()); + wristMotor.setPosition(state.wrist().position); } /** @@ -143,12 +129,11 @@ public void setPosition(ArmState state) { */ public ArmState getPosition() { shoulderMotor.update(shoulderMotorValues); - elbowMotor.update(elbowMotorValues); + wristMotor.update(wristMotorValues); return new ArmState( Rotation2d.fromRotations(shoulderMotorValues.positionRotations), - Rotation2d.fromRotations(elbowMotorValues.positionRotations), - Rotation2d.fromRotations(0)); + Rotation2d.fromRotations(wristMotorValues.positionRotations)); } public ArmState getGoal() { @@ -175,19 +160,10 @@ private void setSetpoint(ArmState setpoint) { this.setpoint = setpoint; shoulderMotor.setSetpoint(setpoint.shoulder().position, setpoint.shoulder().velocity); - elbowMotor.setSetpoint(setpoint.elbow().position, setpoint.elbow().velocity); - // wristMotor.runSetpoint(state.wrist().getRotations()); + wristMotor.setSetpoint(setpoint.wrist().position, setpoint.wrist().velocity); } public Command to(ArmState goal) { return runOnce(() -> setGoal(goal)).andThen(Commands.waitUntil(this::atGoal)); } - - public TwoJointedArmFeedforwardResult calculateFeedforward() { - ArmState position = getPosition(); - - return feedforward.calculateFeedforward( - Rotation2d.fromRotations(position.shoulder().position), - Rotation2d.fromRotations(position.elbow().position)); - } } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index 368c8b8..22953d3 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -29,7 +29,7 @@ public static class ShoulderMotorConstants { 1); /** Minimum angle of the shoulder joint. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(12.5); + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(27.5); /** Maximum angle of the shoulder joint. */ public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90); @@ -59,46 +59,49 @@ public static class ShoulderMotorConstants { public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); } - /** Constants for the elbow motor. */ - public static class ElbowMotorConstants { - /** Elbow motor CAN. */ - public static final CAN CAN = new CAN(32); // TODO + /** Constants for the wrist motor. */ + public static class WristMotorConstants { + /** Wrist motor CAN. */ + public static final CAN CAN = new CAN(34); - /** Joint constants for the elbow joint. */ + /** If true, invert the motor. */ + public static final boolean MOTOR_INVERT = true; + + /** Joint constants for the wrist joint. */ public static final JointConstants JOINT_CONSTANTS = new JointConstants( - Units.lbsToKilograms(13.006), // massKg - Units.inchesToMeters(16.825), // lengthMeters - Units.inchesToMeters(12.251799915), // radiusMeters - 0.5713, - 39.2911765, + Units.lbsToKilograms(8.016), // massKg + Units.inchesToMeters(5.135), // lengthMeters + Units.inchesToMeters(3.47629), // radiusMeters + 0.02835, + 20.454545, DCMotor.getNEO(1), // motor 1); - /** Minimum angle of the elbow joint. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-90); + /** Minimum angle of the wrist joint. */ + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-86.759); - /** Maximum angle of the elbow joint. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(180); + /** Maximum angle of the wrist joint. */ + public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(85.98); - /** Tolerance of the elbow joint. */ + /** Tolerance of the wrist joint. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); /** Proportional gain in volts per rotation. */ public static final double KP = 48.0; - /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 1.5; + /** Maximum speed of the wrist joint in rotations per second. */ + public static final double MAXIMUM_SPEED = 2.4; - /** Maximum acceleration of the shoulder joint in rotations per second per second. */ + /** Maximum acceleration of the wrist joint in rotations per second per second. */ public static final double MAXIMUM_ACCELERATION = - MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3); + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25); - /** Maximum speed and acceleration of the shoulder joint. */ + /** Maximum speed and acceleration of the wrist joint. */ public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); - /** Motion profile of the shoulder joint using constraints. */ + /** Motion profile of the wrist joint using constraints. */ public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); } } diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java index e266328..42e2a12 100644 --- a/src/main/java/frc/robot/arm/ArmFactory.java +++ b/src/main/java/frc/robot/arm/ArmFactory.java @@ -13,8 +13,8 @@ public class ArmFactory { * @return a shoulder motor. */ public static ShoulderMotorIO createShoulderMotor() { - if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) - return new ShoulderMotorIOSparkMax(); + // if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) + // return new ShoulderMotorIOSparkMax(); return new ShoulderMotorIOSim(); } @@ -24,7 +24,10 @@ public static ShoulderMotorIO createShoulderMotor() { * * @return an elbow motor. */ - public static ElbowMotorIO createElbowMotor() { - return new ElbowMotorIOSim(); + public static WristMotorIO createWristMotor() { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) + return new WristMotorIOSparkMax(); + + return new WristMotorIOSim(); } } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index e48dc48..3daa66e 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -4,41 +4,27 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import frc.robot.RobotConstants; -import frc.robot.arm.ArmConstants.ElbowMotorConstants; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; +import frc.robot.arm.ArmConstants.WristMotorConstants; + import java.util.Objects; /** State of the arm. */ -public record ArmState(State shoulder, State elbow, State wrist) { - - public static final ArmState STOW = - new ArmState( - Rotation2d.fromDegrees(12.5), - Rotation2d.fromDegrees(180 - 18.125), - Rotation2d.fromDegrees(0)); - - public static final ArmState SHOOT = STOW.withElbow(Rotation2d.fromDegrees(180 - 35)); - - public static final ArmState INTAKE = - new ArmState( - Rotation2d.fromDegrees(90 - 25.6), - Rotation2d.fromDegrees(-16.325), - Rotation2d.fromDegrees(-50)); +public record ArmState(State shoulder, State wrist) { - public static final ArmState AMP = - new ArmState( - Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(0)); + public static final ArmState UP_SHOOTER_INSIDE = new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, WristMotorConstants.MINIMUM_ANGLE); + public static final ArmState STOW = new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE); + public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(23.265)); + public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(6.81)); /** * Creates an arm state. * * @param shoulder the shoulder's state. - * @param elbow the elbow's state. * @param wrist the wrist's state. */ public ArmState { Objects.requireNonNull(shoulder); - Objects.requireNonNull(elbow); Objects.requireNonNull(wrist); } @@ -46,13 +32,11 @@ public record ArmState(State shoulder, State elbow, State wrist) { * Creates an arm state. * * @param shoulder the shoulder's rotation. - * @param elbow the elbow's rotation. * @param wrist the wrist's rotation. */ - public ArmState(Rotation2d shoulder, Rotation2d elbow, Rotation2d wrist) { + public ArmState(Rotation2d shoulder, Rotation2d wrist) { this( new State(shoulder.getRotations(), 0.0), - new State(elbow.getRotations(), 0.0), new State(wrist.getRotations(), 0.0)); } @@ -73,27 +57,7 @@ public ArmState withShoulder(Rotation2d newShoulder) { * @return a copy of this arm state with a new shoulder state. */ public ArmState withShoulder(State newShoulder) { - return new ArmState(newShoulder, elbow, wrist); - } - - /** - * Copies this arm state with a new elbow rotation. - * - * @param newElbow the new elbow rotation. - * @return a copy of this arm state with a new elbow rotation. - */ - public ArmState withElbow(Rotation2d newElbow) { - return withElbow(new State(newElbow.getRotations(), 0.0)); - } - - /** - * Copies this arm state with a new elbow state. - * - * @param newElbow the new elbow state. - * @return a copy of this arm state with a new elbow state. - */ - public ArmState withElbow(State newElbow) { - return new ArmState(shoulder, newElbow, wrist); + return new ArmState(newShoulder, wrist); } /** @@ -113,7 +77,7 @@ public ArmState withWrist(Rotation2d newWrist) { * @return a copy of this arm state with a new wrist state. */ public ArmState withWrist(State newWrist) { - return new ArmState(shoulder, elbow, newWrist); + return new ArmState(shoulder, newWrist); } /** @@ -123,29 +87,78 @@ public ArmState withWrist(State newWrist) { * @return true if the arm states are equal. */ public boolean at(ArmState other) { - boolean atShoulder = - MathUtil.isNear( + return atShoulderGoal(other) && atWristGoal(other); + } + + /** + * Returns true if the arm is at its shoulder goal. + * + * @param goal goal state of the arm. + * @return true if the arm is at its shoulder goal. + */ + public boolean atShoulderGoal(ArmState goal) { + return MathUtil.isNear( this.shoulder().position, - other.shoulder().position, + goal.shoulder().position, ShoulderMotorConstants.TOLERANCE.getRotations()); - boolean atElbow = - MathUtil.isNear( - this.elbow().position, - other.elbow().position, - ElbowMotorConstants.TOLERANCE.getRotations()); + } + + /** + * Returns the next setpoint involving shoulder-only movement. + * + * @param goal the arm's goal state. + * @return the next setpoint involving shoulder-only movement. + */ + public ArmState nextShoulderSetpoint(ArmState goal) { + return this.withShoulder(ShoulderMotorConstants.MOTION_PROFILE.calculate( + RobotConstants.PERIODIC_DURATION, this.shoulder, goal.shoulder)); + } + + /** + * Returns true if the arm is at its wrist goal. + * + * @param goal goal state of the arm. + * @return true if the arm is at its wrist goal. + */ + public boolean atWristGoal(ArmState goal) { + return MathUtil.isNear( + this.wrist().position, + goal.wrist().position, + WristMotorConstants.TOLERANCE.getRotations()); + } - return atShoulder && atElbow; + /** + * Returns the next setpoint involving wrist-only movement. + * + * @param goal the arm's goal state. + * @return the next setpoint involving wrist-only movement. + */ + public ArmState nextWristSetpoint(ArmState goal) { + return this.withWrist(WristMotorConstants.MOTION_PROFILE.calculate( + RobotConstants.PERIODIC_DURATION, this.wrist, goal.wrist)); } + /** + * Returns the next overall setpoint of the arm's movement. + * + * @param goal the arm's goal state. + * @return the next overall setpoint. + */ public ArmState nextSetpoint(ArmState goal) { - State nextShoulderState = - ShoulderMotorConstants.MOTION_PROFILE.calculate( - RobotConstants.PERIODIC_DURATION, this.shoulder, goal.shoulder); + boolean shooterOnBottom = Rotation2d.fromRotations(wrist.position).getDegrees() < 0.0; + boolean shooterNeedsToBeOnTop = Rotation2d.fromRotations(goal.wrist.position).getDegrees() > 0.0; + boolean shooterOnWrongSide = shooterOnBottom && shooterNeedsToBeOnTop; + + if (shooterOnWrongSide && !atWristGoal(goal)) { + return nextWristSetpoint(goal); + } - State nextElbowState = - ElbowMotorConstants.MOTION_PROFILE.calculate( - RobotConstants.PERIODIC_DURATION, this.elbow, goal.elbow); + if (!atShoulderGoal(goal) ) { + return nextShoulderSetpoint(goal); + } else if (!atWristGoal(goal)) { + return nextWristSetpoint(goal); + } - return new ArmState(nextShoulderState, nextElbowState, new State(0.0, 0.0)); + return this; } } diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java index c9803d5..011b877 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java @@ -4,9 +4,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.AccelerationCalculator; -import frc.lib.ArmFeedforwardCalculator; import frc.lib.Configurator; import frc.lib.SingleJointedArmFeedforward; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; @@ -34,8 +32,7 @@ public ShoulderMotorIOSparkMax() { feedforward = new SingleJointedArmFeedforward( 0, - ArmFeedforwardCalculator.calculateArmGravityCompensation( - Rotation2d.fromDegrees(18.0), 0.1222), + 0, 0); accelerationCalculator = new AccelerationCalculator(); @@ -50,7 +47,7 @@ public void configure() { @Override public void update(ShoulderMotorIOValues values) { - values.positionRotations = getPositionRotations(); + values.positionRotations = getAbsolutePositionRotations(); values.velocityRotationsPerSecond = sparkMax.getEncoder().getVelocity() / ShoulderMotorConstants.JOINT_CONSTANTS.gearing(); values.accelerationRotationsPerSecondPerSecond = @@ -69,14 +66,14 @@ public void setPosition(double positionRotations) { @Override public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - double measuredPositionRotations = getPositionRotations(); + // double measuredPositionRotations = getAbsolutePositionRotations(); - double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); + // double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); - double feedforwardVolts = - feedforward.calculate(Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond); + // double feedforwardVolts = + // feedforward.calculate(Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond); - sparkMax.setVoltage(feedbackVolts + feedforwardVolts); + // sparkMax.setVoltage(feedbackVolts + feedforwardVolts); } /** @@ -84,7 +81,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec * * @return the absolute position of the shoulder in rotations. */ - private double getPositionRotations() { + private double getAbsolutePositionRotations() { return sparkMax.getEncoder().getPosition() / ShoulderMotorConstants.JOINT_CONSTANTS.gearing(); } } diff --git a/src/main/java/frc/robot/arm/ElbowMotorIO.java b/src/main/java/frc/robot/arm/WristMotorIO.java similarity index 92% rename from src/main/java/frc/robot/arm/ElbowMotorIO.java rename to src/main/java/frc/robot/arm/WristMotorIO.java index ffc02d6..80bbbd6 100644 --- a/src/main/java/frc/robot/arm/ElbowMotorIO.java +++ b/src/main/java/frc/robot/arm/WristMotorIO.java @@ -1,9 +1,9 @@ package frc.robot.arm; /** Elbow motor hardware interface. */ -public interface ElbowMotorIO { +public interface WristMotorIO { /** Values for the elbow motor hardware interface. */ - public static class ElbowMotorIOValues { + public static class WristMotorIOValues { /** Position of the elbow motor in rotations. */ public double positionRotations = 0.0; @@ -28,7 +28,7 @@ public static class ElbowMotorIOValues { * * @param values */ - public void update(ElbowMotorIOValues values); + public void update(WristMotorIOValues values); /** * Sets the elbow motor's position. diff --git a/src/main/java/frc/robot/arm/ElbowMotorIOSim.java b/src/main/java/frc/robot/arm/WristMotorIOSim.java similarity index 73% rename from src/main/java/frc/robot/arm/ElbowMotorIOSim.java rename to src/main/java/frc/robot/arm/WristMotorIOSim.java index c40fbf4..299b4a8 100644 --- a/src/main/java/frc/robot/arm/ElbowMotorIOSim.java +++ b/src/main/java/frc/robot/arm/WristMotorIOSim.java @@ -5,10 +5,11 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.lib.AccelerationCalculator; import frc.robot.RobotConstants; -import frc.robot.arm.ArmConstants.ElbowMotorConstants; +import frc.robot.arm.ArmConstants.ShoulderMotorConstants; +import frc.robot.arm.ArmConstants.WristMotorConstants; /** Simulated elbow motor. */ -public class ElbowMotorIOSim implements ElbowMotorIO { +public class WristMotorIOSim implements WristMotorIO { private final SingleJointedArmSim singleJointedArmSim; @@ -19,19 +20,20 @@ public class ElbowMotorIOSim implements ElbowMotorIO { private double appliedVolts; /** Creates a new simulated elbow motor. */ - public ElbowMotorIOSim() { + public WristMotorIOSim() { + // TODO singleJointedArmSim = new SingleJointedArmSim( - ElbowMotorConstants.JOINT_CONSTANTS.motor(), - ElbowMotorConstants.JOINT_CONSTANTS.gearing(), - ElbowMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(), - ElbowMotorConstants.JOINT_CONSTANTS.lengthMeters(), - ElbowMotorConstants.MINIMUM_ANGLE.getRadians(), - ElbowMotorConstants.MAXIMUM_ANGLE.getRadians(), + WristMotorConstants.JOINT_CONSTANTS.motor(), + WristMotorConstants.JOINT_CONSTANTS.gearing(), + ShoulderMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(), + ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters() * 0.5, + WristMotorConstants.MINIMUM_ANGLE.getRadians(), + WristMotorConstants.MAXIMUM_ANGLE.getRadians(), false, 0.0); - feedback = new PIDController(ElbowMotorConstants.KP, 0, 0); + feedback = new PIDController(WristMotorConstants.KP, 0, 0); accelerationCalculator = new AccelerationCalculator(); } @@ -40,7 +42,7 @@ public ElbowMotorIOSim() { public void configure() {} @Override - public void update(ElbowMotorIOValues values) { + public void update(WristMotorIOValues values) { singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION); values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads()); diff --git a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java new file mode 100644 index 0000000..00c7da6 --- /dev/null +++ b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java @@ -0,0 +1,98 @@ +package frc.robot.arm; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.AccelerationCalculator; +import frc.lib.Configurator; +import frc.lib.SingleJointedArmFeedforward; +import frc.robot.arm.ArmConstants.WristMotorConstants; + +public class WristMotorIOSparkMax implements WristMotorIO { + + private final CANSparkMax sparkMax; + + /** Feedback controller for wrist position. */ + private final PIDController feedback; + + /** Feedforward controller for wrist position. */ + private final SingleJointedArmFeedforward feedforward; + + private final AccelerationCalculator accelerationCalculator; + + public WristMotorIOSparkMax() { + sparkMax = new CANSparkMax(WristMotorConstants.CAN.id(), MotorType.kBrushless); + + feedback = new PIDController(WristMotorConstants.KP, 0, 0); + + feedforward = + new SingleJointedArmFeedforward( + 0, + 0, + 0); + + accelerationCalculator = new AccelerationCalculator(); + } + + @Override + public void configure() { + Configurator.configureREV(sparkMax::restoreFactoryDefaults); + + sparkMax.setInverted(WristMotorConstants.MOTOR_INVERT); + + Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake)); + } + + @Override + public void update(WristMotorIOValues values) { + values.positionRotations = getRelativePositionRotations(); + values.velocityRotationsPerSecond = + sparkMax.getEncoder().getVelocity() / WristMotorConstants.JOINT_CONSTANTS.gearing(); + values.accelerationRotationsPerSecondPerSecond = + accelerationCalculator.calculate(values.velocityRotationsPerSecond); + + values.currentAmps = sparkMax.getOutputCurrent(); + values.appliedVolts = sparkMax.getAppliedOutput() * sparkMax.getBusVoltage(); + } + + @Override + public void setPosition(double positionRotations) { + sparkMax + .getEncoder() + .setPosition(positionRotations * WristMotorConstants.JOINT_CONSTANTS.gearing()); + } + + @Override + public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { + double measuredPositionRotations = getRelativePositionRotations(); + + double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); + + double feedforwardVolts = feedforward.calculate(Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond); + + setVoltage(feedbackVolts + feedforwardVolts); + } + + @Override + public void setVoltage(double volts) { + sparkMax.setVoltage(volts); + } + + @Override + public void stop() { + setVoltage(0); + } + + /** + * Gets the relative position of the wrist in rotations. + * + * @return the relative position of the wrist in rotations. + */ + private double getRelativePositionRotations() { + return sparkMax.getEncoder().getPosition() / WristMotorConstants.JOINT_CONSTANTS.gearing(); + } + +} diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index d120cc7..cafe1d5 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -105,6 +105,7 @@ public void addToShuffleboard(ShuffleboardTab tab) { "Setpoint (deg)", () -> Units.rotationsToDegrees(pivotSetpoint.position)); pivot.addDouble( "Goal (deg)", () -> Units.rotationsToDegrees(pivotGoal.position)); + pivot.addBoolean("Is Not Stowed?", this::isNotStowed); ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller"); @@ -158,6 +159,10 @@ private Command pivotTo(Rotation2d angle) { return runOnce(() -> setPivotGoal(angle)).andThen(Commands.waitUntil(this::atPivotGoal)); } + public boolean isNotStowed() { + return pivotMotorValues.positionRotations < PivotMotorConstants.OUT_ANGLE.getRotations(); + } + public Command intake() { return Commands.run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE)) .finallyDo(rollerMotor::stop); diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index d38b555..b8c9d0b 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -39,6 +39,9 @@ public static class PivotMotorConstants { /** Pivot motor's maximum angle. */ public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(86); + /** Pivot motor's "out" angle. */ + public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(-10); + /** Pivot motor's tolerance. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(4.0); diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java index 035b039..85c2fb2 100644 --- a/src/main/java/frc/robot/intake/IntakeFactory.java +++ b/src/main/java/frc/robot/intake/IntakeFactory.java @@ -25,8 +25,8 @@ public static PivotMotorIO createPivotMotor() { * @return a roller motor. */ public static RollerMotorIO createRollerMotor() { - if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) - return new RollerMotorIOSparkMax(); + // if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) + // return new RollerMotorIOSparkMax(); return new RollerMotorIOSim(); } diff --git a/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java b/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java index 6cf2840..bb05caa 100644 --- a/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java +++ b/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java @@ -3,11 +3,8 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import frc.lib.SingleJointedArmFeedforward; import frc.lib.ArmFeedforwardCalculator; import frc.robot.intake.IntakeConstants.PivotMotorConstants; @@ -27,6 +24,7 @@ public PivotMotorIOTalonSRX() { feedback = new PIDController(PivotMotorConstants.KP, 0, 0); + // TODO double kg = ArmFeedforwardCalculator.calculateArmGravityCompensation(Rotation2d.fromDegrees(26), 1.8); feedforward = new SingleJointedArmFeedforward(0, kg, 0); @@ -36,8 +34,8 @@ public PivotMotorIOTalonSRX() { public void configure() { talonSRX.configFactoryDefault(); - talonSRX.setInverted(PivotMotorConstants.IS_MOTOR_INVERTED); talonSRX.setSensorPhase(PivotMotorConstants.IS_SENSOR_INVERTED); + talonSRX.setInverted(PivotMotorConstants.IS_MOTOR_INVERTED); talonSRX.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); }