[MattRain]

Here is our Teleop code from last year... see if this helps. Shorter version below.

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     right,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     left,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     tower,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     none,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     claw,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Brake,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    LeftFlap,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    RightFlap,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Block,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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


void initializeRobot()
{
	servoTarget[Brake] = 65;
	servoTarget[RightFlap] = 241;
  servoTarget[LeftFlap] = 18;
  servoTarget[Block] = 237;

  return;
}

task main()
{
  initializeRobot();

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

  while (true)
  {
		getJoystickSettings(joystick);
		if(joy1Btn(7))// claw Up
   {
    motor[right] = joystick.joy1_y2/4;
    motor[left] = joystick.joy1_y1/4;
   }
   else
   {
    motor[right] = joystick.joy1_y2;
    motor[left] = joystick.joy1_y1;
   }
    motor[tower] = joystick.joy2_y1;

   //****************** DRIVER 2 (James) ***************************

   if(joy2Btn(6))// claw Up
   {
    motor[claw] = 128;
   }
   else if(joy2Btn(8))// claw Down
   {
    motor[claw] = -128;
   }
   else
   {
     motor[claw] = 0;
   }

   if(joy2Btn(2))//flinger
		{
		servoTarget[Block]= 170;
		}
		else
		{
		servoTarget[Block]= 237;
	  }

	 //****************** DRIVER 1 (Jacob) ***************************

   if(joy1Btn(4))// flag up
   {
    motor[flag] = 100;
   }
   else if(joy1Btn(3))// flag down
   {
    motor[flag] = 50;
   }
   else if(joy1Btn(2))// flag down
   {
    motor[flag] = -100;
   }
   else
   {
     motor[flag] = 0;
   }

   if(joy1Btn(5)) //Right FLAP
   {
    servoTarget[LeftFlap] = 80;
   }
   else
   {
     servoTarget[LeftFlap] = 180;
   }

   if(joy1Btn(6))// Left FLAP
   {
    servoTarget[RightFlap] = 146;
   }
   else
   {
     servoTarget[RightFlap] = 47;
   }

   //*********************** BOTH DRIVERS **************************

   if(joy1Btn(1)&joy2Btn(1))// baton mover
   {
    servoTarget[Brake] = 97;
   }
   else
   {
     servoTarget[Brake] = 65;
   }
 		}
 	}

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     right,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     left,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     tower,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     none,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     claw,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Brake,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    LeftFlap,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    RightFlap,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Block,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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


void initializeRobot()
{
	servoTarget[Brake] = 65;
	servoTarget[RightFlap] = 241;
  servoTarget[LeftFlap] = 18;
  servoTarget[Block] = 237;

  return;
}

task main()
{
  initializeRobot();

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

  while (true)

    motor[right] = joystick.joy1_y2;
    motor[left] = joystick.joy1_y1;
    motor[tower] = joystick.joy2_y1;