Yacine Bougrinat / Mbed 2 deprecated GaitDetectionSystem

Gait detection system for exoskeleton control

Dependencies:   Servo mbed

main.cpp@1:3ce5d2797365, 2017-02-21 (annotated)

Committer:
yacine1991
Date:
Tue Feb 21 00:45:21 2017 +0000
Revision:
1:3ce5d2797365
Parent:
0:a40546713f83
Child:
2:786e1897a6ec
Added Actuation of motor inside the GaitPercentage Block

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yacine1991 0:a40546713f83 1 #include "mbed.h"
yacine1991 0:a40546713f83 2 #include <stdio.h>
yacine1991 1:3ce5d2797365 3 #include "Servo.h"
yacine1991 0:a40546713f83 4
yacine1991 0:a40546713f83 5 //Initialize pins
yacine1991 0:a40546713f83 6 AnalogIn ain(p16);
yacine1991 0:a40546713f83 7 Serial device(p28,p27);
yacine1991 1:3ce5d2797365 8 Servo actuator(p21);
yacine1991 0:a40546713f83 9
yacine1991 0:a40546713f83 10 //Variables
yacine1991 0:a40546713f83 11 Timer t;
yacine1991 0:a40546713f83 12 float gaitPeriod[6];
yacine1991 0:a40546713f83 13 int startTime = 0;
yacine1991 0:a40546713f83 14 int gaitCounter = 0;
yacine1991 0:a40546713f83 15 float gaitPeriodMean = 0.00f;
yacine1991 0:a40546713f83 16 float inputReading = 0.00f;
yacine1991 0:a40546713f83 17 float percentage = 0.00f;
yacine1991 1:3ce5d2797365 18 float actuationPercentage = 30.0f;
yacine1991 1:3ce5d2797365 19 Servo myservo(p21);
yacine1991 0:a40546713f83 20
yacine1991 0:a40546713f83 21 //function to calculate the mean
yacine1991 0:a40546713f83 22 float mean (float tab[], int size){
yacine1991 0:a40546713f83 23 float sum = 0.00f;
yacine1991 0:a40546713f83 24 for (int i=1;i<size;i++){
yacine1991 0:a40546713f83 25 sum += tab[i];
yacine1991 0:a40546713f83 26 }
yacine1991 0:a40546713f83 27 return sum/(size-1);
yacine1991 0:a40546713f83 28 }
yacine1991 1:3ce5d2797365 29 //Servo Motor function
yacine1991 1:3ce5d2797365 30 void ServoActuaute(){
yacine1991 1:3ce5d2797365 31 for(float p=0; p<1.0; p += 0.2) {
yacine1991 1:3ce5d2797365 32 myservo = p;
yacine1991 1:3ce5d2797365 33 wait(0.2);
yacine1991 1:3ce5d2797365 34 }
yacine1991 1:3ce5d2797365 35 }
yacine1991 0:a40546713f83 36
yacine1991 0:a40546713f83 37 int main(void)
yacine1991 0:a40546713f83 38 {
yacine1991 0:a40546713f83 39 device.baud(57600);
yacine1991 0:a40546713f83 40
yacine1991 0:a40546713f83 41 while (1) {
yacine1991 0:a40546713f83 42 //Check for Serial Event
yacine1991 0:a40546713f83 43 if (device.readable()){
yacine1991 0:a40546713f83 44 char command = device.getc();
yacine1991 0:a40546713f83 45 /*
yacine1991 0:a40546713f83 46 Menu:
yacine1991 0:a40546713f83 47 Case a: get the mean of gait period over 5 cycles
yacine1991 0:a40546713f83 48 Case b: use the calculated mean to estimate gait
yacine1991 0:a40546713f83 49 cycle percentage while walking
yacine1991 0:a40546713f83 50 */
yacine1991 0:a40546713f83 51 switch (command){
yacine1991 0:a40546713f83 52 case 'a':
yacine1991 0:a40546713f83 53 //Get sensor reading
yacine1991 0:a40546713f83 54 inputReading = ain.read();
yacine1991 1:3ce5d2797365 55 wait_ms(20);
yacine1991 0:a40546713f83 56
yacine1991 0:a40546713f83 57 //Busy waiting for 1st heel strike
yacine1991 0:a40546713f83 58 while (inputReading < 0.20f){
yacine1991 1:3ce5d2797365 59 device.putc(110);//equivalent to printf("n")
yacine1991 1:3ce5d2797365 60 wait_ms(10);
yacine1991 0:a40546713f83 61 inputReading = ain.read();
yacine1991 1:3ce5d2797365 62 wait_ms(20);
yacine1991 0:a40546713f83 63 }
yacine1991 0:a40546713f83 64 t.start();
yacine1991 0:a40546713f83 65 t.reset();
yacine1991 0:a40546713f83 66 gaitCounter = 0;
yacine1991 0:a40546713f83 67
yacine1991 0:a40546713f83 68 //Get the mean of five gait cycles periods
yacine1991 0:a40546713f83 69 while (gaitCounter<6){
yacine1991 0:a40546713f83 70 inputReading = ain.read();
yacine1991 0:a40546713f83 71 //***This "wait" must be minimal***//
yacine1991 1:3ce5d2797365 72 wait_ms(20);
yacine1991 0:a40546713f83 73
yacine1991 0:a40546713f83 74 //Sensor threshold and time more than 0.8 sec
yacine1991 0:a40546713f83 75 if((inputReading>0.20f)&&(t.read()>0.8f)){
yacine1991 0:a40546713f83 76 gaitCounter++;
yacine1991 0:a40546713f83 77 gaitPeriod[gaitCounter] = t.read_ms();//-startTime;
yacine1991 0:a40546713f83 78 t.reset();//reset timer to zero
yacine1991 1:3ce5d2797365 79 device.printf("g|%d",gaitCounter);
yacine1991 1:3ce5d2797365 80 wait_ms(10);
yacine1991 0:a40546713f83 81
yacine1991 0:a40546713f83 82 if (gaitCounter == 5){
yacine1991 0:a40546713f83 83 //mean =(gaitPeriod[2]+gaitPeriod[3]+gaitPeriod[4]+gaitPeriod[5]+gaitPeriod[6])/5.0f;
yacine1991 0:a40546713f83 84 gaitPeriodMean = mean(gaitPeriod,6);
yacine1991 0:a40546713f83 85 //device.printf("Gait period: %1.2f\r\n", mean);
yacine1991 0:a40546713f83 86 device.printf("m|%1.2f", gaitPeriodMean);
yacine1991 1:3ce5d2797365 87 wait_ms(30);
yacine1991 0:a40546713f83 88 gaitCounter = 100;//get out of While
yacine1991 0:a40546713f83 89 }
yacine1991 0:a40546713f83 90 }
yacine1991 0:a40546713f83 91 }
yacine1991 0:a40546713f83 92 break;
yacine1991 0:a40546713f83 93
yacine1991 0:a40546713f83 94 case 'b':
yacine1991 0:a40546713f83 95 t.stop();
yacine1991 0:a40546713f83 96 device.printf("n|b");
yacine1991 1:3ce5d2797365 97 wait_ms(10);
yacine1991 0:a40546713f83 98 inputReading = ain.read();
yacine1991 1:3ce5d2797365 99 wait_ms(20);
yacine1991 0:a40546713f83 100
yacine1991 0:a40546713f83 101 //Busy waiting for the first heel strike to occur
yacine1991 0:a40546713f83 102 while(inputReading<0.20f){
yacine1991 0:a40546713f83 103 inputReading = ain.read();
yacine1991 1:3ce5d2797365 104 wait_ms(20);
yacine1991 0:a40546713f83 105 //device.printf("I am in empty While B\r\n");
yacine1991 0:a40546713f83 106 }
yacine1991 0:a40546713f83 107
yacine1991 0:a40546713f83 108 //device.printf("I just get out of Empty While\r\n");
yacine1991 0:a40546713f83 109
yacine1991 0:a40546713f83 110 //Re-initialize gaitCounter to Zero to start a new cycle
yacine1991 1:3ce5d2797365 111 percentage = 0;
yacine1991 0:a40546713f83 112 t.reset();
yacine1991 0:a40546713f83 113 t.start();
yacine1991 0:a40546713f83 114
yacine1991 1:3ce5d2797365 115 while(percentage<100.00){
yacine1991 0:a40546713f83 116 //device.printf("I am in the Percentage While\r\n");
yacine1991 1:3ce5d2797365 117 percentage = t.read_ms()/gaitPeriodMean*100.0f;
yacine1991 1:3ce5d2797365 118 device.printf("p|%1.2f|",percentage);
yacine1991 1:3ce5d2797365 119 wait_ms(10);
yacine1991 0:a40546713f83 120
yacine1991 0:a40546713f83 121 //***To Do***
yacine1991 0:a40546713f83 122 //Check if reading the sensor is necessary
yacine1991 0:a40546713f83 123 //to ensure starting a new cycle
yacine1991 0:a40546713f83 124 //Real test to determine
yacine1991 0:a40546713f83 125
yacine1991 0:a40546713f83 126 //Check if the cycle is finished
yacine1991 1:3ce5d2797365 127 if(percentage >= 100.0){
yacine1991 0:a40546713f83 128 //Reset percentage
yacine1991 1:3ce5d2797365 129 percentage = 0;
yacine1991 0:a40546713f83 130 t.stop();
yacine1991 0:a40546713f83 131 t.reset();
yacine1991 0:a40546713f83 132 //device.printf("Percentage set to zero,%1.2f\r\n",t.read_ms());
yacine1991 0:a40546713f83 133 //Start timer for another gait cycle
yacine1991 0:a40546713f83 134 t.start();
yacine1991 0:a40546713f83 135 }
yacine1991 0:a40546713f83 136
yacine1991 1:3ce5d2797365 137 if (percentage >= actuationPercentage - 0.2 && percentage <= actuationPercentage + 0.2){
yacine1991 1:3ce5d2797365 138 ServoActuaute();
yacine1991 1:3ce5d2797365 139 device.printf("s|s");
yacine1991 1:3ce5d2797365 140 }
yacine1991 1:3ce5d2797365 141
yacine1991 0:a40546713f83 142 //Check if the user wants to stop the system
yacine1991 0:a40546713f83 143 if (device.readable()){
yacine1991 0:a40546713f83 144 command = device.getc();
yacine1991 0:a40546713f83 145 //Go back to the menu
yacine1991 0:a40546713f83 146 if (command == 'o'){
yacine1991 0:a40546713f83 147 percentage = 100;
yacine1991 0:a40546713f83 148 device.printf("o|%1.2f",percentage);
yacine1991 1:3ce5d2797365 149 wait_ms(10);
yacine1991 0:a40546713f83 150 t.stop();t.reset();t.start();
yacine1991 0:a40546713f83 151 }
yacine1991 1:3ce5d2797365 152 /*/Check for motor actuation request
yacine1991 1:3ce5d2797365 153 else if(command == 'c'){
yacine1991 1:3ce5d2797365 154 if (percentage == actuationPercentage){
yacine1991 1:3ce5d2797365 155 ServoActuaute();
yacine1991 1:3ce5d2797365 156 device.printf("s|s");
yacine1991 1:3ce5d2797365 157 }
yacine1991 1:3ce5d2797365 158 }*/
yacine1991 0:a40546713f83 159 }
yacine1991 0:a40546713f83 160 }
yacine1991 1:3ce5d2797365 161 break;
yacine1991 1:3ce5d2797365 162
yacine1991 0:a40546713f83 163 default:
yacine1991 0:a40546713f83 164 device.printf("Waiting for your commad\r\n");
yacine1991 1:3ce5d2797365 165 wait_ms(10);
yacine1991 0:a40546713f83 166 }
yacine1991 0:a40546713f83 167 }
yacine1991 0:a40546713f83 168 }
yacine1991 0:a40546713f83 169 }