diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index a2e3cce..f97ddfe 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": { @@ -18,34 +18,42 @@ { "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, "buttonCount": 7, "buttonKeys": [ - 90, - 88, - 67, - 86, - 66, - 78, - 77 + 49, + 50, + 51, + 52, + 53, + 54, + 55 ], "povConfig": [ { @@ -75,18 +83,17 @@ "incKey": 76 }, { - "decKey": 73, "incKey": 75 } ], "axisCount": 6, "buttonCount": 5, "buttonKeys": [ - 49, - 50, - 51, - 52, - 53 + -1, + -1, + -1, + -1, + -1 ], "povCount": 0 }, @@ -121,10 +128,17 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true }, { "useGamepad": true + }, + {}, + {}, + { + "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 d52d789..8f4eb4c 100644 --- a/comp/src/main/java/org/team100/frc2026/Climber.java +++ b/comp/src/main/java/org/team100/frc2026/Climber.java @@ -1,45 +1,113 @@ 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_level0 = 0.1; + 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); - 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); + 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(); + + 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); + return runOnce(this::setL1); } - public Command setClimb3() { - return run(this::setL3); + public Command setClimb3() { + return runOnce(this::setL3); } - + private void setL0() { - m_motor.setUnwrappedPosition(0, 0, 0, 0); - + m_servo.setPositionProfiled(m_level0, 0); } - public void setL1() { - m_motor.setUnwrappedPosition(m_level1, 0, 0, 0); + private void setL1() { + m_servo.setPositionProfiled(m_level1, 0); } - public void setL3() { - m_motor.setUnwrappedPosition(m_level3, 0, 0, 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 691de4e..3b1c72b 100644 --- a/comp/src/main/java/org/team100/frc2026/ClimberExtension.java +++ b/comp/src/main/java/org/team100/frc2026/ClimberExtension.java @@ -1,32 +1,110 @@ 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.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 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; + private final double m_minextension = 0.01; public ClimberExtension(LoggerFactory parent) { LoggerFactory log = parent.type(this); - 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); + + switch (Identity.instance) { + case COMP_BOT -> { + + 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; + LinearMechanism climberMech = new LinearMechanism( + log, m_motor, encoder, initialPosition, gearRatio, + 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) + ); + } + + default -> { + 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(); + + 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(m_minextension, 0.01); + } + + @Override + public void periodic() { + m_servo.periodic(); } } 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() { 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/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java index 444dcad..3394a87 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -140,17 +140,23 @@ 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()); 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::b, m_machinery.m_Climber.setClimb3()); + whileTrue(driver::x, + m_machinery.m_ClimberExtension.setPosition() + .andThen(m_machinery.m_Climber.setClimb1())); + whileTrue(driver::b, + m_machinery.m_ClimberExtension.setPosition() + .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()); 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..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() { @@ -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)); //////////////////////////////////////////////////////////// // 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/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