NYP_Auto-Balancing_Robot_FYP_2018
Dependencies: C12832 MPU6050 mbed
Diff: main.cpp
- Revision:
- 0:9dc7ab9daa11
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Aug 22 11:34:26 2018 +0000 @@ -0,0 +1,69 @@ +#include "mbed.h" +#include "MPU6050.h" +#include "C12832.h" + +AnalogOut Aout(p18); +DigitalOut myled(LED1); +Serial pc(USBTX, USBRX); +MPU6050 mpu; +C12832 lcd(p5, p7, p6, p8, p11); + +int16_t ax, ay, az; +int16_t gx, gy, gz; + +float k=0.5; +float x=0; +float xout=0,xold=0; +float x_prev=0; +float sum =0; +float diff =0; +float gain_prop = 1; +float gain_int = 0.0066; +float gain_diff = 0.000028; + + +int main() +{ + lcd.printf("MPU6050 test\n\n"); + lcd.printf("MPU6050 initialize \n"); + + mpu.initialize(); + + pc.printf("MPU6050 testConnection \n"); + + bool mpu6050TestResult = mpu.testConnection(); + if(mpu6050TestResult) {pc.printf("MPU6050 test passed \n");} + else {pc.printf("MPU6050 test failed \n");} + float proportional,integral=0; + float prop=0; + float xAcc = 0; + unsigned char i; + signed int Reading, xsum=0; + lcd.cls(); + while(1) + { + mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + //lcd.locate(0,15); + //writing current accelerometer and gyro position + for(i=1; i<5; i++) + { + Reading = ay; + xsum += Reading; + } + xAcc=Reading*0.0001/4; + //x=ay*0.0001; + diff=gx*0.001; + proportional = 1.0 + xAcc * 1.31;/*x*/ + prop = proportional/3.3; + sum = sum + xAcc ;/*x*/ + integral = sum; + xout = prop*gain_prop + integral*gain_int + diff*gain_diff; + xout=0.5*(xold) +(1-0.5)*xout; + Aout=xout; + xold=xout; + + + //lcd.printf(" Xaxis :%.5f",proportional); + + } +}