Diff: main.cpp

Revision:

3:ec0efa222dfa

Parent:

0:bd0546063b0a

Child:

4:ec2978ff7a1e

diff -r 0af77f74afb2 -r ec0efa222dfa main.cpp
--- a/main.cpp	Tue May 13 18:15:06 2014 +0000
+++ b/main.cpp	Tue Nov 15 14:33:49 2016 +0000
@@ -1,21 +1,287 @@
 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
 
+#define LOG_RATE 25.0
+#define LOOP_RATE 200.0
+#define CMD_TIMEOUT 1.0
+#define GEAR_RATIO (1/2.75)
+#define PI 3.14159
 #include "mbed.h"
-#include "MMA7660.h"
+
+#include "BNO055.h"
+#include "ServoIn.h"
+#include "ServoOut.h"
+
+
+//I2C    i2c(p9, p10); // SDA, SCL
+BNO055 imu(p9, p10);
+
+
+int left;
+float saturateCmd(float cmd);
+void menuFunction(Serial *port);
+DigitalOut status_LED(LED1);
+DigitalOut armed_LED(LED2);
+DigitalOut auto_LED(LED3);
+DigitalOut hall_LED(LED4);
+
+Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
+Serial xbee(p28, p27); // tx, rx for Xbee
+ServoIn CH1(p15);
+ServoIn CH2(p16);
+
+InterruptIn he_sensor(p23);
+ServoOut Steer(p22);
+ServoOut Throttle(p21);
+Timer t; // create timer instance
+Ticker log_tick;
+Ticker heartbeat;
+float t_imu,t_cmd,str_cmd,thr_cmd,str,thr,dt;
+float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop;
+float psi_err,spd_err, psi_err_i,spd_err_i, psi_est, psi_est_dot, psi_comp;
+float L;
+
+bool armed, auto_ctrl;
+float speed;
+float arm_clock;
+bool str_cond,thr_cond,run_ctrl,log_data;
+
+void he_callback()
+{
+    hall_LED = !hall_LED;
+
+    dt_hall = t.read()-t_hall;
+    t_hall = t.read();
+}
+
+int main()
+{
+
+    pc.baud(115200);
+    xbee.baud(115200);
 
-MMA7660 MMA(p28, p27);
+    he_sensor.rise(&he_callback);
+
+    left = 0;
+    str_cmd = 0;
+    str=0;
+    thr=0;
+    L =0;
+    thr_cmd = 0;
+    arm_clock =0;
+    psi_est=0;
+    psi_est_dot=0;    
+    str_cond = false;
+    thr_cond = false;
+    armed = false;
+    auto_ctrl = false;
+    run_ctrl = false;
+    log_data = false;
+
+    t.start();
+    t_log = t.read();
+    t_log_start = t.read();
+    t_cmd = 0;
+
+    status_LED = 1;
+
+    if(imu.check()) {
+        pc.printf("BNO055 connected\r\n");
+        xbee.printf("BNO055 connected\r\n");
+        imu.setmode(OPERATION_MODE_CONFIG);
+        imu.SetExternalCrystal(1);
+        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
+        imu.set_angle_units(DEGREES);
+        imu.set_accel_units(MPERSPERS);
+        imu.set_anglerate_units(DEG_PER_SEC);
+        imu.set_mapping(2);
+       
+    } else {
+        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
+        status_LED = 1;
+        armed_LED = 1;
+        hall_LED = 1;
+        auto_LED = 1;
+        while(1) {
+            status_LED = !status_LED;
+            armed_LED = !armed_LED;
+            hall_LED = !hall_LED;
+            auto_LED = !auto_LED;
+            wait(0.5);
+        }
+    }
+
+    pc.printf("ES456 Vehcile Program\r\n");
 
-DigitalOut connectionLed(LED1);
-PwmOut Zaxis_p(LED2);
-PwmOut Zaxis_n(LED3);
+    while(1) {
+        
+        //menuFunction(&pc);
+        menuFunction(&xbee);
+        
+
+        if(CH1.servoPulse == 0 || CH2.servoPulse ==0) { //RC Reciever must be connected For Car to be armed
+            armed = false;
+        } else {
+            armed = true;
+        }
+
+        str_cond = (CH1.servoPulse > 1800); // If steering is full right
+        thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero
+        //pc.printf("Cond 1: %d Cond 2: %d\r\n",str_cond,thr_cond);
+
+        // pc.printf("Timeer: %f \r\n",t.read()-arm_clock);
+        if(str_cond&thr_cond) {     // Both of the above conditions must be met
+            if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds
+                Steer.write((int)(1500.0));
+                auto_ctrl = true;   //Active auto control loop
+            }
+        } else {
+            arm_clock = t.read();
+        }
+
+        if(armed) { // Is System Armed
+            armed_LED = 1;
+
+            if(auto_ctrl) { // Armed and in Auto Control
+                auto_LED = 1; // Turn on LED to indicate Auto Control
+
+                if(run_ctrl) {
+
+                    float run_time = t.read()-t_run;
+                    if(run_time > 0 && run_time <= 1) {
+                        thr_cmd = 0.2;
+                        str_cmd = 0.0;
+                    } else if(run_time >1 && run_time< 3) {
+                        thr_cmd = 0.2;
+                        str_cmd = -1.0;
+                    } else {
+                        thr_cmd = 0;
+                        str_cmd = 0;
+                        log_data = false;
+                    }
+
+                } // End if run_ctrl
+
+                Steer.write((int)((str_cmd*500.0)+1500.0));
+                Throttle.write((int)((thr_cmd*500.0)+1500.0));
+
+                if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control
+                    auto_ctrl = false;
+                    run_ctrl = false;
+                }
+            } // End if auto_ctrl
 
-int main() {  
-    if (MMA.testConnection())
-        connectionLed = 1;
+            else { // Armed but in Manual Control
+
+                if(xbee.readable()) {
+                    t_run = t.read();
+                    log_data = !log_data;
+                }
+                str_cmd = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
+                thr_cmd = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
+                Steer.write((int)((str_cmd*500.0)+1500.0));  // Write Steering Pulse
+                Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse
+
+                auto_LED = 0;
+            } // end if autocontrol
+
+        } else {
+            armed_LED = 0;          //Turn off armed LED indicator
+            str_cmd = 0;
+            thr_cmd = 0;
+            Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
+            Throttle.write(1500);   //Set Throttle to Low
+        }/// end  else armed
+
+        if(t.read()-t_hall < 0.2) {
+            speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall);
+        } else {
+            speed = 0;
+        }
+
+
+// Read Angles
+        imu.get_angles();
+        imu.get_accel();
+        imu.get_gyro();
+        imu.get_mag();
         
-    while(1) {
-        Zaxis_p = MMA.z();
-        Zaxis_n = -MMA.z();
+        dt = t.read()-t_loop;
+        t_loop = t.read();
+        psi_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x);
+        psi_est_dot = imu.gyro.z + L*(psi_comp - psi_est);
+        
+        psi_est = psi_est + psi_est_dot*dt;
+        
+         if(t.read()-t_log > (1/LOG_RATE)) {
+
+            if(log_data) {
+              /* Data Output Options
+                 imu.accel.x imu.accel.y imu.accel.z
+                 imu.gyro.x imu.gyro.y imu.gyro.z
+                 wheel_spd thr str*/
+              xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, thr_cmd, str_cmd, imu.gyro.z);
+             // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
+             // xbee.printf("$ODO,%.3f, %.3f, %.3f\r\n",wheel_spd,thr,str);
+             }
+             t_log = t.read();
+         }
+        wait(1/LOOP_RATE);
+        status_LED=!status_LED;
     }
 
 }
+
+void menuFunction(Serial *port){
+        if(port->readable()) {
+            char c = port->getc();
+            if(c == 'H' || c == 'h') {
+                port->printf("Command List...\r\n");
+                port->printf("a - set autonomous mode\r\n");              
+                port->printf("l - toggle data logging\r\n");
+                port->printf("r - run auto control loop\r\n");
+                port->printf("h - display this prompt and exit\r\n");
+                port->printf("Enter Command\r\n");
+            }
+            if(c == 'A' || c == 'a') {
+                auto_ctrl = true;
+                Steer.write((int)(1500.0));
+            }           
+
+            if(c == 'R' || c == 'r') {
+                auto_ctrl = true;
+                Steer.write((int)(1500.0));
+                                
+                if(!log_data) {
+                    t_log_start = t.read();
+                    t_run = t.read();
+                    xbee.printf("\r\n\r\n");
+                }
+                log_data = !log_data;
+                run_ctrl = true;
+            }
+
+        }
+
+}
+float saturateCmd(float cmd)
+{
+    if(cmd>1.0) {
+        cmd = 1.0;
+    }
+    if(cmd < -1.0) {
+        cmd = -1.0;
+    }
+    return cmd;
+}
+float saturateCmd(float cmd, float max,float min)
+{
+    if(cmd>max) {
+        cmd = max;
+    }
+    if(cmd < min) {
+        cmd = min;
+    }
+    return cmd;
+}
+
+