Arduino Code Self-Balancing Robot
Arduino Code Self-Balancing Robot
Arduino Code Self-Balancing Robot
com/TARUNKUMARDAHAKE
//facebook.com/TARUNKUMARDAHAKE
#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#endif
#include <NewPing.h>
//enable pin
//Motor pin
int rightpunchH = 3;
int distance=0;
#define MIN_ABS_SPEED 20
MPU6050 mpu;
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
// orientation/motion vars
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
//PID
double Kp = 76;
double Kd = 5 ;
double Ki = 1.5;
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
mpuInterrupt = true;
void setup()
Wire.begin();
Fastwire::setup(400, true);
#endif
Serial.begin(9600);
pinMode(enleft, OUTPUT);
pinMode(enright, OUTPUT);
pinMode(rightpunchH, OUTPUT);
pinMode(rightpunchL, OUTPUT);
pinMode(leftpunchH, OUTPUT);
pinMode(leftpunchH, OUTPUT);
mpu.initialize();
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
if (devStatus == 0)
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
else
// ERROR!
Serial.print(devStatus);
Serial.println(F(")"));
void nopunch()
{analogWrite(enright, 0);
analogWrite(enleft, 0);
digitalWrite(rightpunchH,0);
digitalWrite(rightpunchL,0);
digitalWrite(leftpunchH,0);
digitalWrite(leftpunchL,0);
}
void combo()
movement();
distance = sonar.ping_cm();
if (distance<=20)
{ analogWrite(enright, 80);
analogWrite(enleft, 100);
digitalWrite(rightpunchH,1);
digitalWrite(rightpunchL,0);
digitalWrite(leftpunchH,1);
digitalWrite(leftpunchL,0);
if (distance<=10)
{ analogWrite(enright, 100);
analogWrite(enleft, 130);
digitalWrite(rightpunchH,1);
digitalWrite(rightpunchL,0);
digitalWrite(leftpunchH,1);
digitalWrite(leftpunchL,0);
else
nopunch();
void movement()
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
{
// wait for correct available data length, should be a VERY short wait
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
void loop()
{ combo();