NYP_Auto-Balancing_Robot_FYP_2018
Dependencies: C12832 MPU6050 mbed
main.cpp
- Committer:
- gemsdare
- Date:
- 2018-08-22
- Revision:
- 0:9dc7ab9daa11
File content as of revision 0:9dc7ab9daa11:
#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); } }