Jeremy Dawkins
/
ES456_Labs
Initial Commit
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 20 2022 08:19:56 by 1.7.2