Diff: main.cpp

Revision:

0:df9740ffcf1d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 16 10:31:52 2018 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include <MPU6050.h>
+#include "millis.h"
+#include "PID.h"
+#include "motor.h"
+#define pi 3.14159265358979323846
+
+Serial pc(USBTX,USBRX);
+
+Serial bluetooth(D10, D2);//Rx, Tx (according to the bt module)
+InterruptIn receiver(PC_13);//interupt for bt
+
+I2C i2c(D14, D15);//SDA, SCL
+
+PwmOut pwm1(D13), pwm2(D12);//motor PWM for motor1 and 2 respectively 
+DigitalOut dir1(D7), dir2(D6), dir3(D5), dir4(D4);//motor inputs (dir1 and 2 for motor 1, dir and 4 for motor 2)
+
+MPU6050 mpu;//mpu library
+PID pid;//PID lib
+MOTOR m;//motor lib
+
+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
+int counter = 0, flag = 0;//init counter
+char oneo[2];//char that stores bt command
+
+void bt()
+{
+    oneo[0] = bluetooth.getc();//read char sent to bt module
+    flag = 1;//set interrupt flag to 1 
+}
+
+int main() {
+    mpu.init();//initialize the mpu
+    mpu.whoAmI();//check mpu connection
+    pwm1.period(0.001);//period for motor 1
+    pwm2.period(0.001);//period for motor 2
+    bluetooth.baud(9600);//bt baud rate (same as pc)
+    pc.baud(9600);
+    bluetooth.attach(&bt);//attach the interrupt 
+    for(int c = 0; c<10; c++)//get the initial position of the mpu
+    {
+        mpu.gyro(gyro_val);//read gyro
+        mpu.acc(acc_val);//read accelerometer
+        offset += gyro_val[0];//add gyro value to "offset" to find gyro offset
+        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
+        x = (acos(acc_val[0]/r))*180/pi;//find the roll on x axis
+        sp += x;//find the starting position of the accelerometer
+        if(gyro_val[0] == 0)//test in case the mpu haven't finished initializing
+        {c--;}
+        //pc.printf("test");
+    } 
+    offset = offset/10;//average the offset and starting position
+    sp = sp/10;
+    //pc.printf("%f \n\r", sp);
+    millisStart();//start counter that counts every millisecond 
+    while(1)
+    {
+     if (flag == 0)//if no intterrupt
+          {
+            n_time = millis();//get the time
+            times = (n_time - o_time)*0.001;//how many second(s) since the code is run
+            mpu.filtered(old_p, data, times, offset, sp);//get filtered data
+            old_p = data[0];//store old data (for gyro)
+            pid.control(data[0], out, times, integral);//use the pid
+            m.balance(out[0]);//put the pid output as motor speed (-1 to 1 scale)
+            o_time = n_time;//store the time
+          }
+          else if(flag == 1)//if interrupt flag is triggered
+         {
+             if(oneo[0] == 'w')//if w char is sent
+             {m.forward();}//go forward
+             else if(oneo[0] == 's')
+             {m.backward();}
+             else if(oneo[0] == 'a')
+             {m.left();}
+             else if(oneo[0] == 'd')
+             {m.right();}             
+             flag = 0;//set intterupt flag to 0
+             wait(0.3);//let motor run for 0.3 sec
+         }
+    }
+}
\ No newline at end of file