auto program stops after driving 21 inches
void red_left3() {
intake.setVelocity(90, percent);
conveyor.setVelocity(100, percent);
// set high goal velocity
highgoal.setVelocity(20, percent);
// set max torque
leftfront.setMaxTorque(100, percent);
leftback.setMaxTorque(100, percent);
rightfront.setMaxTorque(100, percent);
rightback.setMaxTorque(100, percent);
// get high goal scoring out of the way
highgoal.spinFor(0.18, turns);
highgoal.setStopping(hold);
//lift intake
intakeair.set(true);
//lift clamp
clampair.set(true);
// Drive up to ring stack
chassis.drive_distance(20);
/*
// Check if 2 seconds have passed
if (Brain.Timer.value() <= 3.5) {
// Continue driving 7 more inches if under 2 sec
} */
chassis.drive_distance(8);
//drop intake
intakeair.set(false);
//intake blue ring
intake.spinFor(reverse, 5, turns, false);
wait(0.3, seconds);
conveyor.spinFor(reverse, 2.25, turns, false);
//turn rear end of robot towards mobile goal
chassis.turn_to_angle(-90);
// drive robot towards mobile goal
chassis.drive_distance(-20.5);
// keep driving backwards and clamp on the mobile goal at the same time
chassis.drive_distance(-3.5);
clampair.set(false);
//spin intake and conveyor until donuts fall into mobile goal
intake.spinFor(reverse, 4, turns, false);
conveyor.spinFor(reverse, 8, turns, false);
// drive forward
chassis.drive_distance(21.5);
//turn to addiotional donuts
chassis.turn_to_angle(0);
//lift intake
intakeair.set(true);
//Drive into addiotional donuts
chassis.drive_distance(13.75);
//drop intake
intakeair.set(false);
//spin intake and conveyor until donuts fall into mobile goal
intake.spinFor(reverse, 15, turns, false);
conveyor.spinFor(reverse, 30, turns, false);
//back out
chassis.drive_distance(-12);
// turn to center ladder
chassis.turn_to_angle(90);
//drive to center ladder
chassis.drive_distance(20);
//drop mobile goal
clampair.set(true);
//drive to center ladder
chassis.drive_distance(13.5);
}