Roy Theussing / Mbed 2 deprecated TestProgramm

to send emg signal to motor with test programm

Dependencies:   HIDScope biquadFilter mbed

Fork of EMGvoorjan by Roy Theussing

main.cpp@37:c0f6e3b44d7b, 2017-10-30 (annotated)

Committer:
Roytsg
Date:
Mon Oct 30 12:28:49 2017 +0000
Revision:
37:c0f6e3b44d7b
Parent:
36:7d7c16c688df 
left emg-signal aangesloten, veel scopes uit gezet;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
Roytsg 21:77998ce2c0dd 3 #include "BiQuad.h"
Roytsg 28:4b22455930ff 4 #include "math.h"
Roytsg 24:fe3825337233 5
vsluiter 4:8b298dfada81 6 //Define objects
Roytsg 28:4b22455930ff 7 AnalogIn emg( A0 ); //EMG
Roytsg 28:4b22455930ff 8 AnalogIn emg1( A1 ); //EMG
Roytsg 37:c0f6e3b44d7b 9 HIDScope scope( 2 ); // aantal scopes dat gemaakt kan worden
Roytsg 28:4b22455930ff 10 DigitalOut ledB(LED_BLUE);
Roytsg 28:4b22455930ff 11 DigitalOut ledG(LED_GREEN);
Roytsg 28:4b22455930ff 12 DigitalIn TestButton(PTA4); // button naast het ledje
Roytsg 28:4b22455930ff 13 DigitalIn onoff(PTC6); // button aan de andere kant
Roytsg 28:4b22455930ff 14 Ticker emgSampleTicker; // Ticker voor de sample frequency
Roytsg 34:008bd226d37e 15 PwmOut motor2MagnitudePin(D5); // magnitude motor 2
Roytsg 29:a48b63e60a40 16 DigitalOut motor2DirectionPin(D4); // direction of the motor 2
Roytsg 34:008bd226d37e 17 InterruptIn MotorOn(D8);
Roytsg 34:008bd226d37e 18
vsluiter 2:e314bb3b2d99 19
Roytsg 28:4b22455930ff 20
Roytsg 28:4b22455930ff 21 int P= 200; // aantal test punten voor de moving average
Roytsg 28:4b22455930ff 22 double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P
Roytsg 37:c0f6e3b44d7b 23 double B[200];
Roytsg 28:4b22455930ff 24 int k = 0; // counter voor de configuratie
Roytsg 28:4b22455930ff 25 double Vvector[200]; // vector voor de Vwaarde configuratie
Roytsg 28:4b22455930ff 26 double Vwaarde[2]; // vector voor waardes van V
Roytsg 28:4b22455930ff 27 int x = 0; // x waarde voor de Vwaarde
Roytsg 37:c0f6e3b44d7b 28 double movmeanR;
Roytsg 37:c0f6e3b44d7b 29 double movmeanL; //moving average for the right arm (first emg-shield)
Roytsg 30:4c6321941515 30 int MotorLock = 0;
Roytsg 36:7d7c16c688df 31 double EMGInputR;
Roytsg 37:c0f6e3b44d7b 32 double EMGInputL;
Roytsg 32:3832f732f17a 33 float ErrorZero = 3;
Roytsg 33:84d986230e15 34 double MotorSpeed = 0.5;
Roytsg 24:fe3825337233 35
Roytsg 34:008bd226d37e 36
Roytsg 28:4b22455930ff 37 // Filters
Roytsg 36:7d7c16c688df 38 BiQuadChain bqcR;
Roytsg 36:7d7c16c688df 39 BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
Roytsg 36:7d7c16c688df 40 BiQuad bq2R( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp?
Roytsg 36:7d7c16c688df 41 BiQuad bq3R( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch?
Roytsg 37:c0f6e3b44d7b 42 BiQuadChain bqcL;
Roytsg 37:c0f6e3b44d7b 43 BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
Roytsg 37:c0f6e3b44d7b 44 BiQuad bq2L( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp?
Roytsg 37:c0f6e3b44d7b 45 BiQuad bq3L( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch?
vsluiter 0:32bb76391d89 46
Roytsg 28:4b22455930ff 47
Roytsg 21:77998ce2c0dd 48
Roytsg 28:4b22455930ff 49 // sample function voor plotten van de emg signalen en moving average
Roytsg 21:77998ce2c0dd 50 void emgSample() {
Roytsg 21:77998ce2c0dd 51
Roytsg 36:7d7c16c688df 52 double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal
Roytsg 37:c0f6e3b44d7b 53 double emgFilteredL = bqcL.step( emg1.read());
Roytsg 36:7d7c16c688df 54 double emgabsR = abs(emgFilteredR); // absolute waarde van het gefilterde signaal
Roytsg 37:c0f6e3b44d7b 55 double emgabsL = abs(emgFilteredL);
Roytsg 37:c0f6e3b44d7b 56 //scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek
Roytsg 37:c0f6e3b44d7b 57 //scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek
Roytsg 34:008bd226d37e 58
Roytsg 24:fe3825337233 59
Roytsg 28:4b22455930ff 60 // deze for-loop maakt de vector voor de moving average
Roytsg 27:674193a62e06 61 for(int i = P-1; i >= 0; i--){
Roytsg 26:97a8adc9b895 62 if (i == 0) {
Roytsg 36:7d7c16c688df 63 A[i] = emgabsR;
Roytsg 26:97a8adc9b895 64 }
Roytsg 26:97a8adc9b895 65 else {
Roytsg 26:97a8adc9b895 66 A[i] = A[i-1];
Roytsg 26:97a8adc9b895 67 }
Roytsg 28:4b22455930ff 68 }
Roytsg 36:7d7c16c688df 69 double sumR = 0;
Roytsg 28:4b22455930ff 70 // deze for-loop sommeert de array
Roytsg 27:674193a62e06 71 for (int n = 0; n < P-1; n++) {
Roytsg 36:7d7c16c688df 72 sumR = sumR + A[n];
Roytsg 26:97a8adc9b895 73 }
Roytsg 24:fe3825337233 74
Roytsg 36:7d7c16c688df 75 movmeanR = sumR/P; //dit is de moving average waarde
Roytsg 24:fe3825337233 76
Roytsg 37:c0f6e3b44d7b 77 // deze for-loop maakt de vector voor de moving average
Roytsg 37:c0f6e3b44d7b 78 for(int i = P-1; i >= 0; i--){
Roytsg 37:c0f6e3b44d7b 79 if (i == 0) {
Roytsg 37:c0f6e3b44d7b 80 B[i] = emgabsL;
Roytsg 37:c0f6e3b44d7b 81 }
Roytsg 37:c0f6e3b44d7b 82 else {
Roytsg 37:c0f6e3b44d7b 83 B[i] = B[i-1];
Roytsg 37:c0f6e3b44d7b 84 }
Roytsg 37:c0f6e3b44d7b 85 }
Roytsg 37:c0f6e3b44d7b 86 double sumL = 0;
Roytsg 37:c0f6e3b44d7b 87 // deze for-loop sommeert de array
Roytsg 37:c0f6e3b44d7b 88 for (int n = 0; n < P-1; n++) {
Roytsg 37:c0f6e3b44d7b 89 sumL = sumL + B[n];
Roytsg 37:c0f6e3b44d7b 90 }
Roytsg 37:c0f6e3b44d7b 91
Roytsg 37:c0f6e3b44d7b 92 movmeanL = sumL/P; //dit is de moving average waarde
Roytsg 37:c0f6e3b44d7b 93
Roytsg 28:4b22455930ff 94 // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
Roytsg 28:4b22455930ff 95 if (TestButton==0 & k<200) {
Roytsg 36:7d7c16c688df 96 Vvector[k] = movmeanR;
Roytsg 28:4b22455930ff 97 ledB = !ledB;
Roytsg 28:4b22455930ff 98 k++;
Roytsg 28:4b22455930ff 99 }
Roytsg 28:4b22455930ff 100 else if (k==200) { // hier moet de test klaar zijn
Roytsg 28:4b22455930ff 101 double sumZ = 0;
Roytsg 28:4b22455930ff 102 for (int n = 0; n < 199; n++) {
Roytsg 28:4b22455930ff 103 sumZ = sumZ + Vvector[n];
Roytsg 28:4b22455930ff 104 } // neemt de som van de zerovector array
Roytsg 28:4b22455930ff 105 Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value
Roytsg 37:c0f6e3b44d7b 106 //scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
Roytsg 28:4b22455930ff 107 if (x == 1) {
Roytsg 37:c0f6e3b44d7b 108 // scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
Roytsg 28:4b22455930ff 109 }
Roytsg 28:4b22455930ff 110 k++;
Roytsg 28:4b22455930ff 111 ledB = 1;
Roytsg 28:4b22455930ff 112 ledG = !ledG;
Roytsg 28:4b22455930ff 113 }
Roytsg 28:4b22455930ff 114 else if (k == 201 && onoff ==0) {// dit is om het ledje uit te doen en om het mogelijk te maken de test opnieuw te doen
Roytsg 28:4b22455930ff 115 ledG = !ledG;
Roytsg 28:4b22455930ff 116 k = 0;
Roytsg 28:4b22455930ff 117 if (x==0) {
Roytsg 28:4b22455930ff 118 x++;
Roytsg 28:4b22455930ff 119 }
Roytsg 28:4b22455930ff 120 else if (x==1) {
Roytsg 28:4b22455930ff 121 x=0;
Roytsg 28:4b22455930ff 122 }
Roytsg 28:4b22455930ff 123 }
Roytsg 37:c0f6e3b44d7b 124 scope.set(0, movmeanR); // stuurt de moving average naar de plot
Roytsg 37:c0f6e3b44d7b 125 scope.set(1, movmeanL);
Roytsg 32:3832f732f17a 126
Roytsg 36:7d7c16c688df 127 if (movmeanR > Vwaarde[1]) {
Roytsg 36:7d7c16c688df 128 movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
Roytsg 32:3832f732f17a 129 }
Roytsg 32:3832f732f17a 130
Roytsg 36:7d7c16c688df 131 double EMGInputRawR = (movmeanR - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero);
Roytsg 36:7d7c16c688df 132 if (EMGInputRawR < 0) {
Roytsg 36:7d7c16c688df 133 EMGInputR = 0;
Roytsg 30:4c6321941515 134 }
Roytsg 30:4c6321941515 135 else {
Roytsg 36:7d7c16c688df 136 EMGInputR = EMGInputRawR;
Roytsg 30:4c6321941515 137 }
Roytsg 37:c0f6e3b44d7b 138 //scope.set(5,EMGInputR);
Roytsg 32:3832f732f17a 139
Roytsg 34:008bd226d37e 140 scope.send();
Roytsg 28:4b22455930ff 141
Roytsg 34:008bd226d37e 142 //motor2MagnitudePin = 0.2;
Roytsg 21:77998ce2c0dd 143 }
Roytsg 21:77998ce2c0dd 144
Roytsg 29:a48b63e60a40 145 float GetReferenceVelocity()
Roytsg 29:a48b63e60a40 146 {
Roytsg 29:a48b63e60a40 147 // Returns reference velocity in rad/s.
Roytsg 29:a48b63e60a40 148 // Positive value means clockwise rotation.
Roytsg 29:a48b63e60a40 149 const float maxVelocity=8.4; // in rad/s of course!
Roytsg 29:a48b63e60a40 150 float referenceVelocity; // in rad/s
Roytsg 36:7d7c16c688df 151 referenceVelocity = (EMGInputR * maxVelocity) * MotorLock;
Roytsg 29:a48b63e60a40 152 return referenceVelocity;
Roytsg 29:a48b63e60a40 153 }
Roytsg 21:77998ce2c0dd 154
Roytsg 29:a48b63e60a40 155 void SetMotor2(float motorValue)
Roytsg 29:a48b63e60a40 156 {
Roytsg 29:a48b63e60a40 157 // Given -1<=motorValue<=1, this sets the PWM and direction
Roytsg 29:a48b63e60a40 158 // bits for motor 1. Positive value makes motor rotating
Roytsg 29:a48b63e60a40 159 // clockwise. motorValues outside range are truncated to
Roytsg 29:a48b63e60a40 160 // within range
Roytsg 29:a48b63e60a40 161 if (fabs(motorValue)>1) motor2MagnitudePin = 1;
Roytsg 29:a48b63e60a40 162 else motor2MagnitudePin = fabs(motorValue);
Roytsg 29:a48b63e60a40 163 }
Roytsg 29:a48b63e60a40 164 float FeedForwardControl(float referenceVelocity) {
Roytsg 29:a48b63e60a40 165 // very simple linear feed-forward control
Roytsg 29:a48b63e60a40 166 const float MotorGain=8.4; // unit: (rad/s) / PWM
Roytsg 29:a48b63e60a40 167 float motorValue = referenceVelocity / MotorGain;
Roytsg 29:a48b63e60a40 168 return motorValue;
Roytsg 29:a48b63e60a40 169 }
Roytsg 29:a48b63e60a40 170
Roytsg 29:a48b63e60a40 171 void MotorLocker() {
Roytsg 29:a48b63e60a40 172 if (MotorLock == 0) {
Roytsg 29:a48b63e60a40 173 MotorLock = 1;
Roytsg 29:a48b63e60a40 174 }
Roytsg 29:a48b63e60a40 175 else if (MotorLock == 1) {
Roytsg 29:a48b63e60a40 176 MotorLock = 0;
Roytsg 29:a48b63e60a40 177 }
Roytsg 29:a48b63e60a40 178 }
Roytsg 28:4b22455930ff 179
vsluiter 0:32bb76391d89 180 int main()
Roytsg 28:4b22455930ff 181 {
Roytsg 28:4b22455930ff 182 ledB = 1;
Roytsg 28:4b22455930ff 183 ledG = 1;
Roytsg 36:7d7c16c688df 184 bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // hier wordt het filter gemaakt
Roytsg 37:c0f6e3b44d7b 185 bqcL.add( &bq1L ).add( &bq2L ).add( &bq3L ); // hier wordt het filter gemaakt
Roytsg 28:4b22455930ff 186 emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
Roytsg 34:008bd226d37e 187 Vwaarde[0] = 0.01;
Roytsg 34:008bd226d37e 188 motor2DirectionPin = 1;
Roytsg 29:a48b63e60a40 189
Roytsg 29:a48b63e60a40 190 while(1) {
Roytsg 35:5a8a7bd8ae59 191 if (Vwaarde[1] == 0 || x==1) {
Roytsg 35:5a8a7bd8ae59 192 MotorLock = 0;
Roytsg 35:5a8a7bd8ae59 193 }
Roytsg 35:5a8a7bd8ae59 194 else MotorLock = 1;
Roytsg 35:5a8a7bd8ae59 195 // MotorOn.rise(&MotorLocker);
Roytsg 36:7d7c16c688df 196 motor2MagnitudePin = EMGInputR*MotorLock;
Roytsg 34:008bd226d37e 197 wait(0.01);
Roytsg 29:a48b63e60a40 198 }
vsluiter 0:32bb76391d89 199 }