task main(){
initializeRobot();
//waitForStart(); //comment whole line when testing and not running FCS
while(true){
getJoystickSettings(joystick); //get current joystick state with each update
//--- SCISSOR LIFT (Controller #2)
if(joy2Btn(L1) || joy2Btn(R1)){ //slow
liftControl(20);
}else if(joy2Btn(L2) || joy2Btn(R2)){ //fast
liftControl(100);
}else{
liftControl(80); //default
}
//--- DRIVE MOTORS (Controller #1)
if(joy1Btn(L1) || joy1Btn(R1)){ //slow
driveControl(15);
}else if(joy1Btn(L2) || joy1Btn(R2)){ //fast
driveControl(100);
}else{
driveControl(50); //default
}
//--- ARM CONTROL (Controller #2)
armControl();
}
}
task usercontrol()
{
while (true){
driveControl(2); //tank = 1 arcade = 2 RC = 3
liftControl();
analogArmControl(elbow, Ch2);
digitalServoControl(claw, Btn6D, Btn6U);
}
}
