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 }
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; } } } }
task main() { wait(3, seconds); setMultipleMotors(50, motorB, motorC, motorC); wait(2, seconds); stopAllMotors(); wait(1, seconds); turnLeft(3, rotations, 50); }
void speed_adjust(int index, int until, int increment){ /* This function is used for slowing down the motor slowly instead of stopping immediatly */ for (int i = index; i >= until; i+=increment){ setMultipleMotors(i, motorA, motorB); wait(0.09); } }
void junction(){ /* This code stops the robot at an intersection to make sure you can chose for it to go straight left or right. */ string s = ""; if (SensorValue[S1] < 38 && SensorValue[S2] < 50){ setMultipleMotors(90, motorA, motorB);//Drive the cart forward a little for 40 miliseconds. This way it ends up more straight on the line after turning wait1Msec(40); setMultipleMotors(0, motorA, motorB);//stop the robot check_bleutooth(&s);//wait for bleutooth input if(s == "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; while(1){ if (SensorValue[S1] < 40){//color sensor check setMultipleMotors(0, motorA, motorB); break; } } } else if(s == "RIGHT"){//the same only then for turning right motor(motorA) = 40; motor(motorB) = 0; while(1){ if (SensorValue[S2] < 60){//light sensor check setMultipleMotors(0, motorA, motorB); break; } } } //For going forward no if statement is needed because the robot wil just continue driving after te bleutooth input /*else if(s == "UP"){ }*/ } }
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 speed_adjust(int index, int until, int increment, int what_motor){ /* what_motor = 0 == both what_motor = 1 == right increment what_motor = 2 == left increment */ if (what_motor == 0){ for (int i = index; i >= until; i+=increment){ setMultipleMotors(i, motorA, motorB); wait(0.1);//0.1 } } else if (what_motor == 1){ //adjust right motor } else if (what_motor == 2){ //adjust left motor } //setMultipleMotors(0, motorA, motorB); }
task main() { /* The main function contains just an infinite while loop that continues to call all the functions before mentioned. In the called functions we check for specific things like the line curving or a bleutooth input. */ int stopcode2 = 0;//for stopping the while loop in case of a remote code shutdown startTask(music);//start the music in a seperate thread string s = "";//create a string TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; while(1){ nSizeOfMessage = cCmdMessageGetSize(INBOX); if (nSizeOfMessage > kMaxSizeOfMessage){//make the message shorter if its to long nSizeOfMessage = kMaxSizeOfMessage; } if (nSizeOfMessage > 0){//if the message is not empty, put the received information in string s nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; s = ""; stringFromChars(s, (char *) nRcvBuffer);//put the received data in string s displayCenteredBigTextLine(4, s); } if (s == "B"){//bluetooth override setMultipleMotors(0, motorA, motorB); stopcode2 = bleutooth_control(&s);//check for bleutooth input. This is the version of bleutooth input that takes over the robot } junction(&s);//check for an intersection linefollow();//check if the line is curving and if not then just drive if(stopcode2 == 1){//if "C" is pressed in the bleutooth_control function then stop the code completely return; } } }
void linefollow(void){ //using float variable for the sensors to have the most accurate float color; float light; float sonar; // reading the sensor value light = SensorValue[S2]; color = SensorValue[S1]; sonar = SensorValue[S3]; // displaying the values from above nxtDisplayTextLine(1, "color: %d", color); nxtDisplayTextLine(2, "light: %d", light); nxtDisplayTextLine(3, "sonar: %d", sonar ); // if an object is closer then 30 centimeters do the folowing if(sonar < 30) { speed_adjust(60, 0, -4);//slow down for (int i = 0; i < 10;){//slowly start turning motor[motorA] = i; motor[motorB] = -i; wait(0.4); i++; } while(1){//wait until you see the line if (SensorValue[S2] < 60){ for (int i = 10; i < 0;){//slow down the turning speed motor[motorA] = i; motor[motorB] = -i; wait(0.4); i+=2; } break; } } } //redirect the robot by using the light sensor else if (light < 69){ float formule; float omrek; float formule2; omrek = light-39; nxtDisplayTextLine(4, "lomrek : %d", omrek); if(omrek<5){ formule =100; formule2=0; } else if (omrek<10){ formule = -0.4*omrek+100; formule2 = -1.428*omrek+50; } else{ formule = -0.9*omrek+100; formule2 = -1.428*omrek+50; } //startTask(music);//starts the music motor(motorA) = formule;//50+((60-light)*2.5) motor(motorB) =formule2;//35 //0 } // redirecting the robot by using hte color sensor else if(color < 51) {//vorige waarde: 4 // float variables for best accurate values float formule; float omrek; float formule2; omrek = color-22; nxtDisplayTextLine(5, "comrek : %d", omrek); if(omrek<5) { formule =100; formule2=0; } else if (omrek<10) { formule = -0.4*omrek+100; formule2 = -1.428*omrek+50; } else { formule = -0.9*omrek+100; formule2 = -1.428*omrek+50; } startTask(music);//starts the music motor(motorA) = formule2;//0 motor(motorB) =formule ;//50+(50-color)*1+(2/3)) } else { setMultipleMotors(50,motorA,motorB);//70 //100 } }
int bleutooth_control(void){ /* 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 for another function. */ TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; int stopcode = 0; nSizeOfMessage = cCmdMessageGetSize(INBOX); if (nSizeOfMessage > kMaxSizeOfMessage){ nSizeOfMessage = kMaxSizeOfMessage; } if (nSizeOfMessage > 0){//Only if an actual message is being received the bleutooth take over system should activate nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; string s = ""; stringFromChars(s, (char *) nRcvBuffer); displayCenteredBigTextLine(4, s); while (s != "A"){//if A is pressed the robot will resume its duty the normal way nSizeOfMessage = cCmdMessageGetSize(INBOX);//misschien onderaan if (nSizeOfMessage > kMaxSizeOfMessage){ nSizeOfMessage = kMaxSizeOfMessage; } if (nSizeOfMessage > 0){ nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; s = ""; stringFromChars(s, (char *) nRcvBuffer); 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; } else if(s == "RIGHT"){//if bleutooth input is right turn right motor(motorA) = 30; motor(motorB) = 0; } else if (s == "UP"){//if bleutooth input is up drive forward setMultipleMotors(50, motorB, motorA); } else if (s == "DOWN"){//if bleutooth input is down drive backwards setMultipleMotors(-50, motorB, motorA); } /*else {//hier setMultipleMotors(0, motorA, motorB); }*/ if (s == "C"){//if the C button is pressed the loop stops and the stopcode == 1 to stop de code if neccesairy stopcode = 1; break; } } } return stopcode;//either 1 or 0 depending on the "C" button being pressed or not }
void need_for_speed(void){ //using float variable for the sensors to have the most accurate float color; float light; float sonar; // reading the sensor value light = SensorValue[S2]; color = SensorValue[S1]; sonar = SensorValue[S3]; // displaying the values from above nxtDisplayTextLine(1, "color: %d", color); nxtDisplayTextLine(2, "light: %d", light); nxtDisplayTextLine(3, "sonar: %d", sonar ); // if an object is less than 15 centimeters the top if(sonar <35) { //setMultipleMotors(0,motorA,motorB); speed_adjust(60, 0, -3, 0); string s = ""; check_bleutooth(&s);//laten omdraaien moet nog } //redirect the robot by using the light sensor else if (light < 65){ float formule; float omrek; float maal = 0; omrek = 65-light; formule = (65+(omrek)*(1+(2/3))); nxtDisplayTextLine(4, "omrekrechts: %d", omrek ); if (omrek< 10){ maal =1.2; } else if(omrek>25){ maal = 2 +((omrek-25)*0.1); } else { (maal = 1.2+(omrek-10)*0.055); } startTask (music);//starts music motor(motorA) = formule;//50+((60-light)*2.5) motor(motorB) =45-(omrek*maal);//35 //0 } // redirecting the robot by using hte color sensor else if(color < 48) {//vorige waarde: 4 // float variables for best accurate values float formule; float omrek; float maal =0; omrek = 48-color; formule = (65+(omrek)*(1+(2/3))); nxtDisplayTextLine(5, "omreklinks: %d", omrek ); if (omrek< 10){ maal = 1.2; } else if(omrek>25){ maal = 2 + ((omrek - 25)*0.1); } else{ maal=(1.2+(omrek-10)*0.055); } startTask (music);//starts music motor(motorA) = 45-(omrek*maal);//0 motor(motorB) =formule ;//50+(50-color)*1+(2/3)) } else { setMultipleMotors(65,motorA,motorB);//70 //100 } }
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() { string sig = ""; TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; while (true) { // Check to see if a message is available nSizeOfMessage = cCmdMessageGetSize(INBOX); writeDebugStream("%c\n", nRcvBuffer); 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); sig = nRcvBuffer; writeDebugStream(sig); if(sig == "U"){ setMotor(motorA, 50); setMotor(motorB, 50); wait1Msec(10); } if(sig=="D"){ setMotor(motorA, -50); setMotor(motorB, -50); wait1Msec(1); } if(sig=="L"){ setMotorTarget(motorA,0, 0); setMotorTarget(motorB,360, 50); waitUntilMotorStop(motorB); startTask(Rijden); k=1; } if(sig=="R"){ setMotorTarget(motorA,360, 50); setMotor(motorB, 0); waitUntilMotorStop(motorA); startTask(Rijden); } if(sig=="F"){ setMotorTarget(motorA, 180,30); setMotorTarget(motorB,180, 30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); k=1; startTask(Rijden); } if(sig=="A"){ setMultipleMotors(motorA, motorB, 0); stopTask(Rijden); } if(sig=="C"){ startTask(Rijden); } wait1Msec(100); } } }
void bluetooth() { string kutzooi = ""; TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; while (true) { // Check to see if a message is available nSizeOfMessage = cCmdMessageGetSize(INBOX); writeDebugStream("%c\n", nRcvBuffer); 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); kutzooi = nRcvBuffer; writeDebugStream(kutzooi); if(kutzooi == "U"){ setMotor(motorA, 30); setMotor(motorB, 30); wait1Msec(10); break; } if(kutzooi=="D"){ setMotorTarget(motorA,1080, 100); setMotorTarget(motorB,1080, 100); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); wait1Msec(1); break; } if(kutzooi=="L"){ setMotorTarget(motorA,0, 0); setMotorTarget(motorB,360, 25); waitUntilMotorStop(motorB); break; } if(kutzooi=="R"){ setMotorTarget(motorA,360, 25); setMotor(motorB, 0); waitUntilMotorStop(motorA); break; } if(kutzooi=="F"){ setMotorTarget(motorA, 1080,100); setMotor(motorB, 0); waitUntilMotorStop(motorA); setMotor(motorA, 0); setMotorTarget(motorB, 1080,100); break; } if(kutzooi=="A"){ setMultipleMotors(motorA, motorB, 0); } wait1Msec(100); } } }
void linefollow(void) { //using float variable for the sensors to have the most accurate float color; float light; float sonar; // reading the sensor value light = SensorValue[S2]; color = SensorValue[S1]; sonar = SensorValue[S3]; // displaying the values from above //nxtDisplayTextLine(1, "color: %d", color); //nxtDisplayTextLine(2, "light: %d", light); //nxtDisplayTextLine(3, "sonar: %d", sonar); // if an object is closer then 30 centimeters do the folowing if (sonar < 30) { speed_adjust(60, 0, -4); //slow down for (int i = 0; i < 10;) {//slowly start turning motor[motorA] = i; motor[motorB] = -i; wait(0.4); i++; } while (1) {//wait until you see the line if (SensorValue[S2] < 60) { for (int i = 10; i < 0;) {//slow down the turning speed motor[motorA] = i; motor[motorB] = -i; wait(0.4); i += 2; } break; } } } //redirect the robot by using the light sensor else if (light < (maxlight - 5)) { // float variable for the formule math float formule; float omrek; float formule2; //omrek is used to can use the formules in every surroundings omrek = light - minlight; // show the result of the omrek on line 4 nxtDisplayTextLine(4, "lomrek : %f", omrek); // if omrek is lower then 5 use 100 and 1 for a real sharp turn if (omrek < 5) { formule = 100; formule2 = 1; }// if the omrek is between 5 and 10 use a formule that gives a heavier control else if (omrek < 10) { formule = -0.4 * omrek + 100; formule2 = -1.428 * omrek + 50; }// the standaard formule for following the line else { formule = -0.9 * omrek + 100; formule2 = -1.428 * omrek + 50; } startTask(music); //starts the music // give the motor a and b the result of the formules motor(motorA) = formule; //50+((60-light)*2.5) motor(motorB) = formule2; //35 //0 } // redirecting the robot by using hte color sensor else if (color < (maxcolor - 5)) {//vorige waarde: 4 // float variables for best accurate values float formule; float omrek; float formule2; //omrek is used to can use the formules in every surroundings omrek = color - mincolor; // show the result of the omrek on line 5 nxtDisplayTextLine(5, "comrek : %d", omrek); // if omrek is lower then 5 use 100 and 1 for a real sharp turn if (omrek < 5) { formule = 100; formule2 = 1; }// if the omrek is between 5 and 10 use a formule that gives a heavier control else if (omrek < 10) { formule = -0.4 * omrek + 100; formule2 = -1.428 * omrek + 50; }// the standaard formule for following the line else { formule = -0.9 * omrek + 100; formule2 = -1.428 * omrek + 50; } startTask(music); //starts the music // give the motor a and b the result of the formules motor(motorA) = formule2; //0 motor(motorB) = formule; //50+(50-color)*1+(2/3)) } else { //if their isn't steering nessasary then both motors gets 50 as speed setMultipleMotors(10, motorA, motorB); //70 //100 } }