Diff: main.cpp

Revision:

3:6a249e8de7d8

Parent:

2:1a0d675eaa6f

Child:

4:91d5db5e8c9f

--- a/main.cpp	Thu Mar 26 20:21:30 2015 +0000
+++ b/main.cpp	Fri Mar 27 03:22:12 2015 +0000
@@ -2,7 +2,7 @@
 #include "math.h"
 #include "bluetoothComm.h"
 #define BT_BAUD 9600
-#define NUM_LRAS 7
+#define NUM_LRAS 6
 #define NUM_ENS NUM_LRAS
 
 // bluetooth serial
@@ -10,22 +10,25 @@
 Timer timer;
 
 //DigitalOut leds[4] = {
-//    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
+//   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(p5), PwmOut(p6),PwmOut(p20),
     PwmOut(p25),PwmOut(p26),PwmOut(p34)
 };
+//,PwmOut(p17)
 
+//int lra[NUM_LRAS];
+//int lra_en[NUM_ENS];
 
 DigitalOut lra_en[NUM_ENS] = {
-    DigitalOut(p7), DigitalOut(p8),DigitalOut(p11),DigitalOut(p12),
+   DigitalOut(p7), DigitalOut(p8),DigitalOut(p12),
     DigitalOut(p13),DigitalOut(p29),DigitalOut(p30)
 };
-
+DigitalOut en3(p11);
 int lraOn_ms[NUM_LRAS];
 int lraPeriod_ms[NUM_LRAS];
 float lraIntensity[NUM_LRAS];
@@ -49,9 +52,6 @@
                 //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: {
@@ -59,11 +59,8 @@
                 //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;
+                newIntensity = (float)(input+253)/508.0;
                 lraIntensity[which] = newIntensity;
-                //bt.putc('b');
-                //bt.printf("%f",newIntensity);
-                //bt.putc(0);
                 break;
             }
             case 2: {
@@ -75,9 +72,6 @@
                 if(newOnTime!=lraOn_ms[which]) {
                     lraOn_ms[which] = newOnTime;
                 }
-                //bt.putc('c');
-                //bt.printf("%d",input);
-                //bt.putc(0);
             }
             case 3: {
                 // Total Period Length
@@ -88,9 +82,6 @@
                 if(newTotalTime!=lraPeriod_ms[which]) {
                     lraPeriod_ms[which] = newTotalTime;
                 }
-                //bt.putc('d');
-                //bt.printf("%d",input);
-                //bt.putc(0);
                 break;
             }
             default: {
@@ -106,26 +97,27 @@
 
 int main (void)
 {
+    en3 = 0;
     //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;
+    timer.reset();
+    int counter_ms = timer.read_ms();
 
     //initialize and start everything
-    unsigned long startTime_ms[NUM_LRAS];
+    int 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);
+        lra[i].period_us(90);
         //initialize values
-        lra[i] = 0.5f;
         //set starting vibration
         lraOn_ms[i] = 100;
         lraPeriod_ms[i] = 1000;
-        lraIntensity[i] = 0.0f;
+        lraIntensity[i] = 0.5f;
         
         lra_en[i] = 0;
         lra[i] = lraIntensity[i]; //set initial intensity
@@ -135,39 +127,29 @@
     }
     while(1){
         if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
-            //read buffer
-            //parse data
-            //strcpy(returnBluetoothData(), btData, 50);
-            console1.printf("btData: ");
-            console1.printf(bluetoothData);
-            console1.printf("\n");
+            //console1.printf("btData: ");
+            //console1.printf(bluetoothData);
+            //console1.printf("\n");
             processData(bluetoothData);
             if(counter_ms % 3 == 0){
                 int num = bluetoothData[0] - '0';
-                console1.printf("processed Data: ");
                 console1.printf("Motor#: %d | ", num);
-                console1.printf("Intensity : ");
-                console1.printf("%f | ", lraIntensity[num]);
-                console1.printf("lraOn : ");
-                console1.printf("%d | ",lraOn_ms[num]);
-                console1.printf("lraPeriod : ");
-                console1.printf("%d ", lraPeriod_ms[num]);
-                console1.printf("\n");
+                console1.printf("Intensity : %f | ", lraIntensity[num]);
+                console1.printf("lraOn : %d | ",lraOn_ms[num]);
+                console1.printf("lraPeriod : %d \n", lraPeriod_ms[num]);
             }
         }
-        counter_ms = timer.read_ms();
         for(int n=0;n<NUM_LRAS;n++){
             // lra_fun
-            // Turn On LRA:
-            //leds[(int)n] = 1;
-            
+            counter_ms = timer.read_ms();
+            elapsed_ms[n] = (int)(counter_ms-startTime_ms[n]);
             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 {
+                    console1.printf("Turning motor off\n");
                     isOn[n] = false;
                     //Set LRA PWM to 0.5
                     lra[n] = 0.5f; // that turns off the motor!
@@ -176,15 +158,19 @@
                 }
             }
             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(n == 1){
+                //    leftToWait_ms[n] = lraPeriod_ms[n] - elapsed_ms[n];
+                //    console1.printf("leftToWait_ms[n] = %d\n",leftToWait_ms[n]);
+                //    console1.printf("elapsed_ms[n] = %d\n",elapsed_ms[n]);
+                //   }
                 if(leftToWait_ms[n] < 0) {
+                    console1.printf("Turning motor on\n");
                     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; 
+                    startTime_ms[n] = timer.read_ms();
                 }
             }
         }