Yacine Bougrinat / Mbed 2 deprecated GaitDetectionSystem

Gait detection system for exoskeleton control

Dependencies:   Servo mbed

main.cpp@2:786e1897a6ec, 2017-02-24 (annotated)

Committer:
yacine1991
Date:
Fri Feb 24 23:52:23 2017 +0000
Revision:
2:786e1897a6ec
Parent:
1:3ce5d2797365 
This version of the program works properly and could be used as reference. The motor is actuated at 30% of gait. This is working the APP_Reference version. This alleviated the problem of hitting twice start/percentage

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