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
}
}
