Created
May 2, 2019 11:16
-
-
Save timlambrecht/2cbdc31f5cae75d53cd9eba723ed48f2 to your computer and use it in GitHub Desktop.
UltrasonicSensorEnMotorForMaze_Speed100
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "MeOrion.h" | |
MeDCMotor motor1(PORT_1); | |
MeDCMotor motor2(PORT_2); | |
MeDCMotor motor3(M1); | |
MeDCMotor motor4(M2); | |
MeUltrasonicSensor SensorL(PORT_7); | |
MeUltrasonicSensor SensorM(PORT_8); | |
MeUltrasonicSensor SensorR(PORT_3); | |
int snelheid = 100; //snelheid vooruit | |
int snelheid2 = 50; //snelheid achteruit en draaien | |
void setup() | |
{ | |
Serial.begin(112500); | |
} | |
void loop() | |
{ | |
//Code sensor & print in seriële monitor | |
Serial.print("SensorL : "); | |
Serial.print(SensorL.distanceCm() ); | |
Serial.println(" cm"); | |
Serial.print("SensorM : "); | |
Serial.print(SensorM.distanceCm() ); | |
Serial.println(" cm"); | |
Serial.print("SensorR : "); | |
Serial.print(SensorR.distanceCm() ); | |
Serial.println(" cm"); | |
delay(100); | |
//VOORUIT-------------------------------------------------------------------------// | |
if (SensorL.distanceCm() > 6 && SensorL.distanceCm() < 35 && | |
SensorM.distanceCm() > 18 && | |
SensorR.distanceCm() > 6 && SensorR.distanceCm() < 35) | |
{ | |
vooruit(); | |
} | |
else if ( | |
SensorL.distanceCm() > 6 && SensorL.distanceCm() < 35 && | |
SensorM.distanceCm() > 18 && | |
SensorR.distanceCm() > 35) | |
{ | |
vooruit(); | |
} | |
else if ( | |
SensorL.distanceCm() > 35 && | |
SensorM.distanceCm() > 18 && | |
SensorR.distanceCm() > 6 && SensorR.distanceCm() < 35) | |
{ | |
vooruit(); | |
} | |
//ACHTERUIT VOORKOMT BOTSEN-------------------------------------------------------------------------// | |
else if (SensorM.distanceCm() > 0 && SensorM.distanceCm() < 4) | |
{ | |
achteruit(); | |
delay(500); | |
} | |
else if (SensorL.distanceCm() > 0 && SensorL.distanceCm() < 4) | |
{ | |
achteruit(); | |
delay(500); | |
draai_rechts(); | |
delay(100); | |
} | |
else if (SensorR.distanceCm() > 0 && SensorR.distanceCm() < 4) | |
{ | |
achteruit(); | |
delay(500); | |
draai_links(); | |
delay(100); | |
} | |
else if ( | |
SensorL.distanceCm() > 35 && SensorL.distanceCm() < 300 && | |
SensorM.distanceCm() > 35 && SensorM.distanceCm() < 300 && | |
SensorR.distanceCm() > 35 && SensorR.distanceCm() < 300) | |
{ | |
achteruit(); | |
} | |
//DRAAI RECHTS-------------------------------------------------------------------------// | |
else if (SensorL.distanceCm() > 4 && SensorL.distanceCm() < 6) | |
{ | |
draai_recht_rechts(); | |
} | |
else if (SensorL.distanceCm() > SensorR.distanceCm() && | |
SensorM.distanceCm() > 399) | |
{ | |
draai_recht_rechts(); | |
} | |
else if (SensorR.distanceCm() > 35 && | |
SensorM.distanceCm() > 6 && SensorM.distanceCm() < 18) | |
{ | |
draai_rechts_90(); | |
} | |
else if (SensorR.distanceCm() > SensorL.distanceCm() && | |
SensorM.distanceCm() > 6 && SensorM.distanceCm() < 18) | |
{ | |
draai_rechts_90(); | |
} | |
//DRAAI LINKS-------------------------------------------------------------------------// | |
else if ( SensorR.distanceCm() > 4 && SensorR.distanceCm() < 6) | |
{ | |
draai_recht_links(); | |
} | |
else if (SensorR.distanceCm() > SensorL.distanceCm() && | |
SensorM.distanceCm() > 399) | |
{ | |
draai_recht_links(); | |
} | |
else if (SensorL.distanceCm() > 35 && | |
SensorM.distanceCm() > 6 && SensorM.distanceCm() < 18) | |
{ | |
draai_links_90(); | |
} | |
else if (SensorL.distanceCm() > SensorR.distanceCm() && | |
SensorM.distanceCm() > 6 && SensorM.distanceCm() < 18) | |
{ | |
draai_links_90(); | |
} | |
//STOP WE ZIJN ER-------------------------------------------------------------------------// | |
else if ( | |
SensorL.distanceCm() > 300 && | |
SensorM.distanceCm() > 300 && | |
SensorR.distanceCm() > 300) | |
{ | |
wacht(); | |
} | |
//ZODAT WE NIET STILVALLEN-------------------------------------------------------------------------// | |
else | |
{ | |
vooruit(); | |
} | |
} | |
void vooruit(){ | |
motor1.run(snelheid+6); /* achterwiel links: between 0 and 255 */ | |
motor2.run(snelheid); /* achterwiel rechts: between 0 and 255 */ | |
motor3.run(snelheid+6); /* voorwiel links: between 0 and 255. */ | |
motor4.run(snelheid); /* voorwiel rechts: between 0 and 255. */ | |
} | |
void achteruit(){ | |
motor1.run(-snelheid2); /* achterwiel links: between 0 and -255 */ | |
motor2.run(-snelheid2); /* achterwiel rechts: between 0 and -255 */ | |
motor3.run(-snelheid2); /* voorwiel links: between 0 and -255. */ | |
motor4.run(-snelheid2); /* voorwiel rechts: between 0 and -255. */ | |
} | |
void draai_links(){ | |
motor1.run(-snelheid2); /* achterwiel links: between 0 and 255 */ | |
motor2.run(snelheid2); /* achterwiel rechts: between 0 and -255 */ | |
motor3.run(-snelheid2); /* voorwiel links: between 0 and 255. */ | |
motor4.run(snelheid2); /* voorwiel rechts: between 0 and -255. */ | |
} | |
void draai_rechts(){ | |
motor1.run(snelheid2); /* achterwiel links: between 0 and -255 */ | |
motor2.run(-snelheid2); /* achterwiel rechts: between 0 and 255 */ | |
motor3.run(snelheid2); /* voorwiel links: between 0 and -255. */ | |
motor4.run(-snelheid2); /* voorwiel rechts: between 0 and 255. */ | |
} | |
void wacht(){ | |
motor1.run(0); /* achterwiel links: between 0 and -255 */ | |
motor2.run(0); /* achterwiel rechts: between 0 and -255 */ | |
motor3.run(0); /* voorwiel links: between 0 and 255. */ | |
motor4.run(0); /* voorwiel rechts: between 0 and 255. */ | |
} | |
void draai_links_90(){ | |
draai_links(); | |
delay(900); | |
vooruit(); | |
delay(200); | |
} | |
void draai_rechts_90(){ | |
draai_rechts(); | |
delay(900); | |
vooruit(); | |
delay(200); | |
} | |
void draai_recht_rechts() { | |
draai_rechts(); | |
delay(300); | |
vooruit(); | |
delay(300); | |
draai_links(); | |
delay(200); | |
} | |
void draai_recht_links() { | |
draai_links(); | |
delay(300); | |
draai_rechts(); | |
vooruit(); | |
delay(300); | |
delay(200); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment