Roy Theussing / Mbed 2 deprecated TestProgrammEND

to send emg signal to motor with test programm

Dependencies:   HIDScope biquadFilter mbed

Fork of TestProgramm by Roy Theussing

main.cpp@30:4c6321941515, 2017-10-25 (annotated)

Committer:
Roytsg
Date:
Wed Oct 25 12:38:09 2017 +0000
Revision:
30:4c6321941515
Parent:
29:a48b63e60a40
Child:
31:620950f7e514
Child:
32:3832f732f17a
motor is aan stuurbaar met spier, motorlock werkt, errorzero naar 1.5 gezet anders is het moeilijk om de motor aan te sturen

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 28:4b22455930ff 5 #include "Serial.h"
vsluiter 0:32bb76391d89 6
Roytsg 24:fe3825337233 7
vsluiter 4:8b298dfada81 8 //Define objects
Roytsg 28:4b22455930ff 9 AnalogIn emg( A0 ); //EMG
Roytsg 28:4b22455930ff 10 AnalogIn emg1( A1 ); //EMG
Roytsg 29:a48b63e60a40 11 HIDScope scope( 6 ); // aantal scopes dat gemaakt kan worden
Roytsg 28:4b22455930ff 12 DigitalOut ledB(LED_BLUE);
Roytsg 28:4b22455930ff 13 DigitalOut ledG(LED_GREEN);
Roytsg 28:4b22455930ff 14 DigitalIn TestButton(PTA4); // button naast het ledje
Roytsg 28:4b22455930ff 15 DigitalIn onoff(PTC6); // button aan de andere kant
Roytsg 28:4b22455930ff 16 Ticker emgSampleTicker; // Ticker voor de sample frequency
Roytsg 29:a48b63e60a40 17 DigitalOut motor2MagnitudePin(D5); // magnitude motor 2
Roytsg 29:a48b63e60a40 18 DigitalOut motor2DirectionPin(D4); // direction of the motor 2
Roytsg 29:a48b63e60a40 19 InterruptIn MotorOn(D10);
vsluiter 2:e314bb3b2d99 20
Roytsg 28:4b22455930ff 21
Roytsg 28:4b22455930ff 22 int P= 200; // aantal test punten voor de moving average
Roytsg 28:4b22455930ff 23 double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P
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 29:a48b63e60a40 28 double movmean;
Roytsg 30:4c6321941515 29 int MotorLock = 0;
Roytsg 30:4c6321941515 30 double EMGInput;
Roytsg 30:4c6321941515 31 float ErrorZero = 1.5;
Roytsg 24:fe3825337233 32
Roytsg 28:4b22455930ff 33 // Filters
Roytsg 28:4b22455930ff 34 BiQuadChain bqc;
Roytsg 28:4b22455930ff 35 BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
Roytsg 28:4b22455930ff 36 BiQuad bq2( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp?
Roytsg 28:4b22455930ff 37 BiQuad bq3( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch?
vsluiter 0:32bb76391d89 38
Roytsg 28:4b22455930ff 39
Roytsg 21:77998ce2c0dd 40
Roytsg 28:4b22455930ff 41 // sample function voor plotten van de emg signalen en moving average
Roytsg 21:77998ce2c0dd 42 void emgSample() {
Roytsg 21:77998ce2c0dd 43
Roytsg 28:4b22455930ff 44 double emgFiltered = bqc.step( emg.read() ); // gefilterde waarde van het emg signaal
Roytsg 28:4b22455930ff 45 double emgabs = abs(emgFiltered); // absolute waarde van het gefilterde signaal
Roytsg 28:4b22455930ff 46 scope.set(0, emgFiltered ); // stuurt de waarden naar de grafiek
Roytsg 28:4b22455930ff 47 scope.set(1, emgabs ); // stuurt de waarden naar de grafiek
Roytsg 24:fe3825337233 48
Roytsg 28:4b22455930ff 49 // deze for-loop maakt de vector voor de moving average
Roytsg 27:674193a62e06 50 for(int i = P-1; i >= 0; i--){
Roytsg 26:97a8adc9b895 51 if (i == 0) {
Roytsg 26:97a8adc9b895 52 A[i] = emgabs;
Roytsg 26:97a8adc9b895 53 }
Roytsg 26:97a8adc9b895 54 else {
Roytsg 26:97a8adc9b895 55 A[i] = A[i-1];
Roytsg 26:97a8adc9b895 56 }
Roytsg 28:4b22455930ff 57 }
Roytsg 26:97a8adc9b895 58 double sum = 0;
Roytsg 28:4b22455930ff 59 // deze for-loop sommeert de array
Roytsg 27:674193a62e06 60 for (int n = 0; n < P-1; n++) {
Roytsg 26:97a8adc9b895 61 sum = sum + A[n];
Roytsg 26:97a8adc9b895 62 }
Roytsg 24:fe3825337233 63
Roytsg 29:a48b63e60a40 64 movmean = sum/P; //dit is de moving average waarde
Roytsg 24:fe3825337233 65
Roytsg 28:4b22455930ff 66 // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
Roytsg 28:4b22455930ff 67 if (TestButton==0 & k<200) {
Roytsg 28:4b22455930ff 68 Vvector[k] = movmean;
Roytsg 28:4b22455930ff 69 ledB = !ledB;
Roytsg 28:4b22455930ff 70 k++;
Roytsg 28:4b22455930ff 71 }
Roytsg 28:4b22455930ff 72 else if (k==200) { // hier moet de test klaar zijn
Roytsg 28:4b22455930ff 73 double sumZ = 0;
Roytsg 28:4b22455930ff 74 for (int n = 0; n < 199; n++) {
Roytsg 28:4b22455930ff 75 sumZ = sumZ + Vvector[n];
Roytsg 28:4b22455930ff 76 } // neemt de som van de zerovector array
Roytsg 28:4b22455930ff 77 Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value
Roytsg 28:4b22455930ff 78 scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
Roytsg 28:4b22455930ff 79 if (x == 1) {
Roytsg 28:4b22455930ff 80 scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
Roytsg 28:4b22455930ff 81 }
Roytsg 28:4b22455930ff 82 k++;
Roytsg 28:4b22455930ff 83 ledB = 1;
Roytsg 28:4b22455930ff 84 ledG = !ledG;
Roytsg 28:4b22455930ff 85 }
Roytsg 28:4b22455930ff 86 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 87 ledG = !ledG;
Roytsg 28:4b22455930ff 88 k = 0;
Roytsg 28:4b22455930ff 89 if (x==0) {
Roytsg 28:4b22455930ff 90 x++;
Roytsg 28:4b22455930ff 91 }
Roytsg 28:4b22455930ff 92 else if (x==1) {
Roytsg 28:4b22455930ff 93 x=0;
Roytsg 28:4b22455930ff 94 }
Roytsg 28:4b22455930ff 95 }
Roytsg 30:4c6321941515 96 double EMGInputRaw = (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero);
Roytsg 30:4c6321941515 97 if (EMGInputRaw < 0) {
Roytsg 30:4c6321941515 98 EMGInput = 0;
Roytsg 30:4c6321941515 99 }
Roytsg 30:4c6321941515 100 else {
Roytsg 30:4c6321941515 101 EMGInput = EMGInputRaw;
Roytsg 30:4c6321941515 102 }
Roytsg 29:a48b63e60a40 103 scope.set(5,EMGInput);
Roytsg 28:4b22455930ff 104 scope.set(2, movmean); // stuurt de moving average naar de plot
Roytsg 28:4b22455930ff 105
Roytsg 21:77998ce2c0dd 106 scope.send();
Roytsg 21:77998ce2c0dd 107 }
Roytsg 21:77998ce2c0dd 108
Roytsg 29:a48b63e60a40 109 float GetReferenceVelocity()
Roytsg 29:a48b63e60a40 110 {
Roytsg 29:a48b63e60a40 111 // Returns reference velocity in rad/s.
Roytsg 29:a48b63e60a40 112 // Positive value means clockwise rotation.
Roytsg 29:a48b63e60a40 113 const float maxVelocity=8.4; // in rad/s of course!
Roytsg 29:a48b63e60a40 114 float referenceVelocity; // in rad/s
Roytsg 30:4c6321941515 115 referenceVelocity = (EMGInput * maxVelocity) * MotorLock;
Roytsg 29:a48b63e60a40 116 return referenceVelocity;
Roytsg 29:a48b63e60a40 117 }
Roytsg 21:77998ce2c0dd 118
Roytsg 29:a48b63e60a40 119 void SetMotor2(float motorValue)
Roytsg 29:a48b63e60a40 120 {
Roytsg 29:a48b63e60a40 121 // Given -1<=motorValue<=1, this sets the PWM and direction
Roytsg 29:a48b63e60a40 122 // bits for motor 1. Positive value makes motor rotating
Roytsg 29:a48b63e60a40 123 // clockwise. motorValues outside range are truncated to
Roytsg 29:a48b63e60a40 124 // within range
Roytsg 29:a48b63e60a40 125 if (fabs(motorValue)>1) motor2MagnitudePin = 1;
Roytsg 29:a48b63e60a40 126 else motor2MagnitudePin = fabs(motorValue);
Roytsg 29:a48b63e60a40 127 }
Roytsg 29:a48b63e60a40 128 float FeedForwardControl(float referenceVelocity) {
Roytsg 29:a48b63e60a40 129 // very simple linear feed-forward control
Roytsg 29:a48b63e60a40 130 const float MotorGain=8.4; // unit: (rad/s) / PWM
Roytsg 29:a48b63e60a40 131 float motorValue = referenceVelocity / MotorGain;
Roytsg 29:a48b63e60a40 132 return motorValue;
Roytsg 29:a48b63e60a40 133 }
Roytsg 29:a48b63e60a40 134
Roytsg 29:a48b63e60a40 135 void MotorLocker() {
Roytsg 29:a48b63e60a40 136 if (MotorLock == 0) {
Roytsg 29:a48b63e60a40 137 MotorLock = 1;
Roytsg 29:a48b63e60a40 138 }
Roytsg 29:a48b63e60a40 139 else if (MotorLock == 1) {
Roytsg 29:a48b63e60a40 140 MotorLock = 0;
Roytsg 29:a48b63e60a40 141 }
Roytsg 29:a48b63e60a40 142 }
Roytsg 28:4b22455930ff 143
vsluiter 0:32bb76391d89 144 int main()
Roytsg 28:4b22455930ff 145 {
Roytsg 28:4b22455930ff 146 ledB = 1;
Roytsg 28:4b22455930ff 147 ledG = 1;
Roytsg 28:4b22455930ff 148 bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt
Roytsg 28:4b22455930ff 149 emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
Roytsg 29:a48b63e60a40 150
Roytsg 28:4b22455930ff 151
Roytsg 29:a48b63e60a40 152 while(1) {
Roytsg 29:a48b63e60a40 153 MotorOn.rise(&MotorLocker);
Roytsg 29:a48b63e60a40 154 motor2DirectionPin = 1;
Roytsg 29:a48b63e60a40 155 SetMotor2(FeedForwardControl(GetReferenceVelocity()));
Roytsg 29:a48b63e60a40 156 }
vsluiter 0:32bb76391d89 157 }