From b5d8181a78f7f99e91b4fb5f682317f19fb8352d Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 21 May 2024 12:45:25 -0700 Subject: [PATCH 1/4] Use lib199 with compliant names for sim devices and data. --- build.gradle | 2 +- src/main/java/org/carlmontrobotics/subsystems/Arm.java | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index 8de1a22e..f8cd2f01 100644 --- a/build.gradle +++ b/build.gradle @@ -80,7 +80,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - implementation "com.github.deepbluerobotics:lib199:69297b35a4fd817d3479963c5819685ddfc775fd" + implementation "com.github.deepbluerobotics:lib199:09795c035fe232fa095e6e1d2bc7d68abe8cb4a1" } test { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index dee74618..30c687a3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -160,7 +160,7 @@ public Arm() { armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); SmartDashboard.putBoolean("arm is at pos", false); if (RobotBase.isSimulation()) { - rotationsSim = new SimDeviceSim("AbsoluteEncoder", ARM_MOTOR_PORT_MASTER).getDouble("rotations"); + rotationsSim = new SimDeviceSim("CANDutyCycle:CANSparkMax", ARM_MOTOR_PORT_MASTER).getDouble("position"); } } @@ -463,9 +463,10 @@ public void setDefaultCommand(TeleopArm teleopArm, Object object) { @Override public void simulationPeriodic() { - // Fake goaling to the goal instantaneously + // Fake going to the goal instantaneously if (rotationsSim != null) { - rotationsSim.set((goalState.position + armMasterEncoder.getZeroOffset()) + rotationsSim.set((goalState.position - armMasterEncoder.getZeroOffset()) + * (armMasterEncoder.getInverted() ? -1.0 : 1.0) / armMasterEncoder.getPositionConversionFactor()); } } From 2a4b389df2179697fd9f881a5b9cfb50996057e7 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 21 May 2024 13:29:35 -0700 Subject: [PATCH 2/4] Bump lib199 commit hash to include latest commit on the compliance branch. --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index f8cd2f01..cadf983d 100644 --- a/build.gradle +++ b/build.gradle @@ -80,7 +80,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - implementation "com.github.deepbluerobotics:lib199:09795c035fe232fa095e6e1d2bc7d68abe8cb4a1" + implementation "com.github.deepbluerobotics:lib199:bf0533fc089c1245059709b1eb78c3b8c99fbfb3" } test { From 5a198cf5aa45f558713339884d5a04223c3ef8fd Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 20 Jun 2024 14:21:13 -0700 Subject: [PATCH 3/4] Bump lib199 version. --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index bc8e70b6..5639c521 100644 --- a/build.gradle +++ b/build.gradle @@ -80,7 +80,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - implementation "com.github.deepbluerobotics:lib199:2accd8b3c2f41d2ac2fea51987c9289e8d5930c5" + implementation "com.github.deepbluerobotics:lib199:400693b52180c82a1f26c08025bc12f144b930f9" } test { From 85ae2c35ffde01a4a94b2170000f2588042612a3 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 20 Jun 2024 14:33:39 -0700 Subject: [PATCH 4/4] Revert unrelated formatting changes. --- .../org/carlmontrobotics/subsystems/Arm.java | 221 ++++++++---------- 1 file changed, 93 insertions(+), 128 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 4d6afc97..4984d6a2 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -49,16 +49,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -// Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 -// and π/2 +// Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2 public class Arm extends SubsystemBase { - + private boolean callDrive = true; - private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory - .createSparkMax(ARM_MOTOR_PORT_MASTER, MotorConfig.NEO); - private final CANSparkMax armMotorFollower/* right */ = - MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_FOLLOWER, - MotorConfig.NEO); + private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_MASTER, + MotorConfig.NEO); + private final CANSparkMax armMotorFollower/* right */ = MotorControllerFactory + .createSparkMax(ARM_MOTOR_PORT_FOLLOWER, MotorConfig.NEO); private final SparkAbsoluteEncoder armMasterEncoder = armMotorMaster .getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); @@ -69,8 +67,7 @@ public class Arm extends SubsystemBase { // rel offset = starting absolute offset private double armFeedVolts;// for SendableBuilder private final ArmFeedforward armFeed = new ArmFeedforward(kS, kG, kV, kA); - private final SparkPIDController armPIDMaster = - armMotorMaster.getPIDController(); + private final SparkPIDController armPIDMaster = armMotorMaster.getPIDController(); private TrapezoidProfile.State setpoint = getCurrentArmState(); private TrapezoidProfile armProfile; @@ -83,12 +80,11 @@ public class Arm extends SubsystemBase { private boolean isArmEncoderConnected = false; private final MutableMeasure voltage = mutable(Volts.of(0)); - private final MutableMeasure> velocity = - mutable(RadiansPerSecond.of(0)); + private final MutableMeasure> velocity = mutable(RadiansPerSecond.of(0)); private final MutableMeasure distance = mutable(Radians.of(0)); private static boolean babyMode; private ShuffleboardTab sysIdTab = Shuffleboard.getTab("arm SysID"); - private boolean setPIDOff; + private boolean setPIDOff; private SimDouble rotationsSim; @@ -106,7 +102,7 @@ public Arm() { armMasterEncoder.setZeroOffset(ENCODER_OFFSET_RAD); // ------------------------------------------------------------ - + armMotorFollower.follow(armMotorMaster, MOTOR_INVERTED_FOLLOWER); setPIDOff = false; armPIDMaster.setP(kP); @@ -128,7 +124,7 @@ public Arm() { armPIDMaster.setPositionPIDWrappingMaxInput((3 * Math.PI) / 2); armPIDMaster.setIZone(IZONE_RAD); - + SmartDashboard.putData("Arm", this); @@ -138,33 +134,31 @@ public Arm() { // sysid // sysid buttons on smartdashbaord; sysid tab name is arm sysid - sysIdTab.add("quasistatic forward", - sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - sysIdTab.add("quasistatic backward", - sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - sysIdTab.add("dynamic forward", - sysIdDynamic(SysIdRoutine.Direction.kForward)); - sysIdTab.add("dynamic backward", - sysIdDynamic(SysIdRoutine.Direction.kReverse)); + sysIdTab.add("quasistatic forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + sysIdTab.add("quasistatic backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + sysIdTab.add("dynamic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + sysIdTab.add("dynamic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); // SmartDashboard.putNumber("arm initial position", goalState.position); // SmartDashboard.putNumber("set arm angle (rad)", 0); // sysid lastArmVel = getArmVel(); lastArmPos = getArmPos(); - + lastMeasuredTime = Timer.getFPGATimestamp(); // SmartDashboard.putNumber("ramp rate (s)", 2); // SmartDashboard.putNumber("soft limit pos (rad)", SOFT_LIMIT_LOCATION_IN_RADIANS); armMotorMaster.setSmartCurrentLimit(80); armMotorFollower.setSmartCurrentLimit(80); - if (SmartDashboard.getBoolean("babymode", babyMode) == true) { - armPIDMaster.setOutputRange(-0.3 / 12, 0.3 / 12); - } else { - armPIDMaster.setOutputRange(MIN_VOLTAGE / 12, MAX_VOLTAGE / 12); + if(SmartDashboard.getBoolean("babymode", babyMode) == true){ + armPIDMaster.setOutputRange(-0.3/12, 0.3/12); + } + else{ + armPIDMaster.setOutputRange(MIN_VOLTAGE/12, MAX_VOLTAGE/12); } TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints( - (MAX_FF_VEL_RAD_P_S), (MAX_FF_ACCEL_RAD_P_S)); + (MAX_FF_VEL_RAD_P_S), + (MAX_FF_ACCEL_RAD_P_S)); armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); SmartDashboard.putBoolean("arm is at pos", false); if (RobotBase.isSimulation()) { @@ -173,7 +167,7 @@ public Arm() { } } - + public void setBooleanDrive(boolean climb) { callDrive = climb; @@ -185,9 +179,9 @@ public void periodic() { SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing babyMode = SmartDashboard.getBoolean("babymode", false); + - - // Aaron was here + //Aaron was here // ^ worst case scenario // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S) @@ -235,29 +229,28 @@ public void periodic() { // SmartDashboard.putNumber("Master RPM", armMotorMaster.getEncoder().getVelocity()); // SmartDashboard.putNumber("Follower RPM", armMotorFollower.getEncoder().getVelocity()); // SmartDashboard.putNumber("Actual Master Arm Volts", - // armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput()); + // armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput()); // SmartDashboard.putNumber("Actual Follower Arm Volts", - // armMotorFollower.getBusVoltage() * armMotorFollower.getAppliedOutput()); + // armMotorFollower.getBusVoltage() * armMotorFollower.getAppliedOutput()); // when the value is different double currentArmPos = getArmPos(); double currentAbsoluteArmVel = armMasterEncoder.getVelocity(); - double currentRelativeArmVel = - armMotorMaster.getEncoder().getVelocity(); + double currentRelativeArmVel = armMotorMaster.getEncoder().getVelocity(); /* - * if (currentArmPos != lastArmPos) { lastMeasuredTime = currTime; lastArmPos = - * currentArmPos; lastArmVel = currentAbsoluteArmVel; } - */ - if ((currentAbsoluteArmVel == currentRelativeArmVel) - || !(currentAbsoluteArmVel != 0 - && currentRelativeArmVel == 0)) { + if (currentArmPos != lastArmPos) { lastMeasuredTime = currTime; lastArmPos = currentArmPos; lastArmVel = currentAbsoluteArmVel; } - isArmEncoderConnected = true;// currTime - lastMeasuredTime < - // DISCONNECTED_ENCODER_TIMEOUT_SEC; + */ + if((currentAbsoluteArmVel == currentRelativeArmVel) || !(currentAbsoluteArmVel != 0 && currentRelativeArmVel == 0)) { + lastMeasuredTime = currTime; + lastArmPos = currentArmPos; + lastArmVel = currentAbsoluteArmVel; + } + isArmEncoderConnected = true;//currTime - lastMeasuredTime < DISCONNECTED_ENCODER_TIMEOUT_SEC; if (isArmEncoderConnected) { if (callDrive) { @@ -267,34 +260,29 @@ public void periodic() { armMotorMaster.set(0); armMotorFollower.set(0); } - - + + + autoCancelArmCommand(); } - public static void setSelector(int num) { numSelector = num; } - public static int getSelector() { return numSelector; } - public void autoCancelArmCommand() { - if (!(getDefaultCommand() instanceof TeleopArm) - || DriverStation.isAutonomous()) + if (!(getDefaultCommand() instanceof TeleopArm) || DriverStation.isAutonomous()) return; - double requestedSpeeds = - ((TeleopArm) getDefaultCommand()).getRequestedSpeeds(); + double requestedSpeeds = ((TeleopArm) getDefaultCommand()).getRequestedSpeeds(); if (requestedSpeeds != 0) { Command currentArmCommand = getCurrentCommand(); - if (currentArmCommand != getDefaultCommand() - && currentArmCommand != null) { + if (currentArmCommand != getDefaultCommand() && currentArmCommand != null) { currentArmCommand.cancel(); } } @@ -311,13 +299,12 @@ private void driveArm() { armFeedVolts = armFeed.calculate(getArmPos(), 0); // kg * cos(arm angle) * arm_COM_length } - - armPIDMaster.setReference((setpoint.position), - CANSparkBase.ControlType.kPosition, 0, armFeedVolts); - + + armPIDMaster.setReference((setpoint.position), CANSparkBase.ControlType.kPosition, 0, armFeedVolts); + // SmartDashboard.putNumber("feedforward volts", armFeedVolts); // SmartDashboard.putNumber("pid volts", - // armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput() - armFeedVolts); + // armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput() - armFeedVolts); } public void stopArm() { @@ -331,11 +318,11 @@ public void driveArm(double volts) { armMotorMaster.setVoltage(volts); // STARTING WITH SLOWER SPEED FOR TESTING } - + public void setLimitsForClimbOn() { armPIDMaster.setOutputRange(-1, 1); armMotorMaster.setSoftLimit(SoftLimitDirection.kReverse, - (float) (CLIMB_FINISH_POS - Units.degreesToRadians(1))); + (float)(CLIMB_FINISH_POS-Units.degreesToRadians(1))); armMotorMaster.setOpenLoopRampRate(1); armMotorMaster.enableSoftLimit(SoftLimitDirection.kReverse, true); } @@ -350,11 +337,9 @@ public void setArmTarget(double targetPos) { goalState.position = targetPos; goalState.velocity = 0; } - public void setPIDOff(boolean setter) { setPIDOff = setter; } - public void resetGoal() { double armPos = getArmPos(); setArmTarget(armPos); @@ -365,40 +350,45 @@ public void driveMotor(Measure volts) { } - private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( - Volts.of(1).per(Seconds.of(1)), Volts.of(2), Seconds.of(10)); + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds.of(1)), + Volts.of(2), Seconds.of(10)); public void logMotor(SysIdRoutineLog log) { log.motor("armMotorMaster") - .voltage(voltage.mut_replace(armMotorMaster.getBusVoltage() - * armMotorMaster.getAppliedOutput(), Volts)) + .voltage(voltage.mut_replace( + armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput(), + Volts)) .angularVelocity(velocity.mut_replace( - armMasterEncoder.getVelocity(), RadiansPerSecond)) - .angularPosition(distance - .mut_replace(armMasterEncoder.getPosition(), Radians)); + armMasterEncoder.getVelocity(), + RadiansPerSecond)) + .angularPosition(distance.mut_replace( + armMasterEncoder.getPosition(), + Radians)); } - private final SysIdRoutine routine = new SysIdRoutine(defaultSysIdConfig, - new SysIdRoutine.Mechanism(this::driveMotor, this::logMotor, this)); + private final SysIdRoutine routine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + this::driveMotor, + this::logMotor, + this)); public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return new SequentialCommandGroup( - new InstantCommand(() -> armMasterEncoder.setZeroOffset(0)), + return new SequentialCommandGroup(new InstantCommand(() -> armMasterEncoder.setZeroOffset(0)), routine.quasistatic(direction)); } public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return new SequentialCommandGroup( - new InstantCommand(() -> armMasterEncoder.setZeroOffset(0)), + return new SequentialCommandGroup(new InstantCommand(() -> armMasterEncoder.setZeroOffset(0)), routine.dynamic(direction)); } // #region Getters public double getArmPos() { - return MathUtil.inputModulus(armMasterEncoder.getPosition(), - ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI);// armMasterEncoder.getPosition();//MathUtil.inputModulus(armMasterEncoder.getPosition(), - // ARM_DISCONT_RAD, + return MathUtil.inputModulus(armMasterEncoder.getPosition(), ARM_DISCONT_RAD, + ARM_DISCONT_RAD + 2 * Math.PI);// armMasterEncoder.getPosition();//MathUtil.inputModulus(armMasterEncoder.getPosition(), + // ARM_DISCONT_RAD, // ARM_DISCONT_RAD + 2 * Math.PI); } @@ -421,21 +411,18 @@ public TrapezoidProfile.State getCurrentArmGoal() { } public boolean armAtSetpoint() { - return Math.abs(getArmPos() - goalState.position) < POS_TOLERANCE_RAD - && Math.abs(getArmVel() - - goalState.velocity) < VEL_TOLERANCE_RAD_P_SEC; + return Math.abs(getArmPos() - goalState.position) < POS_TOLERANCE_RAD && + Math.abs(getArmVel() - goalState.velocity) < VEL_TOLERANCE_RAD_P_SEC; } public double getArmClampedGoal(double goal) { - return MathUtil.clamp( - MathUtil.inputModulus(goal, ARM_DISCONT_RAD, - ARM_DISCONT_RAD + 2 * Math.PI), + return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI), LOWER_ANGLE_LIMIT_RAD, UPPER_ANGLE_LIMIT_RAD); } public double getMaxAccelRad() { return 1; - // return armFeed.maxAchievableAcceleration(MAX_VOLTAGE, getArmPos(), getArmVel()); + //return armFeed.maxAchievableAcceleration(MAX_VOLTAGE, getArmPos(), getArmVel()); } public double getMaxVelocity() { @@ -446,60 +433,38 @@ public double getMaxVelocity() { @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); - builder.addDoubleProperty("armKp", () -> armPIDMaster.getP(), - armPIDMaster::setP); - builder.addDoubleProperty("armKd", () -> armPIDMaster.getD(), - armPIDMaster::setD); + builder.addDoubleProperty("armKp", () -> armPIDMaster.getP(), armPIDMaster::setP); + builder.addDoubleProperty("armKd", () -> armPIDMaster.getD(), armPIDMaster::setD); builder.addDoubleProperty("Current Position", () -> getArmPos(), null); - builder.addBooleanProperty("ArmPIDAtSetpoint", () -> armAtSetpoint(), - null); - builder.addDoubleProperty("Arm Goal Pos (rad)", - () -> goalState.position, null); - builder.addBooleanProperty("ArmEncoderConnected", - () -> isArmEncoderConnected, null); - builder.addDoubleProperty("feedforward volts", () -> armFeedVolts, - null); + builder.addBooleanProperty("ArmPIDAtSetpoint", () -> armAtSetpoint(), null); + builder.addDoubleProperty("Arm Goal Pos (rad)", () -> goalState.position, null); + builder.addBooleanProperty("ArmEncoderConnected", () -> isArmEncoderConnected, null); + builder.addDoubleProperty("feedforward volts", () -> armFeedVolts, null); builder.addDoubleProperty("pid volts", - () -> armMotorMaster.getBusVoltage() - * armMotorMaster.getAppliedOutput() - armFeedVolts, - null); - builder.addDoubleProperty("Arm Volts", - () -> armMotorMaster.getBusVoltage() - * armMotorMaster.getAppliedOutput(), - null); - builder.addDoubleProperty("setpoint goal (rad)", - () -> setpoint.position, null); - builder.addDoubleProperty("setpoint velocity", () -> setpoint.velocity, - null); - builder.addDoubleProperty("Output current ARM", - () -> armMotorMaster.getOutputCurrent(), null); - builder.addDoubleProperty("arm initial position", - () -> goalState.position, null); + () -> armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput() - armFeedVolts, null); + builder.addDoubleProperty("Arm Volts", () -> armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput(), null); + builder.addDoubleProperty("setpoint goal (rad)", () -> setpoint.position, null); + builder.addDoubleProperty("setpoint velocity", () -> setpoint.velocity, null); + builder.addDoubleProperty("Output current ARM", () -> armMotorMaster.getOutputCurrent(), null); + builder.addDoubleProperty("arm initial position", () -> goalState.position, null); // builder.addDoubleProperty("set arm angle (rad)", () -> // armMasterEncoder.getPosition(), setArmTarget()); - builder.addBooleanProperty("ArmPIDAtSetpoint", () -> armAtSetpoint(), - null); - builder.addDoubleProperty("Arm Goal Pos (rad)", - () -> goalState.position, null); - builder.addDoubleProperty("InternalArmVelocity", - () -> armMasterEncoder.getVelocity(), null); - builder.addDoubleProperty("Soft limit Forward", - () -> armMotorMaster.getSoftLimit(SoftLimitDirection.kForward), + builder.addBooleanProperty("ArmPIDAtSetpoint", () -> armAtSetpoint(), null); + builder.addDoubleProperty("Arm Goal Pos (rad)", () -> goalState.position, null); + builder.addDoubleProperty("InternalArmVelocity", () -> armMasterEncoder.getVelocity(), null); + builder.addDoubleProperty("Soft limit Forward", () -> armMotorMaster.getSoftLimit(SoftLimitDirection.kForward), null); - builder.addDoubleProperty("Soft limit Reverse", - () -> armMotorMaster.getSoftLimit(SoftLimitDirection.kReverse), + builder.addDoubleProperty("Soft limit Reverse", () -> armMotorMaster.getSoftLimit(SoftLimitDirection.kReverse), null); } public double getMaxVelRad() { - return armFeed.maxAchievableVelocity(MAX_VOLTAGE, getArmPos(), - getArmVel()); + return armFeed.maxAchievableVelocity(MAX_VOLTAGE, getArmPos(), getArmVel()); } public void setDefaultCommand(TeleopArm teleopArm, Object object) { // TODO Auto-generated method stub - throw new UnsupportedOperationException( - "Unimplemented method 'setDefaultCommand'"); + throw new UnsupportedOperationException("Unimplemented method 'setDefaultCommand'"); } @Override