Joseph Boettcher / Mbed 2 deprecated Glove

Code for Wireless IMU Glove Controller used for SharpShooter II

Dependencies:   LSM9DS1_Library PinDetect mbed-rtos2 mbed

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

Committer:
jboettcher
Date:
Wed Dec 07 05:07:25 2016 +0000
Revision:
1:44b71d096f7a
Parent:
0:f3043bee193a 
Finalized

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jboettcher 0:f3043bee193a 1 #include "mbed.h"
jboettcher 0:f3043bee193a 2 #include "LSM9DS1.h"
jboettcher 0:f3043bee193a 3 #include "rtos.h"
jboettcher 0:f3043bee193a 4 #include "PinDetect.h"
jboettcher 0:f3043bee193a 5
jboettcher 0:f3043bee193a 6
jboettcher 0:f3043bee193a 7 Serial xbee1(p9, p10);
jboettcher 0:f3043bee193a 8 DigitalOut rst1(p11);
jboettcher 0:f3043bee193a 9 DigitalOut led1(LED1);
jboettcher 0:f3043bee193a 10 DigitalOut led2(LED2);
jboettcher 0:f3043bee193a 11 DigitalOut led3(LED3);
jboettcher 0:f3043bee193a 12 DigitalOut led4(LED4);
jboettcher 0:f3043bee193a 13 DigitalOut haptic(p7);
jboettcher 0:f3043bee193a 14 Serial pc(USBTX, USBRX);
jboettcher 0:f3043bee193a 15 PinDetect shoot(p8);
jboettcher 0:f3043bee193a 16 PinDetect reload(p21);
jboettcher 0:f3043bee193a 17 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
jboettcher 0:f3043bee193a 18 Mutex mutex;
jboettcher 0:f3043bee193a 19
jboettcher 0:f3043bee193a 20 //Global Variables
jboettcher 0:f3043bee193a 21 int init_x;
jboettcher 0:f3043bee193a 22 int init_y;
jboettcher 0:f3043bee193a 23 int x = 64;
jboettcher 0:f3043bee193a 24 int y = 64;
jboettcher 0:f3043bee193a 25 int old_x;
jboettcher 0:f3043bee193a 26 int old_y;
jboettcher 0:f3043bee193a 27 int dx;
jboettcher 0:f3043bee193a 28 int dy;
jboettcher 0:f3043bee193a 29 int shots_fired = 0;
jboettcher 0:f3043bee193a 30 bool reload_checksum = 0;
jboettcher 0:f3043bee193a 31 bool send_shoot = 0;
jboettcher 0:f3043bee193a 32 bool send_reload = 0;
jboettcher 0:f3043bee193a 33
jboettcher 0:f3043bee193a 34 //Function Declarations
jboettcher 0:f3043bee193a 35 void computeCursorCoordinates();
jboettcher 0:f3043bee193a 36 void sendCursorSwitchAndPB();
jboettcher 0:f3043bee193a 37 void receiveLEDsAndHaptic();
jboettcher 0:f3043bee193a 38 void setShoot();
jboettcher 0:f3043bee193a 39 void setReload();
jboettcher 0:f3043bee193a 40
jboettcher 0:f3043bee193a 41 int main() {
jboettcher 0:f3043bee193a 42
jboettcher 0:f3043bee193a 43 // Initialize IMU
jboettcher 0:f3043bee193a 44 imu.begin();
jboettcher 0:f3043bee193a 45 if (!imu.begin()) {
jboettcher 0:f3043bee193a 46 pc.printf("Failed to communicate with LSM9DS1.\n");
jboettcher 0:f3043bee193a 47 }
jboettcher 0:f3043bee193a 48 imu.calibrate();
jboettcher 0:f3043bee193a 49 imu.readAccel();
jboettcher 0:f3043bee193a 50 init_x = imu.ax;
jboettcher 0:f3043bee193a 51 init_y = imu.ay;
jboettcher 0:f3043bee193a 52
jboettcher 0:f3043bee193a 53
jboettcher 0:f3043bee193a 54 //Initialize Xbee
jboettcher 0:f3043bee193a 55 rst1 = 0;//Set reset pin to 0
jboettcher 0:f3043bee193a 56 wait_ms(1);
jboettcher 0:f3043bee193a 57 rst1 = 1;//Set reset pin to 1
jboettcher 0:f3043bee193a 58 wait_ms(1);
jboettcher 0:f3043bee193a 59
jboettcher 0:f3043bee193a 60 //Initialize LEDs and Haptic
jboettcher 0:f3043bee193a 61 haptic = 0;
jboettcher 0:f3043bee193a 62 led1 = 1;
jboettcher 0:f3043bee193a 63 led2 = 1;
jboettcher 0:f3043bee193a 64 led3 = 1;
jboettcher 0:f3043bee193a 65 led4 = 1;
jboettcher 0:f3043bee193a 66
jboettcher 0:f3043bee193a 67 //Initialize Shoot PB
jboettcher 0:f3043bee193a 68 shoot.mode(PullDown);
jboettcher 0:f3043bee193a 69 shoot.attach_asserted( &setShoot );
jboettcher 0:f3043bee193a 70 shoot.setSampleFrequency(2000); // Defaults to 20ms.
jboettcher 0:f3043bee193a 71
jboettcher 0:f3043bee193a 72 //Initialize Reload Switch
jboettcher 0:f3043bee193a 73 reload.attach_asserted( &setReload );
jboettcher 0:f3043bee193a 74 reload.setSampleFrequency(2000); // Defaults to 20ms.
jboettcher 0:f3043bee193a 75
jboettcher 0:f3043bee193a 76 //Initialize Threads
jboettcher 0:f3043bee193a 77 Thread thread1(receiveLEDsAndHaptic);
jboettcher 0:f3043bee193a 78 Thread thread2(sendCursorSwitchAndPB);
jboettcher 0:f3043bee193a 79 while(1){}
jboettcher 0:f3043bee193a 80 }
jboettcher 0:f3043bee193a 81
jboettcher 0:f3043bee193a 82 /****************************Helper Functions**********************************/
jboettcher 0:f3043bee193a 83
jboettcher 0:f3043bee193a 84
jboettcher 0:f3043bee193a 85 void computeCursorCoordinates() {
jboettcher 0:f3043bee193a 86 old_x = x;
jboettcher 0:f3043bee193a 87 old_y = y;
jboettcher 0:f3043bee193a 88 imu.readAccel();
jboettcher 0:f3043bee193a 89 imu.readMag();
jboettcher 0:f3043bee193a 90
jboettcher 0:f3043bee193a 91 dx = -imu.ay/2000;
jboettcher 0:f3043bee193a 92 x = old_x + dx;
jboettcher 0:f3043bee193a 93 dy = -imu.ax/2000;
jboettcher 0:f3043bee193a 94 y = old_y + dy;
jboettcher 0:f3043bee193a 95
jboettcher 0:f3043bee193a 96 // Set boundaries for Cursor
jboettcher 0:f3043bee193a 97 if(x<10) { x=10; }
jboettcher 0:f3043bee193a 98 if(y<23) { y=23; }
jboettcher 0:f3043bee193a 99 if(x>117) { x=117; }
jboettcher 0:f3043bee193a 100 if(y>117) { y=117; }
jboettcher 0:f3043bee193a 101 }
jboettcher 0:f3043bee193a 102
jboettcher 0:f3043bee193a 103 void sendCursorSwitchAndPB() {
jboettcher 0:f3043bee193a 104 while(1) {
jboettcher 0:f3043bee193a 105 computeCursorCoordinates();
jboettcher 0:f3043bee193a 106 int x0 = (x/100);
jboettcher 0:f3043bee193a 107 int x1 = ((x%100)/10);
jboettcher 0:f3043bee193a 108 int x2 = ((x%100)%10);
jboettcher 0:f3043bee193a 109 int y0 = (y/100);
jboettcher 0:f3043bee193a 110 int y1 = ((y%100)/10);
jboettcher 0:f3043bee193a 111 int y2 = ((y%100)%10);
jboettcher 0:f3043bee193a 112 mutex.lock();
jboettcher 0:f3043bee193a 113 if(xbee1.writeable()) {
jboettcher 0:f3043bee193a 114 xbee1.putc('@');
jboettcher 0:f3043bee193a 115 xbee1.putc(x0+48);
jboettcher 0:f3043bee193a 116 xbee1.putc(x1+48);
jboettcher 0:f3043bee193a 117 xbee1.putc(x2+48);
jboettcher 0:f3043bee193a 118 xbee1.putc(y0+48);
jboettcher 0:f3043bee193a 119 xbee1.putc(y1+48);
jboettcher 0:f3043bee193a 120 xbee1.putc(y2+48);
jboettcher 0:f3043bee193a 121
jboettcher 0:f3043bee193a 122 //Send Reload Switch - Only send true if rising edge
jboettcher 0:f3043bee193a 123 xbee1.putc(send_reload+48);
jboettcher 0:f3043bee193a 124 reload_checksum = send_reload;
jboettcher 0:f3043bee193a 125 if(send_reload) {
jboettcher 0:f3043bee193a 126 pc.printf("RELOAD\n");
jboettcher 0:f3043bee193a 127 }
jboettcher 0:f3043bee193a 128
jboettcher 0:f3043bee193a 129 //Send Shoot PB
jboettcher 0:f3043bee193a 130 xbee1.putc(send_shoot+48);
jboettcher 0:f3043bee193a 131 if(send_shoot==1) {
jboettcher 0:f3043bee193a 132 shots_fired++;
jboettcher 0:f3043bee193a 133 pc.printf("Shots Fired: %d\n",shots_fired);
jboettcher 0:f3043bee193a 134 }
jboettcher 0:f3043bee193a 135
jboettcher 0:f3043bee193a 136 //Calculate and Send Checksum
jboettcher 0:f3043bee193a 137 int sum = x+y+send_shoot+reload_checksum;
jboettcher 0:f3043bee193a 138 int sum0 = (sum/100);
jboettcher 0:f3043bee193a 139 int sum1 = ((sum%100)/10);
jboettcher 0:f3043bee193a 140 int sum2 = ((sum%100)%10);
jboettcher 0:f3043bee193a 141 xbee1.putc(sum0+48);
jboettcher 0:f3043bee193a 142 xbee1.putc(sum1+48);
jboettcher 0:f3043bee193a 143 xbee1.putc(sum2+48);
jboettcher 0:f3043bee193a 144
jboettcher 0:f3043bee193a 145 //Only send shoot and reload flags once
jboettcher 0:f3043bee193a 146 send_shoot = 0;
jboettcher 0:f3043bee193a 147 send_reload = 0;
jboettcher 0:f3043bee193a 148 }
jboettcher 0:f3043bee193a 149 mutex.unlock();
jboettcher 0:f3043bee193a 150 Thread::wait(5);
jboettcher 0:f3043bee193a 151 }
jboettcher 0:f3043bee193a 152 }
jboettcher 0:f3043bee193a 153
jboettcher 0:f3043bee193a 154 void receiveLEDsAndHaptic() {
jboettcher 0:f3043bee193a 155 while(1) {
jboettcher 0:f3043bee193a 156 if(xbee1.readable()){
jboettcher 0:f3043bee193a 157 char X;
jboettcher 0:f3043bee193a 158 mutex.lock();
jboettcher 0:f3043bee193a 159 X = xbee1.getc();
jboettcher 0:f3043bee193a 160 mutex.unlock();
jboettcher 0:f3043bee193a 161 if(X == '#') {
jboettcher 0:f3043bee193a 162 mutex.lock();
jboettcher 0:f3043bee193a 163 X = xbee1.getc()-48;
jboettcher 0:f3043bee193a 164 mutex.unlock();
jboettcher 0:f3043bee193a 165 led1 = X & (1<<4);
jboettcher 0:f3043bee193a 166 led2 = X & (1<<3);
jboettcher 0:f3043bee193a 167 led3 = X & (1<<2);
jboettcher 0:f3043bee193a 168 led4 = X & (1<<1);
jboettcher 0:f3043bee193a 169 haptic = X & (1<<0);
jboettcher 0:f3043bee193a 170 }
jboettcher 0:f3043bee193a 171 }
jboettcher 0:f3043bee193a 172 Thread::wait(500);
jboettcher 0:f3043bee193a 173 }
jboettcher 0:f3043bee193a 174 }
jboettcher 0:f3043bee193a 175
jboettcher 0:f3043bee193a 176 void setShoot() {
jboettcher 0:f3043bee193a 177 mutex.lock();
jboettcher 0:f3043bee193a 178 send_shoot = 1;
jboettcher 0:f3043bee193a 179 mutex.unlock();
jboettcher 0:f3043bee193a 180 }
jboettcher 0:f3043bee193a 181
jboettcher 0:f3043bee193a 182 void setReload() {
jboettcher 0:f3043bee193a 183 mutex.lock();
jboettcher 0:f3043bee193a 184 send_reload = 1;
jboettcher 0:f3043bee193a 185 mutex.unlock();
jboettcher 0:f3043bee193a 186 }