#pragma config(Sensor, dgtl1,  Touch,          sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           armMotor,      tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*                                    - Dual Joystick Control -                                      *|
|*                                      ROBOTC on VEX 2.0 Cortex                                      *|
|*                                                                                                    *|
|*  This program uses both the Left and the Right joysticks to run the robot using "tank control".    *|
|*                                                                                                    *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)  Ch1 is the X axis and Ch2 is the Y axis for the RIGHT joystick.                             *|
|*    2)  Ch3 is the Y axis and Ch4 is the X axis for the LEFT joystick.                              *|
|*                                                                                                    *|
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]              [Name]              [Type]              [Description]                   *|
|*    Motor - Port 2          rightMotor           VEX Motor           Right motor                    *|
|*    Motor - Port 3          leftMotor            VEX Motor           Left motor                     *|
\*----------------------------------------------------------------------------------------------------*/

//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main ()
{

  while(1 == 1)
  {
    motor[leftMotor]  = vexRT[Ch3];   // Left Joystick Y value
    motor[rightMotor] = vexRT[Ch2];   // Right Joystick Y value

    if (vexRT [Btn6U] == 1)
{
motor [armMotor] = 60;
}

else
{
if (vexRT [Btn6D] == 1)
{
motor [armMotor] = -60;
}
else
{
motor [armMotor] = 0;

if (SensorValue (Touch) ==1)
{

 motor [rightMotor] = 0;
 motor [leftMotor] = 0;
 motor [armMotor] = 0;
 wait1Msec (2000);

 motor [armMotor] = 50;
 wait1Msec (250);

 motor [rightMotor] = 50;
 motor [leftMotor] = 50;
 motor [armMotor] = 0;
 wait1Msec (2500);

 motor [rightMotor] = 0;
 motor [leftMotor] = 0;
 motor [armMotor]= -60;
 wait1Msec (350);

 motor [armMotor]= 0;
 wait1Msec (3000);

 motor [armMotor] = 90;
 motor [rightMotor] = 0;
 motor [leftMotor] = 0;
 wait1Msec (700);

 motor [rightMotor] =-90;
 motor [leftMotor] =-90;
 motor [armMotor] =0;
 wait1Msec (2100);

motor [armMotor] =0;
motor [rightMotor] =0;
motor [leftMotor] =0;
wait1Msec (1970);

motor [armMotor] =0;
motor [rightMotor] =90;
motor [leftMotor] =0;
wait1Msec (1000);

motor [rightMotor] =90;
motor [leftMotor] =90;
wait1Msec (450);

motor [rightMotor] =0;
motor [leftMotor] =0;
motor [armMotor] = -70;
wait1Msec (600);

motor [armMotor] =0;
motor [rightMotor] = 0;
motor [leftMotor] = 0;
wait1Msec (3000);

motor [armMotor] = 60;
motor [rightMotor] =0;
motor [leftMotor] = 0;
wait1Msec (700);

motor [armMotor] =0;
motor [rightMotor] =-90;
motor [leftMotor] = -90;
wait1Msec (800);
 }

    }
}
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++