Diff: main.cpp

Revision:

0:42d1dda7d9c0

diff -r 000000000000 -r 42d1dda7d9c0 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 12 19:03:38 2016 +0000
@@ -0,0 +1,121 @@
+//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 "BNO055.h"
+
+
+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
+
+
+
+
+Timer t; // create timer instance
+
+float t_imu,t_cmd,str_cmd,thr_cmd,str,thr;
+float t_hall, dt_hall,t_run,t_stop,t_log;
+bool armed, auto_ctrl;
+float wheel_spd;
+float arm_clock;
+bool str_cond,thr_cond,run_ctrl,log_data;
+bool log_imu,log_bno,log_odo,log_mag = false;
+
+int main()
+{
+
+    pc.baud(115200);
+    xbee.baud(115200);
+
+//imu.init_MPU_i2c();
+    left = 0;
+    str_cmd = 0;
+    str=0;
+    thr=0;
+    thr_cmd = 0;
+    arm_clock =0;
+    str_cond = false;
+    thr_cond = false;
+    armed = false;
+    auto_ctrl = false;
+    run_ctrl = false;
+    log_data = false;
+
+    t.start();
+    t_imu = t.read();
+    t_cmd = 0;
+
+    status_LED = 1;
+
+    if(imu.check()) {
+        pc.printf("BNO055 connected\r\n");
+        imu.setmode(OPERATION_MODE_CONFIG);
+        imu.SetExternalCrystal(1);
+        //bno.set_orientation(1);
+        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
+        //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
+        imu.set_angle_units(DEGREES);
+        imu.set_mapping(1);
+    } 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");
+    while(1){
+                 
+
+        imu.get_angles();
+        imu.get_accel();
+        imu.get_gyro();
+        imu.get_mag();
+         if(t.read()-t_imu > (1/LOG_RATE)) {
+
+
+           //  pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);
+             pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
+             pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
+             pc.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
+             
+             xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
+             xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
+             xbee.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);             
+             
+             xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);
+
+             t_imu = t.read();
+         } // if t.read
+        wait(1/LOOP_RATE);
+        status_LED=!status_LED;
+    } // while (1)
+
+} // main
+
+
+