-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Update Cheese Stick to use servo and have simulation support.
- Loading branch information
1 parent
bedeb8a
commit f27d4c3
Showing
4 changed files
with
176 additions
and
131 deletions.
There are no files selected for viewing
This file contains 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,115 @@ | ||
| { | ||
| "Keyboard 1 Settings": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| }, | ||
| "keyboardJoysticks": [ | ||
| { | ||
| "axisConfig": [ | ||
| { | ||
| "decKey": 65, | ||
| "incKey": 68 | ||
| }, | ||
| { | ||
| "decKey": 87, | ||
| "incKey": 83 | ||
| }, | ||
| { | ||
| "decKey": 69, | ||
| "decayRate": 0.0, | ||
| "incKey": 82, | ||
| "keyRate": 0.009999999776482582 | ||
| } | ||
| ], | ||
| "axisCount": 3, | ||
| "buttonCount": 4, | ||
| "buttonKeys": [ | ||
| 90, | ||
| 88, | ||
| 67, | ||
| 86, | ||
| -1, | ||
| -1, | ||
| -1, | ||
| -1 | ||
| ], | ||
| "povConfig": [ | ||
| { | ||
| "key0": 328, | ||
| "key135": 323, | ||
| "key180": 322, | ||
| "key225": 321, | ||
| "key270": 324, | ||
| "key315": 327, | ||
| "key45": 329, | ||
| "key90": 326 | ||
| } | ||
| ], | ||
| "povCount": 1 | ||
| }, | ||
| { | ||
| "axisConfig": [ | ||
| { | ||
| "decKey": 74, | ||
| "incKey": 76 | ||
| }, | ||
| { | ||
| "decKey": 73, | ||
| "incKey": 75 | ||
| } | ||
| ], | ||
| "axisCount": 2, | ||
| "buttonCount": 8, | ||
| "buttonKeys": [ | ||
| 77, | ||
| 44, | ||
| 46, | ||
| 47, | ||
| -1, | ||
| -1, | ||
| -1, | ||
| 80, | ||
| -1, | ||
| -1 | ||
| ], | ||
| "povCount": 0 | ||
| }, | ||
| { | ||
| "axisConfig": [ | ||
| { | ||
| "decKey": 263, | ||
| "incKey": 262 | ||
| }, | ||
| { | ||
| "decKey": 265, | ||
| "incKey": 264 | ||
| } | ||
| ], | ||
| "axisCount": 2, | ||
| "buttonCount": 6, | ||
| "buttonKeys": [ | ||
| 260, | ||
| 268, | ||
| 266, | ||
| 261, | ||
| 269, | ||
| 267 | ||
| ], | ||
| "povCount": 0 | ||
| }, | ||
| { | ||
| "axisCount": 0, | ||
| "buttonCount": 0, | ||
| "povCount": 0 | ||
| } | ||
| ], | ||
| "robotJoysticks": [ | ||
| { | ||
| "guid": "Keyboard0" | ||
| }, | ||
| { | ||
| "guid": "Keyboard1" | ||
| } | ||
| ] | ||
| } |
This file contains 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 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
145 changes: 31 additions & 114 deletions
145
src/main/java/frc/robot/subsystems/CheeseStickSubsystem.java
This file contains 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,139 +1,56 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import static com.revrobotics.spark.SparkBase.PersistMode.*; | ||
| import static com.revrobotics.spark.SparkBase.ResetMode.*; | ||
| import static com.revrobotics.spark.SparkLowLevel.MotorType.*; | ||
| import static com.revrobotics.spark.config.SparkBaseConfig.IdleMode.*; | ||
| import static edu.wpi.first.wpilibj2.command.Commands.*; | ||
| import static edu.wpi.first.units.Units.*; | ||
| import static frc.robot.Constants.CheeseStickConstants.*; | ||
|
|
||
| import java.util.function.BooleanSupplier; | ||
| import java.util.function.DoubleSupplier; | ||
|
|
||
| import com.revrobotics.RelativeEncoder; | ||
| import com.revrobotics.spark.SparkMax; | ||
|
|
||
| import edu.wpi.first.math.controller.PIDController; | ||
| import edu.wpi.first.wpilibj.Servo; | ||
| import edu.wpi.first.wpilibj.simulation.PWMSim; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismObject2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
|
|
||
| /** | ||
| * This subsystem is for the cheese stick gripper. | ||
| */ | ||
| public class CheeseStickSubsystem extends SubsystemBase { | ||
| private SparkMax m_motor; | ||
| private RelativeEncoder m_encoder; | ||
| private DoubleSupplier m_position; | ||
|
|
||
| public CheeseStickSubsystem() { | ||
| m_motor = new SparkMax(kMotorPort, kBrushless); | ||
| m_motor.configure(kMotorConfig, kResetSafeParameters, kNoPersistParameters); | ||
| m_encoder = m_motor.getEncoder(); | ||
| m_position = () -> m_encoder.getPosition(); | ||
| } | ||
|
|
||
| /** | ||
| * Resets the subsystem to the default state based on the springs. | ||
| * | ||
| * @return The command. | ||
| */ | ||
| public Command resetPositionCommand() { | ||
| return runOnce( | ||
| () -> m_motor.configure(kMotorConfig.idleMode(kCoast), kResetSafeParameters, kNoPersistParameters)) | ||
| .andThen(waitTime(kResetLength)).andThen(() -> { | ||
| m_motor.configure( | ||
| kMotorConfig.idleMode(kBrake), kResetSafeParameters, kNoPersistParameters); | ||
| m_encoder.setPosition(0); | ||
| }); | ||
| } | ||
|
|
||
| /** | ||
| * Resturns a supplier for the cheese stick position. This might be useful in | ||
| * the future. | ||
| * | ||
| * @return The supplier. | ||
| */ | ||
| public DoubleSupplier getPosition() { | ||
| return m_position; | ||
| } | ||
|
|
||
| /** | ||
| * Creates a command which opens the cheese stick in order to insert into the | ||
| * coral. | ||
| * | ||
| * @return The command. | ||
| */ | ||
| public Command createOpenCommand() { | ||
| return createMoveCommand(kOpenDistance); | ||
| private final Servo m_servo = new Servo(kServoPort); | ||
| private final PWMSim m_sim = new PWMSim(m_servo); | ||
| private final MechanismLigament2d m_ligament = new MechanismLigament2d("Cheeese Stick", 0, 90); | ||
|
|
||
| public CheeseStickSubsystem(MechanismObject2d attachment) { | ||
| // https://docs.revrobotics.com/rev-crossover-products/servo/srs#electrical-specifications | ||
| m_servo.setBoundsMicroseconds(2500, 0, 0, 0, 500); | ||
| m_servo.setZeroLatch(); | ||
| attachment.append(m_ligament); | ||
| } | ||
|
|
||
| /** | ||
| * Creates a command which grips the coral. | ||
| * | ||
| * @return The command. | ||
| * Logs data to the smart dashboard. Displays the simulated mechanism. | ||
| */ | ||
| public Command createGripCommand() { | ||
| return createMoveCommand(0); | ||
| public void periodic() { | ||
| double position = m_sim.getPosition(); | ||
| SmartDashboard.putNumber("Cheese Stick Position", position); | ||
| m_ligament.setLength(kExtensionLength.in(Meters) * position); | ||
| } | ||
|
|
||
| /** | ||
| * Creates a command which moves the cheese stick to the specified position. | ||
| * Returns a command to command the servo to rotate anti-clockwise. This pulls | ||
| * against springs and retracts the cheese stick. Does not wait | ||
| * for it to finish. | ||
| * | ||
| * @param distance The position to go to. | ||
| * @return The command. | ||
| */ | ||
| private Command createMoveCommand(double distance) { | ||
| PIDController PID = new PIDController(kP, 0, 0); | ||
| PID.setTolerance(kTolerance); | ||
| return new Command() { | ||
| /** | ||
| * Evaluate the PID controller. | ||
| */ | ||
| public void execute() { | ||
| m_motor.set(PID.calculate(m_position.getAsDouble())); | ||
| } | ||
|
|
||
| /** | ||
| * Checks if the position is within tolerance or if the timeout has ocurred. | ||
| */ | ||
| public boolean isFinished() { | ||
| return PID.atSetpoint(); | ||
| } | ||
|
|
||
| /** | ||
| * Stops the motor from moving. | ||
| */ | ||
| public void end(boolean interrupted) { | ||
| m_motor.set(0); | ||
| PID.close(); | ||
| } | ||
| }.withTimeout(kGripTimeout); | ||
| public Command retract() { | ||
| return runOnce(() -> m_servo.set(0)).withName("Servo Retract"); | ||
| } | ||
|
|
||
| /** | ||
| * Creates a command which controls the cheese stick based on an activator | ||
| * (likely a button) | ||
| * Returns a command to command the servo to rotate clockwise. This is pushed by | ||
| * springs and extends the cheese stock. Does not wait | ||
| * for it to finish. | ||
| * | ||
| * @param activator The supplier which defines when the cheese stick is open. | ||
| * @return The command. | ||
| */ | ||
| public Command createManualCommand(BooleanSupplier activator) { | ||
| Command open = createOpenCommand(); | ||
| Command grip = createGripCommand(); | ||
| return new Command() { | ||
| public void execute() { | ||
| if (activator.getAsBoolean()) { | ||
| if (!open.isScheduled()) | ||
| open.schedule(); | ||
| if (grip.isScheduled()) | ||
| grip.cancel(); | ||
| } else { | ||
| if (open.isScheduled()) | ||
| open.cancel(); | ||
| if (!grip.isScheduled()) | ||
| grip.schedule(); | ||
| } | ||
| } | ||
| }; | ||
| public Command extend() { | ||
| return runOnce(() -> m_servo.set(1)).withName("Servo Extend"); | ||
| } | ||
| } | ||
| } |