main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "rtos.h"
00004 #include "PinDetect.h"
00005 
00006 
00007 Serial xbee1(p9, p10);
00008 DigitalOut rst1(p11);
00009 DigitalOut led1(LED1);
00010 DigitalOut led2(LED2);
00011 DigitalOut led3(LED3);
00012 DigitalOut led4(LED4);
00013 DigitalOut haptic(p7);
00014 Serial pc(USBTX, USBRX);
00015 PinDetect shoot(p8);
00016 PinDetect reload(p21);
00017 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
00018 Mutex mutex;
00019 
00020 //Global Variables
00021 int init_x;
00022 int init_y;
00023 int x = 64;
00024 int y = 64;
00025 int old_x;
00026 int old_y;
00027 int dx;
00028 int dy;
00029 int shots_fired = 0;
00030 bool reload_checksum = 0;
00031 bool send_shoot = 0;
00032 bool send_reload = 0;
00033 
00034 //Function Declarations
00035 void computeCursorCoordinates();
00036 void sendCursorSwitchAndPB();
00037 void receiveLEDsAndHaptic();
00038 void setShoot();
00039 void setReload();
00040 
00041 int main() {
00042     
00043     // Initialize IMU
00044     imu.begin();
00045     if (!imu.begin()) {
00046         pc.printf("Failed to communicate with LSM9DS1.\n");
00047     }
00048     imu.calibrate();
00049     imu.readAccel();
00050     init_x = imu.ax;
00051     init_y = imu.ay;
00052     
00053     
00054     //Initialize Xbee
00055     rst1 = 0;//Set reset pin to 0
00056     wait_ms(1);
00057     rst1 = 1;//Set reset pin to 1
00058     wait_ms(1);
00059     
00060     //Initialize LEDs and Haptic
00061     haptic = 0;
00062     led1 = 1;
00063     led2 = 1;
00064     led3 = 1;
00065     led4 = 1;
00066     
00067     //Initialize Shoot PB
00068     shoot.mode(PullDown);
00069     shoot.attach_asserted( &setShoot );
00070     shoot.setSampleFrequency(2000); // Defaults to 20ms.
00071     
00072     //Initialize Reload Switch
00073     reload.attach_asserted( &setReload );
00074     reload.setSampleFrequency(2000); // Defaults to 20ms.
00075 
00076     //Initialize Threads
00077     Thread thread1(receiveLEDsAndHaptic);
00078     Thread thread2(sendCursorSwitchAndPB);
00079     while(1){}
00080 }
00081 
00082 /****************************Helper Functions**********************************/
00083 
00084 
00085 void computeCursorCoordinates() {
00086     old_x = x;
00087     old_y = y;
00088     imu.readAccel();
00089     imu.readMag();
00090     
00091     dx = -imu.ay/2000;
00092     x = old_x + dx;
00093     dy = -imu.ax/2000;
00094     y = old_y + dy;
00095     
00096     // Set boundaries for Cursor
00097     if(x<10)   { x=10;  }
00098     if(y<23)   { y=23;  }
00099     if(x>117)  { x=117; }
00100     if(y>117)  { y=117; }
00101 }
00102 
00103 void sendCursorSwitchAndPB() {
00104     while(1) { 
00105         computeCursorCoordinates();
00106         int x0 = (x/100);
00107         int x1 = ((x%100)/10);
00108         int x2 = ((x%100)%10);
00109         int y0 = (y/100);
00110         int y1 = ((y%100)/10);
00111         int y2 = ((y%100)%10);
00112         mutex.lock();
00113         if(xbee1.writeable()) {
00114             xbee1.putc('@');
00115             xbee1.putc(x0+48);
00116             xbee1.putc(x1+48);
00117             xbee1.putc(x2+48);
00118             xbee1.putc(y0+48);
00119             xbee1.putc(y1+48);
00120             xbee1.putc(y2+48);
00121             
00122             //Send Reload Switch - Only send true if rising edge
00123             xbee1.putc(send_reload+48);
00124             reload_checksum = send_reload;
00125             if(send_reload) {
00126                 pc.printf("RELOAD\n");
00127             }
00128             
00129             //Send Shoot PB
00130             xbee1.putc(send_shoot+48);
00131             if(send_shoot==1) {
00132                 shots_fired++;
00133                 pc.printf("Shots Fired: %d\n",shots_fired);
00134             }
00135             
00136             //Calculate and Send Checksum
00137             int sum = x+y+send_shoot+reload_checksum;
00138             int sum0 = (sum/100);
00139             int sum1 = ((sum%100)/10);
00140             int sum2 = ((sum%100)%10);
00141             xbee1.putc(sum0+48);
00142             xbee1.putc(sum1+48);
00143             xbee1.putc(sum2+48);
00144             
00145             //Only send shoot and reload flags once
00146             send_shoot = 0;
00147             send_reload = 0;
00148         }
00149         mutex.unlock();
00150         Thread::wait(5);
00151     }
00152 }
00153 
00154 void receiveLEDsAndHaptic() {
00155     while(1) {
00156         if(xbee1.readable()){
00157             char X;
00158             mutex.lock();
00159             X = xbee1.getc();
00160             mutex.unlock();
00161             if(X == '#') { 
00162                 mutex.lock();
00163                 X = xbee1.getc()-48;
00164                 mutex.unlock();
00165                 led1 = X & (1<<4);
00166                 led2 = X & (1<<3);
00167                 led3 = X & (1<<2);
00168                 led4 = X & (1<<1);
00169                 haptic = X & (1<<0);
00170             }
00171         }
00172         Thread::wait(500);
00173     }
00174 }
00175 
00176 void setShoot() {
00177     mutex.lock();
00178     send_shoot = 1;
00179     mutex.unlock();
00180 }
00181 
00182 void setReload() {
00183     mutex.lock();
00184     send_reload = 1;
00185     mutex.unlock();
00186 }