Diff: main.cpp

Revision:

0:7c2805142589

Child:

1:2af026a7c290

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 26 16:57:07 2015 +0000
@@ -0,0 +1,179 @@
+#include "mbed.h"
+#include "math.h"
+#include "bluetoothComm.h"
+#define BT_BAUD 9600
+#define NUM_LRAS 7
+#define NUM_ENS NUM_LRAS
+
+// bluetooth serial
+// p9 - tx, p10 - rx
+Timer timer;
+
+//DigitalOut leds[4] = {
+//    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
+//};
+
+//int leds[NUM_LRAS];
+
+PwmOut lra[NUM_LRAS] = {
+    PwmOut(p5), PwmOut(p6),PwmOut(p17),PwmOut(p20),
+    PwmOut(p25),PwmOut(p26),PwmOut(p34)
+};
+
+
+DigitalOut lra_en[NUM_ENS] = {
+    DigitalOut(p7), DigitalOut(p8),DigitalOut(p11),DigitalOut(p12),
+    DigitalOut(p13),DigitalOut(p29),DigitalOut(p30)
+};
+
+int lraOn_ms[NUM_LRAS];
+int lraPeriod_ms[NUM_LRAS];
+float lraIntensity[NUM_LRAS];
+
+void processData(void const *n)
+{
+    int index = 0;
+    int which = 0;
+    char input = 0;
+    float newIntensity;
+    int newOnTime;
+    int newTotalTime;
+
+    while(bt.readable()) {
+        input = bt.getc();
+
+        switch ( index ) {
+            case 0: {
+                which = input-('0');
+                //index = (which < 0)? 4 : index;
+                //index = (which > (NUM_LRAS-1))? 4 : index;
+                which = (which < 0)? int(0) : which;
+                which = (which > (NUM_LRAS-1))? (NUM_LRAS-1) : which;
+                //bt.putc('a');
+                //bt.putc(input);
+                //bt.putc(0);
+                break;
+            }
+            case 1: {
+                // Intensity
+                //input = (input < 1)? char(1) : input;
+                //input = (input > 255)? char(255) : input;
+                // scale intensity between 0.5f to 1.0f
+                newIntensity = (float) (input+253)/508.0;
+                lraIntensity[which] = newIntensity;
+                //bt.putc('b');
+                //bt.printf("%f",newIntensity);
+                //bt.putc(0);
+                break;
+            }
+            case 2: {
+                // Period Length Start
+                input = (input < 1)? 1 : input;
+                input = (input > 255)? 255 : input;
+                // scale start length between 50 to 300 - see matlab script "range_calculations.m" in git repo
+                newOnTime = (int) floor( ((input+49.8)/1.016) + 0.5); //floor(...+0.5) = round()
+                if(newOnTime!=lraOn_ms[which]) {
+                    lraOn_ms[which] = newOnTime;
+                }
+                //bt.putc('c');
+                //bt.printf("%d",input);
+                //bt.putc(0);
+            }
+            case 3: {
+                // Total Period Length
+                input = (input < 1)? 1 : input;
+                input = (input > 255)? 255 : input;
+                // scale total period length between 300 to 4000 - see matlab script "range_calculations.m" in git repo
+                newTotalTime = (int) floor( ((input+19.5946)/0.0686) +0.5); //floor(...+0.5) = round()
+                if(newTotalTime!=lraPeriod_ms[which]) {
+                    lraPeriod_ms[which] = newTotalTime;
+                }
+                //bt.putc('d');
+                //bt.printf("%d",input);
+                //bt.putc(0);
+                break;
+            }
+            default: {
+                // do nothing
+                break;
+            }
+        }
+        index++;
+        //Thread::yield();// Pass control to next thread that is in state READY.
+    }
+    //bt.putc(0);
+    index = 0; //reset index
+}
+int main (void)
+{
+    //Init communication
+    robotSetup(BT_BAUD); //set baud rate of bluetooth connection
+    
+    //start universal timer to count up a counter
+    timer.start();
+    int counter_ms = 0;
+
+    //initialize and start everything
+    char btData[50];
+    unsigned long startTime_ms[NUM_LRAS];
+    int elapsed_ms[NUM_LRAS];
+    int leftToWait_ms[NUM_LRAS];
+    bool isOn[NUM_LRAS];
+    for(int i = 0; i < NUM_LRAS; i++) {
+        //set pwm frequency
+        lra[i].period_us(100);
+        //initialize values
+        lra[i] = 0.5f;
+        //set starting vibration
+        lraOn_ms[i] = 100;
+        lraPeriod_ms[i] = 1000;
+        lraIntensity[i] = 0.0f;
+        
+        lra_en[i] = 0;
+        lra[i] = lraIntensity[i]; //set initial intensity
+        startTime_ms[i] = counter_ms; //get start time
+        isOn[i] = false;
+        leftToWait_ms[i] = lraOn_ms[i];
+    }
+    while(1){
+        if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
+            //read buffer
+            //parse data
+            strcpy(returnBluetoothData(), btData, 50);
+            int len = length(btData);
+        }
+        counter_ms = timer.read_ms();
+        for(int n=0;n<NUM_LRAS;n++){
+            // lra_fun
+            // Turn On LRA:
+            //leds[(int)n] = 1;
+            
+            if(isOn[n]) {
+                elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
+                leftToWait_ms[n] = lraOn_ms[n] - elapsed_ms[n];
+                if(leftToWait_ms[n] > 0) {
+                    lra[n] = lraIntensity[n]; //adjust intensity according to current value
+
+                } else {
+                    isOn[n] = false;
+                    //Set LRA PWM to 0.5
+                    lra[n] = 0.5f; // that turns off the motor!
+                    //Turn LRA Off by setting enable pin to 0
+                    lra_en[n] = 0; // no braking happening
+                }
+            }
+            else {    
+            //printf("time: %d\n",leftToWait_ms);
+                elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
+                leftToWait_ms[n] = lraPeriod_ms[n] - elapsed_ms[n];
+                if(leftToWait_ms[n] < 0) {
+                    isOn[n] = true;
+                    //Set LRA PWM to desired intensity
+                    lra[n] = lraIntensity[n]; // that turns on the motor!
+                    //Turn LRA On by setting enable pin to 1
+                    lra_en[n] = 1; 
+                }
+            }
+        }
+    }
+}
\ No newline at end of file