void jumpTo() //a function to jump to a specific point in our directions
{
while(nNxtButtonPressed != 3) //while the orange button is NOT pressed...
{
bNxtLCDStatusDisplay = true;
if(nNxtButtonPressed == 1) //if the right button is pressed...
{
dirIndex ++; //increse the index variable by 1
}
if(nNxtButtonPressed == 2) // if the left button is pressed...
{
dirIndex --; //decrese the index variable by 1.
}
if(dirIndex < 0) //to keep the index variable from going negative
{ //(no negative array slots)
dirIndex = 0;
}
eraseDisplay();
nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo
nxtDisplayBigStringAt(65, 55, "%d", dirIndex);
nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);
wait1Msec(200);
}
}
task main()
{
intDirections();
dirDecode();
StartTask(sensors); //start all our other tasks
StartTask(motors);
StartTask(line);
jumpTo();
bFloatDuringInactiveMotorPWM = false; //make sure the motors brake, not float
// resetMotors = true;
while(true)
{
if(directions[dirIndex][0] == 000)
{
StopAllTasks();
}
eraseDisplay();
nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo
nxtDisplayBigStringAt(65, 55, "%d", dirIndex);
nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);
nMotorPIDSpeedCtrl[left] = mtrSpeedReg; //turn off the PID for the motors,
nMotorPIDSpeedCtrl[right] = mtrSpeedReg; //better reaction time
if(dirIndex>1)
{
if(directions[dirIndex-1][1] == 2 && directions[dirIndex][2] == 2) //if our last turn was a left turn AND we
{ //are going to follow the opposite line, move over to that side
turning = true;
motor[left] = 50;
while(nMotorEncoder[left] <= 200) {}
motor[left] = 0;
motor[right] = 50;
while(nMotorEncoder[right] <= 200) {}
motor[right] = 0;
turning = false;
}
if(directions[dirIndex-1][1] == 3 && directions[dirIndex][2] == 1) //same as last, except this is done if
{ //last turn was a right, AND we are
turning = true; //following the left line
motor[right] = 50;
while(nMotorEncoder[right] <= 200) {}
motor[right] = 0;
motor[left] = 50;
while(nMotorEncoder[left] <= 200) {}
motor[left] = 0;
turning = false;
}
}
resetMotors = true;
targetDPS = 450;
lineFollow = true;
wait1Msec(100);
if(directions[dirIndex][1] == 6 || directions[dirIndex][1] == 7) // if parking...
{
for(int i = 0; i < 30; i = i) //if we have seen blue continually for a
{ //certain amount of time,
if(lineColor == 2)
{
i++;
}
else
{
i = 0;
}
writeDebugStreamLine("lineColor is: %d", lineColor);//white to the debug
wait1Msec(10); //stream, better than
} //viewing variables over BT
resetMotors = true;
wait1Msec(20);
while(lastPos < ((parkDistance * (directions[dirIndex][3] - 1)) + 100))
{
wait1Msec(10);
}
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
switch(directions[dirIndex][1]) // which side of the road are we parking on?
{
case 6:
parkLeft();
break;
case 7:
parkRight();
break;
}
}
else // if not parking, follow the procedure for stopping
{
while(stopSensor != 5) // while the stopSensor is not seeing a stop sign
{
writeDebugStreamLine("stopsensor: %d", stopSensor);
wait1Msec(10); // keep following the line
}
writeDebugStreamLine("!!!STOPPED AT: %d", stopSensor);
PlaySound(soundBeepBeep);
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
// squareLine(1);
switch(directions[dirIndex][1]) // go to the respective turning functioin
{
case 1: goStraight(); break;
case 2: turnLeft(); break;
case 3: turnRight(); break;
case 4: turnLeftL(); break;
case 5: turnRightL(); break;
}
}
if(directions[dirIndex][1] == 9) //for exiting the city
{
while(lineColor != 5) {}
resetMotors = true;
wait1Msec(20);
while(lastPos < 200)
{
wait1Msec(10);
}
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
exit();
}
turning = false;
resetMotors = true;
//directions[dirIndex-1][1] = directions[dirIndex][1]; // needed for if we need to move over to the
dirIndex ++; //other side of the road after turn
// if(dirIndex == 14) // for an endless loop on our practice mat
// {
// dirIndex = 0;
// }
//dirDecode();
}
}
/*
=============================================================================
main task with some testing code
*/
task main() {
// Standard range is set to short range
bool shortrange = true;
tObstacleZone zone = 0;
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "Mindsensors");
nxtDisplayCenteredBigTextLine(1, "SumoEyes");
nxtDisplayCenteredTextLine(3, "Test 2");
nxtDisplayCenteredTextLine(5, "Press enter to");
nxtDisplayCenteredTextLine(6, "switch between");
nxtDisplayCenteredTextLine(7, "ranges");
wait1Msec(2000);
eraseDisplay();
// Set the sensor to short range
MSSUMOsetShortRange(HTMSSUMO);
nxtDisplayRICFile(0, 0, "sumonone.ric");
while(true) {
if (time1[T1] > 1000) {
if (shortrange == false) {
// set the sensor to short range and display this
MSSUMOsetShortRange(HTMSSUMO);
nxtDisplayCenteredTextLine(7, "Short range");
shortrange = true;
} else {
// set the sensor to long range and display this
MSSUMOsetLongRange(HTMSSUMO);
nxtDisplayCenteredTextLine(7, "Long range");
shortrange = false;
}
PlaySound(soundBeepBeep);
while(bSoundActive)
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
zone = MSSUMOreadZone(HTMSSUMO);
if (shortrange) {
switch (zone) {
case MSSUMO_NONE: nxtDisplayRICFile(0, 0, "sumonone.ric"); break;
case MSSUMO_FRONT: nxtDisplayRICFile(0, 0, "sumoSF.ric"); break;
case MSSUMO_RIGHT: nxtDisplayRICFile(0, 0, "sumoSL.ric"); break;
case MSSUMO_LEFT: nxtDisplayRICFile(0, 0, "sumoSR.ric"); break;
}
nxtDisplayCenteredTextLine(7, "Short range");
} else {
switch (zone) {
case MSSUMO_NONE: nxtDisplayRICFile(0, 0, "sumonone.ric"); break;
case MSSUMO_FRONT: nxtDisplayRICFile(0, 0, "sumoLF.ric"); break;
case MSSUMO_RIGHT: nxtDisplayRICFile(0, 0, "sumoLL.ric"); break;
case MSSUMO_LEFT: nxtDisplayRICFile(0, 0, "sumoLR.ric"); break;
}
nxtDisplayCenteredTextLine(7, "Long range");
}
wait1Msec(10);
}
}
}
