TeleOp program as of 12/3/2013

Teleop.c

#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  HTMotor)
#pragma config(Sensor, S2,     SensorIR,       sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     SensorColor,    sensorCOLORFULL)
#pragma config(Sensor, S4,     SensorSonic,    sensorSONAR)
#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_C3_1,     motorF,        tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorG,        tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C4_1,     motorH,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Motor F = Right Back
//Motor G = Right Front
//Motor H = Left Back
//Motor I = Left Front

//Motor D = Flag Manipulator Spin
//Motor E = Block Manipulator Lift

//Servo 1 = Flag Manipulator Tilt

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                               DirectInput Button Notes
//
// Buttons 0=X 1=A 2=B 3=Y 4=LB 5=RB 6=LT 7=RT 8=Back 9=Start 10=LSD 11=RSD
// POV (D-Pad) -1=None 0-7=Positions (0=up, 1-7 +CW)
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

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


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////


task main()
{
  initializeRobot();

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

  bool esMotors = false; //Toggled state of Motor E-Stop mode
	bool esmHold = false; //If the Motor E-Stop button (B) is being held

  while (true)
  {
	  getJoystickSettings(joystick);
	  int btnDebug = joy1Btn(3);

	  //Emergency Stop

		if (joy1Btn(9) == 1 && joy2Btn(9) == 1) //Program will stop when E-Stop button (start) hit on both controllers
		{
			while (joy1Btn(9) == 1 && joy2Btn(9) == 1) //Wait until the buttons are released
			{
				getJoystickSettings(joystick);
			}
			while (joy1Btn(9) != 1 || joy2Btn(9) != 1) //Wait until they are pressed again
			{
				getJoystickSettings(joystick);
			}
			getJoystickSettings(joystick);
		}

		//Motor Emergency Stop

		if (joy1Btn(2) == 1 && !esmHold) //Will toggle motor e-stop when Motor E-Stop button (B) hit on controller 1
		{
			if (esMotors)
			{
				esMotors = false;
				esmHold = true;
			}
			else
			{
				esMotors = true;
				esmHold = true;
			}
		}
		else
		{
			esmHold = false;
		}

	  //Controller 1 - Motors

	  if (joy1Btn(10) == 1 || (joystick.joy1_y2 < 5 && joystick.joy1_y2 > -5) || esMotors) //Threshold & Preset Speed Button - RSD - 0
	  {
	  	motor[motorG] = 0;
	  	motor[motorF] = 0;
		}
		else
		{
			if (joy1Btn(7) == 1 && joystick.joy1_y2 > 0) //Preset Speed Button - RT - 75
		  {
		  	motor[motorG] = 75;
		  	motor[motorF] = 75;
			}
			else if (joy1Btn(5) == 1 && joystick.joy1_y2 > 0) //Preset Speed Button - RB - 30
		  {
		  	motor[motorG] = 30;
		  	motor[motorF] = 30;
			}
			else if (joy1Btn(7) == 1 && joystick.joy1_y2 < 0) //Preset Speed Button - RT - -75
		  {
		  	motor[motorG] = -75;
		  	motor[motorF] = -75;
			}
			else if (joy1Btn(5) == 1 && joystick.joy1_y2 < 0) //Preset Speed Button - RB - -30
		  {
		  	motor[motorG] = -30;
		  	motor[motorF] = -30;
			}
			else //No Presets - RS
			{
				motor[motorG] = joystick.joy1_y2;
			  motor[motorF] = joystick.joy1_y2;
			}
		}

		if (joy1Btn(11) == 1 || (joystick.joy1_y1 < 5 && joystick.joy1_y1 > -5) || esMotors) //Threshold & Preset Speed Button - LSD - 0
		{
			motor[motorH] = 0;
			motor[motorI] = 0;
		}
		else
		{
		  if (joy1Btn(6) == 1 && joystick.joy1_y1 > 0) //Preset Speed Button - LT - 75
		  {
		  	motor[motorH] = 75;
		  	motor[motorI] = 75;
			}
			else if (joy1Btn(4) == 1 && joystick.joy1_y1 > 0) //Preset Speed Button - LB - 30
		  {
		  	motor[motorH] = 30;
		  	motor[motorI] = 30;
			}
			else if (joy1Btn(6) == 1 && joystick.joy1_y1 < 0) //Preset Speed Button - LT - -75
		  {
		  	motor[motorH] = -75;
		  	motor[motorI] = -75;
			}
			else if (joy1Btn(4) == 1 && joystick.joy1_y1 < 0) //Preset Speed Button - LB - -30
		  {
		  	motor[motorH] = -30;
		  	motor[motorI] = -30;
			}
			else //No Presets - LS
			{
				motor[motorH] = joystick.joy1_y1;
			  motor[motorI] = joystick.joy1_y1;
			}
		}

		//Controller 2 - Block Manipulator & Flag Manipulator

		if (joystick.joy2_y1 < 5 && joystick.joy2_y1 > -5) //Threshold
		{
			motor[motorC] = 0;
		}
		else //Tilt Block Manipulator - LS
		{
			motor[motorC] = joystick.joy2_y1;
		}

		if (joystick.joy2_y2 < 5 && joystick.joy2_y2 > -5) //Threshold
		{
			motor[motorE] = 0;
		}
		else //Lift Block Manipulator - RS
		{
			motor[motorE] = joystick.joy2_y2;
		}
  }
}