to send emg signal to motor with test programm
Dependencies: HIDScope biquadFilter mbed
Fork of TestProgramm by
main.cpp@36:7d7c16c688df, 2017-10-30 (annotated)
- Committer:
- Roytsg
- Date:
- Mon Oct 30 11:59:02 2017 +0000
- Revision:
- 36:7d7c16c688df
- Parent:
- 35:5a8a7bd8ae59
- Child:
- 37:c0f6e3b44d7b
added a R after every parameter that works with the right bicep emg-signal ;
Who changed what in which revision?
User | Revision | Line number | New 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 | 29:a48b63e60a40 | 9 | HIDScope scope( 6 ); // 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 | 28:4b22455930ff | 23 | int k = 0; // counter voor de configuratie |
Roytsg | 28:4b22455930ff | 24 | double Vvector[200]; // vector voor de Vwaarde configuratie |
Roytsg | 28:4b22455930ff | 25 | double Vwaarde[2]; // vector voor waardes van V |
Roytsg | 28:4b22455930ff | 26 | int x = 0; // x waarde voor de Vwaarde |
Roytsg | 36:7d7c16c688df | 27 | double movmeanR; //moving average for the right arm (first emg-shield) |
Roytsg | 30:4c6321941515 | 28 | int MotorLock = 0; |
Roytsg | 36:7d7c16c688df | 29 | double EMGInputR; |
Roytsg | 32:3832f732f17a | 30 | float ErrorZero = 3; |
Roytsg | 33:84d986230e15 | 31 | double MotorSpeed = 0.5; |
Roytsg | 24:fe3825337233 | 32 | |
Roytsg | 34:008bd226d37e | 33 | |
Roytsg | 28:4b22455930ff | 34 | // Filters |
Roytsg | 36:7d7c16c688df | 35 | BiQuadChain bqcR; |
Roytsg | 36:7d7c16c688df | 36 | BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp? |
Roytsg | 36:7d7c16c688df | 37 | BiQuad bq2R( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp? |
Roytsg | 36:7d7c16c688df | 38 | BiQuad bq3R( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch? |
vsluiter | 0:32bb76391d89 | 39 | |
Roytsg | 28:4b22455930ff | 40 | |
Roytsg | 21:77998ce2c0dd | 41 | |
Roytsg | 28:4b22455930ff | 42 | // sample function voor plotten van de emg signalen en moving average |
Roytsg | 21:77998ce2c0dd | 43 | void emgSample() { |
Roytsg | 21:77998ce2c0dd | 44 | |
Roytsg | 36:7d7c16c688df | 45 | double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal |
Roytsg | 36:7d7c16c688df | 46 | double emgabsR = abs(emgFilteredR); // absolute waarde van het gefilterde signaal |
Roytsg | 36:7d7c16c688df | 47 | scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek |
Roytsg | 36:7d7c16c688df | 48 | scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek |
Roytsg | 34:008bd226d37e | 49 | |
Roytsg | 24:fe3825337233 | 50 | |
Roytsg | 28:4b22455930ff | 51 | // deze for-loop maakt de vector voor de moving average |
Roytsg | 27:674193a62e06 | 52 | for(int i = P-1; i >= 0; i--){ |
Roytsg | 26:97a8adc9b895 | 53 | if (i == 0) { |
Roytsg | 36:7d7c16c688df | 54 | A[i] = emgabsR; |
Roytsg | 26:97a8adc9b895 | 55 | } |
Roytsg | 26:97a8adc9b895 | 56 | else { |
Roytsg | 26:97a8adc9b895 | 57 | A[i] = A[i-1]; |
Roytsg | 26:97a8adc9b895 | 58 | } |
Roytsg | 28:4b22455930ff | 59 | } |
Roytsg | 36:7d7c16c688df | 60 | double sumR = 0; |
Roytsg | 28:4b22455930ff | 61 | // deze for-loop sommeert de array |
Roytsg | 27:674193a62e06 | 62 | for (int n = 0; n < P-1; n++) { |
Roytsg | 36:7d7c16c688df | 63 | sumR = sumR + A[n]; |
Roytsg | 26:97a8adc9b895 | 64 | } |
Roytsg | 24:fe3825337233 | 65 | |
Roytsg | 36:7d7c16c688df | 66 | movmeanR = sumR/P; //dit is de moving average waarde |
Roytsg | 24:fe3825337233 | 67 | |
Roytsg | 28:4b22455930ff | 68 | // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen |
Roytsg | 28:4b22455930ff | 69 | if (TestButton==0 & k<200) { |
Roytsg | 36:7d7c16c688df | 70 | Vvector[k] = movmeanR; |
Roytsg | 28:4b22455930ff | 71 | ledB = !ledB; |
Roytsg | 28:4b22455930ff | 72 | k++; |
Roytsg | 28:4b22455930ff | 73 | } |
Roytsg | 28:4b22455930ff | 74 | else if (k==200) { // hier moet de test klaar zijn |
Roytsg | 28:4b22455930ff | 75 | double sumZ = 0; |
Roytsg | 28:4b22455930ff | 76 | for (int n = 0; n < 199; n++) { |
Roytsg | 28:4b22455930ff | 77 | sumZ = sumZ + Vvector[n]; |
Roytsg | 28:4b22455930ff | 78 | } // neemt de som van de zerovector array |
Roytsg | 28:4b22455930ff | 79 | Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value |
Roytsg | 28:4b22455930ff | 80 | scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje |
Roytsg | 28:4b22455930ff | 81 | if (x == 1) { |
Roytsg | 28:4b22455930ff | 82 | scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje |
Roytsg | 28:4b22455930ff | 83 | } |
Roytsg | 28:4b22455930ff | 84 | k++; |
Roytsg | 28:4b22455930ff | 85 | ledB = 1; |
Roytsg | 28:4b22455930ff | 86 | ledG = !ledG; |
Roytsg | 28:4b22455930ff | 87 | } |
Roytsg | 28:4b22455930ff | 88 | 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 | 89 | ledG = !ledG; |
Roytsg | 28:4b22455930ff | 90 | k = 0; |
Roytsg | 28:4b22455930ff | 91 | if (x==0) { |
Roytsg | 28:4b22455930ff | 92 | x++; |
Roytsg | 28:4b22455930ff | 93 | } |
Roytsg | 28:4b22455930ff | 94 | else if (x==1) { |
Roytsg | 28:4b22455930ff | 95 | x=0; |
Roytsg | 28:4b22455930ff | 96 | } |
Roytsg | 28:4b22455930ff | 97 | } |
Roytsg | 36:7d7c16c688df | 98 | scope.set(2, movmeanR); // stuurt de moving average naar de plot |
Roytsg | 32:3832f732f17a | 99 | |
Roytsg | 36:7d7c16c688df | 100 | if (movmeanR > Vwaarde[1]) { |
Roytsg | 36:7d7c16c688df | 101 | movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn |
Roytsg | 32:3832f732f17a | 102 | } |
Roytsg | 32:3832f732f17a | 103 | |
Roytsg | 36:7d7c16c688df | 104 | double EMGInputRawR = (movmeanR - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); |
Roytsg | 36:7d7c16c688df | 105 | if (EMGInputRawR < 0) { |
Roytsg | 36:7d7c16c688df | 106 | EMGInputR = 0; |
Roytsg | 30:4c6321941515 | 107 | } |
Roytsg | 30:4c6321941515 | 108 | else { |
Roytsg | 36:7d7c16c688df | 109 | EMGInputR = EMGInputRawR; |
Roytsg | 30:4c6321941515 | 110 | } |
Roytsg | 36:7d7c16c688df | 111 | scope.set(5,EMGInputR); |
Roytsg | 32:3832f732f17a | 112 | |
Roytsg | 34:008bd226d37e | 113 | scope.send(); |
Roytsg | 28:4b22455930ff | 114 | |
Roytsg | 34:008bd226d37e | 115 | //motor2MagnitudePin = 0.2; |
Roytsg | 21:77998ce2c0dd | 116 | } |
Roytsg | 21:77998ce2c0dd | 117 | |
Roytsg | 29:a48b63e60a40 | 118 | float GetReferenceVelocity() |
Roytsg | 29:a48b63e60a40 | 119 | { |
Roytsg | 29:a48b63e60a40 | 120 | // Returns reference velocity in rad/s. |
Roytsg | 29:a48b63e60a40 | 121 | // Positive value means clockwise rotation. |
Roytsg | 29:a48b63e60a40 | 122 | const float maxVelocity=8.4; // in rad/s of course! |
Roytsg | 29:a48b63e60a40 | 123 | float referenceVelocity; // in rad/s |
Roytsg | 36:7d7c16c688df | 124 | referenceVelocity = (EMGInputR * maxVelocity) * MotorLock; |
Roytsg | 29:a48b63e60a40 | 125 | return referenceVelocity; |
Roytsg | 29:a48b63e60a40 | 126 | } |
Roytsg | 21:77998ce2c0dd | 127 | |
Roytsg | 29:a48b63e60a40 | 128 | void SetMotor2(float motorValue) |
Roytsg | 29:a48b63e60a40 | 129 | { |
Roytsg | 29:a48b63e60a40 | 130 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Roytsg | 29:a48b63e60a40 | 131 | // bits for motor 1. Positive value makes motor rotating |
Roytsg | 29:a48b63e60a40 | 132 | // clockwise. motorValues outside range are truncated to |
Roytsg | 29:a48b63e60a40 | 133 | // within range |
Roytsg | 29:a48b63e60a40 | 134 | if (fabs(motorValue)>1) motor2MagnitudePin = 1; |
Roytsg | 29:a48b63e60a40 | 135 | else motor2MagnitudePin = fabs(motorValue); |
Roytsg | 29:a48b63e60a40 | 136 | } |
Roytsg | 29:a48b63e60a40 | 137 | float FeedForwardControl(float referenceVelocity) { |
Roytsg | 29:a48b63e60a40 | 138 | // very simple linear feed-forward control |
Roytsg | 29:a48b63e60a40 | 139 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
Roytsg | 29:a48b63e60a40 | 140 | float motorValue = referenceVelocity / MotorGain; |
Roytsg | 29:a48b63e60a40 | 141 | return motorValue; |
Roytsg | 29:a48b63e60a40 | 142 | } |
Roytsg | 29:a48b63e60a40 | 143 | |
Roytsg | 29:a48b63e60a40 | 144 | void MotorLocker() { |
Roytsg | 29:a48b63e60a40 | 145 | if (MotorLock == 0) { |
Roytsg | 29:a48b63e60a40 | 146 | MotorLock = 1; |
Roytsg | 29:a48b63e60a40 | 147 | } |
Roytsg | 29:a48b63e60a40 | 148 | else if (MotorLock == 1) { |
Roytsg | 29:a48b63e60a40 | 149 | MotorLock = 0; |
Roytsg | 29:a48b63e60a40 | 150 | } |
Roytsg | 29:a48b63e60a40 | 151 | } |
Roytsg | 28:4b22455930ff | 152 | |
vsluiter | 0:32bb76391d89 | 153 | int main() |
Roytsg | 28:4b22455930ff | 154 | { |
Roytsg | 28:4b22455930ff | 155 | ledB = 1; |
Roytsg | 28:4b22455930ff | 156 | ledG = 1; |
Roytsg | 36:7d7c16c688df | 157 | bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // hier wordt het filter gemaakt |
Roytsg | 28:4b22455930ff | 158 | emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz |
Roytsg | 34:008bd226d37e | 159 | Vwaarde[0] = 0.01; |
Roytsg | 34:008bd226d37e | 160 | motor2DirectionPin = 1; |
Roytsg | 29:a48b63e60a40 | 161 | |
Roytsg | 29:a48b63e60a40 | 162 | while(1) { |
Roytsg | 35:5a8a7bd8ae59 | 163 | if (Vwaarde[1] == 0 || x==1) { |
Roytsg | 35:5a8a7bd8ae59 | 164 | MotorLock = 0; |
Roytsg | 35:5a8a7bd8ae59 | 165 | } |
Roytsg | 35:5a8a7bd8ae59 | 166 | else MotorLock = 1; |
Roytsg | 35:5a8a7bd8ae59 | 167 | // MotorOn.rise(&MotorLocker); |
Roytsg | 36:7d7c16c688df | 168 | motor2MagnitudePin = EMGInputR*MotorLock; |
Roytsg | 34:008bd226d37e | 169 | wait(0.01); |
Roytsg | 29:a48b63e60a40 | 170 | } |
vsluiter | 0:32bb76391d89 | 171 | } |