main.cpp

00001 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
00002 
00003 #define LOG_RATE 25.0
00004 #define LOOP_RATE 200.0
00005 #define CMD_TIMEOUT 1.0
00006 #define GEAR_RATIO (1/2.75)
00007 #define PI 3.14159
00008 #include "mbed.h"
00009 
00010 #include "BNO055.h"
00011 
00012 
00013 BNO055 imu(p9, p10);
00014 
00015 
00016 int left;
00017 float saturateCmd(float cmd);
00018 void menuFunction(Serial *port);
00019 DigitalOut status_LED(LED1);
00020 DigitalOut armed_LED(LED2);
00021 DigitalOut auto_LED(LED3);
00022 DigitalOut hall_LED(LED4);
00023 
00024 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
00025 Serial xbee(p28, p27); // tx, rx for Xbee
00026 
00027 
00028 
00029 
00030 Timer t; // create timer instance
00031 
00032 float t_imu,t_cmd,str_cmd,thr_cmd,str,thr;
00033 float t_hall, dt_hall,t_run,t_stop,t_log;
00034 bool armed, auto_ctrl;
00035 float wheel_spd;
00036 float arm_clock;
00037 bool str_cond,thr_cond,run_ctrl,log_data;
00038 bool log_imu,log_bno,log_odo,log_mag = false;
00039 
00040 int main()
00041 {
00042 
00043     pc.baud(115200);
00044     xbee.baud(115200);
00045 
00046 //imu.init_MPU_i2c();
00047     left = 0;
00048     str_cmd = 0;
00049     str=0;
00050     thr=0;
00051     thr_cmd = 0;
00052     arm_clock =0;
00053     str_cond = false;
00054     thr_cond = false;
00055     armed = false;
00056     auto_ctrl = false;
00057     run_ctrl = false;
00058     log_data = false;
00059 
00060     t.start();
00061     t_imu = t.read();
00062     t_cmd = 0;
00063 
00064     status_LED = 1;
00065 
00066     if(imu.check()) {
00067         pc.printf("BNO055 connected\r\n");
00068         imu.setmode(OPERATION_MODE_CONFIG);
00069         imu.SetExternalCrystal(1);
00070         //bno.set_orientation(1);
00071         imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
00072         //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
00073         imu.set_angle_units(DEGREES);
00074         imu.set_mapping(1);
00075     } else {
00076         pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
00077         status_LED = 1;
00078         armed_LED = 1;
00079         hall_LED = 1;
00080         auto_LED = 1;
00081         while(1) {
00082             status_LED = !status_LED;
00083             armed_LED = !armed_LED;
00084             hall_LED = !hall_LED;
00085             auto_LED = !auto_LED;
00086             wait(0.5);
00087         }
00088     }
00089 
00090     pc.printf("ES456 Vehcile Program\r\n");
00091     while(1){
00092                  
00093 
00094         imu.get_angles();
00095         imu.get_accel();
00096         imu.get_gyro();
00097         imu.get_mag();
00098          if(t.read()-t_imu > (1/LOG_RATE)) {
00099 
00100 
00101            //  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);
00102              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);             
00103              pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
00104              pc.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
00105              
00106              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);             
00107              xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
00108              xbee.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);             
00109              
00110              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);
00111 
00112              t_imu = t.read();
00113          } // if t.read
00114         wait(1/LOOP_RATE);
00115         status_LED=!status_LED;
00116     } // while (1)
00117 
00118 } // main
00119 
00120 
00121