Jeremy Dawkins / Mbed 2 deprecated ES456_Labs

Initial Commit

Dependencies:   mbed

main.cpp@1:d56e1d85e6d8, 2016-07-12 (annotated)

Committer:
jdawkins
Date:
Tue Jul 12 19:16:19 2016 +0000
Revision:
1:d56e1d85e6d8
Parent:
0:42d1dda7d9c0 
Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:42d1dda7d9c0 1 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
jdawkins 0:42d1dda7d9c0 2
jdawkins 0:42d1dda7d9c0 3 #define LOG_RATE 25.0
jdawkins 0:42d1dda7d9c0 4 #define LOOP_RATE 200.0
jdawkins 0:42d1dda7d9c0 5 #define CMD_TIMEOUT 1.0
jdawkins 0:42d1dda7d9c0 6 #define GEAR_RATIO (1/2.75)
jdawkins 0:42d1dda7d9c0 7 #define PI 3.14159
jdawkins 0:42d1dda7d9c0 8 #include "mbed.h"
jdawkins 0:42d1dda7d9c0 9
jdawkins 0:42d1dda7d9c0 10 #include "BNO055.h"
jdawkins 0:42d1dda7d9c0 11
jdawkins 0:42d1dda7d9c0 12
jdawkins 0:42d1dda7d9c0 13 BNO055 imu(p9, p10);
jdawkins 0:42d1dda7d9c0 14
jdawkins 0:42d1dda7d9c0 15
jdawkins 0:42d1dda7d9c0 16 int left;
jdawkins 0:42d1dda7d9c0 17 float saturateCmd(float cmd);
jdawkins 0:42d1dda7d9c0 18 void menuFunction(Serial *port);
jdawkins 0:42d1dda7d9c0 19 DigitalOut status_LED(LED1);
jdawkins 0:42d1dda7d9c0 20 DigitalOut armed_LED(LED2);
jdawkins 0:42d1dda7d9c0 21 DigitalOut auto_LED(LED3);
jdawkins 0:42d1dda7d9c0 22 DigitalOut hall_LED(LED4);
jdawkins 0:42d1dda7d9c0 23
jdawkins 0:42d1dda7d9c0 24 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 0:42d1dda7d9c0 25 Serial xbee(p28, p27); // tx, rx for Xbee
jdawkins 0:42d1dda7d9c0 26
jdawkins 0:42d1dda7d9c0 27
jdawkins 0:42d1dda7d9c0 28
jdawkins 0:42d1dda7d9c0 29
jdawkins 0:42d1dda7d9c0 30 Timer t; // create timer instance
jdawkins 0:42d1dda7d9c0 31
jdawkins 0:42d1dda7d9c0 32 float t_imu,t_cmd,str_cmd,thr_cmd,str,thr;
jdawkins 0:42d1dda7d9c0 33 float t_hall, dt_hall,t_run,t_stop,t_log;
jdawkins 0:42d1dda7d9c0 34 bool armed, auto_ctrl;
jdawkins 0:42d1dda7d9c0 35 float wheel_spd;
jdawkins 0:42d1dda7d9c0 36 float arm_clock;
jdawkins 0:42d1dda7d9c0 37 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 0:42d1dda7d9c0 38 bool log_imu,log_bno,log_odo,log_mag = false;
jdawkins 0:42d1dda7d9c0 39
jdawkins 0:42d1dda7d9c0 40 int main()
jdawkins 0:42d1dda7d9c0 41 {
jdawkins 0:42d1dda7d9c0 42
jdawkins 0:42d1dda7d9c0 43 pc.baud(115200);
jdawkins 0:42d1dda7d9c0 44 xbee.baud(115200);
jdawkins 0:42d1dda7d9c0 45
jdawkins 0:42d1dda7d9c0 46 //imu.init_MPU_i2c();
jdawkins 0:42d1dda7d9c0 47 left = 0;
jdawkins 0:42d1dda7d9c0 48 str_cmd = 0;
jdawkins 0:42d1dda7d9c0 49 str=0;
jdawkins 0:42d1dda7d9c0 50 thr=0;
jdawkins 0:42d1dda7d9c0 51 thr_cmd = 0;
jdawkins 0:42d1dda7d9c0 52 arm_clock =0;
jdawkins 0:42d1dda7d9c0 53 str_cond = false;
jdawkins 0:42d1dda7d9c0 54 thr_cond = false;
jdawkins 0:42d1dda7d9c0 55 armed = false;
jdawkins 0:42d1dda7d9c0 56 auto_ctrl = false;
jdawkins 0:42d1dda7d9c0 57 run_ctrl = false;
jdawkins 0:42d1dda7d9c0 58 log_data = false;
jdawkins 0:42d1dda7d9c0 59
jdawkins 0:42d1dda7d9c0 60 t.start();
jdawkins 0:42d1dda7d9c0 61 t_imu = t.read();
jdawkins 0:42d1dda7d9c0 62 t_cmd = 0;
jdawkins 0:42d1dda7d9c0 63
jdawkins 0:42d1dda7d9c0 64 status_LED = 1;
jdawkins 0:42d1dda7d9c0 65
jdawkins 0:42d1dda7d9c0 66 if(imu.check()) {
jdawkins 0:42d1dda7d9c0 67 pc.printf("BNO055 connected\r\n");
jdawkins 0:42d1dda7d9c0 68 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:42d1dda7d9c0 69 imu.SetExternalCrystal(1);
jdawkins 0:42d1dda7d9c0 70 //bno.set_orientation(1);
jdawkins 0:42d1dda7d9c0 71 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 0:42d1dda7d9c0 72 //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer
jdawkins 0:42d1dda7d9c0 73 imu.set_angle_units(DEGREES);
jdawkins 0:42d1dda7d9c0 74 imu.set_mapping(1);
jdawkins 0:42d1dda7d9c0 75 } else {
jdawkins 0:42d1dda7d9c0 76 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 0:42d1dda7d9c0 77 status_LED = 1;
jdawkins 0:42d1dda7d9c0 78 armed_LED = 1;
jdawkins 0:42d1dda7d9c0 79 hall_LED = 1;
jdawkins 0:42d1dda7d9c0 80 auto_LED = 1;
jdawkins 0:42d1dda7d9c0 81 while(1) {
jdawkins 0:42d1dda7d9c0 82 status_LED = !status_LED;
jdawkins 0:42d1dda7d9c0 83 armed_LED = !armed_LED;
jdawkins 0:42d1dda7d9c0 84 hall_LED = !hall_LED;
jdawkins 0:42d1dda7d9c0 85 auto_LED = !auto_LED;
jdawkins 0:42d1dda7d9c0 86 wait(0.5);
jdawkins 0:42d1dda7d9c0 87 }
jdawkins 0:42d1dda7d9c0 88 }
jdawkins 0:42d1dda7d9c0 89
jdawkins 0:42d1dda7d9c0 90 pc.printf("ES456 Vehcile Program\r\n");
jdawkins 0:42d1dda7d9c0 91 while(1){
jdawkins 0:42d1dda7d9c0 92
jdawkins 0:42d1dda7d9c0 93
jdawkins 0:42d1dda7d9c0 94 imu.get_angles();
jdawkins 0:42d1dda7d9c0 95 imu.get_accel();
jdawkins 0:42d1dda7d9c0 96 imu.get_gyro();
jdawkins 0:42d1dda7d9c0 97 imu.get_mag();
jdawkins 0:42d1dda7d9c0 98 if(t.read()-t_imu > (1/LOG_RATE)) {
jdawkins 0:42d1dda7d9c0 99
jdawkins 0:42d1dda7d9c0 100
jdawkins 0:42d1dda7d9c0 101 // 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);
jdawkins 0:42d1dda7d9c0 102 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);
jdawkins 0:42d1dda7d9c0 103 pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 0:42d1dda7d9c0 104 pc.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
jdawkins 0:42d1dda7d9c0 105
jdawkins 0:42d1dda7d9c0 106 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);
jdawkins 0:42d1dda7d9c0 107 xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 0:42d1dda7d9c0 108 xbee.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
jdawkins 0:42d1dda7d9c0 109
jdawkins 0:42d1dda7d9c0 110 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);
jdawkins 0:42d1dda7d9c0 111
jdawkins 0:42d1dda7d9c0 112 t_imu = t.read();
jdawkins 0:42d1dda7d9c0 113 } // if t.read
jdawkins 0:42d1dda7d9c0 114 wait(1/LOOP_RATE);
jdawkins 0:42d1dda7d9c0 115 status_LED=!status_LED;
jdawkins 0:42d1dda7d9c0 116 } // while (1)
jdawkins 0:42d1dda7d9c0 117
jdawkins 0:42d1dda7d9c0 118 } // main
jdawkins 0:42d1dda7d9c0 119
jdawkins 0:42d1dda7d9c0 120
jdawkins 0:42d1dda7d9c0 121