Diff: main.cpp

Revision:

1:3ce5d2797365

Parent:

0:a40546713f83

Child:

2:786e1897a6ec

--- a/main.cpp	Tue Feb 07 19:19:43 2017 +0000
+++ b/main.cpp	Tue Feb 21 00:45:21 2017 +0000
@@ -1,9 +1,11 @@
 #include "mbed.h"
 #include <stdio.h>
+#include "Servo.h"
 
 //Initialize pins
 AnalogIn   ain(p16);
 Serial device(p28,p27);
+Servo actuator(p21);
 
 //Variables
 Timer t;
@@ -13,6 +15,8 @@
 float gaitPeriodMean = 0.00f;
 float inputReading = 0.00f;
 float percentage = 0.00f;
+float actuationPercentage = 30.0f;
+Servo myservo(p21);
 
 //function to calculate the mean 
 float mean (float tab[], int size){
@@ -22,6 +26,13 @@
     }
     return sum/(size-1);
 }
+//Servo Motor function
+void ServoActuaute(){
+    for(float p=0; p<1.0; p += 0.2) {
+        myservo = p;
+        wait(0.2);
+    }    
+}
 
 int main(void)
 {
@@ -41,13 +52,14 @@
             case 'a':                              
                 //Get sensor reading
                 inputReading = ain.read();
-                wait(0.5);
+                wait_ms(20);
                 
                 //Busy waiting for 1st heel strike
                 while (inputReading < 0.20f){
-                    device.printf("n|a");
+                    device.putc(110);//equivalent to printf("n")
+                    wait_ms(10);
                     inputReading = ain.read();
-                    wait(0.5);
+                    wait_ms(20);
                 }
                 t.start();
                 t.reset();
@@ -57,20 +69,22 @@
                 while (gaitCounter<6){  
                     inputReading = ain.read();
                     //***This "wait" must be minimal***// 
-                    wait(0.5);         
+                    wait_ms(20);         
                     
                     //Sensor threshold and time more than 0.8 sec
                     if((inputReading>0.20f)&&(t.read()>0.8f)){
                         gaitCounter++;
                         gaitPeriod[gaitCounter] = t.read_ms();//-startTime;                              
                         t.reset();//reset timer to zero
-                        device.printf("g|%d\r\n",gaitCounter);
+                        device.printf("g|%d",gaitCounter);
+                        wait_ms(10);
                         
                         if (gaitCounter == 5){
                             //mean =(gaitPeriod[2]+gaitPeriod[3]+gaitPeriod[4]+gaitPeriod[5]+gaitPeriod[6])/5.0f;
                             gaitPeriodMean = mean(gaitPeriod,6);
                             //device.printf("Gait period: %1.2f\r\n", mean);
                             device.printf("m|%1.2f", gaitPeriodMean);
+                            wait_ms(30);
                             gaitCounter = 100;//get out of While
                         }                                                          
                     }
@@ -80,27 +94,29 @@
             case 'b':
                 t.stop();
                 device.printf("n|b");
+                wait_ms(10);
                 inputReading = ain.read();
-                wait(0.5);
+                wait_ms(20);
                 
                 //Busy waiting for the first heel strike to occur
                 while(inputReading<0.20f){
                     inputReading = ain.read();
-                    wait(0.5);
+                    wait_ms(20);
                     //device.printf("I am in empty While B\r\n");
                     }
       
                 //device.printf("I just get out of Empty While\r\n");
                 
                 //Re-initialize gaitCounter to Zero to start a new cycle
-                percentage = 0.00;
+                percentage = 0;
                 t.reset();
                 t.start();
                 
-                while(percentage<1.00){
+                while(percentage<100.00){
                     //device.printf("I am in the Percentage While\r\n");
-                    percentage = t.read_ms()/gaitPeriodMean;  
-                    device.printf("p|%1.2f\r\n",percentage); 
+                    percentage = t.read_ms()/gaitPeriodMean*100.0f;  
+                    device.printf("p|%1.2f|",percentage); 
+                    wait_ms(10);
                     
                     //***To Do***
                     //Check if reading the sensor is necessary
@@ -108,9 +124,9 @@
                     //Real test to determine
                     
                     //Check if the cycle is finished                        
-                    if(percentage >= 1.0){                         
+                    if(percentage >= 100.0){                         
                         //Reset percentage
-                        percentage = 0.00;
+                        percentage = 0;
                         t.stop();  
                         t.reset();
                         //device.printf("Percentage set to zero,%1.2f\r\n",t.read_ms());  
@@ -118,6 +134,11 @@
                         t.start();
                     }
                     
+                    if (percentage >= actuationPercentage - 0.2 && percentage <= actuationPercentage + 0.2){
+                        ServoActuaute();
+                        device.printf("s|s");
+                    }
+                        
                     //Check if the user wants to stop the system  
                     if (device.readable()){
                         command = device.getc();
@@ -125,14 +146,23 @@
                         if (command == 'o'){
                             percentage = 100;
                             device.printf("o|%1.2f",percentage);
+                            wait_ms(10);
                             t.stop();t.reset();t.start();
                         }
+                        /*/Check for motor actuation request
+                        else if(command == 'c'){
+                            if (percentage == actuationPercentage){
+                                ServoActuaute();
+                                device.printf("s|s");
+                            }
+                        }*/
                     }
                 }
-                break;
-                
+             break;
+        
             default:
                 device.printf("Waiting for your commad\r\n");
+                wait_ms(10);
             }   
         }
     }