public void teleopPeriodic() {
double throttle = (driveStick.getThrottle() + 1) / -2;
double xAxis = throttle * (Math.abs(driveStick.getX()) > 0.15 ? driveStick.getX() : 0.0);
double yAxis = throttle * (Math.abs(driveStick.getY()) > 0.15 ? driveStick.getY() : 0.0);
double rotation =
throttle * (Math.abs(driveStick.getTwist()) > 0.15 ? driveStick.getTwist() : 0.0);
// Mecanum drive
drive.mecanumDrive_Cartesian(xAxis, yAxis, rotation, 0);
// Elevator control
elevator.set(driveStick.getRawButton(5) ? -1.0 : driveStick.getRawButton(3) ? 1.0 : 0.0);
// Intake control
intakeTensionMotor.set(
driveStick.getRawButton(2) ? -1.0 : driveStick.getRawButton(7) ? 1.0 : 0.0);
if (Math.abs(controlStick.getTwist()) > 0) {
double toteTurnSpeed = -controlStick.getTwist();
intakeMotors[0].set(
toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
intakeMotors[1].set(
toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
} else {
double intakeSpeed =
driveStick.getRawButton(1) ? -1.0 : driveStick.getRawButton(8) ? 0.4 : 0.0;
intakeMotors[0].set(-intakeSpeed);
intakeMotors[1].set(intakeSpeed);
}
}
public double getTwist() {
double leftVal = left.getTwist();
double rightVal = right.getTwist();
return nonZero(leftVal, rightVal);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
Robot.drivetrain.driveWithJoystick(
-pilotStick.getY(), -pilotStick.getTwist(), pilotStick.getThrottle());
}