main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "LSM9DS1.h"
00004 
00005 
00006 ///////////////////////////////////////////////////////////////////////////////
00007 ///////////////////////////// Variable Initialization//////////////////////////
00008 ///////////////////////////////////////////////////////////////////////////////
00009 Ticker bluetooth;
00010 Serial pc(USBTX, USBRX);
00011 
00012 Mutex parametersmutex;
00013 Serial blue(p28, p27);
00014 
00015 LSM9D1 imu(p9, p10, 0xD6, 0x3C);
00016 
00017 
00018 ///////////////////////////////////////////////////////////////////////////////
00019 ///////////////////////////// Control System Variables/////////////////////////
00020 ///////////////////////////////////////////////////////////////////////////////
00021 float rp = 50;
00022 float rd = 51;
00023 float ri = 50;
00024 float desired_angle = 0;
00025 
00026 float speed = 0;
00027 
00028 float pAngle = 0;
00029 float dAngle = 0;
00030 float iAngle = 0;
00031 
00032 time_segment = 5;                        //Update the speed every 5 milliseconds
00033 
00034 
00035 
00036 ///////////////////////////////////////////////////////////////////////////////
00037 ///////////////////////////// Bluetooth Section ///////////////////////////////
00038 ///////////////////////////////////////////////////////////////////////////////
00039 void bluetooth_update() {
00040     char bnum = 0;
00041     char bhit = 0;
00042     while (1) {
00043         if (blue.getc() == '!') {
00044             if (blue.getc() == 'B') { //button data packet
00045                 bnum = blue.getc(); //button number
00046                 //pc.printf("%d",bnum);
00047                 bhit = blue.getc(); //1=hit, 0=release
00048                 parametersmutex.lock();
00049                 if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
00050                     myled = bnum - '0'; //current button number will appear on LEDs
00051                     switch (bnum) {
00052                     case '1': //number button 1
00053                         if (bhit == '1') {
00054                             rd += 1;
00055                         } else {
00056                             //add release code here
00057                         }
00058                         break;
00059                     case '2': //number button 2
00060                         if (bhit == '1') {
00061                             ri += 1;
00062                         } else {
00063                             //add release code here
00064                         }
00065                         break;
00066                     case '3': //number button 3
00067                         if (bhit == '1') {
00068                             rd -= 1;
00069                         } else {
00070                             //add release code here
00071                         }
00072                         break;
00073                     case '4': //number button 4
00074                         if (bhit == '1') {
00075                             ri -= 1;
00076                         } else {
00077                             //add release code here
00078                         }
00079                         break;
00080                     case '5': //button 5 up arrow
00081                         if (bhit == '1') {
00082                             rp += 1;
00083                         } else {
00084                             //add release code here
00085                         }
00086                         break;
00087                     case '6': //button 6 down arrow
00088                         if (bhit == '1') {
00089                             rp -= 1;
00090                         } else {
00091                             //add release code here
00092                         }
00093                         break;
00094                     case '7': //button 7 left arrow
00095                         if (bhit == '1') {
00096                             desired_angle -= 1;
00097                         } else {
00098                             //add release code here
00099                         }
00100                         break;
00101                     case '8': //button 8 right arrow
00102                         if (bhit == '1') {
00103                             desired_angle += 1;
00104                         } else {
00105                             //add release code here
00106                         }
00107                         break;
00108                     default:
00109                         break;
00110                     }
00111                 }
00112                 parametersmutex.unlock();
00113             }
00114         }
00115         Thread::wait(100);
00116     }
00117 }
00118 
00119 
00120 ///////////////////////////////////////////////////////////////////////////////
00121 ///////////////////////////// Control System Updates///////////////////////////
00122 ///////////////////////////////////////////////////////////////////////////////
00123 //make the calls to IMU here and this should be another thread
00124 void update_system() {
00125     return;
00126 }
00127 
00128 
00129 ///////////////////////////////////////////////////////////////////////////////
00130 ///////////////////////////// IMU STUFF////////////////////////////////////////
00131 ///////////////////////////////////////////////////////////////////////////////
00132 void calibrate_imu() {
00133     return;
00134 }
00135 
00136 void get_current_angle() {
00137     imu.readGyro();
00138 }
00139 
00140 
00141 
00142 
00143 ///////////////////////////////////////////////////////////////////////////////
00144 ///////////////////////////// Running Main Function////////////////////////////
00145 ///////////////////////////////////////////////////////////////////////////////
00146 
00147 int main() {
00148     pc.printf("this is running");
00149     Thread bluetooth;
00150     Thread system_update;
00151     
00152     imu.begin();
00153     
00154     bluetooth.start(bluetooth_update);
00155     system_update.start(update_system);
00156     
00157     //bluetooth.attach(&bluetooth_update, 0.1);
00158     while (1) {
00159         //bluetooth_update();
00160         parametersmutex.lock();
00161 //        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
00162         parametersmutex.unlock();
00163         
00164         get_current_angle();
00165         pc.printf("gyro: %d %d %d\n\r", imu.gx, imu.gy, imu.gz);
00166         
00167         Thread::wait(100);
00168     }
00169 }
00170