Jasen Ong / Mbed 2 deprecated NYP_Auto-Balacing_Robot

NYP_Auto-Balancing_Robot_FYP_2018

Dependencies:   C12832 MPU6050 mbed

main.cpp@0:9dc7ab9daa11, 2018-08-22 (annotated)

Committer:
gemsdare
Date:
Wed Aug 22 11:34:26 2018 +0000
Revision:
0:9dc7ab9daa11
NYP_Auto-Balancing_Robot_FYP_2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gemsdare 0:9dc7ab9daa11 1 #include "mbed.h"
gemsdare 0:9dc7ab9daa11 2 #include "MPU6050.h"
gemsdare 0:9dc7ab9daa11 3 #include "C12832.h"
gemsdare 0:9dc7ab9daa11 4
gemsdare 0:9dc7ab9daa11 5 AnalogOut Aout(p18);
gemsdare 0:9dc7ab9daa11 6 DigitalOut myled(LED1);
gemsdare 0:9dc7ab9daa11 7 Serial pc(USBTX, USBRX);
gemsdare 0:9dc7ab9daa11 8 MPU6050 mpu;
gemsdare 0:9dc7ab9daa11 9 C12832 lcd(p5, p7, p6, p8, p11);
gemsdare 0:9dc7ab9daa11 10
gemsdare 0:9dc7ab9daa11 11 int16_t ax, ay, az;
gemsdare 0:9dc7ab9daa11 12 int16_t gx, gy, gz;
gemsdare 0:9dc7ab9daa11 13
gemsdare 0:9dc7ab9daa11 14 float k=0.5;
gemsdare 0:9dc7ab9daa11 15 float x=0;
gemsdare 0:9dc7ab9daa11 16 float xout=0,xold=0;
gemsdare 0:9dc7ab9daa11 17 float x_prev=0;
gemsdare 0:9dc7ab9daa11 18 float sum =0;
gemsdare 0:9dc7ab9daa11 19 float diff =0;
gemsdare 0:9dc7ab9daa11 20 float gain_prop = 1;
gemsdare 0:9dc7ab9daa11 21 float gain_int = 0.0066;
gemsdare 0:9dc7ab9daa11 22 float gain_diff = 0.000028;
gemsdare 0:9dc7ab9daa11 23
gemsdare 0:9dc7ab9daa11 24
gemsdare 0:9dc7ab9daa11 25 int main()
gemsdare 0:9dc7ab9daa11 26 {
gemsdare 0:9dc7ab9daa11 27 lcd.printf("MPU6050 test\n\n");
gemsdare 0:9dc7ab9daa11 28 lcd.printf("MPU6050 initialize \n");
gemsdare 0:9dc7ab9daa11 29
gemsdare 0:9dc7ab9daa11 30 mpu.initialize();
gemsdare 0:9dc7ab9daa11 31
gemsdare 0:9dc7ab9daa11 32 pc.printf("MPU6050 testConnection \n");
gemsdare 0:9dc7ab9daa11 33
gemsdare 0:9dc7ab9daa11 34 bool mpu6050TestResult = mpu.testConnection();
gemsdare 0:9dc7ab9daa11 35 if(mpu6050TestResult) {pc.printf("MPU6050 test passed \n");}
gemsdare 0:9dc7ab9daa11 36 else {pc.printf("MPU6050 test failed \n");}
gemsdare 0:9dc7ab9daa11 37 float proportional,integral=0;
gemsdare 0:9dc7ab9daa11 38 float prop=0;
gemsdare 0:9dc7ab9daa11 39 float xAcc = 0;
gemsdare 0:9dc7ab9daa11 40 unsigned char i;
gemsdare 0:9dc7ab9daa11 41 signed int Reading, xsum=0;
gemsdare 0:9dc7ab9daa11 42 lcd.cls();
gemsdare 0:9dc7ab9daa11 43 while(1)
gemsdare 0:9dc7ab9daa11 44 {
gemsdare 0:9dc7ab9daa11 45 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gemsdare 0:9dc7ab9daa11 46 //lcd.locate(0,15);
gemsdare 0:9dc7ab9daa11 47 //writing current accelerometer and gyro position
gemsdare 0:9dc7ab9daa11 48 for(i=1; i<5; i++)
gemsdare 0:9dc7ab9daa11 49 {
gemsdare 0:9dc7ab9daa11 50 Reading = ay;
gemsdare 0:9dc7ab9daa11 51 xsum += Reading;
gemsdare 0:9dc7ab9daa11 52 }
gemsdare 0:9dc7ab9daa11 53 xAcc=Reading*0.0001/4;
gemsdare 0:9dc7ab9daa11 54 //x=ay*0.0001;
gemsdare 0:9dc7ab9daa11 55 diff=gx*0.001;
gemsdare 0:9dc7ab9daa11 56 proportional = 1.0 + xAcc * 1.31;/*x*/
gemsdare 0:9dc7ab9daa11 57 prop = proportional/3.3;
gemsdare 0:9dc7ab9daa11 58 sum = sum + xAcc ;/*x*/
gemsdare 0:9dc7ab9daa11 59 integral = sum;
gemsdare 0:9dc7ab9daa11 60 xout = prop*gain_prop + integral*gain_int + diff*gain_diff;
gemsdare 0:9dc7ab9daa11 61 xout=0.5*(xold) +(1-0.5)*xout;
gemsdare 0:9dc7ab9daa11 62 Aout=xout;
gemsdare 0:9dc7ab9daa11 63 xold=xout;
gemsdare 0:9dc7ab9daa11 64
gemsdare 0:9dc7ab9daa11 65
gemsdare 0:9dc7ab9daa11 66 //lcd.printf(" Xaxis :%.5f",proportional);
gemsdare 0:9dc7ab9daa11 67
gemsdare 0:9dc7ab9daa11 68 }
gemsdare 0:9dc7ab9daa11 69 }