TeleOp.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S2, HTServo,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     intakeMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     armMotor,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     motorJ,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    latchServo,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

static const int ARM_MOTOR_SPEED = 75;
static const int INTAKE_MOTOR_SPEED = 90;

static const int LATCH_BTN = 1;
static const int UNLATCH_BTN = 2;

static const int RAISE_ARM_BTN = 6;
static const int LOWER_ARM_BTN = 8;

static const int LATCH_POS = 0;
static const int UNLATCH_POS = 180;

static const int ARM_UP_ANGLE = 180;
static const int ARM_DOWN_ANGLE = 0;

static const float SCALE_FACTOR = 40.0 / 127;
void initializeRobot()
{
	nMotorEncoder[armMotor] = 0; // Resets position of armMotor encoder to zero before any commands
	motor[intakeMotor] = INTAKE_MOTOR_SPEED;
  return;
}

task main()
{
  initializeRobot();

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

  while (true) {
  	getJoystickSettings(joystick);
  	if (joy1Btn(LATCH_BTN) == 1) {
  		servo[latchServo] = LATCH_POS;
  	}
  	else if (joy1Btn(UNLATCH_BTN) == 1) {
  		servo[latchServo] = UNLATCH_POS;
  	}
  	else if (joy1Btn(RAISE_ARM_BTN) == 1) {
  		motor[intakeMotor] = 0;
  		while (nMotorEncoder[armMotor] < ARM_UP_ANGLE) {
  			motor[armMotor] = ARM_MOTOR_SPEED;
  		}
  	}
  	else if (joy1Btn(LOWER_ARM_BTN) == 1) {
  		while (nMotorEncoder[armMotor] > ARM_DOWN_ANGLE) {
  			motor[armMotor] = -ARM_MOTOR_SPEED;
  		}
  		motor[intakeMotor] = INTAKE_MOTOR_SPEED;
  	}
  	int xVal = joystick.joy1_x2
  	int yVal = joystick.joy1_y2
  	motor[leftMotor] = (xVal + yVal) * SCALE_FACTOR
  	motor[rightMotor] = (xVal - yVal) * SCALE_FACTOR
  }
}