task usercontrol()
{
// User control code here, inside the loop
int shoulderTarPos = 0;
int elbowTarPos = 0;
while (true){
if(vexRT[Btn7U]){ //ArmUp
shoulderTarPos = 4000;
elbowTarPos = 333; //measure pot for button values
}
else if(vexRT[Btn7D]) { //ArmDown
shoulderTarPos = 2000;
elbowTarPos = 100;
}
else if(vexRT[Btn7L]) { //ArmDrop
shoulderTarPos = 4000;
elbowTarPos = 0;
}
else if(vexRT[Btn7R]){ //ArmSet
shoulderTarPos = 4000;
elbowTarPos = 4000;
}
driveControl(1);
if(false){//!vexRT[Btn6DXmtr2]) {
armControl(elbowTarPos, shoulderTarPos); //this requires the potentiometers to be connected
} else {
manualArmControl();
}
}
}
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 main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
arm_fsm = 1; //arm_fsm = 0;
while (true)
{
getJoystickSettings(joystick); //Required for driver control
if(joy1Btn(9) && joy1Btn(10)) {
arm_fsm = 2;
}
driveControl();
armControl();
manipulatorControl();
flagControl();
}
}
