-
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.
- Loading branch information
1 parent
2a08f6b
commit e9f2276
Showing
14 changed files
with
338 additions
and
15 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
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
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
46 changes: 46 additions & 0 deletions
46
src/main/java/org/team1540/advantagekitdemo/commands/ArcadeDriveCommand.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 |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| package org.team1540.advantagekitdemo.commands; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.math.filter.SlewRateLimiter; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.button.CommandXboxController; | ||
| import org.team1540.advantagekitdemo.Constants; | ||
| import org.team1540.advantagekitdemo.subsystems.drivetrain.Drivetrain; | ||
|
|
||
| public class ArcadeDriveCommand extends Command { | ||
| private final Drivetrain drivetrain; | ||
|
|
||
| private final CommandXboxController xBoxController; | ||
|
|
||
| private final SlewRateLimiter leftRateLimiter = new SlewRateLimiter(5); | ||
| private final SlewRateLimiter rightRateLimiter = new SlewRateLimiter(5); | ||
|
|
||
| public ArcadeDriveCommand(Drivetrain drivetrain, CommandXboxController xBoxController) { | ||
| this.drivetrain = drivetrain; | ||
| this.xBoxController = xBoxController; | ||
| addRequirements(drivetrain); | ||
| } | ||
|
|
||
| public void execute() { | ||
| double throttle = MathUtil.applyDeadband(-xBoxController.getLeftY(), Constants.DEADZONE_RADIUS); | ||
| double turn = MathUtil.applyDeadband(xBoxController.getRightX(), Constants.DEADZONE_RADIUS); | ||
| double left = leftRateLimiter.calculate( | ||
| MathUtil.clamp(throttle + turn,-1, 1) | ||
| ); | ||
| double right = rightRateLimiter.calculate( | ||
| MathUtil.clamp(throttle - turn, -1, 1) | ||
| ); | ||
|
|
||
| drivetrain.drivePercent(left, right); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| drivetrain.stop(); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return false; | ||
| } | ||
| } |
26 changes: 26 additions & 0 deletions
26
src/main/java/org/team1540/advantagekitdemo/commands/ElevatorManualCommand.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 |
|---|---|---|
| @@ -0,0 +1,26 @@ | ||
| package org.team1540.advantagekitdemo.commands; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.button.CommandXboxController; | ||
| import org.team1540.advantagekitdemo.subsystems.elevator.Elevator; | ||
|
|
||
| public class ElevatorManualCommand extends Command { | ||
| private final Elevator elevator; | ||
| private final CommandXboxController controller; | ||
|
|
||
| public ElevatorManualCommand(Elevator elevator, CommandXboxController controller) { | ||
| this.elevator = elevator; | ||
| this.controller = controller; | ||
| addRequirements(elevator); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| elevator.setPercent(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| elevator.stop(); | ||
| } | ||
| } |
26 changes: 26 additions & 0 deletions
26
src/main/java/org/team1540/advantagekitdemo/commands/WristManualCommand.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 |
|---|---|---|
| @@ -0,0 +1,26 @@ | ||
| package org.team1540.advantagekitdemo.commands; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.button.CommandXboxController; | ||
| import org.team1540.advantagekitdemo.subsystems.intake.Intake; | ||
|
|
||
| public class WristManualCommand extends Command { | ||
| private final Intake intake; | ||
| private final CommandXboxController controller; | ||
|
|
||
| public WristManualCommand(Intake intake, CommandXboxController controller) { | ||
| this.intake = intake; | ||
| this.controller = controller; | ||
| addRequirements(intake); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| intake.setWristPercent(controller.getRightX()); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| intake.stopWrist(); | ||
| } | ||
| } |
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
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
73 changes: 73 additions & 0 deletions
73
src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.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 |
|---|---|---|
| @@ -0,0 +1,73 @@ | ||
| package org.team1540.advantagekitdemo.subsystems.intake; | ||
|
|
||
| import edu.wpi.first.math.controller.ArmFeedforward; | ||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
| import edu.wpi.first.wpilibj.RobotState; | ||
| import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; | ||
| import org.littletonrobotics.junction.Logger; | ||
| import org.team1540.advantagekitdemo.util.SuperstructureVisualizer; | ||
|
|
||
| import static org.team1540.advantagekitdemo.Constants.IntakeConstants.*; | ||
|
|
||
| public class Intake extends ProfiledPIDSubsystem { | ||
| private final IntakeIO io; | ||
| private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); | ||
|
|
||
| private final ArmFeedforward wristFeedforward = new ArmFeedforward(WRIST_KS, WRIST_KG, WRIST_KV); | ||
|
|
||
| public Intake(IntakeIO io) { | ||
| super(new ProfiledPIDController(WRIST_KP, WRIST_KI, WRIST_KD, MOTION_CONSTRAINTS)); | ||
| this.io = io; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| super.periodic(); | ||
| io.updateInputs(inputs); | ||
| Logger.processInputs("Intake", inputs); | ||
|
|
||
| SuperstructureVisualizer.setWristPosition(getWristPosition()); | ||
| } | ||
|
|
||
| public void setIntakePercent(double percentOutput) { | ||
| io.setIntakeVoltage(12 * percentOutput); | ||
| } | ||
|
|
||
| public void setWristPercent(double percentOutput) { | ||
| disable(); | ||
| setWristVoltage(12 * percentOutput); | ||
| } | ||
|
|
||
| public void setWristPosition(Rotation2d position) { | ||
| enable(); | ||
| setGoal(position.getRotations()); | ||
| } | ||
|
|
||
| private void setWristVoltage(double volts) { | ||
| io.setWristVoltage(volts); | ||
| } | ||
|
|
||
| public void stopIntake() { | ||
| setIntakePercent(0); | ||
| } | ||
|
|
||
| public void stopWrist() { | ||
| setWristPercent(0); | ||
| } | ||
|
|
||
| public Rotation2d getWristPosition() { | ||
| return inputs.wristPosition; | ||
| } | ||
|
|
||
| @Override | ||
| protected void useOutput(double output, TrapezoidProfile.State setpoint) { | ||
| setWristVoltage(output + wristFeedforward.calculate(getWristPosition().getRadians(), setpoint.velocity)); | ||
| } | ||
|
|
||
| @Override | ||
| protected double getMeasurement() { | ||
| return inputs.wristPosition.getRotations(); | ||
| } | ||
| } |
22 changes: 22 additions & 0 deletions
22
src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIO.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 |
|---|---|---|
| @@ -0,0 +1,22 @@ | ||
| package org.team1540.advantagekitdemo.subsystems.intake; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface IntakeIO { | ||
| @AutoLog | ||
| class IntakeIOInputs { | ||
| public Rotation2d wristPosition = new Rotation2d(); | ||
| public double wristAppliedVolts = 0; | ||
| public double wristCurrentAmps = 0; | ||
|
|
||
| public double intakeAppliedVolts = 0; | ||
| public double intakeCurrentAmps = 0; | ||
| } | ||
|
|
||
| default void updateInputs(IntakeIOInputs inputs) {} | ||
|
|
||
| default void setWristVoltage(double volts) {} | ||
|
|
||
| default void setIntakeVoltage(double volts) {} | ||
| } |
Oops, something went wrong.