Controlling arduino 4WD robot using z-axis"mpu6050 sensor"

[Samuel_451]

w is set to 0 in the beginning , to start the conditions.

  if (w == 0) {
     if ((z==0)||(z < 4)) {
      delay(200);
      left();
     }else if(z>9){
      right();
     }
else if ((z >= 4) && (z <= 9)) {

      //Stop();
      //delay(100);
      forward1();
      /*distance condition*/


      //set bolean to false/true
      q = 0;

    } ```
the robot is moving left to right and it is not able to stick to the forward direction. can any one help. its urgent.

[UKHeliBob]

Your topic was MOVED to its current forum category as it is more suitable than the original as it is not an Introductory Tutorial

[johnwasser]

Sounds like the 'z' value is not staying between 4 and 9. Since the part of the code that you showed doesn't change 'z' at all, the problem is in the part of the code you did not show.

[Samuel_451]

here is the full code
/*
Code Name: Arduino Bluetooth Car with Front and Back Light Control
Code URI: Arduino Projects Archives - CircuitBest
Before uploading the code you have to install the "Adafruit Motor Shield" library
Open Arduino IDE >> Go to sketch >> Include Libray >> Manage Librays... >> Search "Adafruit Motor Shield" >> Install the Library
AFMotor Library: Library Install | Adafruit Motor Shield | Adafruit Learning System
Author: Make DIY
Author URI: admin, Author at CircuitBest
Description: This program is used to control a robot using an app that communicates with Arduino through an HC-05 Bluetooth Module.
App URI: Bluetooth Car Make DIY.apk - Google Drive
Version: 1.0
License: Remixing or Changing this Thing is allowed. Commercial use is not allowed.
*/

#include <AFMotor.h>
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
unsigned long timer = 0;
float radius_of_wheel = 0.033; //Measure the radius of your wheel and enter it here in cm

//initial motors pin
AF_DCMotor motor1(1, MOTOR12_8KHZ);
AF_DCMotor motor2(2, MOTOR12_8KHZ);
AF_DCMotor motor3(3, MOTOR34_8KHZ);
AF_DCMotor motor4(4, MOTOR34_8KHZ);

int val;
int Speeed = 110;
int left_intr = 0;
/boolean conditions/
boolean one = true;
boolean two = true;
boolean three = true;
/Hardware details/
int x;
int y;
int z;
int Y;
/* falues of boolean as 1 and 0 in integer formart*/
int w = 0;
int q = 0;
int e = 0;
int r = 0;
int l = 0;
int p = 0;
int u = 0;
//int pp;
/distance value/
int d;
volatile byte rotation; // variale for interrupt fun must be volatile
float timetaken, rpm, dtime;
float v;
int distance;
unsigned long pevtime;
float rev;
void setup()
{
Serial.begin(9600); //Set the baud rate to your Bluetooth module.
rotation = rpm = pevtime = 0; //Initialize all variable to zero

// mpu setting
Wire.begin();

byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while (status != 0) { } // stop everything if could not connect to MPU6050

Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
// mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
mpu.calcOffsets(); // gyro and accelero
Serial.println("Done!\n");
}
void Left_ISR()
{
left_intr++; delay(10);
rotation++;
dtime = millis();
if (rotation >= 40)
{
timetaken = millis() - pevtime; //timetaken in millisec
rpm = (1000 / timetaken) * 60; //formulae to calculate rpm
pevtime = millis();
rotation = 0;
}
}
void loop() {
// read mpu
mpu.update();

if ((millis() - timer) > 100) { // print data every 10ms
Serial.print("X : ");
Serial.print(x);
Serial.print("\tY : ");
Serial.print(y);
Serial.print("\tZ : ");
Serial.println(z);
x = round(mpu.getAngleX());
y = round(mpu.getAngleY());
z = round(mpu.getAngleZ());
//Serial.print("X : ");
// Serial.print(x);
// Serial.print("\tY : ");
// Serial.print(y);
// Serial.print("\tZ : ");
// Serial.println(z);
timer = millis();
}
/positive is anticlockwie ...negative clockwise/ foward one/
if (w == 0) {
if (z == 0) {
left();
}
else if ((z > 0) && (z <= 4)) {
//delay(200);
left();
}
else if ((z >= 4) && (z <= 9)) {

  //Stop();
  //delay(100);
  forward1();
  /*distance condition*/


  //set bolean to false/true


}

}

// /* move z axis right/ foward 2*/
if (p == 1) {
//Stop();
/edit values of z/
if (z > 0) {
right();
} else if (z < 0) {
//Stop();
Y = abs(z) /+ 1000/;
Serial.println(Y);
}

if ((Y > 57) && (y < 60)) {
  //Serial.print("y is greater than 1030");
  //left_intr=0;
  forward2();

} else if (y < 55) {
  right();
} 

}
/move z axis second right foward 3/
if (e == 1) {
/edit values of z / as z is in negative direction convert to positive/

if (z < 0) {
  //Stop();
  Y = abs(z) /*+ 1000*/;
  Serial.println(Y);
}

if ((Y > 140) && (y < 144)) {
  //Serial.print("y is greater than 1030");
  forward3();

} else if (y < 140) {
  right();
}

}
/move z axis second right foward 4/
if (r == 1) {
/edit values of z / as z is in negative direction convert to positive/
if (z < 0) {
//Stop();
Y = abs(z) /+ 1000/;
Serial.println(Y);
}

if ((Y > 210) && (y < 214)) {
  //Serial.print("y is greater than 1030");
  forward4();

} else if (y < 210) {
  right();
}else if (y > 250) {
  left();
}

}
/move z axis second right foward 4/
/move z axis second right foward 4/
if (u == 1) {
/edit values of z / as z is in negative direction convert to positive/
if (z < 0) {
Stop();
Y = abs(z) /+ 1000/;
Serial.println(Y);
}
//
// if ((Y > 200) && (y < 210)) {
// //Serial.print("y is greater than 1030");
// forward4();
//
// } else if (y < 200) {
// right();
// }

}

}
void forward1()
{
//one=false;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE); //Left_ISR is called when left wheel sensor is triggered

v = radius_of_wheel * rpm * 0.104; //0.033 is the radius of the wheel in meter
distance = (2 * 3.141 * radius_of_wheel) * (left_intr / 40);

Serial.print("rotation"); Serial.print(left_intr);
Serial.print("v"); Serial.print(v);
Serial.print("d"); Serial.print(distance); Serial.println("");
//d=4;
//delay(20000);
if (distance == 1) {
//Serial.println("distance is 4:");
//delay(500);
//l = 1;

e = 0;
w = 1;
p = 1;
r = 0;
u = 0;
detachInterrupt(2);
//Stop();
//noInterrupts();
//interrupts();

}
}
void forward2()
{
//one=false;
// interrupts(); //Left_ISR is called when left wheel sensor is triggered
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise

delay(2000);
e = 1;

p = 0;

//Stop();
// if (distance == 5) {
// Serial.println("distance is 4:");
// //l = z;
// e = 1;
// p = 0;
// Stop();
//
// }
}
void forward3()
{
//one=false;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
delay(2000);
r = 1;
e= 0;
}
void forward4()
{
//one=false;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise

delay(2000);
r=0;
u=1;

}
void forward5()
{
//one=false;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
delay(2000);
e = 0;
w = 1;
p = 0;
r = 0;
u = 1;
}
void back()
{
left_intr = 0;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor anti-clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor anti-clockwise
motor3.setSpeed(Speeed); //Define maximum velocity
motor3.run(BACKWARD); //rotate the motor anti-clockwise
motor4.setSpeed(Speeed); //Define maximum velocity
motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void left()
{
left_intr = 0;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor anti-clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor anti-clockwise
motor3.setSpeed(Speeed); //Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed); //Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
}

void right()
{

motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed); //Define maximum velocity
motor3.run(BACKWARD); //rotate the motor anti-clockwise
motor4.setSpeed(Speeed); //Define maximum velocity
motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void topleft() {
left_intr = 0;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed / 3.1); //Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed / 3.1); //Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
}

void topright()
{
left_intr = 0;
motor1.setSpeed(Speeed / 3.1); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(Speeed / 3.1); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(Speeed);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(Speeed);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
}

void bottomleft()
{
left_intr = 0;
motor1.setSpeed(Speeed); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor anti-clockwise
motor2.setSpeed(Speeed); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor anti-clockwise
motor3.setSpeed(Speeed / 3.1); //Define maximum velocity
motor3.run(BACKWARD); //rotate the motor anti-clockwise
motor4.setSpeed(Speeed / 3.1); //Define maximum velocity
motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void bottomright()
{
left_intr = 0;
motor1.setSpeed(Speeed / 3.1); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor anti-clockwise
motor2.setSpeed(Speeed / 3.1); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor anti-clockwise
motor3.setSpeed(Speeed); //Define maximum velocity
motor3.run(BACKWARD); //rotate the motor anti-clockwise
motor4.setSpeed(Speeed); //Define maximum velocity
motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void Stop()
{
motor1.setSpeed(0); //Define minimum velocity
motor1.run(RELEASE); //stop the motor when release the button
motor2.setSpeed(0); //Define minimum velocity
motor2.run(RELEASE); //rotate the motor clockwise
motor3.setSpeed(0); //Define minimum velocity
motor3.run(RELEASE); //stop the motor when release the button
motor4.setSpeed(0); //Define minimum velocity
motor4.run(RELEASE); //stop the motor when release the button
}

[johnwasser]

@Samuel_451

  / move z axis second right foward 4 /
  / move z axis second right foward 4 /

These two lines don't make any sense. Were they supposed to be comments???

[UKHeliBob]

@Samuel_451

here is the full code

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the </> icon above the compose window) to make it easier to read and copy for examination

How to get the best out of this forum

See also FAQ - Arduino Forum for general rules on forum behaviour and etiquette. Hello, Welcome to the Arduino Forum. This guide explains how to get the best out of this forum. Please read and follow the instructions below. Being new here you might think this is having rules for the sake of rules, but that is not the case. If you don’t follow the guidelines all that happens is there is a long exchange of posts while we try to get you to tell us what we need in order to help you, which is fru…

[johnwasser]

Even after fixing the obvious syntax errors and reformatting, that code is very hard to follow. it contains many un-named constants and variables with one-character names or very short abbreviations. What few comments exist are full of lies. For example:

float radius_of_wheel = 0.033; //Measure the radius of your wheel and enter it here in cm
Where do you find a wheel with a 0.033 cm (0.33mm 0.013") diameter?!?

/*
  Code Name: Arduino Bluetooth Car with Front and Back Light Control
  Code URI: Arduino Projects Archives - CircuitBest
  Before uploading the code you have to install the "Adafruit Motor Shield" library
  Open Arduino IDE >> Go to sketch >> Include Libray >> Manage Librays... >> Search "Adafruit Motor Shield" >> Install the Library
  AFMotor Library: Library Install | Adafruit Motor Shield | Adafruit Learning System
  Author: Make DIY
  Author URI: admin, Author at CircuitBest
  Description: This program is used to control a robot using an app that communicates with Arduino through an HC-05 Bluetooth Module.
  App URI: Bluetooth Car Make DIY.apk - Google Drive
  Version: 1.0
  License: Remixing or Changing this Thing is allowed. Commercial use is not allowed.
*/

#include <AFMotor.h>
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
unsigned long timer = 0;
float radius_of_wheel = 0.033; //Measure the radius of your wheel and enter it here in cm

//initial motors pin
AF_DCMotor motor1(1, MOTOR12_8KHZ);
AF_DCMotor motor2(2, MOTOR12_8KHZ);
AF_DCMotor motor3(3, MOTOR34_8KHZ);
AF_DCMotor motor4(4, MOTOR34_8KHZ);

int val;
int Speeed = 110;
int left_intr = 0;
// boolean conditions /
boolean one = true;
boolean two = true;
boolean three = true;
// Hardware details /
int x;
int y;
int z;
int Y;
/* falues of boolean as 1 and 0 in integer formart*/
int w = 0;
int q = 0;
int e = 0;
int r = 0;
int l = 0;
int p = 0;
int u = 0;
//int pp;
// distance value /
int d;
volatile byte rotation; // variale for interrupt fun must be volatile
float timetaken, rpm, dtime;
float v;
int distance;
unsigned long pevtime;
float rev;

void setup()
{
  Serial.begin(9600); //Set the baud rate to your Bluetooth module.
  rotation = rpm = pevtime = 0; //Initialize all variable to zero

  // mpu setting
  Wire.begin();

  byte status = mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while (status != 0) { } // stop everything if could not connect to MPU6050

  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  Serial.println("Done!\n");
}

void Left_ISR()
{
  left_intr++; 
  delay(10);
  rotation++;
  dtime = millis();
  if (rotation >= 40)
  {
    timetaken = millis() - pevtime; //timetaken in millisec
    rpm = (1000 / timetaken) * 60; //formulae to calculate rpm
    pevtime = millis();
    rotation = 0;
  }
}

void loop()
{
  // read mpu
  mpu.update();

  if ((millis() - timer) > 100)   // print data every 10ms
  {
    Serial.print("X : ");
    Serial.print(x);
    Serial.print("\tY : ");
    Serial.print(y);
    Serial.print("\tZ : ");
    Serial.println(z);
    x = round(mpu.getAngleX());
    y = round(mpu.getAngleY());
    z = round(mpu.getAngleZ());
    //Serial.print("X : ");
    // Serial.print(x);
    // Serial.print("\tY : ");
    // Serial.print(y);
    // Serial.print("\tZ : ");
    // Serial.println(z);
    timer = millis();
  }
  
  // positive is anticlockwie ...negative clockwise / foward one /
  if (w == 0)
  {
    if (z == 0)
    {
      left();
    }
    else if ((z > 0) && (z <= 4))
    {
      //delay(200);
      left();
    }
    else if ((z >= 4) && (z <= 9))
    {

      //Stop();
      //delay(100);
      forward1();
      /*distance condition*/


      //set bolean to false/true


    }
  }

  // /* move z axis right/ foward 2*/
  if (p == 1)
  {
    //Stop();
    // edit values of z /
    if (z > 0)
    {
      right();
    }
    else if (z < 0)
    {
      //Stop();
      Y = abs(z) /* + 1000 */;
      Serial.println(Y);
    }

    if ((Y > 57) && (y < 60))
    {
      //Serial.print("y is greater than 1030");
      //left_intr=0;
      forward2();

    }
    else if (y < 55)
    {
      right();
    }
  }
  
  // move z axis second right foward 3 /
  if (e == 1)
  {
    // edit values of z / as z is in negative direction convert to positive /

    if (z < 0)
    {
      //Stop();
      Y = abs(z) /*+ 1000*/;
      Serial.println(Y);
    }

    if ((Y > 140) && (y < 144))
    {
      //Serial.print("y is greater than 1030");
      forward3();
    }
    else if (y < 140)
    {
      right();
    }
  }
  // move z axis second right foward 4 /
  if (r == 1)
  {
    // edit values of z / as z is in negative direction convert to positive /
    if (z < 0)
    {
      //Stop();
      Y = abs(z) /* + 1000 */;
      Serial.println(Y);
    }

    if ((Y > 210) && (y < 214))
    {
      //Serial.print("y is greater than 1030");
      forward4();

    }
    else if (y < 210)
    {
      right();
    }
    else if (y > 250)
    {
      left();
    }
  }
  // move z axis second right foward 4 /
  // move z axis second right foward 4 /
  if (u == 1)
  {
    // edit values of z / as z is in negative direction convert to positive /
    if (z < 0)
    {
      Stop();
      Y = abs(z) /* + 1000 */;
      Serial.println(Y);
    }
    //
    // if ((Y > 200) && (y < 210)) {
    // //Serial.print("y is greater than 1030");
    // forward4();
    //
    // } else if (y < 200) {
    // right();
    // }

  }

}

void forward1()
{
  //one=false;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed);//Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed);//Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise
  attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE); // Left_ISR is called when left wheel sensor is triggered

  v = radius_of_wheel * rpm * 0.104; //0.033 is the radius of the wheel in meter
  distance = (2 * 3.141 * radius_of_wheel) * (left_intr / 40);

  Serial.print("rotation"); Serial.print(left_intr);
  Serial.print("v"); Serial.print(v);
  Serial.print("d"); Serial.print(distance); Serial.println("");
  //d=4;
  //delay(20000);
  if (distance == 1)
  {
    //Serial.println("distance is 4:");
    //delay(500);
    //l = 1;

    e = 0;
    w = 1;
    p = 1;
    r = 0;
    u = 0;
    detachInterrupt(2);
    //Stop();
    //noInterrupts();
    //interrupts();
  }
}
void forward2()
{
  //one=false;
  // interrupts(); //Left_ISR is called when left wheel sensor is triggered
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed);//Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed);//Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise

  delay(2000);
  e = 1;

  p = 0;

  //Stop();
  // if (distance == 5) {
  // Serial.println("distance is 4:");
  // //l = z;
  // e = 1;
  // p = 0;
  // Stop();
  //
  // }
}
void forward3()
{
  //one=false;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed);//Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed);//Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise
  delay(2000);
  r = 1;
  e = 0;
}
void forward4()
{
  //one=false;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed);//Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed);//Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise

  delay(2000);
  r = 0;
  u = 1;

}
void forward5()
{
  //one=false;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed);//Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed);//Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise
  delay(2000);
  e = 0;
  w = 1;
  p = 0;
  r = 0;
  u = 1;
}
void back()
{
  left_intr = 0;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(BACKWARD); //rotate the motor anti-clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(BACKWARD); //rotate the motor anti-clockwise
  motor3.setSpeed(Speeed); //Define maximum velocity
  motor3.run(BACKWARD); //rotate the motor anti-clockwise
  motor4.setSpeed(Speeed); //Define maximum velocity
  motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void left()
{
  left_intr = 0;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(BACKWARD); //rotate the motor anti-clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(BACKWARD); //rotate the motor anti-clockwise
  motor3.setSpeed(Speeed); //Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed); //Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise
}

void right()
{

  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed); //Define maximum velocity
  motor3.run(BACKWARD); //rotate the motor anti-clockwise
  motor4.setSpeed(Speeed); //Define maximum velocity
  motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void topleft()
{
  left_intr = 0;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise
}

void topright()
{
  left_intr = 0;
  motor1.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
  motor3.setSpeed(Speeed);//Define maximum velocity
  motor3.run(FORWARD); //rotate the motor clockwise
  motor4.setSpeed(Speeed);//Define maximum velocity
  motor4.run(FORWARD); //rotate the motor clockwise
}

void bottomleft()
{
  left_intr = 0;
  motor1.setSpeed(Speeed); //Define maximum velocity
  motor1.run(BACKWARD); //rotate the motor anti-clockwise
  motor2.setSpeed(Speeed); //Define maximum velocity
  motor2.run(BACKWARD); //rotate the motor anti-clockwise
  motor3.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor3.run(BACKWARD); //rotate the motor anti-clockwise
  motor4.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void bottomright()
{
  left_intr = 0;
  motor1.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor1.run(BACKWARD); //rotate the motor anti-clockwise
  motor2.setSpeed(Speeed / 3.1); //Define maximum velocity
  motor2.run(BACKWARD); //rotate the motor anti-clockwise
  motor3.setSpeed(Speeed); //Define maximum velocity
  motor3.run(BACKWARD); //rotate the motor anti-clockwise
  motor4.setSpeed(Speeed); //Define maximum velocity
  motor4.run(BACKWARD); //rotate the motor anti-clockwise
}

void Stop()
{
  motor1.setSpeed(0); //Define minimum velocity
  motor1.run(RELEASE); //stop the motor when release the button
  motor2.setSpeed(0); //Define minimum velocity
  motor2.run(RELEASE); //rotate the motor clockwise
  motor3.setSpeed(0); //Define minimum velocity
  motor3.run(RELEASE); //stop the motor when release the button
  motor4.setSpeed(0); //Define minimum velocity
  motor4.run(RELEASE); //stop the motor when release the button
}