USNA ES456 Autonomous Vehicles / Mbed 2 deprecated MadPulse_Controller_ros

Madpulse ROS Code

Dependencies:   mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn

main.cpp@2:899128d20215, 2016-08-17 (annotated)

Committer:
jdawkins
Date:
Wed Aug 17 20:31:42 2016 +0000
Revision:
2:899128d20215
Parent:
0:42d1dda7d9c0
Child:
3:82e223a4a4e4
Commit Lab Firmware

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 2:899128d20215 22 DigitalOut imu_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 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 2:899128d20215 71 imu.set_angle_units(RADIANS);
jdawkins 2:899128d20215 72 imu.set_mapping(2);
jdawkins 2:899128d20215 73
jdawkins 2:899128d20215 74
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 2:899128d20215 79 imu_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 2:899128d20215 84 imu_LED = !imu_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 2:899128d20215 90 pc.printf("ES456 Vehicle Sensor Logger\r\n");
jdawkins 0:42d1dda7d9c0 91 while(1){
jdawkins 0:42d1dda7d9c0 92
jdawkins 0:42d1dda7d9c0 93 imu.get_angles();
jdawkins 0:42d1dda7d9c0 94 imu.get_accel();
jdawkins 0:42d1dda7d9c0 95 imu.get_gyro();
jdawkins 0:42d1dda7d9c0 96 imu.get_mag();
jdawkins 0:42d1dda7d9c0 97 if(t.read()-t_imu > (1/LOG_RATE)) {
jdawkins 0:42d1dda7d9c0 98
jdawkins 0:42d1dda7d9c0 99 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 100 pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 0:42d1dda7d9c0 101
jdawkins 0:42d1dda7d9c0 102 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 103 xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 0:42d1dda7d9c0 104
jdawkins 0:42d1dda7d9c0 105 t_imu = t.read();
jdawkins 0:42d1dda7d9c0 106 } // if t.read
jdawkins 0:42d1dda7d9c0 107 wait(1/LOOP_RATE);
jdawkins 0:42d1dda7d9c0 108 status_LED=!status_LED;
jdawkins 0:42d1dda7d9c0 109 } // while (1)
jdawkins 0:42d1dda7d9c0 110
jdawkins 0:42d1dda7d9c0 111 } // main
jdawkins 0:42d1dda7d9c0 112
jdawkins 0:42d1dda7d9c0 113
jdawkins 0:42d1dda7d9c0 114