From 551aee02c63646f22bf0ea7c61cb82a0e1982e2b Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Fri, 30 Aug 2024 23:11:28 -0700 Subject: [PATCH 1/3] Arm subystem fixes --- src/main/java/frc/robot/arm/ArmSubsystem.java | 34 +++++-------------- 1 file changed, 9 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 8aeecbf..56d616d 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -18,7 +18,7 @@ public class ArmSubsystem extends StateMachine { private double rightMotorAngle; private double distanceToSpeaker; private double distanceToFeedSpot; - private double lowestSeenAngleLeft = Double.MAX_VALUE; + private double lowestSeenAngleLeft = Double.MIN_VALUE; private double lowestSeenAngleRight = Double.MIN_VALUE; private InterpolatingDoubleTreeMap speakerDistanceToAngle = new InterpolatingDoubleTreeMap(); private InterpolatingDoubleTreeMap feedSpotDistanceToAngle = new InterpolatingDoubleTreeMap(); @@ -60,7 +60,7 @@ public void setState(ArmState newState) { // public void disabledPeriodic() // { // rightMotor.setControl(staticBrake); - // rightMotor.setControl(staticBrake); + // leftMotor.setControl(staticBrake); // } // IF withOverrideBrakeDurNeutral DOESNT WORK public boolean atGoal() { @@ -173,33 +173,17 @@ public void robotPeriodic() { DogLog.log("Arm/Right/AppliedVoltage", rightMotor.getMotorVoltage().getValueAsDouble()); if (DriverStation.isEnabled() && getState() == ArmState.PRE_MATCH_HOMING) { - Double homedAngle = getHomeAngleFromLowestSeen(); - leftMotor.setPosition(Units.degreesToRotations(homedAngle)); - rightMotor.setPosition(Units.degreesToRotations(homedAngle)); - setState(ArmState.IDLE); + leftMotor.setPosition( + Units.degreesToRotations( + RobotConfig.get().arm().minAngle() + (leftMotorAngle - lowestSeenAngleLeft))); + rightMotor.setPosition( + Units.degreesToRotations( + RobotConfig.get().arm().minAngle() + (rightMotorAngle - lowestSeenAngleRight))); + setStateFromRequest(ArmState.IDLE); } // We are enabled and still in pre match homing // Reset the motor positions, and then transition to idle state } - - private double getHomeAngleFromLowestSeen() { - return RobotConfig.get().arm().minAngle() + (getAngle() - lowestSeenAngleLeft); - } - - public double getAngle() { - - return Units.rotationsToDegrees(leftMotor.getPosition().getValueAsDouble()); - } - - private static double clampAngle(double angle) { - if (angle < RobotConfig.get().arm().minAngle()) { - angle = RobotConfig.get().arm().minAngle(); - - } else if (angle > RobotConfig.get().arm().maxAngle()) { - angle = RobotConfig.get().arm().maxAngle(); - } - return angle; - } } From 1f7c52bb8a3aa8cd5d253fd43f760f130ef0ebc9 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sat, 31 Aug 2024 18:47:08 -0700 Subject: [PATCH 2/3] arm homing fix --- src/main/java/frc/robot/arm/ArmSubsystem.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 56d616d..3884d9e 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -18,8 +18,8 @@ public class ArmSubsystem extends StateMachine { private double rightMotorAngle; private double distanceToSpeaker; private double distanceToFeedSpot; - private double lowestSeenAngleLeft = Double.MIN_VALUE; - private double lowestSeenAngleRight = Double.MIN_VALUE; + private double lowestSeenAngleLeft = Double.MAX_VALUE; + private double lowestSeenAngleRight = Double.MAX_VALUE; private InterpolatingDoubleTreeMap speakerDistanceToAngle = new InterpolatingDoubleTreeMap(); private InterpolatingDoubleTreeMap feedSpotDistanceToAngle = new InterpolatingDoubleTreeMap(); private final PositionVoltage positionRequest = @@ -166,14 +166,17 @@ public void robotPeriodic() { DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log("Arm/Left/ArmAngle", leftMotor.getPosition().getValueAsDouble()); DogLog.log("Arm/Left/AppliedVoltage", leftMotor.getMotorVoltage().getValueAsDouble()); + DogLog.log("Arm/Left/LowestSeenAngleLeft", lowestSeenAngleLeft); DogLog.log("Arm/Right/StatorCurrent", rightMotor.getStatorCurrent().getValueAsDouble()); DogLog.log("Arm/Right/SupplyCurrent", rightMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log("Arm/Right/ArmAngle", rightMotor.getPosition().getValueAsDouble()); DogLog.log("Arm/Right/AppliedVoltage", rightMotor.getMotorVoltage().getValueAsDouble()); + DogLog.log("Arm/Right/LowestSeenAngleRight", lowestSeenAngleRight); if (DriverStation.isEnabled() && getState() == ArmState.PRE_MATCH_HOMING) { - + // We are enabled and still in pre match homing + // Reset the motor positions, and then transition to idle state leftMotor.setPosition( Units.degreesToRotations( RobotConfig.get().arm().minAngle() + (leftMotorAngle - lowestSeenAngleLeft))); @@ -182,8 +185,5 @@ public void robotPeriodic() { RobotConfig.get().arm().minAngle() + (rightMotorAngle - lowestSeenAngleRight))); setStateFromRequest(ArmState.IDLE); } - - // We are enabled and still in pre match homing - // Reset the motor positions, and then transition to idle state } } From 93b80b8103bc542076678f37c57f7c0dcf9afc2c Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sun, 1 Sep 2024 18:17:01 -0700 Subject: [PATCH 3/3] doglog name change --- src/main/java/frc/robot/arm/ArmSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 3884d9e..055fabe 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -166,13 +166,13 @@ public void robotPeriodic() { DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log("Arm/Left/ArmAngle", leftMotor.getPosition().getValueAsDouble()); DogLog.log("Arm/Left/AppliedVoltage", leftMotor.getMotorVoltage().getValueAsDouble()); - DogLog.log("Arm/Left/LowestSeenAngleLeft", lowestSeenAngleLeft); + DogLog.log("Arm/Left/LowestSeenAngle", lowestSeenAngleLeft); DogLog.log("Arm/Right/StatorCurrent", rightMotor.getStatorCurrent().getValueAsDouble()); DogLog.log("Arm/Right/SupplyCurrent", rightMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log("Arm/Right/ArmAngle", rightMotor.getPosition().getValueAsDouble()); DogLog.log("Arm/Right/AppliedVoltage", rightMotor.getMotorVoltage().getValueAsDouble()); - DogLog.log("Arm/Right/LowestSeenAngleRight", lowestSeenAngleRight); + DogLog.log("Arm/Right/LowestSeenAngle", lowestSeenAngleRight); if (DriverStation.isEnabled() && getState() == ArmState.PRE_MATCH_HOMING) { // We are enabled and still in pre match homing