Last active
December 15, 2017 14:11
-
-
Save Shashi18/8947a43081d59f7c43e16f8af7716335 to your computer and use it in GitHub Desktop.
Self Balancing Robot
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* Developed By: www.hellocodings.com---------------------------- | |
---Shashi Suman-------------------------------------------------- | |
---Modify as you want-------------------------------------------- | |
---------------------------------------------------------------*/ | |
#include<Wire.h> | |
const int MPU_addr=0x68; | |
double AccelX,AccelY,AccelZ,Tmp,GyroX,GyroY,GyroZ; //These will be the raw data from the MPU6050. | |
uint32_t timer; //it's a timer, saved as a big-ass unsigned int. We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop. | |
double Angle_X, Angle_Y; //These are the angles in the complementary filter | |
#define degconvert 57.29577951 //there are like 57 degrees in a radian. | |
#define m1_left 12 | |
#define m1_right 10 | |
#define m2_left 3 | |
#define m2_right 4 | |
#define Kp 110 | |
#define Kd 0.09 | |
#define Ki 90 | |
#define sampleTime 0.005 | |
#define targetAngle 0 | |
volatile int output = 0; | |
volatile float currentAngle, prevAngle=0, error, prevError=0, errorSum=0; | |
void setup() { | |
pinMode(m1_left, OUTPUT); | |
pinMode(m1_right, OUTPUT); | |
pinMode(m2_left, OUTPUT); | |
pinMode(m2_right, OUTPUT); | |
Wire.begin(); | |
#if ARDUINO >= 157 | |
Wire.setClock(400000UL); // Set I2C frequency to 400kHz | |
#else | |
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz | |
#endif | |
Wire.beginTransmission(MPU_addr); | |
Wire.write(0x6B); // PWR_MGMT_1 register | |
Wire.write(0); // set to zero (wakes up the MPU-6050) | |
Wire.endTransmission(true); | |
Serial.begin(57600); | |
delay(100); | |
//setup starting angle | |
//1) collect the data | |
Wire.beginTransmission(MPU_addr); | |
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) | |
Wire.endTransmission(false); | |
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers | |
AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) | |
AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) | |
AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) | |
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) | |
GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) | |
GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) | |
GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) | |
double roll = atan2(AccelY, AccelZ)*degconvert; | |
double pitch = atan2(-AccelX, AccelZ)*degconvert; | |
double gyroXangle = roll; | |
double gyroYangle = pitch; | |
double Angle_X = roll; | |
double Angle_Y = pitch; | |
//start a timer | |
timer = micros(); | |
} | |
void command(int L_M_Speed, int R_M_Speed) { | |
if(L_M_Speed >= 0) { | |
analogWrite(m2_left, L_M_Speed); | |
digitalWrite(m2_right, LOW); | |
} | |
else { | |
analogWrite(m2_left, 255 + L_M_Speed); | |
digitalWrite(m2_right, HIGH); | |
} | |
if(R_M_Speed >= 0) { | |
analogWrite(m1_right, R_M_Speed); | |
digitalWrite(m1_left, LOW); | |
} | |
else { | |
analogWrite(m1_right, 255 + R_M_Speed); | |
digitalWrite(m1_left, HIGH); | |
} | |
} | |
void emergency(){ | |
digitalWrite(m2_right, LOW); | |
digitalWrite(m1_right, LOW); | |
digitalWrite(m2_left, LOW); | |
digitalWrite(m1_left, LOW); | |
} | |
void loop(){ | |
//Now begins the main loop. | |
//Collect raw data from the sensor. | |
Wire.beginTransmission(MPU_addr); | |
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) | |
Wire.endTransmission(false); | |
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers | |
AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) | |
AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) | |
AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) | |
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) | |
GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) | |
GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) | |
GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) | |
double dt = (double)(micros() - timer) / 1000000; | |
timer = micros(); | |
double roll = atan2(AccelY, AccelZ)*degconvert; | |
double pitch = atan2(-AccelX, AccelZ)*degconvert; | |
double gyroXrate = GyroX/131.0; | |
double gyroYrate = GyroY/131.0; | |
Angle_X = 0.99 * (Angle_X + gyroXrate * dt) + 0.01 * roll; | |
Angle_Y = 0.99 * (Angle_Y + gyroYrate * dt) + 0.01 * pitch; | |
currentAngle = Angle_Y; | |
if(currentAngle > 50 && currentAngle < -50){ | |
emergency(); | |
} | |
else if(currentAngle >-0.3 && currentAngle <0.3){ | |
emergency(); | |
delay(50); | |
} | |
else{ | |
error = currentAngle - targetAngle; | |
errorSum = errorSum + error; | |
errorSum = constrain(errorSum, -300, 300); | |
//calculate output from P, I and D values | |
output = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime; | |
output = constrain(output, -255, 255); | |
command(output, output); | |
prevAngle = currentAngle; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment