teleopkabelo.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     rightMotor,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     intakeMotor,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
//*!!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_BUTTON = 1
static const int RAISE_ARM_BTN = 6
static const int LOWER_ARM_BTN = 8

void initializeRobot()
{
	nMotorEncoder[armMotor] = 0 // Resets position of armMotor encoder to zero before any commands
	motor[intakeMotor] = 90
  return;
}

task main()
{
  initializeRobot();

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

  while (true) {
  	getJoystickSettings(joystick);
  	if (joy1Btn(LATCH_BUTTON) == 1) {
  		servo[latchServo] == ...
  	}
  	else if (joy1Btn(RAISE_ARM_BUTTON) == 1) {
  		motor[intakeMotor] = 0
  		while (nMotorEncoder[armMotor] < ...) {
  			motor[armMotor] = ARM_MOTOR_SPEED
  		}
  		motor[intakeMotor] = INTAKE_MOTOR_SPEED
  	}
  	else if (joy1Btn(LOWER_ARM_BUTTON) == 1) {
  		motor[intakeMotor] = 0
  		while (nMotorEncoder[armMotor] > ...) {
  			motor[armMotor] = -ARM_MOTOR_SPEED
  		}
  		motor[intakeMotor] = INTAKE_MOTOR_SPEED
  	}

  }

}