Calvin Kalintra / Mbed 2 deprecated balance_all

Balance

Dependencies:   MPU6050 Motor mbed millis pid

main.cpp@0:df9740ffcf1d, 2018-05-16 (annotated)

Committer:
ckalintra
Date:
Wed May 16 10:31:52 2018 +0000
Revision:
0:df9740ffcf1d
Balance_code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ckalintra 0:df9740ffcf1d 1 #include "mbed.h"
ckalintra 0:df9740ffcf1d 2 #include <MPU6050.h>
ckalintra 0:df9740ffcf1d 3 #include "millis.h"
ckalintra 0:df9740ffcf1d 4 #include "PID.h"
ckalintra 0:df9740ffcf1d 5 #include "motor.h"
ckalintra 0:df9740ffcf1d 6 #define pi 3.14159265358979323846
ckalintra 0:df9740ffcf1d 7
ckalintra 0:df9740ffcf1d 8 Serial pc(USBTX,USBRX);
ckalintra 0:df9740ffcf1d 9
ckalintra 0:df9740ffcf1d 10 Serial bluetooth(D10, D2);//Rx, Tx (according to the bt module)
ckalintra 0:df9740ffcf1d 11 InterruptIn receiver(PC_13);//interupt for bt
ckalintra 0:df9740ffcf1d 12
ckalintra 0:df9740ffcf1d 13 I2C i2c(D14, D15);//SDA, SCL
ckalintra 0:df9740ffcf1d 14
ckalintra 0:df9740ffcf1d 15 PwmOut pwm1(D13), pwm2(D12);//motor PWM for motor1 and 2 respectively
ckalintra 0:df9740ffcf1d 16 DigitalOut dir1(D7), dir2(D6), dir3(D5), dir4(D4);//motor inputs (dir1 and 2 for motor 1, dir and 4 for motor 2)
ckalintra 0:df9740ffcf1d 17
ckalintra 0:df9740ffcf1d 18 MPU6050 mpu;//mpu library
ckalintra 0:df9740ffcf1d 19 PID pid;//PID lib
ckalintra 0:df9740ffcf1d 20 MOTOR m;//motor lib
ckalintra 0:df9740ffcf1d 21
ckalintra 0:df9740ffcf1d 22 float times, n_time, o_time, offset, sp, r, x, integral, out[3], gyro_val[3], acc_val[3], data[3], old_p;//init floats
ckalintra 0:df9740ffcf1d 23 int counter = 0, flag = 0;//init counter
ckalintra 0:df9740ffcf1d 24 char oneo[2];//char that stores bt command
ckalintra 0:df9740ffcf1d 25
ckalintra 0:df9740ffcf1d 26 void bt()
ckalintra 0:df9740ffcf1d 27 {
ckalintra 0:df9740ffcf1d 28 oneo[0] = bluetooth.getc();//read char sent to bt module
ckalintra 0:df9740ffcf1d 29 flag = 1;//set interrupt flag to 1
ckalintra 0:df9740ffcf1d 30 }
ckalintra 0:df9740ffcf1d 31
ckalintra 0:df9740ffcf1d 32 int main() {
ckalintra 0:df9740ffcf1d 33 mpu.init();//initialize the mpu
ckalintra 0:df9740ffcf1d 34 mpu.whoAmI();//check mpu connection
ckalintra 0:df9740ffcf1d 35 pwm1.period(0.001);//period for motor 1
ckalintra 0:df9740ffcf1d 36 pwm2.period(0.001);//period for motor 2
ckalintra 0:df9740ffcf1d 37 bluetooth.baud(9600);//bt baud rate (same as pc)
ckalintra 0:df9740ffcf1d 38 pc.baud(9600);
ckalintra 0:df9740ffcf1d 39 bluetooth.attach(&bt);//attach the interrupt
ckalintra 0:df9740ffcf1d 40 for(int c = 0; c<10; c++)//get the initial position of the mpu
ckalintra 0:df9740ffcf1d 41 {
ckalintra 0:df9740ffcf1d 42 mpu.gyro(gyro_val);//read gyro
ckalintra 0:df9740ffcf1d 43 mpu.acc(acc_val);//read accelerometer
ckalintra 0:df9740ffcf1d 44 offset += gyro_val[0];//add gyro value to "offset" to find gyro offset
ckalintra 0:df9740ffcf1d 45 r = sqrt(acc_val[0]*acc_val[0]+acc_val[1]*acc_val[1]+acc_val[2]*acc_val[2]);//resultan force from all accelerometer axis
ckalintra 0:df9740ffcf1d 46 x = (acos(acc_val[0]/r))*180/pi;//find the roll on x axis
ckalintra 0:df9740ffcf1d 47 sp += x;//find the starting position of the accelerometer
ckalintra 0:df9740ffcf1d 48 if(gyro_val[0] == 0)//test in case the mpu haven't finished initializing
ckalintra 0:df9740ffcf1d 49 {c--;}
ckalintra 0:df9740ffcf1d 50 //pc.printf("test");
ckalintra 0:df9740ffcf1d 51 }
ckalintra 0:df9740ffcf1d 52 offset = offset/10;//average the offset and starting position
ckalintra 0:df9740ffcf1d 53 sp = sp/10;
ckalintra 0:df9740ffcf1d 54 //pc.printf("%f \n\r", sp);
ckalintra 0:df9740ffcf1d 55 millisStart();//start counter that counts every millisecond
ckalintra 0:df9740ffcf1d 56 while(1)
ckalintra 0:df9740ffcf1d 57 {
ckalintra 0:df9740ffcf1d 58 if (flag == 0)//if no intterrupt
ckalintra 0:df9740ffcf1d 59 {
ckalintra 0:df9740ffcf1d 60 n_time = millis();//get the time
ckalintra 0:df9740ffcf1d 61 times = (n_time - o_time)*0.001;//how many second(s) since the code is run
ckalintra 0:df9740ffcf1d 62 mpu.filtered(old_p, data, times, offset, sp);//get filtered data
ckalintra 0:df9740ffcf1d 63 old_p = data[0];//store old data (for gyro)
ckalintra 0:df9740ffcf1d 64 pid.control(data[0], out, times, integral);//use the pid
ckalintra 0:df9740ffcf1d 65 m.balance(out[0]);//put the pid output as motor speed (-1 to 1 scale)
ckalintra 0:df9740ffcf1d 66 o_time = n_time;//store the time
ckalintra 0:df9740ffcf1d 67 }
ckalintra 0:df9740ffcf1d 68 else if(flag == 1)//if interrupt flag is triggered
ckalintra 0:df9740ffcf1d 69 {
ckalintra 0:df9740ffcf1d 70 if(oneo[0] == 'w')//if w char is sent
ckalintra 0:df9740ffcf1d 71 {m.forward();}//go forward
ckalintra 0:df9740ffcf1d 72 else if(oneo[0] == 's')
ckalintra 0:df9740ffcf1d 73 {m.backward();}
ckalintra 0:df9740ffcf1d 74 else if(oneo[0] == 'a')
ckalintra 0:df9740ffcf1d 75 {m.left();}
ckalintra 0:df9740ffcf1d 76 else if(oneo[0] == 'd')
ckalintra 0:df9740ffcf1d 77 {m.right();}
ckalintra 0:df9740ffcf1d 78 flag = 0;//set intterupt flag to 0
ckalintra 0:df9740ffcf1d 79 wait(0.3);//let motor run for 0.3 sec
ckalintra 0:df9740ffcf1d 80 }
ckalintra 0:df9740ffcf1d 81 }
ckalintra 0:df9740ffcf1d 82 }