diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 0dc93f46..3d55200b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -86,13 +86,13 @@ public static final class Manipulator { public static final int port = 1; // Arm commands - public static final int RAISE_TO_SPEAKER_POD_BUTTON = Button.kY.value; - public static final int RAISE_TO_AMP_BUTTON = Button.kB.value; - public static final int RAISE_TO_SPEAKER_SAFE_BUTTON = Button.kA.value; - public static final int RAISE_TO_SPEAKER_NEXT_BUTTON = Button.kX.value; - public static final int RAISE_TO_GROUND_BUTTON = Button.kStart.value; - public static final int RAISE_TO_CLIMBER_BUTTON = Button.kLeftBumper.value; - public static final int LOWER_TO_CLIMBER_BUTTON = Button.kRightBumper.value; + public static final int raiseToSpeakerPodButton = Button.kY.value; + public static final int raiseToAmpButton = Button.kB.value; + public static final int raiseToSpeakerSafeButton = Button.kA.value; + public static final int raiseToSpeakerNextButton = Button.kX.value; + public static final int raiseToGroundButton = Button.kStart.value; + public static final int raiseToClimberButton = Button.kLeftBumper.value; + public static final int lowerToClimberButton = Button.kRightBumper.value; } } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 2c0e6c74..b743ae21 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -6,8 +6,8 @@ //199 files // import org.carlmontrobotics.subsystems.*; import org.carlmontrobotics.commands.*; -import static org.carlmontrobotics.Constants.OI.Manipulator.*; -import static org.carlmontrobotics.Constants.Arm.*; +import static org.carlmontrobotics.Constants.OI; +import static org.carlmontrobotics.Constants.Arm.ampAngle; import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.subsystems.Arm; @@ -72,12 +72,15 @@ private void setBindingsManipulator() { //right joystick used for manual arm control // Speaker Buttons - new JoystickButton(manipulatorController, RAISE_TO_SPEAKER_POD_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(SPEAKER_ANGLE);})); + new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerPodButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle1);})); + new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerNextButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle2);})); + new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerSafeButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle3);})); // Amp and Intake Buttons - new JoystickButton(manipulatorController, RAISE_TO_AMP_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(AMP_ANGLE);})); - new JoystickButton(manipulatorController, RAISE_TO_GROUND_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(INTAKE_ANGLE);})); + new JoystickButton(manipulatorController, Constants.Arm.raiseToAmpButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.ampAngle);})); + new JoystickButton(manipulatorController, Constants.Arm.raiseToGroundButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.intakeAngle);})); // Cimber Buttons - new JoystickButton(manipulatorController, RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(CLIMB_ANGLE);})); + new JoystickButton(manipulatorController, Constants.Arm.raiseToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberUpAngle);})); + new JoystickButton(manipulatorController, Constants.Arm.lowerToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberDownAngle);})); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java index 9ad9f2ac..07f27e53 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java @@ -14,18 +14,19 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj.DriverStation; + import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; + public class ArmTeleop extends Command { - private final DoubleSupplier joystick; + private final DoubleSupplier joystick; private final Arm armSubsystem; - private final double kDt = 0.02; + private double lastTime = 0; + private Timer armTimer = new Timer(); TrapezoidProfile.State goalState; - /** Creates a new ArmTeleop. */ public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) { // Use addRequirements() here to declare subsystem dependencies. @@ -36,24 +37,24 @@ public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) { // Called when the command is initially scheduled. @Override public void initialize() { - goalState = armSubsystem.getCurrentArmState(); + goalState = new TrapezoidProfile.State(armSubsystem.getArmPos(), armSubsystem.getArmVel()); + lastTime = Timer.getFPGATimestamp(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // use trapazoid math and controllerMoveArm method from arm subsytem to apply - // voltage to the motor - double speed = getRequestedSpeeds(); - - double goalArmRad = goalState.position + speed * kDt; - goalArmRad = MathUtil.clamp(goalArmRad, ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD); - goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, - armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); - - if ((speed != 0) && !DriverStation.isAutonomous()) { - armSubsystem.setArmTarget(goalArmRad); - } + // use trapazoid math and controllerMoveArm method from arm subsytem to apply voltage to the motor + double speeds = getRequestedSpeeds(); + double currTime = Timer.getFPGATimestamp(); + double deltaT = currTime - lastTime; + + double goalArmRad = goalState.position + speeds * deltaT; + goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT); + goalState = new TrapezoidProfile.State(goalArmRad, 0); + TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(armTimer.get(), armSubsystem.getCurrentArmState(), goalState); + armSubsystem.driveArm(setpoint.position); + lastTime = currTime; } public double getRequestedSpeeds() { @@ -70,6 +71,8 @@ public double getRequestedSpeeds() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + armTimer.stop(); + armTimer.reset(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 81e618fb..b424fb41 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -1,5 +1,4 @@ package org.carlmontrobotics.subsystems; - import static org.carlmontrobotics.Constants.Arm.*; import org.carlmontrobotics.commands.ArmTeleop; @@ -30,21 +29,20 @@ // TODO: FIGURE OUT ANGLES // Arm angle is measured from horizontal on the intake side of the robot and bounded between __ and __ public class Arm extends SubsystemBase { - + private final CANSparkMax masterArmMotor = MotorControllerFactory.createSparkMax(MASTER_ARM_MOTOR, MotorConfig.NEO); private final CANSparkMax followArmMotor = MotorControllerFactory.createSparkMax(FOLLOW_ARM_MOTOR, MotorConfig.NEO); - private final SparkAbsoluteEncoder armEncoder = masterArmMotor - .getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + private final SparkAbsoluteEncoder armEncoder = masterArmMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); private static double kDt = 0.02; - - // PID, feedforward, trap profile + + //PID, feedforward, trap profile private final ArmFeedforward armFeed = new ArmFeedforward(kG, kS, kV, kA); private final SparkPIDController armPID = masterArmMotor.getPIDController(); private TrapezoidProfile armProfile = new TrapezoidProfile(armConstraints); - - // TODO: put in correct initial position + + //TODO: put in correct initial position // rad, rad/s private static TrapezoidProfile.State goalState; private static TrapezoidProfile.State setpointState; @@ -58,7 +56,7 @@ public Arm() { armEncoder.setVelocityConversionFactor(rotationToRad); armEncoder.setInverted(encoderInverted); followArmMotor.follow(masterArmMotor); - + armEncoder.setZeroOffset(ENCODER_OFFSET); armPID.setFeedbackDevice(armEncoder); armPID.setPositionPIDWrappingEnabled(true); @@ -68,7 +66,7 @@ public Arm() { armPID.setP(kP); armPID.setI(kI); armPID.setD(kD); - + SmartDashboard.putData("Arm", this); setpointState = getCurrentArmState(); @@ -79,21 +77,19 @@ public Arm() { @Override public void periodic() { - if (DriverStation.isDisabled()) - resetGoal(); + if(DriverStation.isDisabled()) resetGoal(); driveArm(); autoCancelArmCommand(); } - public void autoCancelArmCommand() { - if (!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) - return; + public void autoCancelArmCommand() { + if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return; double requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds(); - if (requestedSpeeds != 0) { + if(requestedSpeeds != 0) { Command currentArmCommand = getCurrentCommand(); - if (currentArmCommand != getDefaultCommand() && currentArmCommand != null) { + if(currentArmCommand != getDefaultCommand() && currentArmCommand != null) { currentArmCommand.cancel(); } } @@ -103,23 +99,19 @@ public void autoCancelArmCommand() { // uses trapezoid profiles which supplies goal states to pid controller // feedforward controller is used to supply additional voltage to keep it // at its current position - // YOU DO NOT HAVE TO WORRY ABOUT HOW THE ARM DRIVES OUTSIDE OF SUBSYSTEM - public void driveArm() { + public void driveArm(){ setpointState = armProfile.calculate(kDt, setpointState, goalState); var currentPosition = getCurrentArmState(); double armFeedVolts = armFeed.calculate(currentPosition.position, currentPosition.velocity); // code to stop arm from moving past certain bounds - if ((getArmPos() > ARM_UPPER_LIMIT_RAD && currentPosition.velocity > 0) || - (getArmPos() < ARM_LOWER_LIMIT_RAD && currentPosition.velocity < 0)) { + if ((getArmPos() > ARM_UPPER_LIMIT_RAD && currentPosition.velocity > 0) || + (getArmPos() < ARM_LOWER_LIMIT_RAD && currentPosition.velocity < 0)) { armFeedVolts = armFeed.calculate(currentPosition.position, 0); } armPID.setReference(setpointState.position, CANSparkBase.ControlType.kPosition, 0, armFeedVolts); } - // sets target arm position - // automatically clamps target positions such that they do not exceed lower and - // upper limit public void setArmTarget(double targetPos) { targetPos = getArmClampedGoal(targetPos); goalState.position = targetPos; @@ -131,9 +123,9 @@ public void resetGoal() { setArmTarget(armPos); } - // #endregion + //#endregion - // #region Getters + //#region Getters public double getArmPos() { return MathUtil.inputModulus(armEncoder.getPosition(), ARM_DISCONTINUITY_RAD, @@ -143,22 +135,21 @@ public double getArmPos() { public double getArmVel() { return armEncoder.getVelocity(); } - + public TrapezoidProfile.State getCurrentArmState() { return new TrapezoidProfile.State(getArmPos(), getArmVel()); } - + public TrapezoidProfile.State getCurrentArmGoal() { return goalState; } - + public boolean armAtGoal() { return Math.abs(getArmPos() - goalState.position) < posToleranceRad && - Math.abs(getArmVel() - goalState.velocity) < velToleranceRadPSec; + Math.abs(getArmVel() - goalState.velocity) < velToleranceRadPSec; } public double getArmClampedGoal(double goal) { - return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI), - ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD); + return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI), ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD); } } \ No newline at end of file