Programming :: "Sword" Driver Control

#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  leftLift,       sensorNone)
#pragma config(Sensor, I2C_2,  rightLift,      sensorNone)
#pragma config(Motor,  port1,           leftStrafe,    tmotorVex269, openLoop)
#pragma config(Motor,  port2,           leftDrive,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           leftBottomLift, tmotorVex393, openLoop)
#pragma config(Motor,  port4,           leftTopLift,   tmotorVex393, openLoop)
#pragma config(Motor,  port5,           leftFeed,      tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port6,           rightFeed,     tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor,  port7,           rightTopLift,  tmotorVex393, openLoop)
#pragma config(Motor,  port8,           rightBottomLift, tmotorVex393, openLoop)
#pragma config(Motor,  port9,           rightDrive,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port10,          rightStrafe,   tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/****************************************************************
        Mount San Antonio College VEX Team
****************************************************************/
/****************************************************************
Program for Robot A (15" x 15" x 15") - "Sword"

Remote Control Configuration:
- Left joystick, Y-axis will control robot's forward and backward movement
- Left joystick, X-axis will control robot's side-to-side strafe movement
- Right joystick, X-axis will control robot's clockwise and counterclockwise rotation
- Button 6 (U/D) will control feed
- Button 5 (U/D) will control arm

/****************************************************************
        Mount San Antonio College VEX Team
****************************************************************/


task main()
{
while (1 == 1)
{

//Initialize Values

int Y1 = 0, X2 = 0, X1 = 0, threshold=25;

//Arm Function

if(vexRT[Btn5U] == 1) //Lift arm
      {
    motor[leftTopLift] = 115;
    motor[leftBottomLift] = 115;
    motor[rightTopLift] = 115;
    motor[rightBottomLift] = 115;
      }
  else if(vexRT[Btn5D] == 1) //Lower arm
      {
    motor[leftTopLift] = -80;
    motor[leftBottomLift] = -80;
    motor[rightTopLift] = -80;
    motor[rightBottomLift] = -80;
      }
  else //Stop motors if no button
      {
    motor[leftTopLift] = 0;
    motor[leftBottomLift] = 0;
    motor[rightTopLift] = 0;
    motor[rightBottomLift] = 0;
      }
//Feed Fucntion

if(vexRT[Btn6U] == 1) //Intake
    {
    motor[leftFeed] = 127;
    motor[rightFeed] = 127;
    }
  else if(vexRT[Btn6D] == 1) //Score
    {
    motor[leftFeed] = -127;
    motor[rightFeed] = -127;
    }
  else //Stop motor if no button
    {
    motor[leftFeed] = 0;
    motor[rightFeed] = 0;
    }

//Drive Function

  if(abs(vexRT[Ch3]) > threshold)Y1 = vexRT[Ch3];
 else Y1 = 0;
  if(abs(vexRT[Ch1]) > threshold)X2 = vexRT[Ch1];
 else X2 = 0;
  if(abs(vexRT[Ch4]) > threshold)X1 = vexRT[Ch4];
 else X1 = 0;

motor[leftDrive] = Y1 + X2;
motor[rightDrive] = Y1 - X2;
  motor[leftStrafe] = X1;
  motor[rightStrafe] = X1;
  }
}