ArcadeProp (2).c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S3,     IR,             sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Motor,  mtr_S1_C1_1,     LeftF,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightF,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     LeftR,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     RightR,        tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Clamp,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    IRlift,               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"
#define DEADBAND(n, t)	((abs(n) > t)? (n): 0)
#define BOUND(n, lo, hi)	(((n) < (lo))? lo: ((n) > (hi))? (hi): (n))

//Arcade Joystick;
//void ArcadeControl(int move, int turn)  //Setup Left Joystick on Contoller 2 for forward/backward movement.  Right Joystick for left/right
//{
//	int left = move - turn;
//	int right = move + turn;

//	motor[LeftF] = left;
//	motor[RightF] = right;
//}
int deadZone = 10;
//
// Main task - program entry point
//
task main()
{

 	while (true)
  {
  	getJoystickSettings(joystick);

//  	 	ArcadeControl((joystick.joy1_y1 * 100/128), joystick.joy1_x2);  //y1 is forward/backward.  x2 is left/right.
		int drivePower = DEADBAND(joystick.joy1_y1, deadZone)*100/128;
		int turnPower = DEADBAND(joystick.joy1_x2, deadZone)*100/128;

		motor[LeftF] = BOUND(drivePower - turnPower, -78, 100);
		motor[RightF] = BOUND(drivePower + turnPower, -78, 100);
		motor[LeftR] = BOUND(-drivePower - turnPower, -78, 100);
		motor[RightR] = BOUND(-drivePower + turnPower, -78, 100);

		wait1Msec(10);


				if (joy2Btn(4) == 1)
			// turn servo to position 210 Opened
			{
				servo[Clamp] = 210;        // changes the position of servo1 to 160
				servoTarget[Clamp] = 210;  // changes the position of servo1 to 160
			}
		else if (joy2Btn(2) == 1)
			// The servo to position 255 Cloesed
			{
				servo[Clamp] = 255;        // changes the position of servo1 to 160
				servoTarget[Clamp] = 255;  // changes the position of servo1 to 160
			}

			// Close Servo IR Lift
			if (joy2Btn(10) == 1)
			{
				servo[IRlift] = 250;        // changes the position of servo1 to 160
				servoTarget[IRlift] = 250;  // changes the position of servo1 to 160
			}

			// slow speed to grab towers
			if (joy1Btn(1) == 1)
			{
				motor[LeftR] = 40;
				motor[RightR] = 40;
				motor[LeftF] = -40;
				motor[RightF] = -40;
			}
  }
}