 | Code: #pragma config(Sensor, S1, MoveStat, sensorCOLORFULL) #pragma config(Sensor, S2, TrnStatLft, sensorLightInactive) #pragma config(Sensor, S3, TrnStatRht, sensorLightInactive) #pragma config(Sensor, S4, LineLead, sensorI2CMindsensorsLineFollow) #pragma config(Motor, motorB, MotorB, tmotorNormal, openLoop, encoder)//Right motor. #pragma config(Motor, motorC, MotorC, tmotorNormal, openLoop, encoder)//Left motor.
#include "drivers/MSLL-Driver.h";
bool NextTry=false;
bool TurnDone=false;
bool FollowLine=true;
int Optimized=0;
int MoveCount=0;
int Result;
int Params[4]={16,0,2,100};
char MazeSolution[150];//Large # of moves just in case.
string LineColor;
task FollowTheLine() { int SpeedB; int SpeedC; bMotorReflected[MotorB]=true; bMotorReflected[MotorC]=true; byte steering = 0;
motor[MotorB]=50; motor[MotorC]=50;
while (FollowLine==true) { steering = LLreadSteering(LineLead);
SpeedB = (Params[3] + steering*2); SpeedC = (Params[3] - steering*2);
SpeedB = clip(SpeedB, -100, 100); SpeedC = clip(SpeedC, -100, 100);
motor[motorB] = (byte)SpeedB; motor[motorC] = (byte)SpeedC; } motor[motorB] = 0; motor[motorC] = 0; } task main() { LLinit(LineLead); LLsetKp(LineLead, Params[0], 32); LLsetKi(LineLead, Params[1], 32); LLsetKd(LineLead, Params[2], 32);
SensorType[MoveStat]=sensorCOLORNONE;
nxtDisplayCenteredBigTextLine(2, "%s", "Calbrate"); nxtDisplayCenteredBigTextLine(4, "%s", "white.");
wait1Msec(500);
while(nNxtButtonPressed!=3);
wait1Msec(500); LLcalWhite(LineLead);
eraseDisplay();
nxtDisplayCenteredBigTextLine(2, "%s", "Calbrate"); nxtDisplayCenteredBigTextLine(4, "%s", "black.");
while(nNxtButtonPressed!=3); wait1Msec(500); LLcalBlack(LineLead);
eraseDisplay();
nxtDisplayCenteredTextLine(1, "%s", "If white line"); nxtDisplayCenteredTextLine(2, "%s", "left Button."); nxtDisplayCenteredTextLine(5, "%s", "If black line"); nxtDisplayCenteredTextLine(6, "%s", "right Button.");
wait1Msec(500);
while(nNxtButtonPressed==-1);
eraseDisplay();
if(nNxtButtonPressed==2)//White line. { LineColor="White"; eraseDisplay(); nxtDisplayCenteredBigTextLine(2, "%s", "White"); nxtDisplayCenteredBigTextLine(4, "%s", "line."); DeadEnd(); while(nNxtButtonPressed!=3); TurnLeft(); while(nNxtButtonPressed!=3); TurnRight(); } else if(nNxtButtonPressed==1)//Black line. { LineColor="Black"; eraseDisplay(); nxtDisplayCenteredBigTextLine(2, "%s", "Black"); nxtDisplayCenteredBigTextLine(4, "%s", "line."); DeadEnd(); while(nNxtButtonPressed!=3); TurnLeft(); while(nNxtButtonPressed!=3); TurnRight(); } }
int OptimizePath(void) { for(int OptimizeCount=1; OptimizeCount<=MoveCount; OptimizeCount++) { if(MazeSolution[OptimizeCount]=='B') { if(MazeSolution[OptimizeCount-1]=='L' && MazeSolution[OptimizeCount+1]=='R') { MazeSolution[OptimizeCount-1]='B'; while(OptimizeCount+1<=MoveCount) { MazeSolution[OptimizeCount]=MazeSolution[OptimizeCount+2]; OptimizeCount++; } MoveCount=MoveCount-2; OptimizeCount=1; } else if(MazeSolution[OptimizeCount-1]=='L' && MazeSolution[OptimizeCount+1]=='S') { MazeSolution[OptimizeCount-1]='R'; while(OptimizeCount+1<=MoveCount) { MazeSolution[OptimizeCount]=MazeSolution[OptimizeCount+2]; OptimizeCount++; } MoveCount=MoveCount-2; OptimizeCount=1; } else if(MazeSolution[OptimizeCount-1]=='R' && MazeSolution[OptimizeCount+1]=='L') { MazeSolution[OptimizeCount-1]='B'; while(OptimizeCount+1<=MoveCount) { MazeSolution[OptimizeCount]=MazeSolution[OptimizeCount+2]; OptimizeCount++; } MoveCount=MoveCount-2; OptimizeCount=1; } else if(MazeSolution[OptimizeCount-1]=='S' && MazeSolution[OptimizeCount+1]=='L') { MazeSolution[OptimizeCount-1]='R'; while(OptimizeCount+1<=MoveCount) { MazeSolution[OptimizeCount]=MazeSolution[OptimizeCount+2]; OptimizeCount++; } MoveCount=MoveCount-2; OptimizeCount=1; } else if(MazeSolution[OptimizeCount-1]=='S' && MazeSolution[OptimizeCount+1]=='S') { MazeSolution[OptimizeCount-1]='B'; while(OptimizeCount+1<=MoveCount) { MazeSolution[OptimizeCount]=MazeSolution[OptimizeCount+2]; OptimizeCount++; } MoveCount=MoveCount-2; OptimizeCount=1; } else if(MazeSolution[OptimizeCount-1]=='L' && MazeSolution[OptimizeCount+1]=='L') { MazeSolution[OptimizeCount-1]='S'; while(OptimizeCount+1<=MoveCount) { MazeSolution[OptimizeCount]=MazeSolution[OptimizeCount+2]; OptimizeCount++; } MoveCount=MoveCount-2; OptimizeCount=1; } } } Optimized=1; return Optimized; } void TurnRight() { SensorType[TrnStatLft]=sensorLightActive; SensorType[MoveStat]=sensorCOLORBLUE;
bMotorReflected[MotorB]=true; bMotorReflected[MotorC]=true;
nMotorEncoder[MotorB]=0; nMotorEncoder[MotorC]=0; nMotorEncoderTarget[motorB]=60; nMotorEncoderTarget[motorC]=60;
motor[MotorB]=Params[3]; motor[MotorC]=Params[3];
while(nMotorRunState[MotorB]!=runStateIdle && nMotorRunState[MotorC]!=runStateIdle);
bMotorReflected[MotorB]=true; bMotorReflected[MotorC]=false;
motor[MotorB]=Params[3]; motor[MotorC]=Params[3];
if(LineColor=="Black") { Result=LLreadResult(LineLead); while(Result!=0) { Result=LLreadResult(LineLead); } while(Result==0) { Result=LLreadResult(LineLead); } while(TurnDone==false) { Result=LLreadResult(LineLead);
switch(Result) { case 1: motor[MotorB]=35; motor[MotorC]=35; break; case 3: motor[MotorB]=30; motor[MotorC]=30; break; case 7: motor[MotorB]=25; motor[MotorC]=25; break; case 15: motor[MotorB]=20; motor[MotorC]=20; break; case 14: motor[MotorB]=15; motor[MotorC]=15; break; case 30: motor[MotorB]=10; motor[MotorC]=10; break; case 28: motor[MotorB]=5; motor[MotorC]=5; TurnDone=true; break; case 60: motor[MotorB]=0; motor[MotorC]=0; TurnDone=true; break; } } TurnDone=false; } else if(LineColor=="White") { Result=LLreadResult(LineLead); while(Result!=255) { Result=LLreadResult(LineLead); } while(Result==255) { Result=LLreadResult(LineLead); } while(TurnDone==false) { Result=LLreadResult(LineLead);
switch(Result) { case 254: motor[MotorB]=35; motor[MotorC]=35; break; case 252: motor[MotorB]=30; motor[MotorC]=30; break; case 248: motor[MotorB]=25; motor[MotorC]=25; break; case 240: motor[MotorB]=20; motor[MotorC]=20; break; case 241: motor[MotorB]=15; motor[MotorC]=15; break; case 225: motor[MotorB]=10; motor[MotorC]=10; break; case 227: motor[MotorB]=5; motor[MotorC]=5; TurnDone=true; break; case 195: motor[MotorB]=0; motor[MotorC]=0; TurnDone=true; break; } } TurnDone=false; } SensorType[TrnStatLft]=sensorLightInactive; SensorType[MoveStat]=sensorCOLORNONE; } void TurnLeft() { SensorType[TrnStatRht]=sensorLightActive; SensorType[MoveStat]=sensorCOLORBLUE;
bMotorReflected[MotorB]=true; bMotorReflected[MotorC]=true;
nMotorEncoder[MotorB]=0; nMotorEncoder[MotorC]=0; nMotorEncoderTarget[motorB]=60; nMotorEncoderTarget[motorC]=60;
motor[MotorB]=Params[3]; motor[MotorC]=Params[3];
while(nMotorRunState[MotorB]!=runStateIdle && nMotorRunState[MotorC]!=runStateIdle);
bMotorReflected[MotorB]=false; bMotorReflected[MotorC]=true;
motor[MotorB]=Params[3]; motor[MotorC]=Params[3];
if(LineColor=="Black") { Result=LLreadResult(LineLead); while(Result!=0) { Result=LLreadResult(LineLead); } while(Result==0) { Result=LLreadResult(LineLead); } while(TurnDone==false) { Result=LLreadResult(LineLead);
switch(Result) { case 128: motor[MotorB]=35; motor[MotorC]=35; break; case 192: motor[MotorB]=30; motor[MotorC]=30; break; case 224: motor[MotorB]=25; motor[MotorC]=25; break; case 240: motor[MotorB]=20; motor[MotorC]=20; break; case 112: motor[MotorB]=15; motor[MotorC]=15; break; case 120: motor[MotorB]=10; motor[MotorC]=10; break; case 56: motor[MotorB]=5; motor[MotorC]=5; TurnDone=true; break; case 60: motor[MotorB]=0; motor[MotorC]=0; TurnDone=true; break; } } TurnDone=false; } else if(LineColor=="White") { Result=LLreadResult(LineLead); while(Result!=255) { Result=LLreadResult(LineLead); } while(Result==255) { Result=LLreadResult(LineLead); } while(TurnDone==false) { Result=LLreadResult(LineLead);
switch(Result) { case 127: motor[MotorB]=35; motor[MotorC]=35; break; case 63: motor[MotorB]=30; motor[MotorC]=30; break; case 31: motor[MotorB]=25; motor[MotorC]=25; break; case 15: motor[MotorB]=20; motor[MotorC]=20; break; case 143: motor[MotorB]=15; motor[MotorC]=15; break; case 135: motor[MotorB]=10; motor[MotorC]=10; break; case 199: motor[MotorB]=5; motor[MotorC]=5; TurnDone=true; break; case 195: motor[MotorB]=0; motor[MotorC]=0; TurnDone=true; break; } } TurnDone=false; } SensorType[TrnStatRht]=sensorLightInactive; SensorType[MoveStat]=sensorCOLORNONE; } void DeadEnd() { SensorType[MoveStat]=sensorCOLORRED;
bMotorReflected[MotorB]=true; bMotorReflected[MotorC]=false;
motor[MotorB]=Params[3]; motor[MotorC]=Params[3];
wait1Msec(200);
motor[MotorB]=Params[3]; motor[MotorC]=Params[3];
if(LineColor=="Black") { Result=LLreadResult(LineLead); while(Result!=0) { Result=LLreadResult(LineLead); } while(Result==0) { Result=LLreadResult(LineLead); } while(TurnDone==false) { Result=LLreadResult(LineLead);
switch(Result) { case 128: motor[MotorB]=35; motor[MotorC]=35; break; case 192: motor[MotorB]=30; motor[MotorC]=30; break; case 224: motor[MotorB]=25; motor[MotorC]=25; break; case 240: motor[MotorB]=20; motor[MotorC]=20; break; case 112: motor[MotorB]=15; motor[MotorC]=15; break; case 120: motor[MotorB]=10; motor[MotorC]=10; break; case 56: motor[MotorB]=5; motor[MotorC]=5; TurnDone=true; break; case 60: motor[MotorB]=0; motor[MotorC]=0; TurnDone=true; break; } } TurnDone=false; } else if(LineColor=="White") { Result=LLreadResult(LineLead); while(Result!=255) { Result=LLreadResult(LineLead); } while(Result==255) { Result=LLreadResult(LineLead); } while(TurnDone==false) { Result=LLreadResult(LineLead);
switch(Result) { case 127: motor[MotorB]=35; motor[MotorC]=35; break; case 63: motor[MotorB]=30; motor[MotorC]=30; break; case 31: motor[MotorB]=25; motor[MotorC]=25; break; case 15: motor[MotorB]=20; motor[MotorC]=20; break; case 143: motor[MotorB]=15; motor[MotorC]=15; break; case 135: motor[MotorB]=10; motor[MotorC]=10; break; case 199: motor[MotorB]=5; motor[MotorC]=5; TurnDone=true; break; case 195: motor[MotorB]=0; motor[MotorC]=0; TurnDone=true; break; } } TurnDone=false; } SensorType[TrnStatRht]=sensorLightInactive; SensorType[MoveStat]=sensorCOLORNONE; } void MazeFinish() { motor[MotorB]=0; motor[MotorC]=0;
PlayTone(695, 14); while(bSoundActive); PlayTone(695, 14); while(bSoundActive); PlayTone(695, 14); while(bSoundActive); PlayTone(929, 83); while(bSoundActive); PlayTone(1401, 83); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1188, 14); while(bSoundActive); PlayTone(1054, 14); while(bSoundActive); PlayTone(1841, 83); while(bSoundActive); PlayTone(1401, 41); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1188, 14); while(bSoundActive); PlayTone(1054, 14); while(bSoundActive); PlayTone(1841, 83); while(bSoundActive); PlayTone(1401, 41); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1188, 14); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1054, 55); while(bSoundActive); wait1Msec(280); PlayTone(695, 14); while(bSoundActive); PlayTone(695, 14); while(bSoundActive); PlayTone(695, 14); while(bSoundActive); PlayTone(929, 83); while(bSoundActive); PlayTone(1401, 83); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1188, 14); while(bSoundActive); PlayTone(1054, 14); while(bSoundActive); PlayTone(1841, 83); while(bSoundActive); PlayTone(1401, 41); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1188, 14); while(bSoundActive); PlayTone(1054, 14); while(bSoundActive); PlayTone(1841, 83); while(bSoundActive); PlayTone(1401, 41); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1188, 14); while(bSoundActive); PlayTone(1251, 14); while(bSoundActive); PlayTone(1054, 55); while(bSoundActive); } |  |