// Called repeatedly when this Command is scheduled to run
protected void execute() {
OurTimer time = OurTimer.getTimer("KajDrive");
shooter.setLight(false);
double rightSpeed, leftSpeed, x, y;
if (!InputConstants.competitionCode) {
if (operator.getRawButton(InputConstants.r1Button)) {
halfSpeed = !halfSpeed;
}
if (!driveTrain.locked && operator.getRawAxis(InputConstants.leftXAxis) > 0.2
|| operator.getRawAxis(InputConstants.leftXAxis) < -0.2
|| operator.getRawAxis(InputConstants.leftYAxis) > 0.2
|| operator.getRawAxis(InputConstants.leftYAxis) < -0.2
|| operator.getRawAxis(InputConstants.rightXAxis) > 0.2
|| operator.getRawAxis(InputConstants.rightXAxis) < -0.2
|| operator.getRawAxis(InputConstants.rightYAxis) > 0.2
|| operator.getRawAxis(InputConstants.rightYAxis) < -0.2) {
driveTrain.locked = true;
}
if (driveTrain.locked) {
x = operator.getRawAxis(InputConstants.rightXAxis);
y = operator.getRawAxis(InputConstants.leftYAxis);
} else {
x = driver.getRawAxis(InputConstants.rightXAxis);
y = driver.getRawAxis(InputConstants.leftYAxis);
}
if (operator.getRawButton(InputConstants.triangleButton)) {
driveTrain.locked = false;
}
x = x * x * x;
y = y * y * y;
if (halfSpeed) {
leftSpeed = (x - y) / 2;
rightSpeed = (-x - y) / 2;
} else {
leftSpeed = x - y;
rightSpeed = -x - y;
}
} else {
x = driver.getRawAxis(InputConstants.rightXAxis);
y = driver.getRawAxis(InputConstants.leftYAxis);
x = x * x * x;
y = y * y * y;
leftSpeed = x - y;
rightSpeed = -x - y;
}
// System.out.println("Operator axis: " + operator.getRawAxis(InputConstants.rightXAxis) + " " +
// operator.getRawAxis(InputConstants.leftYAxis));
// System.out.println("Driver axis: " + driver.getRawAxis(InputConstants.rightXAxis) + " " +
// driver.getRawAxis(InputConstants.leftYAxis));
System.out.println("Locked: " + driveTrain.locked);
if (driver.getRawButton(InputConstants.l1Button)) {
driveTrain.setLeftVBus(leftSpeed / 2.0);
driveTrain.setRightVBus(rightSpeed / 2.0);
} else {
driveTrain.setLeftVBus(leftSpeed);
driveTrain.setRightVBus(rightSpeed);
}
time.stop();
}
// Called just before this Command runs the first time
protected void initialize() {
shooter.setLight(false);
}