-
Notifications
You must be signed in to change notification settings - Fork 1
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Arm Brake & 2-Note Auto with Simulated Note #110
Changes from 7 commits
51c9357
e0bc12d
77647e1
0ddac3f
06736e9
2fc74f8
4ef5d0e
b639725
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,105 @@ | ||
package competition.auto_programs; | ||
|
||
import competition.subsystems.collector.commands.EjectCollectorCommand; | ||
import competition.subsystems.collector.commands.IntakeCollectorCommand; | ||
import competition.subsystems.collector.commands.StopCollectorCommand; | ||
import competition.subsystems.drive.commands.StopDriveCommand; | ||
import competition.subsystems.pose.PoseSubsystem; | ||
import competition.subsystems.shooter.ShooterWheelSubsystem; | ||
import competition.subsystems.shooter.commands.FireWhenReadyCommand; | ||
import competition.subsystems.shooter.commands.WarmUpShooterCommand; | ||
import edu.wpi.first.wpilibj2.command.Commands; | ||
import edu.wpi.first.wpilibj2.command.InstantCommand; | ||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; | ||
import xbot.common.subsystems.autonomous.AutonomousCommandSelector; | ||
import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; | ||
import xbot.common.subsystems.pose.BasePoseSubsystem; | ||
import xbot.common.trajectory.XbotSwervePoint; | ||
|
||
import javax.inject.Inject; | ||
import javax.inject.Provider; | ||
import java.util.ArrayList; | ||
|
||
public class FromMidShootCollectShoot extends SequentialCommandGroup { | ||
|
||
final AutonomousCommandSelector autoSelector; | ||
|
||
@Inject | ||
public FromMidShootCollectShoot( | ||
PoseSubsystem pose, | ||
Provider<WarmUpShooterCommand> warmUpShooterCommandProvider, | ||
Provider<SwerveSimpleTrajectoryCommand> swerveProvider, | ||
Provider<FireWhenReadyCommand> fireWhenReadyCommandProvider, | ||
IntakeCollectorCommand intake, | ||
EjectCollectorCommand eject, | ||
StopCollectorCommand stopCollector, | ||
StopDriveCommand stopDrive, | ||
AutonomousCommandSelector autoSelector) { | ||
this.autoSelector = autoSelector; | ||
// Force our location | ||
var startInFrontOfSpeaker = pose.createSetPositionCommand( | ||
() -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation)); | ||
this.addCommands(startInFrontOfSpeaker); | ||
|
||
// Shoot the pre-loaded note from the subwoofer | ||
queueMessageToAutoSelector("Shoot pre-loaded note from subwoofer"); | ||
var warmUpForFirstSubwooferShot = warmUpShooterCommandProvider.get(); | ||
warmUpForFirstSubwooferShot.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER); | ||
var fireFirstShot = fireWhenReadyCommandProvider.get(); | ||
|
||
this.addCommands(Commands.deadline(fireFirstShot, | ||
warmUpForFirstSubwooferShot)); | ||
|
||
// Drive to the middle note while collecting | ||
queueMessageToAutoSelector("Drive to middle note"); | ||
var driveToMiddleSpike = swerveProvider.get(); | ||
driveToMiddleSpike.logic.setEnableConstantVelocity(true); | ||
driveToMiddleSpike.logic.setConstantVelocity(1); | ||
driveToMiddleSpike.logic.setAimAtGoalDuringFinalLeg(true); | ||
driveToMiddleSpike.logic.setKeyPointsProvider(() -> { | ||
ArrayList<XbotSwervePoint> points = new ArrayList<>(); | ||
var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SpikeMiddle); | ||
points.add(new XbotSwervePoint(target.getTranslation(), target.getRotation(), 10)); | ||
return points; | ||
}); | ||
var stopShooter = warmUpShooterCommandProvider.get(); | ||
stopShooter.setTargetRpm(ShooterWheelSubsystem.TargetRPM.STOP); | ||
|
||
this.addCommands(Commands.deadline(driveToMiddleSpike, | ||
intake, stopShooter)); | ||
|
||
// Return to subwoofer, and also (eject for 0.1 seconds, warm up shooter) | ||
queueMessageToAutoSelector("Return to subwoofer"); | ||
var driveToSubwoofer = swerveProvider.get(); | ||
driveToSubwoofer.logic.setEnableConstantVelocity(true); | ||
driveToSubwoofer.logic.setConstantVelocity(1); | ||
driveToSubwoofer.logic.setKeyPointsProvider(() -> { | ||
ArrayList<XbotSwervePoint> points = new ArrayList<>(); | ||
var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation); | ||
points.add(new XbotSwervePoint(target.getTranslation(), target.getRotation(), 10)); | ||
return points; | ||
}); | ||
|
||
var moveNoteAwayFromShooterWheels = | ||
eject.withTimeout(0.1).andThen(stopCollector.withTimeout(0.05)); | ||
var warmUpForSecondSubwooferShot = warmUpShooterCommandProvider.get(); | ||
warmUpForSecondSubwooferShot.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER); | ||
|
||
this.addCommands(Commands.deadline(driveToSubwoofer, | ||
moveNoteAwayFromShooterWheels.andThen(warmUpForSecondSubwooferShot))); | ||
|
||
// Fire note | ||
queueMessageToAutoSelector("Fire second note"); | ||
var fireSecondShot = fireWhenReadyCommandProvider.get(); | ||
this.addCommands(Commands.deadline(fireSecondShot, | ||
stopDrive)); | ||
} | ||
|
||
private void queueMessageToAutoSelector(String message) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. ⭐ fun idea! But maybe something that should live on a base class? |
||
this.addCommands( | ||
new InstantCommand(() -> { | ||
autoSelector.setAutonomousState(message); | ||
}) | ||
); | ||
} | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -25,6 +25,8 @@ public class ArmMaintainerCommand extends BaseMaintainerCommand<Double> { | |
TimeStableValidator calibrationValidator; | ||
final double calibrationStallDurationSec = 0.5; | ||
|
||
private boolean dynamicBrakingEnabled = false; | ||
|
||
@Inject | ||
public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf, | ||
HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory, | ||
|
@@ -52,10 +54,17 @@ protected void coastAction() { | |
@Override | ||
protected void calibratedMachineControlAction() { | ||
// The arms can draw huge currents when trying to move small values, so if we are on target | ||
// then we need to kill power. In the future, we could potentially engage the brake here. | ||
// then we need to kill power. | ||
if (isMaintainerAtGoal()) { | ||
if (dynamicBrakingEnabled) { | ||
// Engage the brake so we don't backdrive away from this point | ||
// (vibrations from the shooter can cause that) | ||
arm.setBrakeEnabled(true); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. ⭐ just out of curiosity, why not manage the break inside the subsystem based on the power that's sent, how would it be different to disengage with non-zero power and engage with zero-ish power? |
||
} | ||
arm.setPower(0.0); | ||
} else { | ||
// If we need to move, disengage the brake | ||
arm.setBrakeEnabled(false); | ||
double power = positionPid.calculate(arm.getTargetValue(), arm.getCurrentValue()); | ||
arm.setPower(power); | ||
} | ||
|
@@ -68,6 +77,8 @@ protected void uncalibratedMachineControlAction() { | |
aKitLog.record("Started Calibration", startedCalibration); | ||
aKitLog.record("Given Up On Calibration", givenUpOnCalibration); | ||
|
||
arm.setBrakeEnabled(false); | ||
|
||
// Try to auto-calibrate. | ||
if (!startedCalibration) { | ||
calibrationStartTime = XTimer.getFPGATimestamp(); | ||
|
@@ -109,6 +120,15 @@ protected void uncalibratedMachineControlAction() { | |
} | ||
} | ||
|
||
@Override | ||
protected void humanControlAction() { | ||
// Disable the brake before letting the human take over. | ||
if (dynamicBrakingEnabled) { | ||
arm.setBrakeEnabled(false); | ||
} | ||
super.humanControlAction(); | ||
} | ||
|
||
@Override | ||
protected double getErrorMagnitude() { | ||
double current = arm.getCurrentValue(); | ||
|
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.
⭐ I think these longer command group constructors start getting hard to read. What do you think about logic like all this stuff for setting up a particular command ending up in a function so the top level constructor is just "do these 5 commands in this sequence"