task one(){
driverControl();
armUserControl();
wristUserControl();
position();
}
task main()
{
displayLCDCenteredString(0, "Autonomus");
displayLCDCenteredString(1, "Recorder V1.0");
wait1Msec(2000);
clearLCD();
displayLCDCenteredString(0, "Start");
displayLCDCenteredString(1, "\/");
waitPress(centerButton);
waitRelease();
clearLCD();
displayLCDCenteredString(0, "Time Remaining");
ClearTimer(T1);
int i = -1;
while(time1[T1] < 1000*AUTONLENGTH)
{
i++;
clearLCDLine(1);
timeRemaining = AUTONLENGTH-(time1[T1]*0.001);
sprintf(display, "%1.1f%c", timeRemaining);
displayLCDCenteredString(1, display);
baseEncoderReset();
liftArmEncoderReset();
driverControl(false);
temp[tempRightSpeed] = motor[backRight]; //Right Speed
temp[tempLeftSpeed] = motor[backLeft]; //Left Speed
temp[tempRightDistance] = nMotorEncoder[backRight]; //Right Distance
temp[tempLeftDistance] = nMotorEncoder[backLeft]; //Left Distance
temp[tempHangingArmDistance] = SensorValue[liftArmEncoder]; //Lift Encoder
temp[tempHangingArmSpeed] = motor[liftTopRight]; //Lift Speed
temp[tempIntakeSpeed] = motor[intakeRight]; //Intake Speed
if(abs(temp[0] - temp[2]) < 11) //Straight
{
if(temp[0] > 0) //Forward
{
data[i][dataRightSpeed] = (temp[tempRightSpeed] + temp[tempLeftSpeed])/2; //Right Speed
data[i][dataRightDistance] = (temp[tempRightDistance] + temp[tempLeftDistance])/2; //Right Distance
data[i][dataLeftSpeed] = data[i][dataRightSpeed]; //Left Speed
data[i][dataLeftDistance] = data[i][dataRightDistance]; //Left Distance
data[i][dataMovementCode] = codeForward; //Forward Code
}
else if(temp[0] < 0) //Backward
{
data[i][dataRightSpeed] = (temp[tempRightSpeed] + temp[tempLeftSpeed])/2; //Right Speed
data[i][dataRightDistance] = (temp[tempRightDistance] + temp[tempLeftDistance])/2; //Right Distance
data[i][dataLeftSpeed] = data[i][dataRightSpeed]; //Left Speed
data[i][dataLeftDistance] = data[i][dataRightDistance]; //Left Distance
data[i][dataMovementCode] = codeBackward; //Backward Code
}
else //Not moving
{
data[i][dataRightSpeed] = 0;
data[i][dataRightDistance] = temp[tempRightDistance];
data[i][dataLeftSpeed] = 0;
data[i][dataLeftDistance] = temp[tempLeftDistance];
data[i][dataMovementCode] = codeWait; //Wait Code
}
}
else if((temp[tempRightSpeed] < temp[tempLeftSpeed] && temp[tempRightSpeed] > 0) || (temp[tempRightSpeed] > temp[tempLeftSpeed] && temp[tempRightSpeed] < 0)) //Right Turn
{
data[i][dataRightSpeed] = temp[tempRightSpeed];
data[i][dataLeftSpeed] = temp[tempLeftSpeed];
data[i][dataRightDistance] = temp[tempRightDistance];
data[i][dataLeftDistance] = temp[tempLeftDistance];
data[i][dataMovementCode] = codeRightTurn;
}
else if((temp[tempRightSpeed] > temp[tempLeftSpeed] && temp[tempRightSpeed] > 0) || (temp[tempRightSpeed] < temp[tempLeftSpeed] && temp[tempRightSpeed] < 0)) //Left Turn
{
data[i][dataRightSpeed] = temp[tempRightSpeed];
data[i][dataLeftSpeed] = temp[tempLeftSpeed];
data[i][dataRightDistance] = temp[tempRightDistance];
data[i][dataLeftDistance] = temp[tempLeftDistance];
data[i][dataMovementCode] = codeLeftTurn;
}
if(temp[tempHangingArmSpeed] != 0)
{
data[i][dataMovementCode] += codeHangingArm;
if(temp[tempHangingArmSpeed] > 0)
{
data[i][dataMovementCode] += codePlus;
}
else
{
data[i][dataMovementCode] += codeMinus;
}
}
if(tempIntakeSpeed != 0)
{
data[i][dataMovementCode] += codeIntake;
if(temp[tempIntakeSpeed] > 0)
{
data[i][dataMovementCode] += codePlus;
}
else
{
data[i][dataMovementCode] += codeMinus;
}
}
if(temp[tempBallLift] != 0)
{
data[i][dataMovementCode] += codeBallLift;
data[i][dataMovementCode] += codePlus;
}
data[i][dataHangingArmDistance] = temp[tempHangingArmDistance];
if(!firstRun)
{
if((data[i][dataMovementCode]/10) == (data[i-1][dataMovementCode]/10))
{
i--;
data[i][dataMovementCode] = data[i+1][dataMovementCode];
data[i][dataRightSpeed] = (data[i+1][dataRightSpeed] + data[i][dataRightSpeed]) / 2;
data[i][dataLeftSpeed] = (data[i+1][dataLeftSpeed] + data[i][dataLeftSpeed]) / 2;
}
i++;
data[i][dataRightDistance] += data[i+1][dataRightDistance];
data[i][dataLeftDistance] += data[i+1][dataLeftDistance];
data[i][dataHangingArmDistance] += data[i+1][dataHangingArmDistance];
}
else
{
firstRun = false;
}
}
for(int i=1;i<AUTONLENGTH*20;i++)
{
data[i][dataRightDistance] = 127;
data[i][dataMovementCode] = codeForward+1;
data[i][dataRightSpeed] = 63;
if((data[i][dataMovementCode]/10)*10 == codeForward)
{
writeDebugStreamLine("");
writeDebugStream("forward(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(");");
}
else if((data[i][dataMovementCode]/10)*10 == codeBackward)
{
writeDebugStreamLine("");
writeDebugStream("backward(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(");");
}
else if((data[i][dataMovementCode]/10)*10 == codeLeftTurn)
{
writeDebugStreamLine("");
writeDebugStream("leftTurn(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftDistance]);
writeDebugStream(");");
}
else if((data[i][dataMovementCode]/10)*10 == codeRightTurn)
{
writeDebugStreamLine("");
writeDebugStream("rightTurn(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftDistance]);
writeDebugStream(");");
}
}
}
