From d2de0a50ab39b83bf6ef139dc1aa181606ca37c1 Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Wed, 11 Feb 2026 17:38:21 -0800 Subject: [PATCH 1/8] changed it to match intake extend better --- .../java/org/team100/frc2026/Climber.java | 92 +++++++++++++++---- .../org/team100/lib/config/PIDConstants.java | 2 +- .../r1/IncrementalProfileReferenceR1.java | 3 +- .../servo/OnboardAngularPositionServo.java | 13 ++- 4 files changed, 90 insertions(+), 20 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/Climber.java b/comp/src/main/java/org/team100/frc2026/Climber.java index d52d789..7b0f2d0 100644 --- a/comp/src/main/java/org/team100/frc2026/Climber.java +++ b/comp/src/main/java/org/team100/frc2026/Climber.java @@ -1,45 +1,105 @@ package org.team100.frc2026; +import org.team100.lib.config.Friction; +import org.team100.lib.config.Identity; +import org.team100.lib.config.PIDConstants; +import org.team100.lib.config.SimpleDynamics; +import org.team100.lib.controller.r1.PIDFeedback; import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.mechanism.RotaryMechanism; import org.team100.lib.motor.BareMotor; +import org.team100.lib.motor.MotorPhase; import org.team100.lib.motor.sim.SimulatedBareMotor; +import org.team100.lib.profile.r1.IncrementalProfile; +import org.team100.lib.profile.r1.TrapezoidIncrementalProfile; +import org.team100.lib.reference.r1.IncrementalProfileReferenceR1; +import org.team100.lib.reference.r1.ProfileReferenceR1; +import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor; +import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder; +import org.team100.lib.sensor.position.incremental.ctre.Talon6Encoder; +import org.team100.lib.servo.OnboardAngularPositionServo; +import org.team100.lib.util.CanId; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import org.team100.lib.motor.ctre.KrakenX44Motor; +import org.team100.lib.motor.ctre.KrakenX60Motor; +import org.team100.lib.motor.NeutralMode100; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +@SuppressWarnings("unused") public class Climber extends SubsystemBase { private final BareMotor m_motor; - // these are for the degrees that the motor moves - private final double m_level1 = 90; - private final double m_level3 = 180; + private final OnboardAngularPositionServo m_servo; + private static final double m_level1 = 90; + private static final double m_level3 = 180; public Climber(LoggerFactory parent) { + LoggerFactory log = parent.type(this); - m_motor = new SimulatedBareMotor(log, 600); + switch (Identity.instance) { + case COMP_BOT -> { + m_motor = new KrakenX60Motor( + log, + new CanId(0), + NeutralMode100.BRAKE, + MotorPhase.FORWARD, + 60, 1, + new SimpleDynamics(log, 0, 0), + new Friction(log, 0, 0, 0, 0), + new PIDConstants(log, 0, 0, 0, 0, 0, 0)); + IncrementalBareEncoder encoder = m_motor.encoder(); + + TrapezoidIncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, profile, 0.05, 0.05); + double initialPosition = 0; + double gearRatio = 2; + RotaryMechanism climberMech = new RotaryMechanism( + log, m_motor, encoder, initialPosition, gearRatio, + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + m_servo = new OnboardAngularPositionServo(log, climberMech, ref); + }: + break; + + default ->{ + m_motor = new SimulatedBareMotor(log, 600); + IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, profile, 0.05, 0.05); + PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); + IncrementalBareEncoder encoder = m_motor.encoder(); + + SimulatedRotaryPositionSensor sensor = + new SimulatedRotaryPositionSensor(log, encoder, 1); + RotaryMechanism climberMech = + new RotaryMechanism( + log, + m_motor, + sensor, + 1, + Double.NEGATIVE_INFINITY, + Double.POSITIVE_INFINITY); + m_servo = new OnboardAngularPositionServo( log, climberMech, ref, feedback ); + } + } } public Command setClimb0() { - return run(this::setL0); + return runOnce(this::setL0); } public Command setClimb1() { - return run(this::setL1); - } - - public Command setClimb3() { - return run(this::setL3); + return runOnce(this::setL1); } private void setL0() { - m_motor.setUnwrappedPosition(0, 0, 0, 0); - + m_servo.setPosition(0); } - public void setL1() { - m_motor.setUnwrappedPosition(m_level1, 0, 0, 0); + private void setL1() { + m_servo.setPosition(m_level1); } - public void setL3() { - m_motor.setUnwrappedPosition(m_level3, 0, 0, 0); + private void setL3() { + m_servo.setPosition(m_level3); } } diff --git a/lib/src/main/java/org/team100/lib/config/PIDConstants.java b/lib/src/main/java/org/team100/lib/config/PIDConstants.java index 50341dd..b74b24b 100644 --- a/lib/src/main/java/org/team100/lib/config/PIDConstants.java +++ b/lib/src/main/java/org/team100/lib/config/PIDConstants.java @@ -103,7 +103,7 @@ public void register(Runnable listener) { ////////////////////////////////////////////////////// - PIDConstants(LoggerFactory log, + public PIDConstants(LoggerFactory log, double positionP, double positionI, double positionD, double velocityP, double velocityI, double velocityD) { // m_positionP = positionP; diff --git a/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java b/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java index 93822f1..b7d1e73 100644 --- a/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java +++ b/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java @@ -9,6 +9,7 @@ import org.team100.lib.logging.LoggerFactory.BooleanLogger; import org.team100.lib.logging.LoggerFactory.SetpointsR1Logger; import org.team100.lib.profile.r1.IncrementalProfile; +import org.team100.lib.profile.r1.TrapezoidIncrementalProfile; import org.team100.lib.state.ControlR1; import org.team100.lib.state.ModelR1; @@ -29,7 +30,7 @@ public class IncrementalProfileReferenceR1 implements ProfileReferenceR1 { public IncrementalProfileReferenceR1( LoggerFactory parent, - Supplier profile, + TrapezoidIncrementalProfile profile, double positionTolerance, double velocityTolerance) { LoggerFactory log = parent.type(this); diff --git a/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java b/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java index b73636c..bcbb0d6 100644 --- a/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java +++ b/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java @@ -35,8 +35,7 @@ public class OnboardAngularPositionServo extends AngularPositionServoImpl { public OnboardAngularPositionServo( LoggerFactory parent, RotaryMechanism mech, - ProfileReferenceR1 ref, - FeedbackR1 feedback) { + ProfileReferenceR1 ref) { super(parent, mech, ref); if (feedback.handlesWrapping()) throw new IllegalArgumentException("Do not supply wrapping feedback"); @@ -96,4 +95,14 @@ void actuate(SetpointsR1 unwrappedSetpoint, double feedForwardTorqueNm) { m_log_velocity_error.log(() -> currentUnwrappedSetpoint.v() - unwrappedMeasurement.v()); } + public void setPosition(double mLevel1) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); + } + + public void setPosition(double mLevel3) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); + } + } From a9674792e224922bfd1fa3fe13d1b2ca83706b74 Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Sat, 14 Feb 2026 10:56:49 -0800 Subject: [PATCH 2/8] down to one error in climb extend --- .../java/org/team100/frc2026/Climber.java | 75 +++++++++---------- .../org/team100/frc2026/ClimberExtension.java | 67 ++++++++++++++++- .../r1/IncrementalProfileReferenceR1.java | 3 +- .../servo/OnboardAngularPositionServo.java | 13 +--- .../servo/OutboardLinearPositionServo.java | 4 + 5 files changed, 110 insertions(+), 52 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/Climber.java b/comp/src/main/java/org/team100/frc2026/Climber.java index 7b0f2d0..b37e2bd 100644 --- a/comp/src/main/java/org/team100/frc2026/Climber.java +++ b/comp/src/main/java/org/team100/frc2026/Climber.java @@ -23,7 +23,6 @@ import org.team100.lib.motor.ctre.KrakenX44Motor; import org.team100.lib.motor.ctre.KrakenX60Motor; import org.team100.lib.motor.NeutralMode100; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -34,54 +33,54 @@ public class Climber extends SubsystemBase { private static final double m_level1 = 90; private static final double m_level3 = 180; - public Climber(LoggerFactory parent) { - + public Climber(LoggerFactory parent, CanId CanId) { LoggerFactory log = parent.type(this); + switch (Identity.instance) { + case COMP_BOT -> { m_motor = new KrakenX60Motor( - log, - new CanId(0), - NeutralMode100.BRAKE, - MotorPhase.FORWARD, - 60, 1, - new SimpleDynamics(log, 0, 0), - new Friction(log, 0, 0, 0, 0), - new PIDConstants(log, 0, 0, 0, 0, 0, 0)); - IncrementalBareEncoder encoder = m_motor.encoder(); + log, + new CanId(0), + NeutralMode100.BRAKE, + MotorPhase.FORWARD, + 60, 1, + new SimpleDynamics(log, 0, 0), + new Friction(log, 0, 0, 0, 0), + new PIDConstants(log, 0, 0, 0, 0, 0, 0)); + IncrementalBareEncoder encoder = m_motor.encoder(); TrapezoidIncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); - ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, profile, 0.05, 0.05); + + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05); double initialPosition = 0; double gearRatio = 2; - RotaryMechanism climberMech = new RotaryMechanism( - log, m_motor, encoder, initialPosition, gearRatio, + RotaryMechanism climberMech = new RotaryMechanism( + log, m_motor, encoder, initialPosition, gearRatio, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); - m_servo = new OnboardAngularPositionServo(log, climberMech, ref); - }: - break; - - default ->{ + PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); + m_servo = new OnboardAngularPositionServo(log, climberMech, ref, feedback); + } + + default -> { m_motor = new SimulatedBareMotor(log, 600); IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); - ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, profile, 0.05, 0.05); - PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); - IncrementalBareEncoder encoder = m_motor.encoder(); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, ()-> profile, 0.05, 0.05); + PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); + IncrementalBareEncoder encoder = m_motor.encoder(); - SimulatedRotaryPositionSensor sensor = - new SimulatedRotaryPositionSensor(log, encoder, 1); - RotaryMechanism climberMech = - new RotaryMechanism( - log, - m_motor, - sensor, - 1, - Double.NEGATIVE_INFINITY, - Double.POSITIVE_INFINITY); - m_servo = new OnboardAngularPositionServo( log, climberMech, ref, feedback ); + SimulatedRotaryPositionSensor sensor = new SimulatedRotaryPositionSensor(log, encoder, 1); + RotaryMechanism climberMech = new RotaryMechanism( + log, + m_motor, + sensor, + 1, + Double.NEGATIVE_INFINITY, + Double.POSITIVE_INFINITY); + m_servo = new OnboardAngularPositionServo(log, climberMech, ref, feedback); + } } } - } public Command setClimb0() { return runOnce(this::setL0); @@ -92,14 +91,14 @@ public Command setClimb1() { } private void setL0() { - m_servo.setPosition(0); + m_servo.setPositionProfiled(0, 0); } private void setL1() { - m_servo.setPosition(m_level1); + m_servo.setPositionProfiled(m_level1,0 ); } private void setL3() { - m_servo.setPosition(m_level3); + m_servo.setPositionProfiled(m_level3, 0); } } diff --git a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java index 691de4e..50c9f9c 100644 --- a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java +++ b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java @@ -1,8 +1,27 @@ package org.team100.frc2026; +import org.team100.lib.config.Friction; +import org.team100.lib.config.Identity; +import org.team100.lib.config.PIDConstants; +import org.team100.lib.config.SimpleDynamics; +import org.team100.lib.controller.r1.PIDFeedback; import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.mechanism.LinearMechanism; +import org.team100.lib.mechanism.RotaryMechanism; import org.team100.lib.motor.BareMotor; +import org.team100.lib.motor.MotorPhase; +import org.team100.lib.motor.NeutralMode100; +import org.team100.lib.motor.ctre.KrakenX60Motor; import org.team100.lib.motor.sim.SimulatedBareMotor; +import org.team100.lib.profile.r1.IncrementalProfile; +import org.team100.lib.profile.r1.TrapezoidIncrementalProfile; +import org.team100.lib.reference.r1.IncrementalProfileReferenceR1; +import org.team100.lib.reference.r1.ProfileReferenceR1; +import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder; +import org.team100.lib.servo.OutboardLinearPositionServo; +import org.team100.lib.util.CanId; + +import com.ctre.phoenix6.swerve.utility.LinearPath; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -14,8 +33,54 @@ public class ClimberExtension extends SubsystemBase { public ClimberExtension(LoggerFactory parent) { LoggerFactory log = parent.type(this); - m_motor = new SimulatedBareMotor(log, 600); + + switch (Identity.instance) { + case COMP_BOT -> { + + m_motor = new KrakenX60Motor( + log, + new CanId(0), + NeutralMode100.BRAKE, + MotorPhase.FORWARD, + 60, 1, + new SimpleDynamics(log, 0, 0), + new Friction(log, 0, 0, 0, 0), + new PIDConstants(log, 0, 0, 0, 0, 0, 0)); + IncrementalBareEncoder encoder = m_motor.encoder(); + + TrapezoidIncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, ()-> profile, 0.05); + double initialPosition = 0; + double gearRatio = 2; + m_servo = new OutboardLinearPositionServo( log, climberMech, ref, feedback ); + LinearPath climberMech = new LinearPath( + log, m_servo, encoder, initialPosition, gearRatio, + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + }: + break; + + default -> { + m_motor = new SimulatedBareMotor(log, 600); + IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, profile, 0.05, 0.05); + PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); + IncrementalBareEncoder encoder = m_motor.encoder(); + + SimulatedLinearPositionSensor sensor = + new SimulatedLinearPositionSensor(log, encoder, 1); + RotaryMechanism climberMech = + new LinearMechanism( + log, + m_motor, + sensor, + 1, + Double.NEGATIVE_INFINITY, + Double.POSITIVE_INFINITY); + m_servo = new OnboardLinearPositionServo( log, climberMech, ref, feedback ); + } + } } + public Command setPosition() { return run(this::setOutPosition); } diff --git a/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java b/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java index b7d1e73..93822f1 100644 --- a/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java +++ b/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java @@ -9,7 +9,6 @@ import org.team100.lib.logging.LoggerFactory.BooleanLogger; import org.team100.lib.logging.LoggerFactory.SetpointsR1Logger; import org.team100.lib.profile.r1.IncrementalProfile; -import org.team100.lib.profile.r1.TrapezoidIncrementalProfile; import org.team100.lib.state.ControlR1; import org.team100.lib.state.ModelR1; @@ -30,7 +29,7 @@ public class IncrementalProfileReferenceR1 implements ProfileReferenceR1 { public IncrementalProfileReferenceR1( LoggerFactory parent, - TrapezoidIncrementalProfile profile, + Supplier profile, double positionTolerance, double velocityTolerance) { LoggerFactory log = parent.type(this); diff --git a/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java b/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java index bcbb0d6..b73636c 100644 --- a/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java +++ b/lib/src/main/java/org/team100/lib/servo/OnboardAngularPositionServo.java @@ -35,7 +35,8 @@ public class OnboardAngularPositionServo extends AngularPositionServoImpl { public OnboardAngularPositionServo( LoggerFactory parent, RotaryMechanism mech, - ProfileReferenceR1 ref) { + ProfileReferenceR1 ref, + FeedbackR1 feedback) { super(parent, mech, ref); if (feedback.handlesWrapping()) throw new IllegalArgumentException("Do not supply wrapping feedback"); @@ -95,14 +96,4 @@ void actuate(SetpointsR1 unwrappedSetpoint, double feedForwardTorqueNm) { m_log_velocity_error.log(() -> currentUnwrappedSetpoint.v() - unwrappedMeasurement.v()); } - public void setPosition(double mLevel1) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); - } - - public void setPosition(double mLevel3) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); - } - } diff --git a/lib/src/main/java/org/team100/lib/servo/OutboardLinearPositionServo.java b/lib/src/main/java/org/team100/lib/servo/OutboardLinearPositionServo.java index b83edeb..ccca1b1 100644 --- a/lib/src/main/java/org/team100/lib/servo/OutboardLinearPositionServo.java +++ b/lib/src/main/java/org/team100/lib/servo/OutboardLinearPositionServo.java @@ -5,11 +5,14 @@ import org.team100.lib.logging.LoggerFactory.ControlR1Logger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.mechanism.LinearMechanism; +import org.team100.lib.mechanism.RotaryMechanism; import org.team100.lib.reference.r1.ProfileReferenceR1; import org.team100.lib.reference.r1.SetpointsR1; import org.team100.lib.state.ControlR1; import org.team100.lib.state.ModelR1; +import com.ctre.phoenix6.swerve.utility.LinearPath; + /** * Profiled or direct position control using the feedback controller in the * motor controller hardware. @@ -48,6 +51,7 @@ public OutboardLinearPositionServo( m_log_velocity = log.doubleLogger(Level.COMP, "velocity (m_s)"); } + @Override public void reset() { // using the current velocity sometimes includes a whole lot of noise, and then From 6f240a689b0394a9d23d517ca0e40cdc865691f9 Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Sat, 14 Feb 2026 11:56:45 -0800 Subject: [PATCH 3/8] NO MORE ERRORS!!! --- .../org/team100/frc2026/ClimberExtension.java | 93 ++++++++++--------- 1 file changed, 50 insertions(+), 43 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java index 50c9f9c..6daa16d 100644 --- a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java +++ b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java @@ -17,81 +17,88 @@ import org.team100.lib.profile.r1.TrapezoidIncrementalProfile; import org.team100.lib.reference.r1.IncrementalProfileReferenceR1; import org.team100.lib.reference.r1.ProfileReferenceR1; +import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor; import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder; +import org.team100.lib.servo.AngularPositionServo; +import org.team100.lib.servo.LinearPositionServo; +import org.team100.lib.servo.OnboardAngularPositionServo; +import org.team100.lib.servo.OutboardAngularPositionServo; import org.team100.lib.servo.OutboardLinearPositionServo; import org.team100.lib.util.CanId; -import com.ctre.phoenix6.swerve.utility.LinearPath; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ClimberExtension extends SubsystemBase { - private final BareMotor m_motor; + private final LinearPositionServo m_servo; - private final double m_maxExtension=20; + private final double m_maxExtension = 20; public ClimberExtension(LoggerFactory parent) { LoggerFactory log = parent.type(this); - + IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05); + switch (Identity.instance) { case COMP_BOT -> { - - m_motor = new KrakenX60Motor( - log, - new CanId(0), - NeutralMode100.BRAKE, - MotorPhase.FORWARD, - 60, 1, - new SimpleDynamics(log, 0, 0), - new Friction(log, 0, 0, 0, 0), - new PIDConstants(log, 0, 0, 0, 0, 0, 0)); - IncrementalBareEncoder encoder = m_motor.encoder(); - - TrapezoidIncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); - ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, ()-> profile, 0.05); + + KrakenX60Motor m_motor = new KrakenX60Motor(log, new CanId(0), NeutralMode100.BRAKE, MotorPhase.FORWARD, + 60, 1, + new SimpleDynamics(log, 0, 0), new Friction(log, 0, 0, 0, 0), + new PIDConstants(log, 0, 0, 0, 0, 0, 0)); + + IncrementalBareEncoder encoder = m_motor.encoder(); + double initialPosition = 0; double gearRatio = 2; - m_servo = new OutboardLinearPositionServo( log, climberMech, ref, feedback ); - LinearPath climberMech = new LinearPath( - log, m_servo, encoder, initialPosition, gearRatio, + LinearMechanism climberMech = new LinearMechanism( + log, m_motor, encoder, initialPosition, gearRatio, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); - }: - break; + m_servo = new OutboardLinearPositionServo( + log, // LoggerFactory parent, + climberMech,// LinearMechanism mechanism, + ref, // ProfileReferenceR1 ref, + 0.01, // double positionTolerance, + 0.01 // double velocityTolerance) + ); + } default -> { - m_motor = new SimulatedBareMotor(log, 600); - IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); - ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, profile, 0.05, 0.05); + SimulatedBareMotor m_motor = new SimulatedBareMotor(log, 600); PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); IncrementalBareEncoder encoder = m_motor.encoder(); - SimulatedLinearPositionSensor sensor = - new SimulatedLinearPositionSensor(log, encoder, 1); - RotaryMechanism climberMech = - new LinearMechanism( - log, - m_motor, - sensor, - 1, - Double.NEGATIVE_INFINITY, - Double.POSITIVE_INFINITY); - m_servo = new OnboardLinearPositionServo( log, climberMech, ref, feedback ); + SimulatedRotaryPositionSensor sensor = new SimulatedRotaryPositionSensor(log, encoder, 1); + Double minimumPosition = 0.1; + LinearMechanism climberMech = new LinearMechanism( + log, m_motor, encoder, 1, 2, + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + + m_servo = new OutboardLinearPositionServo( + log, // LoggerFactory parent, + climberMech, // LinearMechanism mechanism, + ref, // ProfileReferenceR1 ref, + 0.01, // double positionTolerance, + 0.01 // double velocityTolerance + ); } - } + } } - + public Command setPosition() { return run(this::setOutPosition); } + public Command setHomePosition() { return run(this::setInPosition); } + private void setOutPosition() { - m_motor.setUnwrappedPosition(m_maxExtension,0,0,0); - + m_servo.setPositionProfiled(m_maxExtension, 0); + } + public void setInPosition() { - m_motor.setUnwrappedPosition(0,0,0,0); + m_servo.setPositionProfiled(0, 0); } } From 704cfee5a85fcbfdd476c8d664086a332c69c325 Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Sat, 14 Feb 2026 13:41:17 -0800 Subject: [PATCH 4/8] finished binding buttons --- comp/simgui-ds.json | 35 ++++++++++--------- .../org/team100/frc2026/ClimberExtension.java | 5 +++ .../org/team100/frc2026/robot/Binder.java | 5 ++- .../org/team100/frc2026/robot/Machinery.java | 2 +- 4 files changed, 27 insertions(+), 20 deletions(-) diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index a2e3cce..575a846 100644 --- a/comp/simgui-ds.json +++ b/comp/simgui-ds.json @@ -4,9 +4,9 @@ "visible": false } }, - "Joysticks": { + "Keyboard 0 Settings": { "window": { - "visible": false + "visible": true } }, "System Joysticks": { @@ -39,13 +39,13 @@ "axisCount": 6, "buttonCount": 7, "buttonKeys": [ - 90, - 88, - 67, - 86, - 66, - 78, - 77 + 49, + 50, + 51, + 52, + 53, + 54, + 55 ], "povConfig": [ { @@ -82,11 +82,11 @@ "axisCount": 6, "buttonCount": 5, "buttonKeys": [ - 49, - 50, - 51, - 52, - 53 + -1, + -1, + -1, + -1, + -1 ], "povCount": 0 }, @@ -120,11 +120,14 @@ } ], "robotJoysticks": [ + {}, { - "guid": "Keyboard0" + "useGamepad": true }, + {}, + {}, { - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java index 6daa16d..b2e2b6e 100644 --- a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java +++ b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java @@ -101,4 +101,9 @@ private void setOutPosition() { public void setInPosition() { m_servo.setPositionProfiled(0, 0); } + + @Override + public void periodic() { + m_servo.periodic(); + } } diff --git a/comp/src/main/java/org/team100/frc2026/robot/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java index 444dcad..91d57e5 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -140,9 +140,9 @@ public void bind() { /// whileTrue(driver::b, m_machinery.m_shooter.shoot()); - whileTrue(driver::x, m_machinery.m_intake.intake()); + // whileTrue(driver::x, m_machinery.m_intake.intake()); - whileTrue(driver::y, m_machinery.m_serializer.serialize()); + // whileTrue(driver::y, m_machinery.m_serializer.serialize()); // Test bindings whileTrue(driver::leftBumper, m_machinery.m_extender.goToExtendedPosition()); @@ -150,7 +150,6 @@ public void bind() { whileTrue(driver::rightTrigger, m_machinery.m_ClimberExtension.setPosition()); whileTrue(driver::x, m_machinery.m_Climber.setClimb0()); whileTrue(driver::y, m_machinery.m_Climber.setClimb1()); - whileTrue(driver::b, m_machinery.m_Climber.setClimb3()); // The real bindings whileTrue(driver::leftBumper, m_machinery.m_extender.goToRetractedPosition()); diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java index 081029c..1d5e768 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -93,7 +93,7 @@ public Machinery() { m_extender = new IntakeExtend(driveLog, new CanId(19)); m_serializer = new Serializer(driveLog); m_ClimberExtension = new ClimberExtension(driveLog); - m_Climber = new Climber(driveLog); + m_Climber = new Climber(driveLog, new CanId(32)); //////////////////////////////////////////////////////////// // From a64ab490aadc31d96918fcde49134c38f8e0b75e Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Sun, 15 Feb 2026 10:10:17 -0800 Subject: [PATCH 5/8] binded and made climber go up & down --- comp/simgui-ds.json | 34 +++++++++++-------- .../java/org/team100/frc2026/Climber.java | 4 +++ .../org/team100/frc2026/robot/Binder.java | 8 +++++ 3 files changed, 31 insertions(+), 15 deletions(-) diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index 575a846..4d75015 100644 --- a/comp/simgui-ds.json +++ b/comp/simgui-ds.json @@ -9,31 +9,34 @@ "visible": true } }, - "System Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ { - "decKey": 69, - "incKey": 82 + "decKey": 65, + "incKey": 81 }, - {}, { + "decKey": 83, + "incKey": 87 + }, + { + "decKey": 68, "decayRate": 0.0, + "incKey": 69, "keyRate": 0.009999999776482582 }, - {}, { - "decKey": 65, - "incKey": 68 + "decKey": 70, + "incKey": 82 + }, + { + "decKey": 71, + "incKey": 84 }, { - "decKey": 87, - "incKey": 83 + "decKey": 72, + "incKey": 89 } ], "axisCount": 6, @@ -75,7 +78,6 @@ "incKey": 76 }, { - "decKey": 73, "incKey": 75 } ], @@ -122,12 +124,14 @@ "robotJoysticks": [ {}, { + "guid": "78696e70757401000000000000000000", "useGamepad": true }, {}, {}, { - "guid": "Keyboard0" + "guid": "Keyboard0", + "name": "keyboard" } ] } diff --git a/comp/src/main/java/org/team100/frc2026/Climber.java b/comp/src/main/java/org/team100/frc2026/Climber.java index b37e2bd..11912e4 100644 --- a/comp/src/main/java/org/team100/frc2026/Climber.java +++ b/comp/src/main/java/org/team100/frc2026/Climber.java @@ -90,6 +90,10 @@ public Command setClimb1() { return runOnce(this::setL1); } + public Command setClimb3() { + return runOnce(this::setL3); + } + private void setL0() { m_servo.setPositionProfiled(0, 0); } diff --git a/comp/src/main/java/org/team100/frc2026/robot/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java index 91d57e5..b82c67a 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -150,6 +150,14 @@ public void bind() { whileTrue(driver::rightTrigger, m_machinery.m_ClimberExtension.setPosition()); whileTrue(driver::x, m_machinery.m_Climber.setClimb0()); whileTrue(driver::y, m_machinery.m_Climber.setClimb1()); + whileTrue(driver::a, + m_machinery.m_ClimberExtension.setPosition() + .andThen(m_machinery.m_Climber.setClimb1() + .andThen(m_machinery.m_Climber.setClimb0()))); + whileTrue(driver::b, + m_machinery.m_ClimberExtension.setPosition() + .andThen(m_machinery.m_Climber.setClimb3() + .andThen(m_machinery.m_Climber.setClimb0()))); // The real bindings whileTrue(driver::leftBumper, m_machinery.m_extender.goToRetractedPosition()); From b910c27d13cfb7ffa6bbeb7ff5f3f21e85f29683 Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Sun, 15 Feb 2026 12:14:46 -0800 Subject: [PATCH 6/8] fixed bindings, made a 'return to home' button when done climbing --- comp/simgui-ds.json | 9 ++++++++- comp/src/main/java/org/team100/frc2026/Climber.java | 9 +++++++-- .../java/org/team100/frc2026/ClimberExtension.java | 3 ++- .../main/java/org/team100/frc2026/robot/Binder.java | 13 ++++++------- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index 4d75015..f97ddfe 100644 --- a/comp/simgui-ds.json +++ b/comp/simgui-ds.json @@ -9,6 +9,11 @@ "visible": true } }, + "System Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -122,11 +127,13 @@ } ], "robotJoysticks": [ - {}, { "guid": "78696e70757401000000000000000000", "useGamepad": true }, + { + "useGamepad": true + }, {}, {}, { diff --git a/comp/src/main/java/org/team100/frc2026/Climber.java b/comp/src/main/java/org/team100/frc2026/Climber.java index 11912e4..8f4eb4c 100644 --- a/comp/src/main/java/org/team100/frc2026/Climber.java +++ b/comp/src/main/java/org/team100/frc2026/Climber.java @@ -30,6 +30,7 @@ public class Climber extends SubsystemBase { private final BareMotor m_motor; private final OnboardAngularPositionServo m_servo; + private static final double m_level0 = 0.1; private static final double m_level1 = 90; private static final double m_level3 = 180; @@ -95,14 +96,18 @@ public Command setClimb3() { } private void setL0() { - m_servo.setPositionProfiled(0, 0); + m_servo.setPositionProfiled(m_level0, 0); } private void setL1() { - m_servo.setPositionProfiled(m_level1,0 ); + m_servo.setPositionProfiled(m_level1, 0); } private void setL3() { m_servo.setPositionProfiled(m_level3, 0); } + @Override + public void periodic() { + m_servo.periodic(); + } } diff --git a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java index b2e2b6e..3b1c72b 100644 --- a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java +++ b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java @@ -33,6 +33,7 @@ public class ClimberExtension extends SubsystemBase { private final LinearPositionServo m_servo; private final double m_maxExtension = 20; + private final double m_minextension = 0.01; public ClimberExtension(LoggerFactory parent) { LoggerFactory log = parent.type(this); @@ -99,7 +100,7 @@ private void setOutPosition() { } public void setInPosition() { - m_servo.setPositionProfiled(0, 0); + m_servo.setPositionProfiled(m_minextension, 0.01); } @Override diff --git a/comp/src/main/java/org/team100/frc2026/robot/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java index b82c67a..3394a87 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -148,16 +148,15 @@ public void bind() { whileTrue(driver::leftBumper, m_machinery.m_extender.goToExtendedPosition()); whileTrue(driver::rightBumper, m_machinery.m_extender.goToRetractedPosition()); whileTrue(driver::rightTrigger, m_machinery.m_ClimberExtension.setPosition()); - whileTrue(driver::x, m_machinery.m_Climber.setClimb0()); - whileTrue(driver::y, m_machinery.m_Climber.setClimb1()); - whileTrue(driver::a, + whileTrue(driver::x, m_machinery.m_ClimberExtension.setPosition() - .andThen(m_machinery.m_Climber.setClimb1() - .andThen(m_machinery.m_Climber.setClimb0()))); + .andThen(m_machinery.m_Climber.setClimb1())); whileTrue(driver::b, m_machinery.m_ClimberExtension.setPosition() - .andThen(m_machinery.m_Climber.setClimb3() - .andThen(m_machinery.m_Climber.setClimb0()))); + .andThen(m_machinery.m_Climber.setClimb3())); + whileTrue(driver::a, + m_machinery.m_Climber.setClimb0() + .andThen(m_machinery.m_ClimberExtension.setHomePosition())); // The real bindings whileTrue(driver::leftBumper, m_machinery.m_extender.goToRetractedPosition()); From 0cdf672b7c5073850c1c094781cb00dc03433490 Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Mon, 16 Feb 2026 11:16:39 -0800 Subject: [PATCH 7/8] started work on auton --- .../team100/frc2026/auton/AutonPositions.java | 11 +++ .../team100/frc2026/auton/ClimberAuton.java | 98 +++++++++++++++++++ .../org/team100/frc2026/robot/Machinery.java | 4 +- 3 files changed, 111 insertions(+), 2 deletions(-) create mode 100644 comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java create mode 100644 comp/src/main/java/org/team100/frc2026/auton/ClimberAuton.java diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java new file mode 100644 index 0000000..7b9ea25 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java @@ -0,0 +1,11 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class AutonPositions { + + public static final Pose2d CLIMB_LEFT = new Pose2d(1.175, 4.25, new Rotation2d(-135 * (Math.PI / 180))); + public static final Pose2d CLIMB_RIGHT = new Pose2d(1.175, 3.1, new Rotation2d(135 * (Math.PI / 180))); + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/ClimberAuton.java b/comp/src/main/java/org/team100/frc2026/auton/ClimberAuton.java new file mode 100644 index 0000000..f81a2b9 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/ClimberAuton.java @@ -0,0 +1,98 @@ +package org.team100.frc2026.auton; + +import static edu.wpi.first.wpilibj2.command.Commands.parallel; +import static edu.wpi.first.wpilibj2.command.Commands.sequence; +import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; + +import java.util.List; +import java.util.ArrayList; + +import org.team100.frc2026.Intake; +import org.team100.frc2026.robot.Machinery; +import org.team100.lib.config.AnnotatedCommand; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; +import org.team100.lib.trajectory.TrajectorySE2; +import org.team100.lib.trajectory.TrajectorySE2Factory; +import org.team100.lib.trajectory.TrajectorySE2Planner; +import org.team100.lib.trajectory.constraint.TimingConstraint; +import org.team100.lib.trajectory.constraint.TimingConstraintFactory; +import org.team100.lib.trajectory.path.PathSE2Factory; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberAuton implements AnnotatedCommand { + private final LoggerFactory log; + private final ControllerSE2 controller; + private final Machinery machinery; + private final List constraints; + private final TrajectorySE2Factory trajectoryFactory; + private final PathSE2Factory pathFactory; + private final TrajectorySE2Planner planner; + + public ClimberAuton( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + log = parent.name(name()); + this.controller = controller; + this.machinery = machinery; + constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + trajectoryFactory = new TrajectorySE2Factory(constraints); + pathFactory = new PathSE2Factory(); + planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + } + + @Override + public String name() { + return "Climber Auton"; + } + + TrajectorySE2 t1(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2(AutonPositions.CLIMB_LEFT, + new DirectionSE2(1, 1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t2(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(0, -1, 0), 1), + new WaypointSE2(AutonPositions.CLIMB_RIGHT, + new DirectionSE2(0, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + @Override + public Command command() { + DriveWithTrajectoryFunction n1 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t1); + DriveWithTrajectoryFunction n2 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t2); + return sequence( + n1.until(n1::isDone), + waitSeconds(1), + parallel( + n2.until(n2::isDone), + machinery.m_ClimberExtension.setPosition()), + waitSeconds(1)); + } + + @Override + public Pose2d start() { + return StartingPositions.LEFT_BUMP; + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java index 1d5e768..ace02ca 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -74,8 +74,8 @@ public class Machinery { final Intake m_intake; final IntakeExtend m_extender; final Serializer m_serializer; - final ClimberExtension m_ClimberExtension; - final Climber m_Climber; + public final ClimberExtension m_ClimberExtension; + public final Climber m_Climber; public Machinery() { From e73dfd1b682722e8587cf724be71f6c882662c9f Mon Sep 17 00:00:00 2001 From: Harpsicord Date: Mon, 16 Feb 2026 11:33:13 -0800 Subject: [PATCH 8/8] added to selector --- comp/src/main/java/org/team100/frc2026/auton/AllAutons.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java index 7977b2a..30f02b6 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -35,6 +35,11 @@ public AllAutons(Machinery machinery, ControllerSE2 controller) { machinery.m_swerveKinodynamics, controller, machinery)); + m_autonChooser.add(new ClimberAuton( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); } public Command get() {