We have seen how we can extract the data from IMU. Now its time to put that data to work. In this chapter we will be controlling our robotic vehicle just with the tilt of our hand. So in essence it will be a gesture-controlled robotic vehicle. Now to do so, lets go ahead and connect the Raspberry Pi as follows:
Make sure you attach a sufficiently long wire for the sensor, do not exceed 1 meter at any point and use it as a remote control for your vehicle. Once connected upload the following code:
import smbus
from time import sleep
import RPi.GPIO as GPIO
int1 = 12
int2 = 16
int3 = 18
int4 = 15
GPIO.setup(int1, GPIO.OUT)
GPIO.setup(int2, GPIO.OUT)
GPIO.setup(int3, GPIO.OUT)
GPIO.setup(int4, GPIO.OUT)
PWM1 = GPIO.PWM(12, 100)
PWM2 = GPIO.PWM(16, 100)
PWM3 = GPIO.PWM(18, 100)
PWM4 = GPIO.PWM(15, 100)
PWM1.start(0)
PWM2.start(0)
PWM3.start(0)
PWM4.start(0)
PWR_MGMT_1 = 0x6B
SMPLRT_DIV = 0x19
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
INT_ENABLE = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H = 0x43
GYRO_YOUT_H = 0x45
GYRO_ZOUT_H = 0x47
def MPU_Init():
bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
bus.write_byte_data(Device_Address, CONFIG, 0)
bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
bus.write_byte_data(Device_Address, INT_ENABLE, 1)
def read_raw_data(addr):
high = bus.read_byte_data(Device_Address, addr)
low = bus.read_byte_data(Device_Address, addr+1)
value = ((high << 8) | low)
if(value > 32768):
value = value - 65536
return value
bus = smbus.SMBus(1)
Device_Address = 0x68
MPU_Init()
while True:
acc_x = read_raw_data(ACCEL_XOUT_H)
acc_y = read_raw_data(ACCEL_YOUT_H)
acc_z = read_raw_data(ACCEL_ZOUT_H)
gyro_x = read_raw_data(GYRO_XOUT_H)
gyro_y = read_raw_data(GYRO_YOUT_H)
gyro_z = read_raw_data(GYRO_ZOUT_H)
Ax = (gyro_x/327)
Ay = (gyro_y/327)
for Ax > 20:
PWM1.changeDutyCycle(Ax)
PWM3.changeDutyCycle(Ax)
for Ax < -20:
PWM2.changeDutyCycle(Ax)
PWM4.changeDutyCycle(Ax)
for Ay > 20:
PWM1.changeDutyCycle(Ax)
PWM4.changeDutyCycle(Ax)
for Ay < -20:
PWM2.changeDutyCycle(Ax)
PWM3.changeDutyCycle(Ax)
As you would have seen the code is almost the same till the time we reach the while True loop. Thereafter we have done a small trick. So let's see what it is.
Ax = (gyro_x/327)
Ay = (gyro_y/327)
When the accelerometer is in the middle or in the flat lying position the value would be close to 32768, which would be the central reading. Hence to find out what percentage we have tilted front or back we are using this line. To do that we are dividing it by 327. What it does is, it gives a reading in between 0 - 100. For example, if raw reading is gryo_x = 21000, gyro_x/327 = 64.22. Now 64.22 would be reading of tilt in the percentage value. This is important step for us as, this reading will help us determine the DutyCycle that we have to provide to the motors drivers.
for Ax < -20:
PWM2.changeDutyCycle(Ax)
PWM4.changeDutyCycle(Ax)
This step is very simple, what we have done is simply taken a threshold for the readings beyond which the PWM would be provided to the motors. The DutyCycle for the PWM being provided to the motors would be directly proportional to the angle of tilt.
for Ay < -20:
PWM2.changeDutyCycle(Ax)
PWM3.changeDutyCycle(Ax)
Just like we did before, in case the tilt is in another direction, then the motor drivers pins corresponding to the rear direction would be made high making it go backwards. The speed as before would be proportional to the tilt.
for Ay > 20:
PWM1.changeDutyCycle(Ax)
PWM4.changeDutyCycle(Ax)
In this line, we are making the robotic vehicle turn in one direction. As before, due to the percentage value of tilt the DutyCycle will change and hence change the rate of turn. As you can see the robot's wheels will turn opposite to each other, hence the turn will be made while being on its axis.
Go ahead, see how you can control it just by your hand gestures.