#pragma config(Sensor, in1, lineTrackerLeft, sensorLineFollower)
#pragma config(Sensor, in2, lineTrackerCenter, sensorLineFollower)
#pragma config(Sensor, in3, lineTrackerRight, sensorLineFollower)
#pragma config(Sensor, dgtl1, Sonar, sensorSONAR_inch)
#pragma config(Sensor, dgtl3, limitSwitch, sensorTouch)
#pragma config(Motor, port2, rightMotor, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port3, leftMotor, tmotorNormal, openLoop)
#pragma config(Motor, port4, rightElbow, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port5, leftElbow, tmotorNormal, openLoop)
#pragma config(Motor, port6, wrist, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
task main()
{ while (vexRT[Btn7D]==0)
{
}
ClearTimer (T1);
int threshold;
threshold= 3013;
while (time1 [T1]<60000)
{
if (SensorValue (lineTrackerLeft) < threshold)
{
motor [leftMotor] = 0;
motor [rightMotor] = 65;
}
if (SensorValue (lineTrackerCenter) < threshold)
{
motor [leftMotor] = 65;
motor [rightMotor] =65;
}
if (SensorValue (lineTrackerRight) < threshold)
{
motor [leftMotor] = 65;
motor [rightMotor] = 0;
}
}
while (time1[T1]<60000)
{
startMotor (wrist, 60);
wait (.01);
startMotor (rightMotor, -65);
startMotor (leftMotor, -65);
wait (.5);
startMotor (rightElbow, 50);
startMotor (leftElbow, 50);
startMotor (wrist, -50);
untilButtonPress (limitSwitch);
startMotor (leftMotor, -65);
startMotor (rightMotor, 65);
wait (5);
startMotor (rightMotor, 65);
startMotor (leftMotor, 65);
untilSonarLessThan(0.0625);
startMotor (rightElbow, -65);
startMotor (leftElbow, -65);
startMotor (wrist, -35);
wait (1);
stopMotor (rightElbow);
stopMotor (leftElbow);
stopMotor (wrist);
startMotor (leftMotor, -65);
startMotor (rightMotor, 65);
wait(5);
stopMotor (leftMotor);
stopMotor (rightMotor);
wait(.01);
startMotor (rightMotor, 65);
startMotor (leftMotor, 65);
untilSonarLessThan (4);
startMotor (rightElbow, 30);
startMotor (leftElbow, 30);
startMotor (wrist, -30);
wait(.5);
stopMotor (rightElbow);
stopMotor (leftElbow);
startMotor (wrist, 50);
wait(.5);
}
while (time1[T1]>60000)
{
motor(rightMotor) = vexRT (Ch2);
motor(leftMotor) = vexRT (Ch3);
if(vexRT [Btn6D] == 1)
{
motor [rightElbow] = 65;
}
else if(vexRT [Btn6U] == 1)
{
motor [rightElbow] = -65;
}
else
{
motor [rightElbow] = 0;
}
if(vexRT [Btn5D] == 1)
{
motor [leftElbow] = 65;
}
else if(vexRT [Btn5U] == 1)
{
motor [leftElbow] = -65;
}
else
{
motor [leftElbow] = 0;
}
**** if (vexRT[btn8U] ==1)
{
motor [wrist] =60;
}
else if(vexRT [btn8D] ==1)
{
motor [wrist] =-60;
}
else
{
motor [wrist] =0;
}
}
}