From 0a8c5ecc89809ce6b39aaf04f586b9f66df090ba Mon Sep 17 00:00:00 2001 From: isabelle Date: Mon, 10 Mar 2025 20:12:41 -0700 Subject: [PATCH 1/5] climb testing --- .../org/ironriders/climb/ClimbConstants.java | 8 ++--- .../org/ironriders/climb/ClimbSubsystem.java | 30 +++++++------------ 2 files changed, 14 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/ironriders/climb/ClimbConstants.java b/src/main/java/org/ironriders/climb/ClimbConstants.java index 75bef05..a037295 100644 --- a/src/main/java/org/ironriders/climb/ClimbConstants.java +++ b/src/main/java/org/ironriders/climb/ClimbConstants.java @@ -6,16 +6,14 @@ public class ClimbConstants { public static final double GEAR_RATIO = 1/100; - public static final double P = 0; + public static final double P = 0.1; public static final double I = 0; public static final double D = 0; public static final double MAX_ACC = 10; public static final double MAX_VEL = 10; - public static final double T = .02; - //public static final double CLIMBER_LIMIT = 45; - + public static final double T = .02; public enum State { UP(-0.3), @@ -30,7 +28,7 @@ public enum State { public enum Targets { HOME(0), MAX(-123.5), - TARGET(2); //TEST and figure out (2 should be safe and visible) (This is supposed to be the angle where the robot is off the ground but not touching the chain) + TARGET(40); //TEST and figure out (40 should be safe and visible) (This is supposed to be the angle where the robot is off the ground but not touching the chain) public final double pos; diff --git a/src/main/java/org/ironriders/climb/ClimbSubsystem.java b/src/main/java/org/ironriders/climb/ClimbSubsystem.java index bcc2dc9..1e5f77f 100644 --- a/src/main/java/org/ironriders/climb/ClimbSubsystem.java +++ b/src/main/java/org/ironriders/climb/ClimbSubsystem.java @@ -1,28 +1,18 @@ package org.ironriders.climb; -import java.lang.annotation.Target; -import java.util.DuplicateFormatFlagsException; import org.ironriders.lib.IronSubsystem; -import org.ironriders.lib.data.PID; -import org.ironriders.wrist.algae.AlgaeWristConstants; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkRelativeEncoder; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import com.revrobotics.spark.config.LimitSwitchConfig; -import com.revrobotics.spark.config.SoftLimitConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; public class ClimbSubsystem extends IronSubsystem { @@ -36,10 +26,13 @@ public class ClimbSubsystem extends IronSubsystem { private final ClimbCommands commands; private final RelativeEncoder encoder; + private double pidOutput; + private TrapezoidProfile.State goalSetpoint = new TrapezoidProfile.State(); private TrapezoidProfile.State periodicSetpoint = new TrapezoidProfile.State(); - + private SoftLimitConfig softLimitConfig = new SoftLimitConfig(); // should force stop motor if it gets out of bounds + public ClimbSubsystem() { publish("Climber P", ClimbConstants.P); publish("Climber I", ClimbConstants.I); @@ -49,8 +42,6 @@ public ClimbSubsystem() { encoder = climbMotor.getEncoder(); encoder.setPosition(0); // Set pos to zero on deploy - - var softLimitConfig = new SoftLimitConfig(); // should force stop motor if it gets out of bounds softLimitConfig .reverseSoftLimitEnabled(true) .reverseSoftLimit(ClimbConstants.Targets.MAX.pos) @@ -84,6 +75,7 @@ public void periodic() { publish("Climb Motor Val", getPostion()); publish("Climber target pos", goalSetpoint.position); publish("Climber target velo", goalSetpoint.velocity); + publish("Climber PID output", pidOutput); pidController.setP(SmartDashboard.getNumber("Climber P", ClimbConstants.P)); pidController.setI(SmartDashboard.getNumber("Climber I", ClimbConstants.I)); @@ -102,14 +94,14 @@ public void set(ClimbConstants.State state) { public void goTo(ClimbConstants.Targets limit) { setGoal(limit); - double pidOutput = pidController.calculate(getPostion() /* Encoder pos with motor gearing */, periodicSetpoint.position); + pidOutput = pidController.calculate(getPostion() /* Encoder pos with motor gearing */, periodicSetpoint.position); if (pidOutput == 0) { climbMotor.stopMotor(); return; } climbMotor.set(pidOutput); - publish("Climber PID output", pidOutput); + } public void setGoal(ClimbConstants.Targets limit) { From cfe3b437e041723afc1a1f41f22bdf0b36b1fc06 Mon Sep 17 00:00:00 2001 From: isabelle Date: Mon, 10 Mar 2025 20:33:51 -0700 Subject: [PATCH 2/5] d --- src/main/java/org/ironriders/climb/ClimbConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/ironriders/climb/ClimbConstants.java b/src/main/java/org/ironriders/climb/ClimbConstants.java index 24663b7..50c097a 100644 --- a/src/main/java/org/ironriders/climb/ClimbConstants.java +++ b/src/main/java/org/ironriders/climb/ClimbConstants.java @@ -10,8 +10,8 @@ public class ClimbConstants { public static final double I = 0; public static final double D = 0; - public static final double MAX_ACC = 10; - public static final double MAX_VEL = 10; + public static final double MAX_ACC = 2; + public static final double MAX_VEL = 2; public static final double T = .02; From 8ad486143a7d3019354c594414de6a2df2e22869 Mon Sep 17 00:00:00 2001 From: isabelle Date: Mon, 10 Mar 2025 20:38:21 -0700 Subject: [PATCH 3/5] climber test prep --- src/main/java/org/ironriders/climb/ClimbConstants.java | 2 +- src/main/java/org/ironriders/climb/ClimbSubsystem.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/ironriders/climb/ClimbConstants.java b/src/main/java/org/ironriders/climb/ClimbConstants.java index 50c097a..a7f510c 100644 --- a/src/main/java/org/ironriders/climb/ClimbConstants.java +++ b/src/main/java/org/ironriders/climb/ClimbConstants.java @@ -10,7 +10,7 @@ public class ClimbConstants { public static final double I = 0; public static final double D = 0; - public static final double MAX_ACC = 2; + public static final double MAX_ACC = 2; // test velocitys public static final double MAX_VEL = 2; public static final double T = .02; diff --git a/src/main/java/org/ironriders/climb/ClimbSubsystem.java b/src/main/java/org/ironriders/climb/ClimbSubsystem.java index 17db6b0..8a09a53 100644 --- a/src/main/java/org/ironriders/climb/ClimbSubsystem.java +++ b/src/main/java/org/ironriders/climb/ClimbSubsystem.java @@ -46,7 +46,7 @@ public ClimbSubsystem() { .reverseSoftLimitEnabled(true) .reverseSoftLimit(ClimbConstants.Targets.MAX.pos) .forwardSoftLimitEnabled(true) - .forwardSoftLimit(ClimbConstants.Targets.HOME.pos); + .forwardSoftLimit(ClimbConstants.Targets.HOME.pos); // Home is also the minimum position climbMotorConfig.idleMode(IdleMode.kBrake); climbMotorConfig.smartCurrentLimit(ClimbConstants.CURRENT_LIMIT); @@ -74,7 +74,9 @@ public void periodic() { periodicSetpoint = profile.calculate(ClimbConstants.T, periodicSetpoint, goalSetpoint); publish("Climber set postion", periodicSetpoint.position); - publish("Climb Motor Val", getPostion()); + //publish("Climb Motor Val", getPostion()); + publish("Climb Motor Val", encoder.getPosition()); // for testing + publish("Climber target pos", goalSetpoint.position); publish("Climber target velo", goalSetpoint.velocity); publish("Climber PID output", pidOutput); From f48d03d05d20b8fa268a0bc7c036360a4a24e986 Mon Sep 17 00:00:00 2001 From: isabelle Date: Tue, 11 Mar 2025 06:29:46 -0700 Subject: [PATCH 4/5] make coast defualt --- src/main/java/org/ironriders/climb/ClimbSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/ironriders/climb/ClimbSubsystem.java b/src/main/java/org/ironriders/climb/ClimbSubsystem.java index 8a09a53..4b6c458 100644 --- a/src/main/java/org/ironriders/climb/ClimbSubsystem.java +++ b/src/main/java/org/ironriders/climb/ClimbSubsystem.java @@ -48,7 +48,7 @@ public ClimbSubsystem() { .forwardSoftLimitEnabled(true) .forwardSoftLimit(ClimbConstants.Targets.HOME.pos); // Home is also the minimum position - climbMotorConfig.idleMode(IdleMode.kBrake); + climbMotorConfig.idleMode(IdleMode.kCoast); // for testing set to coast climbMotorConfig.smartCurrentLimit(ClimbConstants.CURRENT_LIMIT); climbMotor.configure(climbMotorConfig.apply(softLimitConfig), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); From 63e338827ba208714ad80d8f75e75ed0756b1ca8 Mon Sep 17 00:00:00 2001 From: isabelle Date: Tue, 11 Mar 2025 07:10:53 -0700 Subject: [PATCH 5/5] small --- ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes .../org/ironriders/climb/ClimbCommands.java | 9 ++++----- .../org/ironriders/climb/ClimbConstants.java | 6 ++++++ .../org/ironriders/climb/ClimbSubsystem.java | 6 ++++++ .../org/ironriders/core/RobotCommands.java | 3 +-- 8 files changed, 17 insertions(+), 7 deletions(-) diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat index 7799a404ffb4f2a69805e751571247a0e65f9676..8c157e9a8519fbe9d1e312182fd6cd235d1c145c 100644 GIT binary patch delta 72 zcmZn=Xb|9d&anH{*)wNsCtqYtpQyk(*@V%D;|0S`WU+}QK5PdWF8|J9o+xO=c8K8; OkOfiBz1f241SK+>a delta 72 zcmZn=Xb|9d%&_m(*)wNsCtqYtpQyk(*@V%D;}OFFWU+}QK5XY1j{eSJo+xO=c8=i~ OkOfiBz1f241SZaVvY96eTCtsDIQ~1E N86w5K*@Ec=D*$wP7|Q?v diff --git a/src/main/java/org/ironriders/climb/ClimbCommands.java b/src/main/java/org/ironriders/climb/ClimbCommands.java index ed12293..0dd88f5 100644 --- a/src/main/java/org/ironriders/climb/ClimbCommands.java +++ b/src/main/java/org/ironriders/climb/ClimbCommands.java @@ -13,11 +13,6 @@ public ClimbCommands(ClimbSubsystem climb) { climb.publish("Climb to TARGET", goTo(ClimbConstants.Targets.TARGET)); climb.publish("Re-zero (TESTING ONLY)", reZero()); } - - public Command set(ClimbConstants.State state) { - return climb - .runOnce(() -> climb.set(state)); - } public Command goTo(ClimbConstants.Targets limit) { return climb @@ -27,4 +22,8 @@ public Command goTo(ClimbConstants.Targets limit) { private Command reZero() { return climb.runOnce(() -> climb.reZero()); } + + public Command getGoalpoint() { + return climb.runOnce(() -> climb.getGoal()); + } } \ No newline at end of file diff --git a/src/main/java/org/ironriders/climb/ClimbConstants.java b/src/main/java/org/ironriders/climb/ClimbConstants.java index a7f510c..c9ea910 100644 --- a/src/main/java/org/ironriders/climb/ClimbConstants.java +++ b/src/main/java/org/ironriders/climb/ClimbConstants.java @@ -1,5 +1,8 @@ package org.ironriders.climb; +import java.util.Arrays; +import java.util.Optional; + public class ClimbConstants { public static final int CLIMBER_MOTOR_CAN_ID = 17; public static final int CURRENT_LIMIT = 40; @@ -34,5 +37,8 @@ public enum Targets { Targets(double pos) { this.pos = pos; } + public static Optional findByCode(Double code) { + return Arrays.stream(values()).filter(target -> target.pos == code).findFirst(); + } } } diff --git a/src/main/java/org/ironriders/climb/ClimbSubsystem.java b/src/main/java/org/ironriders/climb/ClimbSubsystem.java index 4b6c458..9f1ea77 100644 --- a/src/main/java/org/ironriders/climb/ClimbSubsystem.java +++ b/src/main/java/org/ironriders/climb/ClimbSubsystem.java @@ -1,5 +1,7 @@ package org.ironriders.climb; +import java.util.Optional; + import org.ironriders.lib.IronSubsystem; import com.revrobotics.RelativeEncoder; @@ -111,6 +113,10 @@ public void goTo(ClimbConstants.Targets limit) { } + public double getGoal() { + return goalSetpoint.position; + } + public void setGoal(ClimbConstants.Targets limit) { this.goalSetpoint = new TrapezoidProfile.State(limit.pos, 0d); } diff --git a/src/main/java/org/ironriders/core/RobotCommands.java b/src/main/java/org/ironriders/core/RobotCommands.java index 4e008b4..9cef53d 100644 --- a/src/main/java/org/ironriders/core/RobotCommands.java +++ b/src/main/java/org/ironriders/core/RobotCommands.java @@ -6,7 +6,6 @@ import org.ironriders.drive.DriveCommands; import org.ironriders.elevator.ElevatorCommands; import org.ironriders.elevator.ElevatorConstants; -import org.ironriders.lib.GameState; import org.ironriders.targeting.TargetingCommands; import org.ironriders.wrist.algae.AlgaeIntakeCommands; import org.ironriders.wrist.algae.AlgaeIntakeConstants; @@ -107,7 +106,7 @@ public Command rumble() { public Command toggleClimber() { return Commands.none(); - // TODO + //return climbCommands.goTo(Targets.TARGET); } public Command prepareToScoreCoral(ElevatorConstants.Level level) {