Project:

Materials

1-Chassis:

2- Motor driver:

3-Battery:

4-IMU sensor:

5-Arduino UNO:

6-two motor:

1- Chassis: For create the main structure of the robot i used wood chassis with
some nuts and screws, two wheels with two motors, one battery socket, one
caster wheel, and even 2 little wheels for encoders.
2- Motor driver: Description: This dual bidirectional motor driver is based on the
very popular L298 Dual H-Bridge Motor Driver Integrated Circuit. The circuit will
allow you to easily and independently control two motors of up to 2A each in both
directions.

It is ideal for robotic applications and well suited for connection to a microcontroller
requiring just a couple of control lines per motor. It can also be interfaced with
simple manual switches, TTL logic gates, relays, etc.

BMU sensor:
Code: #include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
#define M1rpwm 11

#define M2rpwm 4

#define M1lpwm 10

#define M2lpwm 5

#define ena 9

#define enb 3

struct AngleGyroData {

float angle;

int16_t gyro;

};

AngleGyroData _angleData = {0, 0};

float _initAngle;

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)

// AD0 high = 0x69 This is the ONLY way I can make my cheap banggood.com board
work, wile also connectiong AD0 to VCC

MPU6050 mpu(0x68);

/* Connect VCC, GND, SDA, SCL and the MPU-6050's INT pin to Arduino's external
interrupt #0.

On the Arduino Uno and Mega 2560, this is digital I/O pin 2. */
// MPU control/status vars

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorFloat gravity; // [x, y, z] gravity vector

float yawPitchRoll[3]; // [z, y, x] yaw/pitch/roll container

int16_t gyros[3]; // [x, y, z] gyros container

bool mpu_setup() {

Wire.begin(); // join I2C bus (I2Cdev library doesn't do this automatically)

Serial.print(F("Init I2C "));

mpu.initialize();

Serial.print(F("ID")); Serial.println(mpu.getDeviceID());

// verify connection

Serial.println(F("Test conns"));

Serial.println(mpu.testConnection() ? F("Conn success") : F("Conn failed"));

// load and configure the DMP

Serial.println(F("Init DMP"));

uint8_t devStatus = mpu.dmpInitialize();

 // 0 = success, !0 = error

if (devStatus == 0) {

Serial.println(F("Enable DMP"));

mpu.setDMPEnabled(true);

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("Set INTrrpts"));

return true;

} else {

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed (if it's going to break, usually the code
will be 1)

Serial.print(F("DMP ERR ")); Serial.println(devStatus);

return false;

int8_t mpu_getData(AngleGyroData* data){

uint8_t mpuIntStatus = mpu.getIntStatus();

 // get current FIFO count (all bytes currently in the FIFO)

uint16_t fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & 0x10) || fifoCount >= 1024) {

mpu.resetFIFO(); // reset so we can continue cleanly

return -1; //FIFO overflow

// otherwise, check for DMP data ready interrupt (this should happen frequently)

} else if (mpuIntStatus & 0x02) {

if (fifoCount >= packetSize) {

// read 1st packet from FIFO, don't bother with the rest

mpu.getFIFOBytes(fifoBuffer, packetSize);

mpu.resetFIFO(); // reset so we can continue cleanly

// in case there were plenty of packets in the FIFO, only transform the last one,
to avoid wasting CPU for nothing

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(yawPitchRoll, &q, &gravity);

mpu.dmpGetGyro(gyros, fifoBuffer);

// all we care is the Y axis data

data->angle = yawPitchRoll[2];

data->gyro = gyros[2];
 return 0; // all good

}else {

return -2; //Wrong Status

// angle is in radians

// gyro is in radians per sec, doesn't normally go beyond 1000

// the 2 have opposite signs

void setup(){

Serial.begin(9600);

mpu_setup();

pinMode(ena, OUTPUT);

pinMode(enb, OUTPUT);

pinMode(M1rpwm, OUTPUT);

pinMode(M1lpwm, OUTPUT);

pinMode(M2lpwm, OUTPUT);

pinMode(M2rpwm, OUTPUT);
 // configure the initial angle, which is assumed to be the "stable" position

delay(1000);

while(mpu_getData(&_angleData));//wait to get valid data

_initAngle = _angleData.angle;

long currentTime, lastTime = 0;

float integralErr = 0, INTEGRAL_ERR_MAX = 20, actualAngle, ANGLE_OFFSET =

0.035;

void loop(){

currentTime = millis();

if(currentTime - lastTime > 1){

// get the MPU data if available

int8_t res = mpu_getData(&_angleData);

// if res != 0 the data is not yet ready or there were errors, so ignore and keep
trying

if(res == 0){

actualAngle = _angleData.angle - ANGLE_OFFSET;

// 1. update the speed, apply PID algo (for the D, we don't need to do it
manually as the sensor already gives us the gyro value which is the derivative of
the angle)

int16_t speed = 10 * actualAngle + 6 * integralErr - 0.35 * _angleData.gyro;

//if(speed > -50 && speed < 50) speed = 0;

speed = constrain(speed, -150, 150);

 \

//analogWrite(MOTOR_ENABLE_PIN, abs(speed));

integralErr = constrain(integralErr + actualAngle, -INTEGRAL_ERR_MAX,

INTEGRAL_ERR_MAX);

// 2. figure out which DIRECTION to go

if(speed > 0){

digitalWrite(ena,HIGH);

digitalWrite(enb,HIGH);

analogWrite(M1rpwm, map(abs(speed), 0, 255, 240, 100));

analogWrite(M2lpwm, map(abs(speed), 0, 255, 240, 100));

digitalWrite(M1lpwm,HIGH);

digitalWrite(M2rpwm,HIGH);

Serial.println("innn");

}else{

digitalWrite(ena,HIGH);

digitalWrite(enb,HIGH);

digitalWrite(M1rpwm,HIGH);

digitalWrite(M2lpwm,HIGH);

analogWrite(M1lpwm, map(abs(speed), 0, 255, 240, 100));

analogWrite(M2rpwm, map(abs(speed), 0, 255, 240, 100));

Serial.println("outtt");

}
 //Serial.print(_angleData.angle); Serial.print(" / "); Serial.print(_angleData.gyro);
Serial.print(" @ "); Serial.println(speed);

// 3. keep track of the timings so that we do the update at regular intervals

lastTime = currentTime;