banana.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     motorH,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

int c = 150;
int d = 200;
int sprint = 8;
int dectoggle = 0;
int decstate = 0;
int counter = 0;
bool claw1, claw2, claw3, claw4, nudge, togg1, togg2, togg3, togg4, togg5, place;

void toggle_button(int num)
{
	static TServoIndex s[4] = {servo2, servo3, servo4, servo5};
	static int state = 0;
	if (joy1Btn(num + 1) == 1) {
		state = !state;
		if (state == 0) {
			servo[s[num]] = 0;
		} else {
			servo[s[num]] = 180;
		}
	}
}
task main()
{

 //waitForStart();   // wait for start of tele-op phase

  while (1) {
  	getJoystickSettings(joystick);  // Update Buttons and Joysticks

    if (dectoggle) {
    counter += 1;
    counter = counter > 3 ? 0 : counter;
    if (!counter) {

    if (joy2Btn(5) == 1){
      c=c-2;
    }

    if (joy2Btn(8) == 1){
      d=d+1;
    }

    if (joy2Btn(6) == 1){
   		d=d-1;
   	}
    }
    } else {
    if (joy2Btn(7) == 1){
      c=c+2;
    }

    if (joy2Btn(5) == 1){
      c=c-2;
    }

    if (joy2Btn(8) == 1){
      d=d+1;
    }

    if (joy2Btn(6) == 1){
   		d=d-1;
   	}
    }

    if (joy2Btn(1) == 1){
      if (!dectoggle) {motor[motorA] = 25;} else {motor[motorA] = 10;}
    }

    if (joy2Btn(3) == 1){
      if (!dectoggle) {motor[motorA] = -25;} else {motor[motorA] = -10;}
    }

    if (joy2Btn(4) == 1){
    	if (!decstate) {
    	  dectoggle = !dectoggle;
    	  decstate = 1;
      }
    } else {
      decstate = 0;
    }

    if (joy1Btn(6) == 1){
      sprint = 2;
    }

    if (joy1Btn(6) == 0){
      sprint = 5;
    }

    if ((joy2Btn(1) == 0) && (joy2Btn(3) == 0)){
      motor[motorA] = 0;
    }

    if ((joy1Btn(7) == 0) && (joy1Btn(8) == 0)){
      motor[motorF] = 0;
    }

    if (joy1Btn(7) == 1){
      motor[motorF] = 10;
    }

    if (joy1Btn(8) == 1){
      motor[motorF] = -10;
    }

    if (joy2Btn(1) == 1) {
    	if (togg1==false){
      	place=claw1;
      	if (place==false) {claw1=true;}
      	if (place==true) {claw1=false;}
				togg1=true;
			}
    } else {togg1=false;}

    if (joy2Btn(2) == 1) {
    	if (togg2==false){
      	place=claw2;
      	if (place==false) {claw2=true;}
      	if (place==true) {claw2=false;}
      	togg2=true;
      }
    } else {togg2=false;}

    if (joy2Btn(3) == 1) {
    	if (togg3==false){
      	place=claw3;
      	if (place==false) {claw3=true;}
      	if (place==true) {claw3=false;}
      	togg3=true;
      }
    } else {togg3=false;}

    if (joy2Btn(4) == 1) {
    	if (togg4==false){
      	place=claw4;
      	if (place==false) {claw4=true;}
      	if (place==true) {claw4=false;}
      	togg4=true;
      }
    } else {togg4=false;}

    if (joy2Btn(8) == 1) {
    	if (togg5==false){
      	place=nudge;
      	if (place==false) {nudge=true;}
      	if (place==true) {nudge=false;}
      	togg5=true;
      }
    } else {togg5=false;}

    if (claw1==true) {servo[servo3]=50;} else {servo[servo3]=165;}
    if (claw2==true) {servo[servo4]=125;} else {servo[servo4]=250;}
    if (claw3==true) {servo[servo5]=0;} else {servo[servo5]=130;}
    if (claw4==true) {servo[servo6]=100;} else {servo[servo6]=225;}
    if (nudge==true) {servo[servo2]=175;} else {servo[servo2]=125;}

		c = c > 220 ? 220 : c;
		c = c < 0 ? 0 : c;

		d = d > 255 ? 255 : d;
		d = d < 0 ? 0 : d;

	  servo[servo1] = c;
	  //servo[servo2] = d;
	  getJoystickSettings(joystick);  // Update Buttons and Joysticks
	  motor[motorH] = (joystick.joy2_y2);
    motor[motorF] = (joystick.joy2_y1);
    motor[motorG] = -(joystick.joy2_y1);
    motor[motorD] = (joystick.joy1_y1/sprint);
    motor[motorE] = -(joystick.joy1_y2/sprint);

    toggle_button(0);
    toggle_button(1);
    toggle_button(2);
    toggle_button(3);

	  wait10Msec(1);
  }
}