int bleutooth_control(string *s){
/*
This code is for taking over the robot completely using bleutooth to make sure you can stop de robot in case its needed.
pressing the middle button ("FIRE") on the phone stops the robot due to it not being included in the code here.
To get in to this function you have to press "B"
*/
TFileIOResult nBTCmdRdErrorStatus;
int nSizeOfMessage;
ubyte nRcvBuffer[kMaxSizeOfMessage];
int stopcode = 0;
while (*s != "A"){//if A is pressed the robot will resume its duty the normal way
nSizeOfMessage = cCmdMessageGetSize(INBOX);
if (nSizeOfMessage > kMaxSizeOfMessage){
nSizeOfMessage = kMaxSizeOfMessage;
}
if (nSizeOfMessage > 0){
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
*s = "";
stringFromChars(*s, (char *) nRcvBuffer);//put bluetooth input in string s
displayCenteredBigTextLine(4, *s);
}
//The next 4 if statements are for controlling the robot manually
if(*s == "LEFT"){//if bleutooth input is left turn left
motor(motorA) = 0;
motor(motorB) = 30;
startTask(music);//make sure the music is running
}
else if(*s == "RIGHT"){//if bleutooth input is right turn right
motor(motorA) = 30;
motor(motorB) = 0;
startTask(music);
}
else if (*s == "UP"){//if bleutooth input is up drive forward
setMultipleMotors(50, motorB, motorA);
startTask(music);
}
else if (*s == "DOWN"){//if bleutooth input is down drive backwards
setMultipleMotors(-50, motorB, motorA);
startTask(music);
}
else {//if there is no correct input turn off the motors
setMultipleMotors(0, motorA, motorB);
stopTask(music);//make sure the music is stopped
clearSounds();//empty the buffer
}
if (*s == "C"){//if the C button is pressed the loop stops and the stopcode == 1 to stop de code from running
stopcode = 1;
stopTask(music);
clearSounds();
break;
}
}
*s = "";//make sure s is empty
return stopcode;//output of the function used to stop the code if chosen
}
task main()
{
startTask(geluid);
while(1)
{
startTask(accelerate);
while (SensorValue(sonarsensor) > 5)
{
startTask(geluid);
while (SensorValue(sonarsensor) > 30)
{
if ((SensorValue(EyeLeft) == BLACKCOLOR) && (SensorValue(EyeRight) <= rightThreshold))
{
//
// Beide sensoren zien lijn, oftewel kruispunt.
//
clearSounds();
motor[left] = 0;
motor[right] = 0;
}
if ((SensorValue(EyeLeft) == BLACKCOLOR)&&(SensorValue(EyeRight)> rightThreshold))
{
//
// Only left sensor sees the line, we must turn left.
//
motor[left] = i;
motor[right] = i/(0.1 * speedlimit);
}
if ((SensorValue(EyeLeft) == WHITECOLOR) &&(SensorValue(EyeRight)< rightThreshold))
{
//
// Only right sensor sees the line, turn right.
//
motor[left] = i/(0.1 * speedlimit);
motor[right] = i;
}
if ((SensorValue(EyeLeft) == WHITECOLOR)&&(SensorValue(EyeRight) > rightThreshold))
{
motor[left] = i/2;
motor[right] = i/2;
}
wait1Msec(5);
}
clearSounds();
startTask(deaccelerate);
}
clearSounds();
i = 0;
}
}
/*
Plays a part of the Game of Thrones intro in a loop
*/
task Sound(){
while(true){
playSoundFile("charge1.rso");
wait1Msec(7300);
clearSounds();
}
}
void junction(string *junction_string){
/*
This code stops the robot at an intersection to make sure you can chose for it to go straight left or right.
In case the direction is already chosen it just goes to the already chosen direction
*/
if (SensorValue[S1] < 38 && SensorValue[S2] < 55){//was 50
setMultipleMotors(50, motorA, motorB);//Drive the cart forward a little for 40 miliseconds. This way it ends up more straight on the line after turning
wait1Msec(50);
stopTask(music);//stops the music
clearSounds();//clears the sound buffer
setMultipleMotors(0, motorA, motorB);//stop the robot
wait(0.02);
while (1){
if(*junction_string == "LEFT"){//if the input is "LEFT" turn left until you read the black line color and then continue your normal duty
motor(motorA) = 0;
motor(motorB) = 40;
*junction_string = "";
while(1){
if (SensorValue[S1] < 40){//color sensor check
setMultipleMotors(0, motorA, motorB);
break;
}
}
break;
}
else if(*junction_string == "RIGHT"){//the same only then for turning right
motor(motorA) = 40;
motor(motorB) = 0;
*junction_string = "";
while(1){
if (SensorValue[S2] < 65){//light sensor check
setMultipleMotors(0, motorA, motorB);
break;
}
}
break;
}
//if up just stop the loop to exit
else if(*junction_string == "UP"){
setMultipleMotors(50, motorA, motorB);
wait(0.2);
break;
}
else if (*junction_string == ""){//When no input stop the robot
setMultipleMotors(0, motorA, motorB);
check_bleutooth(junction_string);
}
}
}
}
void junction() {
/*
This code stops the robot at an intersection to make sure you can chose for it to go straight left or right.
In case the direction is already chosen it just goes to the already chosen direction
*/
if (SensorValue[S1] < (mincolor + 10) && SensorValue[S2] < (minlight + 10)) {//the minimul light value + 10 for the correct moment to stop at a line
setMultipleMotors(50, motorA, motorB); //Drive the cart forward a little for 50 miliseconds. This way it ends up more straight on the line after turning
wait1Msec(50);
stopTask(music); //stops the music
clearSounds(); //clears the sound buffer
setMultipleMotors(0, motorA, motorB); //stop the robot
wait(1);
while (1) {
nxtDisplayTextLine(0, "Counter: %d %c", counter, new_matrix[counter]);
if (new_matrix[counter] == 'L') {//if the input is "LEFT" turn left until you read the black line color and then continue your normal duty
motor(motorA) = 0;
motor(motorB) = 40;
while (1) {
if (SensorValue[S1] < 30) {//color sensor check
wait1Msec(5);
setMultipleMotors(0, motorA, motorB);
counter++;
break;
}
}
break;
} else if (new_matrix[counter] == 'R') {//the same only then for turning right
motor(motorA) = 40;
motor(motorB) = 0;
while (1) {
if (SensorValue[S2] < 50) {//light sensor check
wait1Msec(5);
setMultipleMotors(0, motorA, motorB);
counter++;
break;
}
}
break;
} //if up just stop the loop to exit
else if (new_matrix[counter] == 'U') {
setMultipleMotors(50, motorA, motorB);
wait(0.2);
counter++;
break;
}
}
}
}
// Interupts the task sound if it is playing.
void StopSound(){
stopTask(Sound);
clearSounds();
}
task main()
{
int timer_release = 1;
int app_timeout = 3000;
int stop_functie = 1;
bool intersection = false;
nVolume = 3;
string app_message = "";
wait1Msec(50); // The program waits 50 milliseconds to initialize the light sensor.
while(true) // Infinite loop
{
/** check if there is a message available from app */
nSizeOfMessage = cCmdMessageGetSize(INBOX);
if (nSizeOfMessage > kMaxSizeOfMessage){
nSizeOfMessage = kMaxSizeOfMessage;
}
/** store the message of the app into a variable*/
if (nSizeOfMessage > 0) {
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
stringFromChars(app_message, (char *) nRcvBuffer);
displayCenteredBigTextLine(4, app_message);
if (app_message == "FIRE" && timer_release == 1) {
if (stop_functie == 0) {
stop_functie = 1;
app_message = "STOP";
app_move(app_message, false);
continue;
}
else if (stop_functie == 1){
//motor[motorA] = 0;
//motor[motorB] = 0;
stop_functie = 0;
timer_release = 1;
}
}
}
/** check the sonar sensor and stop the robot if it sees sth */
if (stop_functie == 0 && intersection == false && SensorValue[sonar1] < 30) {
int i = -15;
clearSounds();
while (i < 0) {
playSound(soundBeepBeep);
motor[motorA] = i;
motor[motorB] = i;
i += 4;
wait1Msec(300);
}
motor[motorA] = 0;
motor[motorB] = 0;
stop_functie = true;
}
/** check the color sensor if it's black stop the robot */
else if (stop_functie == 0 && SensorValue[colorSensor] == BLACKCOLOR && SensorValue[lightSensor] < 50) {
int i = -15;
clearSounds();
while (i < 0) {
playSound(soundException);
motor[motorA] = i;
motor[motorB] = i;
i += 4;
wait1Msec(300);
}
motor[motorA] = 0;
motor[motorB] = 0;
intersection = true;
}
/** if the robot is at intersection, wait for a reactie from app
The time-out is max "app_timeout" */
else if (stop_functie == 0 && intersection == true) {
/* Reset Timer1 value (only the first time (when the timer is free)
and check if its reached app_timeout) */
if (timer_release == 1){
ClearTimer(T1);
timer_release = 0;
}
if (timer_release == 0 && time1[T1] < app_timeout) {
if (nSizeOfMessage > 0){
/* Release the timer */
timer_release = 1;
intersection = false;
stop_functie = 1;
if (app_message == "FIRE") {
stop_functie = 0;
}
else if (app_message != "UP") {
app_move(app_message, true);
}
}
}
else if (timer_release == 0 && time1[T1] > app_timeout){
intersection = false;
/* Release the timer */
timer_release = 1;
}
}
else if (stop_functie == 0 && intersection == false) {
displayCenteredBigTextLine(4, "%d", SensorValue[lightSensor]);
motor[motorA] = -22 + (power((float)(60 - SensorValue[lightSensor]) / 12, 3) * 12);
motor[motorB] = -22 - (power((float)(60 - SensorValue[lightSensor]) / 12, 3) * 12);
playSound(soundBlip);
}
else if (stop_functie == 1) {
app_move(app_message, false);
app_message = "";
clearSounds();
}
}
}
void app_move(string s, bool bocht){
int tmp_right;
int tmp_left;
if (bocht == true) {
tmp_right = -60;
tmp_left = -90;
//playSound(soundBeepBeep);
}
else {
tmp_left = -45;
tmp_right = -45;
//playSound(soundBeepBeep);
}
if (s == "UP"){
nMotorEncoder[motorA] = 0;
//nMotorEncoder[motorB] = 0;
while (nMotorEncoder[motorA] > -45) {
motor[motorA] = -30;
motor[motorB] = -30;
playSound(soundBeepBeep);
clearSounds();
}
motor[motorA] = 0;
motor[motorB] = 0;
}
else if (s == "LEFT") {
nMotorEncoder[motorA] = 0;
while (nMotorEncoder[motorA] > tmp_left) {
motor[motorA] = -20;
motor[motorB] = 20;
playSound(soundBeepBeep);
clearSounds();
}
motor[motorA] = 0;
motor[motorB] = 0;
}
else if (s == "RIGHT") {
nMotorEncoder[motorB] = 0;
while (nMotorEncoder[motorB] > tmp_right) {
motor[motorA] = 20;
motor[motorB] = -20;
playSound(soundBeepBeep);
clearSounds();
}
motor[motorA] = 0;
motor[motorB] = 0;
}
else if (s == "DOWN") {
nMotorEncoder[motorA] = 0;
//nMotorEncoder[motorB] = 0;
while (nMotorEncoder[motorA] < 45) {
motor[motorA] = 30;
motor[motorB] = 30;
playSound(soundBeepBeep);
clearSounds();
}
motor[motorA] = 0;
motor[motorB] = 0;
}
else if (s == "STOP") {
motor[motorA] = 0;
motor[motorB] = 0;
}
}
JvSound::~JvSound()
{
clearSounds();
}
task Rijden()
{
// Geluid dat de robot maakt tijdens het rijden, in loop.
k = 0; // Reset kruispuntboolean naar 0
//l = 0;
int var = 0; // variabele die naar een waarde wordt geset afhankelijk van welke sensor een signaal geeft.
while(true){
playSoundFile("hehe.rso");
// Linker sensor: Trigger wanneer de zwart-wit sensor (links) te weinig wit registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar links.
if(SensorValue[WIT] < 55)
var=1;
// Rechter sensor: Trigger wanneer de kleurensensor (rechts) zwart registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar rechts.
if((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))
var=2;
// kruispuntherkenning: Trigger wanneer zowel de kleurensensor als de zwartwit sensor getriggerd worden. Voer case 3 uit: stop, rij achteruit, wacht op commando.
if(((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))&&((SensorValue[WIT] <55)))
var=3;
// Tegenovergestelde van kruispuntherkenning: Trigger als de sensoren alleen wit registreren. Voer case 4 uit: rij naar voren.
if(((SensorValue[ZWART]!=blackcolor)&&(SensorValue[ZWART]!=greencolor))&&((SensorValue[WIT] >55)))
var=4;
// Obstakelsensor: Trigger wanneer de sonar een obstakel registreert. Voer case 5 uit: geef geluid en stop.
if (SensorValue[sonar] <30)
var=5;
switch(var)
{
case 1: //stuur naar links
setMotor(motorB,30);
setMotor(motorA,-5);
var=0;
k=0;
break;
case 2: //stuur naar rechts
setMotor(motorB,-5);
setMotor(motorA,30);
var=0;
k=0;
break;
case 3: //stoppen, ga achteruit, wacht op BlueTooth signaal
setMotor(motorB,0);
setMotor(motorA,0);
wait1Msec(100);
clearSounds();
setMotorTarget(motorA,85,-30); // ga achteruit
setMotorTarget(motorB,85,-30);
waitUntilMotorStop(motorA);
waitUntilMotorStop(motorB);
waitUntil(k==1); // wacht op een BlueTooth signaal dat links, rechts of vooruit zegt en k=1 triggert.
var=0;
k=0;
break;
case 4: // rij naar voren
setMotor(motorB,30);
setMotor(motorA,30);
var=0;
k=0;
break;
case 5: // als sonar wat ziet: schreeuw, rem rustig af en roteer 180*.
clearSounds();
playSoundFile("scream4.rso");
for (int i=3000;i>0 ; i--)
{
setMotor(motorA,i/100);
setMotor(motorB,i/100);
}
//waitUntil(l==1);
//nxtDisplayTextLine(1,"%d",i);
waitUntilMotorStop(motorA);
waitUntilMotorStop(motorB);
// oorspronkelijk hadden we code om een object te ontwijken, zowel linksom als rechtsom,
// met als alternatief om 180* te draaien. Dit kregen we echter niet op tijd volledig
// aan de praat, waardoor we hebben gekozen om hardcoded 180* te draaien.
// zie hiervoor de AvoidObject.c en AvoidObject.h files.
//
//AvoidMain();
//waitUntil(avoidDone==1);
//waitUntil(l==1);
//avoidDone = 0;
//
// einde ontwijkcode
// hardcoded rotate robot 180*
//setMotorTarget(motorA, 360, 30); // rechtsom draaien
//setMotorTarget(motorB, 360, -30);
//waitUntilMotorStop(motorA);
//waitUntilMotorStop(motorB);
// end hardcoded rotate robot 180*
setMotorTarget(motorA, 360, 30);
waitUntilMotorStop(motorA);
setMotorTarget(motorA, 135, 30);
setMotorTarget(motorB, 135, 30);
waitUntilMotorStop(motorA);
setMotorTarget(motorB, 360,30);
waitUntilMotorStop(motorB);
setMotorTarget(motorA, 540, 30);
setMotorTarget(motorB, 540, 30);
waitUntilMotorStop(motorA);
setMotorTarget(motorB, 360,30);
waitUntilMotorStop(motorB);
setMotorTarget(motorA, 135, 30);
setMotorTarget(motorB, 135, 30);
waitUntilMotorStop(motorA);
var=0;
l=0;
break;
}
}
}
task main()
{
// Gekregen code voor BlueTooth
string sig = "";
TFileIOResult nBTCmdRdErrorStatus;
int nSizeOfMessage;
ubyte nRcvBuffer[kMaxSizeOfMessage];
// Einde gekregen code voor BlueTooth
while(true) // Main loop
{
/*
* Gekregen code voor BlueTooth
* leest berichten die BlueTooth stuurt uit en stopt die in een string.
*/
nSizeOfMessage = cCmdMessageGetSize(INBOX);
if (nSizeOfMessage > kMaxSizeOfMessage)
nSizeOfMessage = kMaxSizeOfMessage;
if (nSizeOfMessage > 0){
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
string s = "";
stringFromChars(s, (char *) nRcvBuffer);
displayCenteredBigTextLine(4, s);
/*
* Einde gekregen code voor BlueTooth
*/
sig = nRcvBuffer;// zet bluetoothsignaal in variabele
//writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven
// naar voren rijden zonder lijndetectie
if(sig == "U"){
setMotor(motorA, 30);
setMotor(motorB, 30);
wait1Msec(10);
}
// naar achter rijden zonder lijndetectie
if(sig=="D"){
setMotor(motorA, -30);
setMotor(motorB, -30);
wait1Msec(10);
}
// maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten)
if(sig=="L"){
setMotorTarget(motorA, 0, 0);
setMotorTarget(motorB, 360, 30);
waitUntilMotorStop(motorB);
startTask(Rijden);
k=1; // zet anti-lijnhump op 1
l=1; // zet anti-lijnhump op 1
}
// maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten)
if(sig=="R"){
setMotorTarget(motorA, 360, 30);
setMotor(motorB, 0);
waitUntilMotorStop(motorA);
startTask(Rijden);
k=1; // zet anti-lijnhump op 1
l=1; // zet anti-lijnhump op 1
}
// rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten)
if(sig=="F"){
setMotorTarget(motorA, 180, 30);
setMotorTarget(motorB, 180, 30);
waitUntilMotorStop(motorA);
waitUntilMotorStop(motorB);
k=1; // zet anti-lijnhump op 1
startTask(Rijden);
}
// Stoppen
if(sig=="A"){
setMultipleMotors(motorA, motorB, 0);
stopTask(Rijden);
clearSounds();
}
// Zet lijndetectie aan
if(sig=="C"){
k=1; // zet anti-lijnhump op 1
l=1; // zet anti-lijnhump op 1
startTask(Rijden);
}
/*
* nog 1 knop ongebruikt (B).
*/
// Na elk commando, wacht 0,1s.
wait1Msec(100);
}
}
}
task main()
{
int EmptiesArray[MAXCOL] = {3, 3, 3, 3, 3, 3, 3};
SensorType[S1] = sensorI2CCustom9V;
SensorType[S2] = sensorColorNxtRED;
SensorType[S3] = sensorTouch; //used for recalibration
SensorType[S4] = sensorSONAR;
while(SensorValue(S4) == 255){}
playSound(soundDownwardTones);
time1[T1] = 0;
int curPos = -2;
motor[motorA] = 10;
motor[motorB] = 10;
while(SensorValue[S3] != 1)
{}
motor[motorA] = 0;
motor[motorB] = 0;
displayGameBoard();
do
{
updateEmpties(EmptiesArray);
robotPlace(EmptiesArray, curPos);
if (!isFinish())
{
clearTimer(T1);
while (time1[T1]<5000)
{
if (time1[T1]%1000==0)
{
playTone (650,50);
wait1Msec(800);
clearSounds();
}
}
playTone (850, 50);
curPos = P_moveUpdate(EmptiesArray);
}
} while(!isFinish());
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);//[3]
wait1Msec(280);
}
task main()
{
nVolume = 3;
//motor[motorC] = -20;
/** PDI Algorithm variables */
int Kp = 138;
int Ki = 0;
int Kd = 2800;
int offset = 56;
int Tp = 45;
int integral = 0;
int lastError = 0;
int derivative = 0;
int LightValue = 0;
int error = 0;
int turn = 0;
int powerA = 0;
int powerB = 0;
int app_timeout = 3000;
bool stop_functie = true; // if it's false the robot drives by itself and if it's true it drives by the app
bool intersection = false; // if true we are at intersection
string app_message = ""; // the message from app
wait1Msec(50); // The program waits 50 milliseconds to initialize the light sensor.
while(true) // Infinite loop
{
/*** check if there is a message available from app ***/
nSizeOfMessage = cCmdMessageGetSize(INBOX);
if (nSizeOfMessage > kMaxSizeOfMessage){
nSizeOfMessage = kMaxSizeOfMessage;
}
/** store the message of the app into a variable*/
if (nSizeOfMessage > 0) {
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
stringFromChars(app_message, (char *) nRcvBuffer);
displayCenteredBigTextLine(4, app_message);
/* Stop or Start the robot by tapping on "Fire" */
if (app_message == "FIRE" && intersection == false) {
if (stop_functie == false) {
stop_functie = true;
app_message = "STOP";
app_move(app_message, false);
continue;
}
else if (stop_functie == true){
stop_functie = false;
}
}
}
/*** if the robot drives by itself ***/
if (stop_functie == false) {
/** check the sonar sensor and stop the robot if it see's sth **/
if (intersection == false && SensorValue[sonar1] < 30) {
int i = -15;
clearSounds();
while (i < 0) {
playSound(soundBeepBeep);
motor[motorA] = i;
motor[motorB] = i;
i += 4;
wait1Msec(300);
}
motor[motorA] = 0;
motor[motorB] = 0;
stop_functie = true;
}
/** Detect intersections for the first time **/
else if (SensorValue[colorSensor] == BLACKCOLOR && SensorValue[lightSensor] < 50) {
playSound(soundException);
int i = -20;
clearSounds();
while (i < 0) {
playSound(soundException);
motor[motorA] = i;
motor[motorB] = i;
i += 4;
wait1Msec(300);
}
motor[motorA] = 0;
motor[motorB] = 0;
intersection = true;
/* Reset Timer1 value (only the first time the robot sees a intersection **/
ClearTimer(T1);
}
/** if the robot is at intersection, wait for a message from app
The time-out is max "app_timeout" **/
else if (intersection == true) {
if (time1[T1] < app_timeout) {
if (nSizeOfMessage > 0){
intersection = false;
stop_functie = true;
if (app_message == "FIRE") {
stop_functie = false;
}
else if (app_message != "UP") {
app_move(app_message, true);
}
}
}
else if (time1[T1] > app_timeout){
intersection = false;
}
}
/** if there is no other condition: (NO intersection, No sonar) **/
else if (intersection == false) {
//motor[motorA] = -23 + power((float)(58 - SensorValue[lightSensor]) / 5.15, 3);
//motor[motorB] = -23 - power((float)(58 - SensorValue[lightSensor]) / 5.15, 3);
//playSound(soundBlip);
LightValue = SensorValue(lightSensor);
error = LightValue - offset;
//integral = integral + error;
derivative = error - lastError;
Turn = (Kp*error) + (Ki*integral) + (Kd*derivative);
Turn = Turn/100;
powerA = Tp + Turn;
powerB = Tp - Turn;
motor[motorA] = -1*powerA;
motor[motorB] = -1*powerB;
lastError = error;
displayCenteredBigTextLine(4, "%d", error);
}
}
/*** if the robot drives by the app ***/
else if (stop_functie == true) {
clearSounds();
app_move(app_message, false);
app_message = "";
}
}
}
