Hand Gesture Controlled robot

Make this robot moves by just moving your hand!

Aug 26, 2021

•

64217 views

•

13 respects

bluetooth

gyro

robotics

wireless communication

robots

Components and supplies

Switch Actuator, APEM A01 series Illuminated Push-Button Switches

HC-05 Bluetooth Module

Dual H-Bridge motor drivers L293D

Ultrasonic Sensor - HC-SR04 (Generic)

Inertial Measurement Unit (IMU) (6 deg of freedom)

Slide Switch

Arduino UNO

Arduino Nano R3

Robot wheels

PCB's

Rechargeable Battery, Lithium Ion

9V battery (generic)

DC Motor, 12 V

Robot chassis

Tools and machines

Soldering Station, 110 V

Solder Flux, Soldering

Solder Wire, Lead Free

Apps and platforms

Arduino IDE

Project description

Code

Receiver code

arduino

Upload it code to the reception unit

1#include <SoftwareSerial.h>
2
3SoftwareSerial BTSerial(10, 11); //  CONNECT BT RX PIN TO ARDUINO 12 PIN | CONNECT BT TX PIN TO ARDUINO 11 PIN
4#include<SPI.h>
5
6  
7int trigPin = 12;    // Trig
8int echoPin = 13;    // Echo
9long duration,  cm, inches;
10int direct;
11int l1=3;
12int r1=5;
13int l2=6;
14int r2=9;
15
16int  forword_corr;
17
18
19int a = 0 ;
20int s = 3; 
21
22
23int b = 0 ;
24int  c = 2; 
25
26
27
28
29
30void setup() {
31  Serial.begin(9600);      
32  BTSerial.begin(9600);    
33  
34 
35  pinMode(l1, OUTPUT);
36  pinMode(r1,  OUTPUT);
37  pinMode(l2, OUTPUT);
38  pinMode(r2, OUTPUT);
39  
40  digitalWrite(l1,  LOW);
41  digitalWrite(r1, LOW);
42  digitalWrite(l2, LOW);
43  digitalWrite(r2,  LOW);
44  
45  pinMode(trigPin, OUTPUT);
46  pinMode(echoPin, INPUT);
47}
48
49void  loop() {
50  
51   digitalWrite(trigPin, LOW);
52  delayMicroseconds(5);
53  digitalWrite(trigPin, HIGH);
54  delayMicroseconds(10);
55  digitalWrite(trigPin,  LOW);
56
57  pinMode(echoPin, INPUT);
58  duration = pulseIn(echoPin, HIGH);
59
60  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
61  Serial.print(cm);
62  Serial.print("cm");
63  Serial.println();
64  if(BTSerial.available()>0){  // Witing for data incoming from the other XBee module
65   direct=BTSerial.read();
66    
67                if(direct == 'B'){
68            Serial.println("Backward");
69            backward();
70          }else if (direct=='F'){
71              Serial.println("Forward");
72            forward();
73          }else if(direct == 'R'){
74            Serial.println("Right");
75            left();
76          }else if(direct == 'L'){
77            Serial.println("left");
78            right();
79
80           }else if(direct == 'r'){
81            Serial.println("rotright");
82            rotr();
83            
84          }else if(direct == 'l'){
85            Serial.println("rotleft");
86            rotl();
87            }else if  (direct == 'S'){
88            Serial.println("Stop");
89            stopCar();
90          }
91                
92     
93           
94     
95  }
96}
97    
98  
99
100
101 
102void forward()
103{
104  if(cm<=15){
105     digitalWrite(l1, LOW);
106      digitalWrite(r2, LOW);
107
108    }
109    else{
110    digitalWrite(l1, HIGH);
111  digitalWrite(r1, LOW);
112  digitalWrite(l2, LOW);
113  
114  
115    digitalWrite(r2, LOW);
116 delay(2);
117    digitalWrite(r2, HIGH);
118   delay(2);
119}
120}
121void backward()
122{
123  digitalWrite(l1, LOW);
124  digitalWrite(l2,  HIGH);
125  digitalWrite(r2, LOW);
126  if(b%c == 0){
127    digitalWrite(r1, HIGH);
128  }else{
129    digitalWrite(r1, LOW);
130   }
131   
132
133    if(b > 1000 ) {
134     b=0;
135   }
136}
137void right()
138{
139  digitalWrite(l1, HIGH);
140  digitalWrite(r1,  LOW);
141  digitalWrite(l2, LOW);
142  analogWrite(r2, 100);
143 
144    
145
146}
147void  left()
148{
149   analogWrite(l1, 100);
150  digitalWrite(r1, LOW);
151  digitalWrite(l2,  LOW);
152  digitalWrite(r2, HIGH);
153}
154
155void stopCar() {
156    digitalWrite(l1,  LOW);
157
158  digitalWrite(r1, LOW);
159  digitalWrite(l2, LOW);
160  digitalWrite(r2,  LOW);
161}
162void rotr() {
163
164  digitalWrite(l1, HIGH);
165  digitalWrite(r1,  LOW);
166  digitalWrite(l2, HIGH);
167  digitalWrite(r2, LOW);
168}
169void rotl(){
170
171  digitalWrite(l1, LOW);
172  digitalWrite(r1, HIGH);
173  digitalWrite(l2, LOW);
174  digitalWrite(r2, HIGH);
175}
176

#include <SoftwareSerial.h>

SoftwareSerial BTSerial(10, 11); //  CONNECT BT RX PIN TO ARDUINO 12 PIN | CONNECT BT TX PIN TO ARDUINO 11 PIN
#include<SPI.h>

  
int trigPin = 12;    // Trig
int echoPin = 13;    // Echo
long duration,  cm, inches;
int direct;
int l1=3;
int r1=5;
int l2=6;
int r2=9;

int  forword_corr;


int a = 0 ;
int s = 3; 


int b = 0 ;
int  c = 2; 





void setup() {
  Serial.begin(9600);      
  BTSerial.begin(9600);    
  
 
  pinMode(l1, OUTPUT);
  pinMode(r1,  OUTPUT);
  pinMode(l2, OUTPUT);
  pinMode(r2, OUTPUT);
  
  digitalWrite(l1,  LOW);
  digitalWrite(r1, LOW);
  digitalWrite(l2, LOW);
  digitalWrite(r2,  LOW);
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void  loop() {
  
   digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin,  LOW);

  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  if(BTSerial.available()>0){  // Witing for data incoming from the other XBee module
   direct=BTSerial.read();
    
                if(direct == 'B'){
            Serial.println("Backward");
            backward();
          }else if (direct=='F'){
              Serial.println("Forward");
            forward();
          }else if(direct == 'R'){
            Serial.println("Right");
            left();
          }else if(direct == 'L'){
            Serial.println("left");
            right();

           }else if(direct == 'r'){
            Serial.println("rotright");
            rotr();
            
          }else if(direct == 'l'){
            Serial.println("rotleft");
            rotl();
            }else if  (direct == 'S'){
            Serial.println("Stop");
            stopCar();
          }
                
     
           
     
  }
}
    
  


 
void forward()
{
  if(cm<=15){
     digitalWrite(l1, LOW);
      digitalWrite(r2, LOW);

    }
    else{
    digitalWrite(l1, HIGH);
  digitalWrite(r1, LOW);
  digitalWrite(l2, LOW);
  
  
    digitalWrite(r2, LOW);
 delay(2);
    digitalWrite(r2, HIGH);
   delay(2);
}
}
void backward()
{
  digitalWrite(l1, LOW);
  digitalWrite(l2,  HIGH);
  digitalWrite(r2, LOW);
  if(b%c == 0){
    digitalWrite(r1, HIGH);
  }else{
    digitalWrite(r1, LOW);
   }
   

    if(b > 1000 ) {
     b=0;
   }
}
void right()
{
  digitalWrite(l1, HIGH);
  digitalWrite(r1,  LOW);
  digitalWrite(l2, LOW);
  analogWrite(r2, 100);
 
    

}
void  left()
{
   analogWrite(l1, 100);
  digitalWrite(r1, LOW);
  digitalWrite(l2,  LOW);
  digitalWrite(r2, HIGH);
}

void stopCar() {
    digitalWrite(l1,  LOW);

  digitalWrite(r1, LOW);
  digitalWrite(l2, LOW);
  digitalWrite(r2,  LOW);
}
void rotr() {

  digitalWrite(l1, HIGH);
  digitalWrite(r1,  LOW);
  digitalWrite(l2, HIGH);
  digitalWrite(r2, LOW);
}
void rotl(){

  digitalWrite(l1, LOW);
  digitalWrite(r1, HIGH);
  digitalWrite(l2, LOW);
  digitalWrite(r2, HIGH);
}

Receiver code

arduino

Upload it code to the reception unit

1#include <SoftwareSerial.h>
2
3SoftwareSerial BTSerial(10, 11); //   CONNECT BT RX PIN TO ARDUINO 12 PIN | CONNECT BT TX PIN TO ARDUINO 11 PIN
4#include<SPI.h>
5
6   
7int trigPin = 12;    // Trig
8int echoPin = 13;    // Echo
9long duration,   cm, inches;
10int direct;
11int l1=3;
12int r1=5;
13int l2=6;
14int r2=9;
15
16int   forword_corr;
17
18
19int a = 0 ;
20int s = 3; 
21
22
23int b = 0 ;
24int   c = 2; 
25
26
27
28
29
30void setup() {
31  Serial.begin(9600);      
32   BTSerial.begin(9600);    
33  
34 
35  pinMode(l1, OUTPUT);
36  pinMode(r1,   OUTPUT);
37  pinMode(l2, OUTPUT);
38  pinMode(r2, OUTPUT);
39  
40  digitalWrite(l1,   LOW);
41  digitalWrite(r1, LOW);
42  digitalWrite(l2, LOW);
43  digitalWrite(r2,   LOW);
44  
45  pinMode(trigPin, OUTPUT);
46  pinMode(echoPin, INPUT);
47}
48
49void   loop() {
50  
51   digitalWrite(trigPin, LOW);
52  delayMicroseconds(5);
53   digitalWrite(trigPin, HIGH);
54  delayMicroseconds(10);
55  digitalWrite(trigPin,   LOW);
56
57  pinMode(echoPin, INPUT);
58  duration = pulseIn(echoPin, HIGH);
59
60   cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
61  Serial.print(cm);
62   Serial.print("cm");
63  Serial.println();
64  if(BTSerial.available()>0){   // Witing for data incoming from the other XBee module
65   direct=BTSerial.read();
66     
67                if(direct == 'B'){
68            Serial.println("Backward");
69             backward();
70          }else if (direct=='F'){
71              Serial.println("Forward");
72             forward();
73          }else if(direct == 'R'){
74            Serial.println("Right");
75             left();
76          }else if(direct == 'L'){
77            Serial.println("left");
78             right();
79
80           }else if(direct == 'r'){
81            Serial.println("rotright");
82             rotr();
83            
84          }else if(direct == 'l'){
85            Serial.println("rotleft");
86             rotl();
87            }else if  (direct == 'S'){
88            Serial.println("Stop");
89             stopCar();
90          }
91                
92     
93           
94      
95  }
96}
97    
98  
99
100
101 
102void forward()
103{
104  if(cm<=15){
105      digitalWrite(l1, LOW);
106      digitalWrite(r2, LOW);
107
108    }
109    else{
110     digitalWrite(l1, HIGH);
111  digitalWrite(r1, LOW);
112  digitalWrite(l2, LOW);
113   
114  
115    digitalWrite(r2, LOW);
116 delay(2);
117    digitalWrite(r2, HIGH);
118    delay(2);
119}
120}
121void backward()
122{
123  digitalWrite(l1, LOW);
124  digitalWrite(l2,   HIGH);
125  digitalWrite(r2, LOW);
126  if(b%c == 0){
127    digitalWrite(r1, HIGH);
128   }else{
129    digitalWrite(r1, LOW);
130   }
131   
132
133    if(b > 1000 ) {
134      b=0;
135   }
136}
137void right()
138{
139  digitalWrite(l1, HIGH);
140  digitalWrite(r1,   LOW);
141  digitalWrite(l2, LOW);
142  analogWrite(r2, 100);
143 
144    
145
146}
147void   left()
148{
149   analogWrite(l1, 100);
150  digitalWrite(r1, LOW);
151  digitalWrite(l2,   LOW);
152  digitalWrite(r2, HIGH);
153}
154
155void stopCar() {
156    digitalWrite(l1,   LOW);
157
158  digitalWrite(r1, LOW);
159  digitalWrite(l2, LOW);
160  digitalWrite(r2,   LOW);
161}
162void rotr() {
163
164  digitalWrite(l1, HIGH);
165  digitalWrite(r1,   LOW);
166  digitalWrite(l2, HIGH);
167  digitalWrite(r2, LOW);
168}
169void rotl(){
170
171   digitalWrite(l1, LOW);
172  digitalWrite(r1, HIGH);
173  digitalWrite(l2, LOW);
174   digitalWrite(r2, HIGH);
175}
176

#include <SoftwareSerial.h>

SoftwareSerial BTSerial(10, 11); //   CONNECT BT RX PIN TO ARDUINO 12 PIN | CONNECT BT TX PIN TO ARDUINO 11 PIN
#include<SPI.h>

   
int trigPin = 12;    // Trig
int echoPin = 13;    // Echo
long duration,   cm, inches;
int direct;
int l1=3;
int r1=5;
int l2=6;
int r2=9;

int   forword_corr;


int a = 0 ;
int s = 3; 


int b = 0 ;
int   c = 2; 





void setup() {
  Serial.begin(9600);      
   BTSerial.begin(9600);    
  
 
  pinMode(l1, OUTPUT);
  pinMode(r1,   OUTPUT);
  pinMode(l2, OUTPUT);
  pinMode(r2, OUTPUT);
  
  digitalWrite(l1,   LOW);
  digitalWrite(r1, LOW);
  digitalWrite(l2, LOW);
  digitalWrite(r2,   LOW);
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void   loop() {
  
   digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
   digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin,   LOW);

  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

   cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  Serial.print(cm);
   Serial.print("cm");
  Serial.println();
  if(BTSerial.available()>0){   // Witing for data incoming from the other XBee module
   direct=BTSerial.read();
     
                if(direct == 'B'){
            Serial.println("Backward");
             backward();
          }else if (direct=='F'){
              Serial.println("Forward");
             forward();
          }else if(direct == 'R'){
            Serial.println("Right");
             left();
          }else if(direct == 'L'){
            Serial.println("left");
             right();

           }else if(direct == 'r'){
            Serial.println("rotright");
             rotr();
            
          }else if(direct == 'l'){
            Serial.println("rotleft");
             rotl();
            }else if  (direct == 'S'){
            Serial.println("Stop");
             stopCar();
          }
                
     
           
      
  }
}
    
  


 
void forward()
{
  if(cm<=15){
      digitalWrite(l1, LOW);
      digitalWrite(r2, LOW);

    }
    else{
     digitalWrite(l1, HIGH);
  digitalWrite(r1, LOW);
  digitalWrite(l2, LOW);
   
  
    digitalWrite(r2, LOW);
 delay(2);
    digitalWrite(r2, HIGH);
    delay(2);
}
}
void backward()
{
  digitalWrite(l1, LOW);
  digitalWrite(l2,   HIGH);
  digitalWrite(r2, LOW);
  if(b%c == 0){
    digitalWrite(r1, HIGH);
   }else{
    digitalWrite(r1, LOW);
   }
   

    if(b > 1000 ) {
      b=0;
   }
}
void right()
{
  digitalWrite(l1, HIGH);
  digitalWrite(r1,   LOW);
  digitalWrite(l2, LOW);
  analogWrite(r2, 100);
 
    

}
void   left()
{
   analogWrite(l1, 100);
  digitalWrite(r1, LOW);
  digitalWrite(l2,   LOW);
  digitalWrite(r2, HIGH);
}

void stopCar() {
    digitalWrite(l1,   LOW);

  digitalWrite(r1, LOW);
  digitalWrite(l2, LOW);
  digitalWrite(r2,   LOW);
}
void rotr() {

  digitalWrite(l1, HIGH);
  digitalWrite(r1,   LOW);
  digitalWrite(l2, HIGH);
  digitalWrite(r2, LOW);
}
void rotl(){

   digitalWrite(l1, LOW);
  digitalWrite(r1, HIGH);
  digitalWrite(l2, LOW);
   digitalWrite(r2, HIGH);
}

Transmitter code

arduino

Upload it to the transmission unit.

1/*
2   Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
3   by Dejan, https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/
4*/
5
6#include  <Wire.h>
7#include <SoftwareSerial.h>
8SoftwareSerial BTSerial(10, 11); // CONNECT  BT RX PIN TO ARDUINO 11 PIN | CONNECT BT TX PIN TO ARDUINO 10 PIN
9
10
11const  int MPU_ADDRESS = 0x68; // MPU6050 I2C address
12
13float AccX, AccY, AccZ;
14float  GyroX, GyroY, GyroZ;
15
16float accAngleX, accAngleY, gyroAngleX, gyroAngleY,  gyroAngleZ;
17float roll, pitch, yaw;
18
19float AccErrorX, AccErrorY, GyroErrorX,  GyroErrorY, GyroErrorZ;
20
21float elapsedTime, currentTime, previousTime;
22
23int  c = 0;
24
25
26void setup()
27{
28  
29  Serial.begin(9600);
30  BTSerial.begin(9600);  // HC-05 default speed in AT command more
31
32  Wire.begin();                      //  Initialize comunication
33  Wire.beginTransmission(MPU_ADDRESS);       // Start  communication with MPU6050 // MPU=0x68
34  Wire.write(0x6B);                  //  Talk to the register 6B
35  Wire.write(0x00);                  // Make reset -  place a 0 into the 6B register
36  Wire.endTransmission(true);        //end the  transmission
37  
38  
39  // Configure Accelerometer Sensitivity - Full Scale  Range (default +/- 2g)
40/*  Wire.beginTransmission(MPU);
41  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
42  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
43  Wire.endTransmission(true);
44  
45  // Configure Gyro Sensitivity - Full Scale  Range (default +/- 250deg/s)
46  Wire.beginTransmission(MPU);
47  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
48  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
49  Wire.endTransmission(true);
50  delay(20);
51  
52  
53  //Call this function  if you need to get the IMU error values for your module*/
54 //while(true) calculate_IMU_error();
55  
56 // delay(20);
57  
58}
59
60void loop()
61{
62  
63  MPU_read_accel_data();
64  MPU_read_gyro_data();
65
66 
67  // calculate time elapsed since last time  we were here 
68  previousTime = currentTime;        // Previous time is stored  before the actual time read
69  currentTime = millis();            // Current time  actual time read
70  elapsedTime = (currentTime - previousTime) / 1000; // Divide  by 1000 to get seconds
71
72
73  // Complementary filter - combine acceleromter  and gyro angle values
74  roll  = 0.92 * (roll  + (GyroX * elapsedTime)) + 0.08  * accAngleX;
75  pitch = 0.92 * (pitch + (GyroY * elapsedTime)) + 0.08 * accAngleY;
76  //yaw   = gyroAngleZ;
77
78
79  // Currently the raw values are in degrees  per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
80  gyroAngleX += GyroX * elapsedTime; // deg/s * s = deg
81  gyroAngleY += GyroY  * elapsedTime;
82  gyroAngleZ += GyroZ * elapsedTime;
83  
84  /*by me */
85  // Print the values on the serial monitor
86    Serial.print("roll:");
87    Serial.print(roll);
88    Serial.print("    ");
89
90    Serial.print("pitch:");
91    Serial.println(pitch);
92    
93   /* Serial.print("    ");
94    Serial.print("yaw:");
95    Serial.println(yaw);*/
96  
97  
98
99
100  /*Serial.print(" ");
101/*  made by me
102  // Print the values on the serial monitor
103  Serial.print(pitch);
104  Serial.print(" ");
105  Serial.print(accAngleY);
106  Serial.print(" ");
107  Serial.println(gyroAngleY);
108*/
109if(pitch<-17){
110      BTSerial.write('F');
111    }else if(pitch>20){
112      BTSerial.write('B');
113    }else if(roll>30){
114      BTSerial.write('R');
115    }else if(roll<-30){
116      BTSerial.write('L');
117    }else if(yaw>30){
118      BTSerial.write('r');
119    }else if(yaw<-30){
120      BTSerial.write('l');
121    }else 
122      BTSerial.write('S');
123}
124
125void  MPU_read_accel_data()
126{
127  // === Read acceleromter data === //
128  Wire.beginTransmission(MPU_ADDRESS);
129  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
130  Wire.endTransmission(false);
131  Wire.requestFrom(MPU_ADDRESS, 6, true); // Read 6 registers total, each axis value  is stored in 2 registers
132  
133  //For a range of +-2g, we need to divide the  raw values by 16384, according to the datasheet
134  AccX = (Wire.read() << 8 |  Wire.read()) / 16384.0; // X-axis value
135  AccY = (Wire.read() << 8 | Wire.read())  / 16384.0; // Y-axis value
136  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;  // Z-axis value
137  
138  // Calculating Roll and Pitch from the accelerometer  data
139  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI)      - (-0.40);  // AccErrorX ~(0.58) See the calculate_IMU_error()custom function  for more details
140  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ,  2))) * 180 / PI) - (-3.75); // AccErrorY ~(-1.58)
141  
142}
143
144
145void MPU_read_gyro_data()
146{
147  // === Read gyroscope data === //
148  Wire.beginTransmission(MPU_ADDRESS);
149  Wire.write(0x43); // Gyro data first register address 0x43
150  Wire.endTransmission(false);
151  Wire.requestFrom(MPU_ADDRESS, 6, true); // Read 4 registers total, each axis value  is stored in 2 registers
152
153  // For a 250deg/s range we have to divide first  the raw value by 131.0, according to the datasheet
154  GyroX = (Wire.read() <<  8 | Wire.read()) / 131.0;
155  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
156  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
157  
158  // Correct the outputs  with the calculated error values
159  GyroX = GyroX - (-0.68); // GyroErrorX = ~  (-2.12) 
160  GyroY = GyroY - (-2.48);  // GyroErrorY = ~ (4.12)
161  GyroZ = GyroZ  - (-0.12);  // GyroErrorZ = ~ (1.20)
162
163
164}
165
166
167void calculate_IMU_error()
168{
169  
170  // We can call this funtion in the setup section to calculate the accelerometer  and gyro data error. From here we will get the error values used in the above equations  printed on the Serial Monitor.
171  // Note that we should place the IMU flat in  order to get the proper values, so that we then can the correct values
172  // Read  accelerometer values 200 times
173  while (c < 200)
174  {
175    Wire.beginTransmission(MPU_ADDRESS);
176    Wire.write(0x3B);
177    Wire.endTransmission(false);
178    Wire.requestFrom(MPU_ADDRESS,  6, true);
179    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
180    AccY  = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
181    AccZ = (Wire.read() << 8 |  Wire.read()) / 16384.0 ;
182    // Sum all readings
183    AccErrorX = AccErrorX  + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
184    AccErrorY  = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180  / PI));
185    c++;
186  }
187  
188  //Divide the sum by 200 to get the error value
189  AccErrorX = AccErrorX / 200;
190  AccErrorY = AccErrorY / 200;
191  
192  c =  0;
193  
194  // Read gyro values 200 times
195  while (c < 200)
196  {
197    Wire.beginTransmission(MPU_ADDRESS);
198    Wire.write(0x43);
199    Wire.endTransmission(false);
200    Wire.requestFrom(MPU_ADDRESS,  6, true);
201    
202    GyroX = Wire.read() << 8 | Wire.read();
203    GyroY =  Wire.read() << 8 | Wire.read();
204    GyroZ = Wire.read() << 8 | Wire.read();
205    
206    // Sum all readings
207    GyroErrorX = GyroErrorX + (GyroX / 131.0);
208    GyroErrorY = GyroErrorY + (GyroY / 131.0);
209    GyroErrorZ = GyroErrorZ +  (GyroZ / 131.0);
210    
211    c++;
212    
213  }
214  
215  //Divide the sum  by 200 to get the error value
216  GyroErrorX = GyroErrorX / 200;
217  GyroErrorY  = GyroErrorY / 200;
218  GyroErrorZ = GyroErrorZ / 200;
219  
220  // Print the  error values on the Serial Monitor
221  
222  Serial.print("AccErrorX: ");
223  Serial.print(AccErrorX);
224  Serial.print(" | AccErrorY: ");
225  Serial.print(AccErrorY);
226  Serial.print(" | GyroErrorX: ");
227  Serial.print(GyroErrorX);
228  Serial.print("  | GyroErrorY: ");
229  Serial.print(GyroErrorY);
230  Serial.print(" | GyroErrorZ:  ");
231  Serial.println(GyroErrorZ);
232  
233
234  delay(1000);
235}
236

/*
   Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
   by Dejan, https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/
*/

#include  <Wire.h>
#include <SoftwareSerial.h>
SoftwareSerial BTSerial(10, 11); // CONNECT  BT RX PIN TO ARDUINO 11 PIN | CONNECT BT TX PIN TO ARDUINO 10 PIN


const  int MPU_ADDRESS = 0x68; // MPU6050 I2C address

float AccX, AccY, AccZ;
float  GyroX, GyroY, GyroZ;

float accAngleX, accAngleY, gyroAngleX, gyroAngleY,  gyroAngleZ;
float roll, pitch, yaw;

float AccErrorX, AccErrorY, GyroErrorX,  GyroErrorY, GyroErrorZ;

float elapsedTime, currentTime, previousTime;

int  c = 0;


void setup()
{
  
  Serial.begin(9600);
  BTSerial.begin(9600);  // HC-05 default speed in AT command more

  Wire.begin();                      //  Initialize comunication
  Wire.beginTransmission(MPU_ADDRESS);       // Start  communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  //  Talk to the register 6B
  Wire.write(0x00);                  // Make reset -  place a 0 into the 6B register
  Wire.endTransmission(true);        //end the  transmission
  
  
  // Configure Accelerometer Sensitivity - Full Scale  Range (default +/- 2g)
/*  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  
  // Configure Gyro Sensitivity - Full Scale  Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  delay(20);
  
  
  //Call this function  if you need to get the IMU error values for your module*/
 //while(true) calculate_IMU_error();
  
 // delay(20);
  
}

void loop()
{
  
  MPU_read_accel_data();
  MPU_read_gyro_data();

 
  // calculate time elapsed since last time  we were here 
  previousTime = currentTime;        // Previous time is stored  before the actual time read
  currentTime = millis();            // Current time  actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide  by 1000 to get seconds


  // Complementary filter - combine acceleromter  and gyro angle values
  roll  = 0.92 * (roll  + (GyroX * elapsedTime)) + 0.08  * accAngleX;
  pitch = 0.92 * (pitch + (GyroY * elapsedTime)) + 0.08 * accAngleY;
  //yaw   = gyroAngleZ;


  // Currently the raw values are in degrees  per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX += GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY += GyroY  * elapsedTime;
  gyroAngleZ += GyroZ * elapsedTime;
  
  /*by me */
  // Print the values on the serial monitor
    Serial.print("roll:");
    Serial.print(roll);
    Serial.print("    ");

    Serial.print("pitch:");
    Serial.println(pitch);
    
   /* Serial.print("    ");
    Serial.print("yaw:");
    Serial.println(yaw);*/
  
  


  /*Serial.print(" ");
/*  made by me
  // Print the values on the serial monitor
  Serial.print(pitch);
  Serial.print(" ");
  Serial.print(accAngleY);
  Serial.print(" ");
  Serial.println(gyroAngleY);
*/
if(pitch<-17){
      BTSerial.write('F');
    }else if(pitch>20){
      BTSerial.write('B');
    }else if(roll>30){
      BTSerial.write('R');
    }else if(roll<-30){
      BTSerial.write('L');
    }else if(yaw>30){
      BTSerial.write('r');
    }else if(yaw<-30){
      BTSerial.write('l');
    }else 
      BTSerial.write('S');
}

void  MPU_read_accel_data()
{
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDRESS, 6, true); // Read 6 registers total, each axis value  is stored in 2 registers
  
  //For a range of +-2g, we need to divide the  raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 |  Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read())  / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;  // Z-axis value
  
  // Calculating Roll and Pitch from the accelerometer  data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI)      - (-0.40);  // AccErrorX ~(0.58) See the calculate_IMU_error()custom function  for more details
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ,  2))) * 180 / PI) - (-3.75); // AccErrorY ~(-1.58)
  
}


void MPU_read_gyro_data()
{
  // === Read gyroscope data === //
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDRESS, 6, true); // Read 4 registers total, each axis value  is stored in 2 registers

  // For a 250deg/s range we have to divide first  the raw value by 131.0, according to the datasheet
  GyroX = (Wire.read() <<  8 | Wire.read()) / 131.0;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
  
  // Correct the outputs  with the calculated error values
  GyroX = GyroX - (-0.68); // GyroErrorX = ~  (-2.12) 
  GyroY = GyroY - (-2.48);  // GyroErrorY = ~ (4.12)
  GyroZ = GyroZ  - (-0.12);  // GyroErrorZ = ~ (1.20)


}


void calculate_IMU_error()
{
  
  // We can call this funtion in the setup section to calculate the accelerometer  and gyro data error. From here we will get the error values used in the above equations  printed on the Serial Monitor.
  // Note that we should place the IMU flat in  order to get the proper values, so that we then can the correct values
  // Read  accelerometer values 200 times
  while (c < 200)
  {
    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_ADDRESS,  6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY  = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 |  Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX  + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY  = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180  / PI));
    c++;
  }
  
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  
  c =  0;
  
  // Read gyro values 200 times
  while (c < 200)
  {
    Wire.beginTransmission(MPU_ADDRESS);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_ADDRESS,  6, true);
    
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY =  Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ +  (GyroZ / 131.0);
    
    c++;
    
  }
  
  //Divide the sum  by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY  = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  
  // Print the  error values on the Serial Monitor
  
  Serial.print("AccErrorX: ");
  Serial.print(AccErrorX);
  Serial.print(" | AccErrorY: ");
  Serial.print(AccErrorY);
  Serial.print(" | GyroErrorX: ");
  Serial.print(GyroErrorX);
  Serial.print("  | GyroErrorY: ");
  Serial.print(GyroErrorY);
  Serial.print(" | GyroErrorZ:  ");
  Serial.println(GyroErrorZ);
  

  delay(1000);
}

Downloadable files

Interfacing of L293D with Arduino

Comments

Only logged in users can leave comments

abdelkader_ch

0 Followers

•

0 Projects

Intro

Components and supplies

Tools and machines

Apps and platforms

Project description

Code

Downloadable files