changed it to match intake extend better #43
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| 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, 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(); | ||
|
|
||
| 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 runOnce(this::setL0); | ||
|
Member
Author
There was a problem hiding this comment. this should be "run". the difference is that "runOnce" executes the lambda once on command initialize and then never does anything else. you want this lambda (which is ultimately "set position") to run on each iteration until it's finished. |
||
| } | ||
|
|
||
| public Command setClimb1() { | ||
| return runOnce(this::setL1); | ||
| } | ||
|
|
||
| public Command setClimb3() { | ||
| return runOnce(this::setL3); | ||
| } | ||
| private void setL0() { | ||
| m_servo.setPositionProfiled(m_level0, 0); | ||
| } | ||
|
|
||
| private void setL1() { | ||
| m_servo.setPositionProfiled(m_level1, 0); | ||
| } | ||
|
|
||
| private void setL3() { | ||
| m_servo.setPositionProfiled(m_level3, 0); | ||
| } | ||
| @Override | ||
| public void periodic() { | ||
| m_servo.periodic(); | ||
| } | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 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); | ||
| 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_servo.setPositionProfiled(m_maxExtension, 0); | ||
|
|
||
| } | ||
|
|
||
| public void setInPosition() { | ||
| m_servo.setPositionProfiled(m_minextension, 0.01); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| m_servo.periodic(); | ||
| } | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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))); | ||
|
|
||
| } |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
you can pull the common elements (e.g. the profile) out of the switch statement