Diff: main.cpp

Revision:

0:3e074f9f4063

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 26 15:07:05 2018 +0000
@@ -0,0 +1,190 @@
+#include "mbed.h"
+#include "PinDetect.h"
+ 
+//Analog input. Finger 1-5.  Thumb is 1.  Pinky is 5.
+AnalogIn a1(p16);
+AnalogIn a2(p17);
+AnalogIn a3(p18);
+AnalogIn a4(p19);
+AnalogIn a5(p20);
+
+//Coordinator Setup for XBEE
+Serial xbee1(p9,p10);
+DigitalOut rst1(p11);
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED2);
+DigitalOut myled4(LED2);
+
+// Global count variable
+float volatile af1=0.5;
+float volatile af2=0.5;
+float volatile af3=0.5;
+float volatile af4=0.5;
+float volatile af5=0.5;
+ 
+float volatile afs1=0.5;
+float volatile afs2=0.5;
+float volatile afs3=0.5;
+float volatile afs4=0.5;
+float volatile afs5=0.5;
+ 
+Serial pc(USBTX, USBRX);
+ 
+//calibration constants
+float const c1_low = 0.524;
+float const c2_low = 0.469;
+float const c3_low = 0.477;
+float const c4_low = 0.424;
+float const c5_low = 0.471;
+ 
+float const c1_high = 0.661;
+float const c2_high = 0.642;
+float const c3_high = 0.693;
+float const c4_high = 0.690;
+float const c5_high = 0.713;
+ 
+//function prototypes:
+float myConstrain(float val, float min, float max);
+float myScale(float val, float min, float max);
+float myConstrainScale(float val, float min, float max);
+ 
+int main() {
+    const float dampingCoeff = 0.5; //tweak this
+    rst1 = 0;   //Set reset pin to 0
+    myled1 = 0;
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0;
+    wait_ms(1);
+    rst1 = 1;   //Set reset pin to 1
+    wait_ms(1);
+    
+    while(1) {        
+        //filter:
+        af1 += dampingCoeff * ( a1 - af1 );
+        af2 += dampingCoeff * ( a2 - af2 );
+        af3 += dampingCoeff * ( a3 - af3 );
+        af4 += dampingCoeff * ( a4 - af4 );
+        af5 += dampingCoeff * ( a5 - af5 );
+        
+        //constrain and scale:
+        afs1 = myConstrainScale(af1, c1_low, c1_high);
+        afs2 = myConstrainScale(af2, c2_low, c2_high);
+        afs3 = myConstrainScale(af3, c3_low, c3_high);
+        afs4 = myConstrainScale(af4, c4_low, c4_high);
+        afs5 = myConstrainScale(af5, c5_low, c5_high);
+        
+
+        if (0.3 >= afs1 > 0.5) {  // Thumb
+            xbee1.putc ('A');  // Move back speed 1
+            
+            myled1 = 1;
+            myled2 = 0;
+            myled3 = 0;
+            myled4 = 0;
+        } 
+        else if (afs1 >= 0.5) { // Thumb
+            xbee1.putc ('B');  // Move back speed 2
+            
+            myled1 = 1;
+            myled2 = 0;
+            myled3 = 0;
+            myled4 = 0;
+        }
+        else if (0.3 >= afs2 > 0.5) { // Pointer Finger
+            xbee1.putc ('C');  // Move right speed 1
+            
+            myled1 = 0;
+            myled2 = 1;
+            myled3 = 0;
+            myled4 = 0;
+        }
+        else if (afs2 >= 0.5) { // Pointer Finger
+            xbee1.putc ('D');  // Move right speed 2
+            
+            myled1 = 0;
+            myled2 = 1;
+            myled3 = 0;
+            myled4 = 0;
+        }
+        else if (0.3 >= afs3 > 0.5) { // Middle Finger
+            xbee1.putc ('E');  // Move Forward speed 1
+            
+            myled1 = 0;
+            myled2 = 0;
+            myled3 = 1;
+            myled4 = 0;
+        }
+        else if (afs3 >= 0.5) { // Middle Finger
+            xbee1.putc ('F');  // Move Forward speed 1
+            
+            myled1 = 0;
+            myled2 = 0;
+            myled3 = 1;
+            myled4 = 0;
+        }
+        else if (0.3 >= afs4 > 0.5) { //Ring Finger
+            xbee1.putc ('G');  // Move Left speed 1
+            
+            myled1 = 1;
+            myled2 = 0;
+            myled3 = 0;
+            myled4 = 0;
+        }
+        else if (afs4 >= 0.5) { // Ring Finger
+            xbee1.putc ('H');  // Move Left speed 2
+            
+            myled1 = 1;
+            myled2 = 0;
+            myled3 = 0;
+            myled4 = 0;
+        }
+        else if (afs5 >= 0.5) { // Pinky
+            xbee1.putc ('I');  // Move back speed 2
+            
+            myled1 = 1;
+            myled2 = 1;
+            myled3 = 1;
+            myled4 = 1;
+        }
+        else {                // Otherwise just stop
+            xbee1.putc ('I'); 
+            
+            myled1 = 0;
+            myled2 = 0;
+            myled3 = 0;
+            myled4 = 0;
+        }
+        
+    }
+}
+ 
+ 
+float myConstrain(float val, float min, float max) {
+    float output = 0;
+    
+    if (val < min) {
+        output = min;
+    } else if (val > max) {
+        output = max;
+    } else {
+        output = val;
+    }
+    return output;
+}
+ 
+float myScale(float val, float min, float max) { //not used
+    float output = (val - min) / (max - min);
+    return output;
+}
+ 
+float myConstrainScale(float val, float min, float max) {
+    float output = 0;
+    
+    val = myConstrain(val, min, max);
+    output = (val - min) / (max - min);
+    
+    return output;
+}