@Override
public void robotInit() {
// We could set up Strongback using its configurator, but this is entirely optional since all
// defaults are acceptable.
// Strongback.configure().initialize();
// Set up the robot hardware ...
Motor left =
Motor.compose(Hardware.Motors.talon(LF_MOTOR_PORT), Hardware.Motors.talon(LR_MOTOR_PORT));
Motor right =
Motor.compose(Hardware.Motors.talon(RF_MOTOR_PORT), Hardware.Motors.talon(RR_MOTOR_PORT))
.invert();
drive = new TankDrive(left, right);
// Set up the human input controls for teleoperated mode. We want to use the Logitech Attack
// 3D's throttle as a
// "sensitivity" input to scale the drive speed and throttle, so we'll map it from it's native
// [-1,1] to a simple scale
// factor of [0,1] ...
FlightStick joystick = Hardware.HumanInterfaceDevices.logitechAttack3D(JOYSTICK_PORT);
ContinuousRange sensitivity = joystick.getThrottle().map(t -> (t + 1.0) / 2.0);
driveSpeed = joystick.getPitch().scale(sensitivity::read); // scaled
turnSpeed = joystick.getRoll().scale(sensitivity::read).invert(); // scaled and inverted
// Set up the command we'll use during autonomous. We'd probably use a custom CommandGroup that
// did something more
// complicated, but for this very simple example we'll just drive forward at 50% speed for 5
// seconds,
// so we can use a simple lambda ...
autonomousCommand = Command.create(5.0, () -> drive.tank(0.5, 0.5));
// Set up the data recorder to capture the left & right motor speeds (since both motors on the
// same side should
// be at the same speed, we can just use the composed motors for each) and the sensitivity. We
// have to do this
// before we start Strongback...
Strongback.dataRecorder()
.register("Left motors", left)
.register("Right motors", right)
.register("Sensitivity", sensitivity.scaleAsInt(1000));
}
private void recordCommand(Command command, CommandState state) {
eventRecorder.record(command.getClass().getName(), state.ordinal());
}
/**
* Submit to Strongback's internal scheduler a {@link Command} that runs the supplied function one
* or more times until it returns <code>false</code> or until the prescribed maximum time has
* passed, and then calls the second function.
*
* @param first the first function to be called at least one time and that should return <code>
* true</code> if it is to be called again; may not be null
* @param maxDurationInSeconds the maximum amount of time that the first function should be
* repeatedly called; must be positive
* @param second the second function to be called after the delay; may be null if not needed
*/
public static void submit(BooleanSupplier first, double maxDurationInSeconds, Runnable second) {
submit(Command.create(maxDurationInSeconds, first, second));
}
/**
* Submit to Strongback's internal scheduler a {@link Command} that runs the supplied function one
* or more times until it returns <code>false</code> or until the prescribed maximum time has
* passed, whichever comes first.
*
* @param function the function to be called at least one time and that should return <code>true
* </code> if it is to be called again; may not be null
* @param maxDurationInSeconds the maximum amount of time that the first function should be
* repeatedly called; must be positive
*/
public static void submit(BooleanSupplier function, double maxDurationInSeconds) {
submit(Command.create(maxDurationInSeconds, function));
}
/**
* Submit to Strongback's internal scheduler a {@link Command} that runs the supplied function one
* time, waits the prescribed amount of time, and then calls the second function.
*
* @param first the first function to be called; may not be null
* @param delayInSeconds the delay in seconds after the first function completes; must be positive
* @param second the second function to be called after the delay; may be null if not needed
*/
public static void submit(Runnable first, double delayInSeconds, Runnable second) {
submit(Command.create(delayInSeconds, first, second));
}
/**
* Submit to Strongback's internal scheduler a {@link Command} that runs the supplied function one
* time and completes immediately.
*
* @param executeFunction the function to be called during execution; may not be null
*/
public static void submit(Runnable executeFunction) {
submit(Command.create(executeFunction));
}