task main(){
// Initialize variables here //
int myInt=0;
int in=0;
int out=0;
//Debug2File(); //Send the debug information to the file debug.txt
//Debug2NXT(); //Send the debug information to the NXT screen
Debug2Stream(); //Send the debug information to the PC Screen
// End of initialize //
while(true){
clearTimer(T1);
hogCPU(); //Prevent other tasks from running when this one is.
// ------------- Put Unit Test code here -------------------//
// xxxxxxx [] Describe test 1 here. Put an X inside of [] when the test passes.
// xxxxxxx [] Describe test 2 here. Put an X inside of [] when the test passes.
// USAGE NOTES:
// The units for a are encoder clicks
// Set #define NO_UNIT_TEST
if (i<5) in=0;
if (i<10) in=1;
out=MySoftwareModule(in);
DebugInt("In",in);
DebugInt("Out",out);
i+=1; // Increment the frame counter for unit test
// ------------- Unit code test is done here ---------------//
DebugPrint();
timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
releaseCPU(); // Let other tasks run now.
wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
}// While Loop
}// Main Task
void OEditor::UpdateCursor() {
int i;
int nls; // newlines
int l;
// find the number of newlines
nls = 0;
for (i=bufpos; i<=CursorPos; i++) {
//if (i>0)
if (getChar(i) == '\n') {
nls++;
}
}
cury = nls;
l = buf.lastIndexOf('\n', CursorPos); // search backwards
DebugInt(l);
if (l==-1) {
curx = CursorPos; // we are on first row
} else {
//curx = CursorPos - buf.lastIndexOf('\n', CursorPos) ;
curx = CursorPos - l;
}
}
task Foreground(){
//servoChangeRate[servo2] = 2;
int timeLeft;
int state=0;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtWheelMotor] + nMotorEncoder[ltWheelMotor];
long robotDir = nMotorEncoder[ltWheelMotor] - nMotorEncoder[rtWheelMotor];
long distInches = robotDist/IN2CLK;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(speedCmd)<3 && abs(dirCmd)<3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>48)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 1 || SensorValue[IR] == 2);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>55)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>74)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 3)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
countState++;
if(countState == 3)
state=8;
}
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
if(pathIdx == 3)
{
state=9;
}
}
}
if (state==9)
{
pathIdx = 3;
}
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("dist",distInches);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
motor[ltWheelMotor]=speedCmd+dirCmd;
motor[rtWheelMotor]=speedCmd-dirCmd;
motor[blockthrower]=armSpd;
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task main(){
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtMotor, (tMotor)ltMotor);
DirectionReset();
nMotorEncoder[blockthrower] = 0;
speedCmdZ1=0;
pathIdx=0;
delayatruecount=0;
int state=0;
Pid_Init1();
Pid_Init2();
//--------------------End INIT Code--------------------------//
waitForStart(); // Wait for the beginning of autonomous phase.
int iFrameCnt=0;
int timeLeft;
servo[irArm]=243;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/27;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd = Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// Calculate the IR value
IRval = Delayatrue(1, SensorValue[IR] == 5 || SensorValue[IR] == 6);
if (state==0)// State O Follow Path
{
if (distInches>28)
{
state=1;
}
}
if(state==1)// State 1 Swing out irArm
{
servo[irArm]=150;
if (distInches>34)
{
state=2;
}
}
if (state==2)// state 2 look for ir under box 1
{
if ( IRval||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=12;
servo[irArm]=243;
}
else
{
state=3;
}
}
if (state==12)//follows path before flipping arm
{
//speedCmd=0;
if(distInches>44)
{
state = 8;
}
}
if (state==3)//state 3 look for box 2 and follow path
{
if (distInches>46)
{
state=4;
}
}
if (state==4)//state 4 look for ir under box 2
{
if ( IRval==true||SensorValue[IR] == 3)
{
state=13;
servo[irArm]=243;
}
else
{
state=15;
}
servo[irArm]=243;
}
if (state==13) // waits for distance before flipping
{
if(distInches>57)
{
state = 8;
}
}
if (state==15) // pulls servo arm out
{
if(distInches>57)
{
servo[irArm] = 80;
state =5;
}
}
if (state==5)//state 5 look for box 3 and follow path
{
if (distInches>66)//36
{
state=6;
}
}
if (state==6)// State 6 Look for ir under box 3
{
if ( IRval||SensorValue[IR] == 4||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=14;
}
else
{
state=7;
}
servo[irArm]=243;
}
if (state==14)// waits distance before flipping arm
{
if(distInches>69)
state = 8;
}
if (state==7)// State 7 look for box 4
{
if (pathIdx == 3)//45
{
state=8;
}
}
if (state==8)// state 8 flip arm
{
servo[irArm]=243;
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
state=9;
}
}
if (state==9)//state 9 lower arm
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos - armEncoder) < 400)
{
pathIdx=3;
state=10;
}
}
if(state==10)//state 10 follow path
{
}
//DebugInt("spd",speedCmd);
//DebugInt("dir",robotDir/DEG2CLK);
//DebugInt("dist",distInches);
//DebugInt("path",pathIdx);
//DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
if (pathIdx>s) pathIdx=s; // Protect the path index
// Ramp the command up
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1);
leftmotors=Pid1(speedCmd+dirCmd);
rightmotors=Pid2(speedCmd-dirCmd);
//rightmotors=speedCmd-dirCmd;
//leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
//DebugInt("rightmotors",rightmotors);
//DebugInt("leftmotors",leftmotors);
//DebugInt("rightencoder",nMotorEncoder[rtMotor]);
//DebugInt("leftencoder",nMotorEncoder[ltMotor]);
if (iFrameCnt==0)
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
iFrameCnt++;
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task Foreground() {
//servoChangeRate[servo2] = 2;
Pid_Init1();
Pid_Init2();
int timeLeft;
int state=0;
int speedCmd = 0;
int dirCmd = 0;
servo[irArm]=105;
const tMUXSensor LEGOUS = msensor_S4_3;
while(true) {
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/DEG2CLK;
// Calculate the speed and direction commands
if(shortPathTrue == false)
{
speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
}
else
{
speedCmd = ForwardDist(shortPath[pathIdx][DIST_IDX], robotDist, shortPath[pathIdx][SPD_IDX]);
dirCmd=Direction(shortPath[pathIdx][DIR_IDX], robotDir);
}
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
bool SonarVal;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>18)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 4 || SensorValue[IR] == 3);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>27)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>46)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 1)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 1900;
if (abs(armSetPos - armEncoder) <20)
{
state=8;
}
servo[irArm]=243;
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
state=9;
}
}
SonarVal = Delayatrue2(1, USreadDist(LEGOUS) > 40);
if (state==9)
{
if ((distInches>90) && (distInches<115))
{
if(SonarVal)
{
state=10;
}
else
{
state=11;
}
}
}
if(state==10)
{
shortPathTrue=true;
}
if(state==11)
{
}
/*
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("sonarval",SonarVal);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("dist",distInches);
DebugInt("ir", SensorValue[IR]);
*/
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
rightmotors=speedCmd-dirCmd;
leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
DebugInt("rightmotors",rightmotors);
DebugInt("leftmotors",leftmotors);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task Foreground(){
int timeLeft;
// int iFrameCnt=0;
// int out,in=0;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------UNIT TEST Code---------------------------//
int spdCmdPct=40; // Percent of full motor speed command
// Calculate robot and motor information (speed, distance, direction)
mrtLtClk = nMotorEncoder[ltWheelMotor];
mtrRtClk = nMotorEncoder[rtWheelMotor];
mtrLtSpdClk = mrtLtClk - mrtLtClkOld;mrtLtClkOld = mrtLtClk;
mtrRtSpdClk = mtrRtClk - mtrRtClkOld;mtrRtClkOld = mtrRtClk;
DebugInt(" sLt",mtrLtSpdClk);
DebugInt(" sRt",mtrRtSpdClk);
rbtDistClk = mtrRtClk + mrtLtClk;
rbtDirClk = mrtLtClk - mtrRtClk;
// ------------- The path state machine --------------------//
switch (sm) {
case FWDa: //Drive Forward
dirCmdClk=DIR0;
spdCmdPct=40;
if (rbtDistClk>DST12) sm=TURNa;
break;
case TURNa: //Drive Forward
dirCmdClk=DIR90;
spdCmdPct=40;
bool fall;
fall=FallEdge(abs(dirCmdPct)>5,fallOld);
if (fall) {
sm=FWDb;
tmpDist=rbtDistClk;
}
break;
case FWDb: //Drive Forward
dirCmdClk=DIR90;
spdCmdPct=40;
if (rbtDistClk>tmpDist+DST12) sm=TURNb;
break;
case TURNb: //Drive Forward
dirCmdClk=DIRm90;
spdCmdPct=0;
fall=FallEdge(abs(dirCmdPct)>5,fallOld);
DebugInt(" fall",(int)fall);
if (fall) {
sm=STOP;
tmpDist=rbtDistClk;
}
break;
case STOP:
spdCmdPct=0;
break;
default:
spdCmdPct=40;
}
// Calculate the direction command
DebugInt(" rbtDirC",rbtDirClk);
dirCmdPct = LimitSym((dirCmdClk-rbtDirClk)/DIR_KP, TURN_SPEED);
// Calculate the motor commands
int mtrCmdLtPct;
int mtrCmdRtPct;
mtrCmdLtPct=spdCmdPct+dirCmdPct;
mtrCmdRtPct=spdCmdPct-dirCmdPct;
// Limit the rate of change of the motor commands to prevent slipping.
// DebugInt(" mlc1",mtrCmdLtPct);
// DebugInt(" mrc1",mtrCmdRtPct);
mtrCmdLtPct=RateLimit( mtrCmdLtPct, 4, mrtLtRlPct);
mtrCmdRtPct=RateLimit( mtrCmdRtPct, 4, mtrRtRlPct );
/*
// Protect against stalled motors
switch (sm2) {
case MTR_OK: //Drive Forward
#define MTR_TRP 45
if ((abs(mtrLtSpdClk)<MTR_TRP && abs(mtrCmdLtPct)>30)||(abs(mtrRtSpdClk)<MTR_TRP && abs(mtrCmdRtPct)>30)){
mtrBadTmr+=1;
if (mtrBadTmr>7) sm2=MTR_BAD;
} else {
mtrBadTmr=0;
}
break;
case MTR_BAD: //Drive Forward
mtrCmdLtPct=0;
mtrCmdRtPct=0;
break;
default:
}
*/
// Power the drive motors
motor[ltWheelMotor]=mtrCmdLtPct;
motor[rtWheelMotor]=mtrCmdRtPct;
// DebugInt(" mlc2",mtrCmdLtPct);
// DebugInt(" mrc2",mtrCmdRtPct);
// DebugInt(" SM", sm);
DebugInt(" tmr", mtrBadTmr);
DebugInt(" SM2", sm2);
//--------------------------End UNIT TEST Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
if (timeLeft<0) timeLeft=0;
DebugInt(" time", timeLeft);
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
