Diff: main.cpp

Revision:

0:b95af9fb38a2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 29 05:50:52 2014 +0000
@@ -0,0 +1,198 @@
+#define USB_KEYBOARD 0
+
+#include "mbed.h"
+#include <math.h> 
+ 
+#if USB_KEYBOARD 
+#include "USBKeyboard.h"
+#endif
+ 
+
+
+#if USB_KEYBOARD
+//USBKeyboard keyboard;
+USBKeyboard keyboard(USBTX, USBRX);
+#else
+Serial pc(USBTX, USBRX);
+#endif 
+
+// Leg sensors
+AnalogIn leg_left(A1);
+AnalogIn leg_right(A0);
+ 
+// Arm sensors
+AnalogIn arm_left(A3);
+AnalogIn arm_right(A2);
+ 
+DigitalOut ledRed(LED1);
+DigitalOut ledGreen(LED2);
+DigitalOut ledBlue(LED3);
+// Total count
+const unsigned int TOTAL_COUNTS = 10;
+ 
+// Clock count
+unsigned int clock_count = 0;
+ 
+// Threshold values
+float al_low = 0.457f;  // left arm low
+float al_high = 0.530f; // left arm high
+float ar_low = 0.39f;  // right arm low
+float ar_high = 0.420f; // right arm high
+float ll_low = 0.579f;  // left leg low
+float ll_high = 0.60f; // left leg high
+float lr_low = 0.490f;  // right leg low
+float lr_high = 0.510f; // right leg high
+ 
+unsigned int map_reading_to_modulo(float low, float high, float reading) {
+    //(low, 1), (high, TOTAL_COUNTS - 1);
+    float slope = ((TOTAL_COUNTS - 1) / (high - low));  // calculate slope
+    float result = slope * reading - (slope * low);  // transform reading from 1 to TOTAL_COUNTS
+    return (int(floor(result)) + 1);
+}
+ 
+void check_arm_left(float reading) {
+    if (reading < al_low) {
+        // Lower than the threshold
+        // Fire an event every single clock count
+#if USB_KEYBOARD        
+        keyboard.keyCode('o');
+#else
+        pc.putc('o');
+#endif
+        
+    } else if (al_low <= reading && reading < al_high ) {
+        // Find mapping
+        int mod = map_reading_to_modulo(al_low, al_high, reading);
+        //int real_reading = int(floor(reading));
+        if (clock_count % mod == 0) {
+#if USB_KEYBOARD            
+            keyboard.keyCode('o');
+#else
+            pc.putc('o');
+#endif
+        } 
+    } else {
+        // Don't do anything
+    }
+}
+ 
+ 
+void check_arm_right(float reading) {
+    if (reading < ar_low) {
+        // Lower than the threshold
+        // Fire an event every single clock count
+#if USB_KEYBOARD        
+        keyboard.keyCode('p');
+#else
+        pc.putc('p');
+#endif
+    } else if (ar_low <= reading && reading < ar_high  ) {
+        // Find mapping
+        int mod = map_reading_to_modulo(ar_low, ar_high, reading);
+        //int real_reading = int(floor(reading));
+        if (clock_count % mod == 0) {
+#if USB_KEYBOARD
+            keyboard.keyCode('p');
+#else
+            pc.putc('p');
+#endif
+        }
+    } else {
+        // Don't do anything
+    }
+}
+ 
+ 
+ 
+void check_leg_left(float reading) {
+    ledGreen = 1;
+    ledBlue = 1;
+    ledRed = 1;
+    if (reading < ll_low) {
+        // Lower than the threshold
+        // Fire an event every single clock count
+#if USB_KEYBOARD        
+        keyboard.keyCode('q');
+#else
+        pc.putc('q');
+#endif
+        ledRed = 0;
+        
+    } else if (ll_low <= reading && reading < ll_high ) {
+        // Find mapping
+        ledGreen = 0;
+        int mod = map_reading_to_modulo(al_low, al_high, reading);
+        //int real_reading = int(floor(reading));
+        if (clock_count % mod == 0) {
+#if USB_KEYBOARD
+            keyboard.keyCode('q');
+#else
+            pc.putc('q');
+#endif
+        }
+    } else {
+        ledBlue = 0;
+        // Don't do anything
+    }
+}
+ 
+ 
+void check_leg_right(float reading) {
+    if (reading < lr_low) {
+        // Higher than the threshold
+        // Fire an event every single clock count
+#if USB_KEYBOARD        
+        keyboard.keyCode('w');
+#else
+        pc.putc('w');
+#endif
+
+    } else if (lr_low <= reading && reading < lr_high ) {
+        // Find mapping
+        int mod = map_reading_to_modulo(al_low, al_high, reading);
+        //int real_reading = int(floor(reading));
+        if (clock_count % mod == 0) {
+#if USB_KEYBOARD
+            keyboard.keyCode('w');
+#else
+            pc.putc('w');
+#endif
+        }
+    } else {
+
+        // Don't do anything
+    }
+}
+ 
+ 
+int main() {
+    ledRed = 1;
+    ledGreen = 1;
+    ledBlue = 0;
+    wait(1.0f);
+    while(1) {
+#if 1
+
+        ledRed = 1;
+        ledGreen = 1;
+        
+        // Check for movement
+        check_arm_left(arm_left.read());
+        check_arm_right(arm_right.read());
+        check_leg_left(leg_left.read());
+        check_leg_right(leg_right.read());
+        
+        // Cycle the Clock
+        wait(0.1f); // Should be around 25 iterations pers second
+        clock_count = (clock_count + 1) % TOTAL_COUNTS;
+#else
+        //Debug code
+        keyboard.keyCode('q');
+        keyboard.keyCode('w');
+        keyboard.keyCode('o');
+        keyboard.keyCode('p');    
+        wait(1.0f);  
+#endif
+    }
+}
+