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?
User | Revision | Line number | New 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 | } |