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@33:84d986230e15, 2017-10-26 (annotated)

Committer:
Roytsg
Date:
Thu Oct 26 07:55:14 2017 +0000
Revision:
33:84d986230e15
Parent:
32:3832f732f17a
Child:
34:008bd226d37e
test if the motor speed can change

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 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 29:a48b63e60a40 15 DigitalOut motor2MagnitudePin(D5); // magnitude motor 2
Roytsg 29:a48b63e60a40 16 DigitalOut motor2DirectionPin(D4); // direction of the motor 2
Roytsg 29:a48b63e60a40 17 InterruptIn MotorOn(D10);
Roytsg 33:84d986230e15 18 InterruptIn SpeedUp(D12);
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 29:a48b63e60a40 27 double movmean;
Roytsg 30:4c6321941515 28 int MotorLock = 0;
Roytsg 30:4c6321941515 29 double EMGInput;
Roytsg 32:3832f732f17a 30 float ErrorZero = 3;
Roytsg 33:84d986230e15 31 double MotorSpeed = 0.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 32:3832f732f17a 48 Vwaarde[0] = 0.01;
Roytsg 24:fe3825337233 49
Roytsg 28:4b22455930ff 50 // deze for-loop maakt de vector voor de moving average
Roytsg 27:674193a62e06 51 for(int i = P-1; i >= 0; i--){
Roytsg 26:97a8adc9b895 52 if (i == 0) {
Roytsg 26:97a8adc9b895 53 A[i] = emgabs;
Roytsg 26:97a8adc9b895 54 }
Roytsg 26:97a8adc9b895 55 else {
Roytsg 26:97a8adc9b895 56 A[i] = A[i-1];
Roytsg 26:97a8adc9b895 57 }
Roytsg 28:4b22455930ff 58 }
Roytsg 26:97a8adc9b895 59 double sum = 0;
Roytsg 28:4b22455930ff 60 // deze for-loop sommeert de array
Roytsg 27:674193a62e06 61 for (int n = 0; n < P-1; n++) {
Roytsg 26:97a8adc9b895 62 sum = sum + A[n];
Roytsg 26:97a8adc9b895 63 }
Roytsg 24:fe3825337233 64
Roytsg 29:a48b63e60a40 65 movmean = sum/P; //dit is de moving average waarde
Roytsg 24:fe3825337233 66
Roytsg 28:4b22455930ff 67 // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
Roytsg 28:4b22455930ff 68 if (TestButton==0 & k<200) {
Roytsg 28:4b22455930ff 69 Vvector[k] = movmean;
Roytsg 28:4b22455930ff 70 ledB = !ledB;
Roytsg 28:4b22455930ff 71 k++;
Roytsg 28:4b22455930ff 72 }
Roytsg 28:4b22455930ff 73 else if (k==200) { // hier moet de test klaar zijn
Roytsg 28:4b22455930ff 74 double sumZ = 0;
Roytsg 28:4b22455930ff 75 for (int n = 0; n < 199; n++) {
Roytsg 28:4b22455930ff 76 sumZ = sumZ + Vvector[n];
Roytsg 28:4b22455930ff 77 } // neemt de som van de zerovector array
Roytsg 28:4b22455930ff 78 Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value
Roytsg 28:4b22455930ff 79 scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
Roytsg 28:4b22455930ff 80 if (x == 1) {
Roytsg 28:4b22455930ff 81 scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
Roytsg 28:4b22455930ff 82 }
Roytsg 28:4b22455930ff 83 k++;
Roytsg 28:4b22455930ff 84 ledB = 1;
Roytsg 28:4b22455930ff 85 ledG = !ledG;
Roytsg 28:4b22455930ff 86 }
Roytsg 28:4b22455930ff 87 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 88 ledG = !ledG;
Roytsg 28:4b22455930ff 89 k = 0;
Roytsg 28:4b22455930ff 90 if (x==0) {
Roytsg 28:4b22455930ff 91 x++;
Roytsg 28:4b22455930ff 92 }
Roytsg 28:4b22455930ff 93 else if (x==1) {
Roytsg 28:4b22455930ff 94 x=0;
Roytsg 28:4b22455930ff 95 }
Roytsg 28:4b22455930ff 96 }
Roytsg 32:3832f732f17a 97 scope.set(2, movmean); // stuurt de moving average naar de plot
Roytsg 32:3832f732f17a 98
Roytsg 32:3832f732f17a 99 if (movmean > Vwaarde[1]) {
Roytsg 32:3832f732f17a 100 movmean = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
Roytsg 32:3832f732f17a 101 }
Roytsg 32:3832f732f17a 102
Roytsg 30:4c6321941515 103 double EMGInputRaw = (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero);
Roytsg 30:4c6321941515 104 if (EMGInputRaw < 0) {
Roytsg 30:4c6321941515 105 EMGInput = 0;
Roytsg 30:4c6321941515 106 }
Roytsg 30:4c6321941515 107 else {
Roytsg 30:4c6321941515 108 EMGInput = EMGInputRaw;
Roytsg 30:4c6321941515 109 }
Roytsg 29:a48b63e60a40 110 scope.set(5,EMGInput);
Roytsg 32:3832f732f17a 111
Roytsg 28:4b22455930ff 112
Roytsg 21:77998ce2c0dd 113 scope.send();
Roytsg 21:77998ce2c0dd 114 }
Roytsg 21:77998ce2c0dd 115
Roytsg 29:a48b63e60a40 116 float GetReferenceVelocity()
Roytsg 29:a48b63e60a40 117 {
Roytsg 29:a48b63e60a40 118 // Returns reference velocity in rad/s.
Roytsg 29:a48b63e60a40 119 // Positive value means clockwise rotation.
Roytsg 29:a48b63e60a40 120 const float maxVelocity=8.4; // in rad/s of course!
Roytsg 29:a48b63e60a40 121 float referenceVelocity; // in rad/s
Roytsg 30:4c6321941515 122 referenceVelocity = (EMGInput * maxVelocity) * MotorLock;
Roytsg 29:a48b63e60a40 123 return referenceVelocity;
Roytsg 29:a48b63e60a40 124 }
Roytsg 21:77998ce2c0dd 125
Roytsg 29:a48b63e60a40 126 void SetMotor2(float motorValue)
Roytsg 29:a48b63e60a40 127 {
Roytsg 29:a48b63e60a40 128 // Given -1<=motorValue<=1, this sets the PWM and direction
Roytsg 29:a48b63e60a40 129 // bits for motor 1. Positive value makes motor rotating
Roytsg 29:a48b63e60a40 130 // clockwise. motorValues outside range are truncated to
Roytsg 29:a48b63e60a40 131 // within range
Roytsg 29:a48b63e60a40 132 if (fabs(motorValue)>1) motor2MagnitudePin = 1;
Roytsg 29:a48b63e60a40 133 else motor2MagnitudePin = fabs(motorValue);
Roytsg 29:a48b63e60a40 134 }
Roytsg 29:a48b63e60a40 135 float FeedForwardControl(float referenceVelocity) {
Roytsg 29:a48b63e60a40 136 // very simple linear feed-forward control
Roytsg 29:a48b63e60a40 137 const float MotorGain=8.4; // unit: (rad/s) / PWM
Roytsg 29:a48b63e60a40 138 float motorValue = referenceVelocity / MotorGain;
Roytsg 29:a48b63e60a40 139 return motorValue;
Roytsg 29:a48b63e60a40 140 }
Roytsg 29:a48b63e60a40 141
Roytsg 29:a48b63e60a40 142 void MotorLocker() {
Roytsg 29:a48b63e60a40 143 if (MotorLock == 0) {
Roytsg 29:a48b63e60a40 144 MotorLock = 1;
Roytsg 29:a48b63e60a40 145 }
Roytsg 29:a48b63e60a40 146 else if (MotorLock == 1) {
Roytsg 29:a48b63e60a40 147 MotorLock = 0;
Roytsg 29:a48b63e60a40 148 }
Roytsg 29:a48b63e60a40 149 }
Roytsg 28:4b22455930ff 150
Roytsg 33:84d986230e15 151 void TestMotorSpeed() {
Roytsg 33:84d986230e15 152 if (MotorSpeed == 0.5) {
Roytsg 33:84d986230e15 153 MotorSpeed = 1.0;
Roytsg 33:84d986230e15 154 }
Roytsg 33:84d986230e15 155 else if (MotorSpeed == 1.0) {
Roytsg 33:84d986230e15 156 MotorSpeed == 0.5;
Roytsg 33:84d986230e15 157 }
Roytsg 33:84d986230e15 158 }
Roytsg 33:84d986230e15 159
vsluiter 0:32bb76391d89 160 int main()
Roytsg 28:4b22455930ff 161 {
Roytsg 28:4b22455930ff 162 ledB = 1;
Roytsg 28:4b22455930ff 163 ledG = 1;
Roytsg 28:4b22455930ff 164 bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt
Roytsg 28:4b22455930ff 165 emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
Roytsg 29:a48b63e60a40 166
Roytsg 28:4b22455930ff 167
Roytsg 29:a48b63e60a40 168 while(1) {
Roytsg 29:a48b63e60a40 169 MotorOn.rise(&MotorLocker);
Roytsg 29:a48b63e60a40 170 motor2DirectionPin = 1;
Roytsg 33:84d986230e15 171 //SetMotor2(FeedForwardControl(GetReferenceVelocity()));
Roytsg 33:84d986230e15 172 SpeedUp.rise(&TestMotorSpeed);
Roytsg 33:84d986230e15 173 motor2MagnitudePin = MotorSpeed;
Roytsg 29:a48b63e60a40 174 }
vsluiter 0:32bb76391d89 175 }