UltrasonicSensorEnMotorForMaze_Speed80

UltrasonicSensorEnMotorForMaze_Speed80.ino

#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 = 80; //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() > 15 && 
    SensorR.distanceCm() > 6 && SensorR.distanceCm() < 35) 
    {
  vooruit();
  }
       
else if (
        SensorL.distanceCm() > 6 && SensorL.distanceCm() < 20 &&
        SensorM.distanceCm() > 15 &&
        SensorR.distanceCm() > 35)        
    {
  vooruit();
  }  

else if (
        SensorL.distanceCm() > 35 &&
        SensorM.distanceCm() > 15 &&
        SensorR.distanceCm() > 6 && SensorR.distanceCm() < 20)        
    {
  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);
  }  

//DRAAI RECHTS-------------------------------------------------------------------------//
  
else if (SensorL.distanceCm() > 4 && SensorL.distanceCm() < 6) 
    {
  draai_rechts();
  }

else if (SensorR.distanceCm() > 35 &&
         SensorM.distanceCm() > 6 && SensorM.distanceCm() < 15)
    {
  draai_rechts_90();
  }

else if (SensorR.distanceCm() > SensorL.distanceCm() &&
         SensorM.distanceCm() > 6 && SensorM.distanceCm() < 15)
    {
  draai_rechts_90();
  }  

//DRAAI LINKS-------------------------------------------------------------------------//

else if ( SensorR.distanceCm() > 4 && SensorR.distanceCm() < 6) 
    {
  draai_links();
  }

else if (SensorL.distanceCm() > 35 &&
         SensorM.distanceCm() > 6 && SensorM.distanceCm() < 15) 
    {
  draai_links_90();
  }

else if (SensorL.distanceCm() > SensorR.distanceCm() &&
         SensorM.distanceCm() > 6 && SensorM.distanceCm() < 15)
    {
  draai_links_90();
  }  

//STOP-------------------------------------------------------------------------//

else if (
        SensorL.distanceCm() > 35 &&
        SensorM.distanceCm() > 35 && 
        SensorR.distanceCm() > 35) 
    {
  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(1000);
  vooruit();
  delay(200);
  }

void draai_rechts_90(){
  draai_rechts();
  delay(1000);
  vooruit();
  delay(200);
  }