ArcadeProp5.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#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,     LiftR,         tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     LiftF,         tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Spinner,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     LeftR,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     RightR,        tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C2_1,    Clamp,                tServoStandard)
#pragma config(Servo,  srvo_S2_C2_2,    IRlift,               tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    SpinLift,             tServoStandard)
#pragma config(Servo,  srvo_S2_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo6,               tServoNone)
#pragma config(SrvoPosition,  Position01,            166, 128,  36, 128, 128, 128, 128, 128)
//*!!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
//

	void Liftup(int Height)  //How tall to go
				{

  			nMotorEncoder[LiftR] = 0;  //clear the TETRIX encoders in motors left and right
  			nMotorEncoder[LiftF] = 0;  //clear the TETRIX encoders in motors left and right
  	    wait1Msec(200);							// Delay before next command

 				while (nMotorEncoder[LiftF] < Height) //while the encoder wheel turns one revolution//
	        {
	        motor[LiftR] = 50;
					motor[LiftF] = 50;
	  			}

				motor[LiftR] = 0;
				motor[LiftF] = 0;
				wait1Msec(200);							// Delay before next command
			}

			void BasketUp(int Height3)
					{
					nMotorEncoder[LiftR] = 0;  		//clear the TETRIX encoders in rear motor
 					while (nMotorEncoder[LiftR] < Height3) //Lift up the back end of basket
	        {
	    		motor[LiftR] = 75;
	  			}
				  motor[LiftR] =0;
					wait1Msec(200);	//wait to drop balls in tube
				}

		void BasketDown(int Height5)
					{
					nMotorEncoder[LiftR] = 0;  		//clear the TETRIX encoders in rear motor
 					while (nMotorEncoder[LiftR] < Height5) //Lift up the back end of basket
	        {
	    		motor[LiftR] = -75;
	  			}
				  motor[LiftR] =0;
					wait1Msec(200);	//wait to drop balls in tube
				}

			void Liftdown(int Height2)
				{
				nMotorEncoder[LiftR] = 0;  //clear the TETRIX encoders in motors left and right
  			nMotorEncoder[LiftF] = 0;  //clear the TETRIX encoders in motors left and right
  	    wait1Msec(200);							// Delay before next command

				while (nMotorEncoder[LiftF] > Height2) //while the encoder wheel turns one revolution//
	        {
	    		motor[LiftR] = -75;
					motor[LiftF] = -75;
	  			}

				motor[LiftR] = 0;
				motor[LiftF] = 0;
				}

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] = 223;        // changes the position of servo1 to 160
				servoTarget[Clamp] = 223;  // changes the position of servo1 to 160
			}

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

			// spin tower out
			if (joy2Btn(1) == 1)
			{
				servo[SpinLift] = 70;
			}
			//spin tower in
			if (joy2Btn(3) == 1)
			{
				servo[SpinLift] = 23;
			}
			//activate spinner
			if (joy2Btn(9) == 1)
			{
				motor[Spinner] = 70;
			}

			else
			{
				motor[Spinner] = 0;
			}


			if (joy2Btn(7) == 1)
			{
			motor[LiftF] = -60;
			}
			else
			{
				motor[LiftF] = 0;
			}

			if (joy2Btn(8) == 1)
			{
			motor[LiftR] = -60;
			}
			else
			{
				motor[LiftR] = 0;
			}

			if (joy2Btn(5) == 1)
			{
				motor[LiftF] = -50;
			}
			else
			{
				motor[LiftF] = 0;
			}

			if (joy2Btn(6) == 1)
			{
				motor[LiftR] = -50;
			}
			else
			{
				motor[LiftR] = 0;
			}

	}
}