void initializeRobot() {
bDisplayDiagnostics=false;
setGyroPos(false);
if (HTSMUXreadPowerStatus(MUX)) { // Multiplexer is off or dead?
nxtDisplayBigStringAt(0,45," MUX ");
nxtDisplayBigStringAt(0,25,"Dead Batt");
PlayImmediateTone(440, 50);
wait1Msec(1500);
eraseDisplay();
fallbackMode=true;
selection[SONAR_MENU]=3;
}
if (SensorRaw[gyro]>=640 || SensorRaw[gyro]<=590) { // Gyro moving or disconnected
nxtDisplayBigStringAt(21,45,"Gyro");
nxtDisplayBigStringAt(15,25,"Error");
nxtDisplayCenteredTextLine(7, "%i", SensorRaw[gyro]);
PlayImmediateTone(440, 50);
wait1Msec(1500);
setGyroEnabled(false);
} else {
wait1Msec(100);
nxtDisplayBigStringAt(15,45,"Wait...");
HTGYROstartCal(gyro);
PlaySound(soundBeepBeep);
setGyroEnabled(true);
}
eraseDisplay();
setClamp(true);
initMenus();
return;
}
task main() {
int fGyroAngle;
while(true){
SensorValue[gyrosense] = fGyroAngle;
nxtDisplayBigStringAt(0, 23, " %3d ", (long) fGyroAngle);
}
}
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();
}
}
//************************
// Sobe a Rampa
//************************
void F_STATE_SOBE()
{
PlayTone(500,10);
MTASK_SET_RUN(MT_ACCEL,MT_STOP_BUTTON);
int USminDist = 999, USdist,USdelta,Lado1,Lado2;
float Error = 0, vI = 0, vP = 0, vD = 0, kI = 0.3, kP = 1.5, kD = 1, lD=4;
tByteArray USwall;
MV_StopMotors();
GARRA_V(PORT_ARD,GARRA_V_DESCE);
wait10Msec(30);
GARRA_V(PORT_ARD,GARRA_V_PARA);
time1[T1]=0;
while(ESTADO_IS_CURRENT())
{
//=====================================
// Le arduino para pingar os ultrasons
wait10Msec(3);
ReadAllUS_short(PORT_ARD,USwall);
// Filtra dados
//// Seta as variaveis locais como locais
//// para nao dar bug (alterar o valor na outra task)
Lado1=USwall[0];
Lado2=USwall[2];
//// Acha a menor distancia que o robo fica das paredes
//if(Lado1+Lado2<USminDist)USminDist=USdist;
//// Filtra paredes (existe ou nao)
if(Lado1>60)Lado1=60-Lado2;
if(Lado2>60)Lado2=60-Lado1;
// Calcula o Error
vD = time1[T1] * ((Lado1 - Lado2) - vD) / 1000;
lD = vD;
vP = (Lado1 - Lado2);
vI += time1[T1] * vP / 1000;
//Aplica constantes
vP *= kP;
vI *= kI;
vD *= kD;
if(vI>20) vI = 20;
if(vI<-20) vI = -20;
Error = vP+
vI-
vD;
time1[T1]=0;
// Aplica erro aos motores
motor[MA]=85 - Error;
motor[MC]=85 + Error;
//Mostra no display os valores P, I, D e Stering final (Error)
eraseDisplay();
nxtDisplayString(2,"P = %i", (int)vP);
nxtDisplayString(3,"I = %i", (int)vI);
nxtDisplayString(4,"D = %i", (int)vD);
nxtDisplayBigStringAt((46 - Error)/2, 24, "|");
//Se saiu da rampa com acceleracao, entra na sala
if(!ACCEL_Rampa)ESTADO_SET_TARGET(ST_ENTRA);
//=====================================
}
}
//==========================================
//==========================================
//==========================================
task selector()
{
wait1Msec(1000);
while((nNxtButtonPressed != kEnterButton))
{
if (nNxtButtonPressed == kLeftButton)
{
while(nNxtButtonPressed ==kLeftButton){};
MissionNumber = MissionNumber-1;
PlaySoundFile("! Click.rso");
}
if (MissionNumber < 1)
{
MissionNumber = 1;
}
if (MissionNumber > 20)
{
MissionNumber = 20;
}
if (nNxtButtonPressed == kRightButton)
{
while(nNxtButtonPressed ==kRightButton){};
MissionNumber = MissionNumber+1;
PlaySoundFile("! Click.rso");
}
nxtDisplayBigTextLine(2, "Mission");
nxtDisplayBigTextLine(5, "%2d", MissionNumber);
}
eraseDisplay();
string tmp = "";
StringFormat(tmp, "%3d", MissionNumber);
nxtDisplayBigStringAt(20,18,tmp);
while(nNxtButtonPressed == kEnterButton)
{}
eraseDisplay();
while(nNxtButtonPressed != kEnterButton)
{
nxtDisplayBigTextLine(6, "SecDelay");
nxtDisplayBigTextLine(4, "%3d", time_selector);
if (nNxtButtonPressed == kLeftButton)
{
while(nNxtButtonPressed ==kLeftButton){};
time_selector = time_selector-1;
PlaySoundFile("! Click.rso");
}
if (nNxtButtonPressed == kRightButton)
{
while(nNxtButtonPressed ==kRightButton){}
time_selector = time_selector+1;
PlaySoundFile("! Click.rso");
}
if(time_selector > 30) time_selector = 30;
if(time_selector < 0) time_selector = 0;
}
PlaySound(soundException);
wait1Msec(200);
eraseDisplay();
while(nNxtButtonPressed == kEnterButton){}
while(nNxtButtonPressed != kEnterButton)
{
nxtDisplayBigTextLine(6, "gyro_cal");
nxtDisplayBigTextLine(4, "%2d", gyroCalTime);
if(nNxtButtonPressed == kLeftButton)
{
while(nNxtButtonPressed == kLeftButton){}
gyroCalTime = gyroCalTime-1;
PlaySoundFile("! Click.rso");
}
if(nNxtButtonPressed == kRightButton)
{
while(nNxtButtonPressed == kRightButton){}
gyroCalTime = gyroCalTime+1;
PlaySoundFile("! Click.rso");
}
if(gyroCalTime > 15) gyroCalTime = 15;
if(gyroCalTime < 0) gyroCalTime = 0;
}
if(gyroCalTime > 0) calibrate = 1;
PlaySound(soundException);
eraseDisplay();
}
