task main() {
float r0 = getIRDir(sensorIR)-8, r1;
if(r0 > 0) rbtArcRight(-7); else rbtArcLeft(20);
rbtMoveFdDist(-10, 5000);
ClearTimer(T1); while(time1[T1] < 2000) {
r1 = getIRDir(sensorIR)-8; int acS[5]; HTIRS2readAllACStrength(sensorIR, acS[0], acS[1], acS[2], acS[3], acS[4]);
if(r0 > 0) {setLeftMotors(acS[4] > acS[3] ? -6 : -50); setRightMotors(acS[4] > acS[3] ? -50 : -8);}
else {setLeftMotors(acS[4] > acS[3] ? -6 : -90); setRightMotors(acS[4] > acS[3] ? -30 : 0);}
} setLeftMotors(0); setRightMotors(0); int cr = (r0 > 0 ? (r1 > 0 ? 1 : 2) : (r1 > 0 ? 3 : 4));
nxtDisplayBigTextLine(3, "%f", cr); for(;;);
}
void goBackwardFor_time(int time){ //Goes Backwards for a set ammount of time
setLeftMotors(-autoSpeed);
setRightMotors(-autoSpeed);
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();}
stopLeftMotors();
stopRightMotors();
}
void AnesthesiologistDrive::drive()
{
leftCmd = linearVelocity + turnSpeed;
rightCmd = linearVelocity - turnSpeed;
setLeftMotors(leftCmd);
setRightMotors(rightCmd);
}
void manualControl(void){ //For manual control
if (abs(vexRT(Ch2)) > 15 || abs(vexRT(Ch3)) > 15){ //Only move if we pop over 15 units on the controls
//Run the motors at the joystick input
setLeftMotors(vexRT(Ch2));
setRightMotors(vexRT(Ch3));
} else {
//Kill the motors
stopLeftMotors();
stopRightMotors();
}
if (vexRT(Btn8U)){
setMotor(vertBelt,- 0);
}
// Starts and stops verticle belt
if (vexRT(Btn5D)||vexRT(Btn6D)){
if (vexRT(Btn5D)){setMotor(vertBelt,-100);} else {setMotor(vertBelt,100);}
}
// Starts and stops verticle belt
if (vexRT(Btn5U)||vexRT(Btn6U)){
if (vexRT(Btn5U)){setMotor(hozBelt,-100);} else {setMotor(hozBelt,100);}
}
//Kill the belts
if (vexRT(Btn8D)){stopMotor(hozBelt);stopMotor(vertBelt);}
//Start Auto
if (vexRT(Btn8L) && vexRT(Btn8R)){roboControl();}
//Quick start, all on
if (vexRT(Btn7L)){
setMotor(hozBelt,100);
setMotor(vertBelt,100);
launcherSpeed_new = 127;
}
/* Launcher speeds */
if (vexRT(Btn7D)){ stopLauncher();} //Emergency launcher Stop
//if (vexRT(Btn7D)){ safeLauncherStop();} //Safe launcher Stop
if (vexRT(Btn7R)){launcherSpeed_new = 75;}//Low speed mode
if (vexRT(Btn7U)){ launcherSpeed_new = 127;} //Sets launcher speed to 127 (max)
updateLauncherSpeed();
}
void manualControl(void){ //For manual control
if (abs(vexRT(Ch2)) > 15 || abs(vexRT(Ch3)) > 15){ //Only move if we pop over 15 units on the controls
//Run the motors at the joystick input
setLeftMotors(vexRT(Ch2));
setRightMotors(vexRT(Ch3));
} else {
//Kill the motors
stopLeftMotors();
stopRightMotors();
}
// Starts and stops verticle belt
if (vexRT(Btn5D)||vexRT(Btn6D)){
if (vexRT(Btn5D)){setMotor(vertBelt,-100);} else {setMotor(vertBelt,100);}
}
// Starts and stops verticle belt
if (vexRT(Btn5U)||vexRT(Btn6U)){
if (vexRT(Btn5U)){setMotor(hozBelt,-100);} else {setMotor(hozBelt,100);}
}
//Kill the belts
if (vexRT(Btn8D)){stopMotor(hozBelt);stopMotor(vertBelt);}
// Starts and stops the horizontal belt
/*
if (vexRT(Btn8L)){hozBeltBtn_beenPressed = true;} //We have pressed it
if (!vexRT(Btn8L) && hozBeltBtn_beenPressed){ //When we let go lets flip
if (!hozBelt_on){hozBelt_on = true; setMotor(hozBelt,100);}else{hozBelt_on = false;stopMotor(hozBelt);}
hozBeltBtn_beenPressed = false;
}
*/
//Quick start, all on
if (vexRT(Btn7L)){
setMotor(hozBelt,100);
setMotor(vertBelt,100);
launcherSpeed_new = 127;
}
/* Launcher speeds */
if (vexRT(Btn7D)){ stopLauncher();} //Emergency launcher Stop
//if (vexRT(Btn7D)){ safeLauncherStop();} //Safe launcher Stop
if (vexRT(Btn7R)){launcherSpeedHop();}//Pop up 15
if (vexRT(Btn7U)){ launcherSpeed_new = 127;} //Sets launcher speed to 127 (max)
updateLauncherSpeed();
}
task main() {
waitForStart();
setLeftMotors(-60);
setRightMotors(-60);
wait1Msec(3200);
}
