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 7799a40..8c157e9 100644 Binary files a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat index b9d72a5..6a7a26b 100644 Binary files a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat index 7f36dce..a7f2c7d 100644 Binary files a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat index 3a70554..928429d 100644 Binary files a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat differ 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 229cd82..c9ea910 100644 --- a/src/main/java/org/ironriders/climb/ClimbConstants.java +++ b/src/main/java/org/ironriders/climb/ClimbConstants.java @@ -1,17 +1,20 @@ 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; 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 MAX_ACC = 2; // test velocitys + public static final double MAX_VEL = 2; public static final double T = .02; @@ -28,11 +31,14 @@ 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; 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 afcc7c1..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; @@ -27,9 +29,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); @@ -38,14 +44,13 @@ 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) .forwardSoftLimitEnabled(true) - .forwardSoftLimit(ClimbConstants.Targets.HOME.pos); + .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); @@ -71,9 +76,12 @@ 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); pidController.setP(SmartDashboard.getNumber("Climber P", ClimbConstants.P)); pidController.setI(SmartDashboard.getNumber("Climber I", ClimbConstants.I)); @@ -93,11 +101,20 @@ public void set(ClimbConstants.State state) { public void goTo(ClimbConstants.Targets limit) { setGoal(limit); - double pidOutput = pidController.calculate(getPostion() /* Encoder pos times motor gearing */, + pidOutput = pidController.calculate(getPostion() /* Encoder pos times motor gearing */, periodicSetpoint.position); + if (pidOutput == 0) { + climbMotor.stopMotor(); + return; + } + climbMotor.set(pidOutput); - publish("Climber PID output", pidOutput); + + } + + public double getGoal() { + return goalSetpoint.position; } public void setGoal(ClimbConstants.Targets limit) { 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) {