Margreeth de Breij / Mbed 2 deprecated frdm_MotorEMG

Motor programma met EMG

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Margreeth de Breij

main.cpp@20:f5091e29cd26, 2015-10-02 (annotated)

Committer:
Rvs94
Date:
Fri Oct 02 17:22:17 2015 +0000
Revision:
20:f5091e29cd26
Parent:
19:9417d2011e8b
Child:
21:cd7eb62183da
Child:
22:56c3a5918bfc
Child:
24:d0af4b2be295
Programma selectie werkend, alleen is het script instabiel (vermoedelijk door de hoeveelheid tickers). Volgende stap is het installeren van Go flags.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Margreeth95 0:284ed397e046 1 #include "mbed.h"
Margreeth95 0:284ed397e046 2 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 3 #include "HIDScope.h"
Margreeth95 0:284ed397e046 4 #include "QEI.h"
Rvs94 12:69ab81cf5b7d 5 #include "biquadFilter.h"
Margreeth95 0:284ed397e046 6
Margreeth95 0:284ed397e046 7 Serial pc(USBTX, USBRX); // tx, rx
Rvs94 20:f5091e29cd26 8 QEI Encoder(D3, D2, NC, 128);
Rvs94 20:f5091e29cd26 9 HIDScope scope(3);
Rvs94 20:f5091e29cd26 10
Rvs94 20:f5091e29cd26 11 //Ledjes
Margreeth95 19:9417d2011e8b 12 DigitalOut LedR(LED_RED);
Margreeth95 19:9417d2011e8b 13 DigitalOut LedG(LED_GREEN);
Margreeth95 19:9417d2011e8b 14 DigitalOut LedB(LED_BLUE);
Rvs94 20:f5091e29cd26 15
Rvs94 20:f5091e29cd26 16 //Motor
Margreeth95 0:284ed397e046 17 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
Margreeth95 0:284ed397e046 18 PwmOut motor2speed(D5);
Rvs94 12:69ab81cf5b7d 19
Rvs94 20:f5091e29cd26 20 //Tickers
Margreeth95 0:284ed397e046 21 Ticker ScopeTime;
Rvs94 9:774fc3c6a39e 22 Ticker myControllerTicker;
Rvs94 20:f5091e29cd26 23 Ticker PrintTime;
Rvs94 7:67b50d4fb03c 24
Rvs94 20:f5091e29cd26 25 //Startwaarden
Rvs94 10:80fe931a71e4 26 double reference;
Rvs94 10:80fe931a71e4 27 double position;
Rvs94 15:7fbee317af2d 28 double m2_ref = 0;
Margreeth95 19:9417d2011e8b 29 int count = 0;
Rvs94 2:099da0fc31b6 30
Rvs94 20:f5091e29cd26 31 //Sample time (motor2-step)
Rvs94 20:f5091e29cd26 32 const double m2_Ts = 0.01;
Rvs94 20:f5091e29cd26 33
Rvs94 20:f5091e29cd26 34 //Controller gain Motor 2
Rvs94 20:f5091e29cd26 35 const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
Rvs94 20:f5091e29cd26 36 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 20:f5091e29cd26 37
Rvs94 20:f5091e29cd26 38 //Derivative filter coeffs Motor 2
Rvs94 20:f5091e29cd26 39 const double BiGain = 0.016955;
Rvs94 20:f5091e29cd26 40 const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
Rvs94 20:f5091e29cd26 41
Rvs94 20:f5091e29cd26 42 // Filter variables
Rvs94 20:f5091e29cd26 43 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 20:f5091e29cd26 44
Rvs94 20:f5091e29cd26 45
Rvs94 20:f5091e29cd26 46 //HIDScope
Margreeth95 0:284ed397e046 47 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 48 {
Margreeth95 0:284ed397e046 49 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 50 scope.set(1, motor2speed.read());
Rvs94 15:7fbee317af2d 51 scope.set(2, position);
Rvs94 4:0d4aff8b57b3 52
Margreeth95 0:284ed397e046 53 scope.send();
Rvs94 1:48aba8d5610a 54
Margreeth95 0:284ed397e046 55 }
Rvs94 12:69ab81cf5b7d 56
Rvs94 20:f5091e29cd26 57 // PC Printf
Rvs94 20:f5091e29cd26 58 void PCPrintf()
Rvs94 20:f5091e29cd26 59 {
Rvs94 20:f5091e29cd26 60 pc.printf("position = %f aantal degs = %f, count = %i\n",reference,position,count);
Rvs94 20:f5091e29cd26 61 }
Rvs94 10:80fe931a71e4 62
Rvs94 12:69ab81cf5b7d 63 // Biquad filter
Rvs94 12:69ab81cf5b7d 64 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
Rvs94 9:774fc3c6a39e 65 {
Rvs94 13:a6770307a5d2 66 double v = u - a1*v1 - a2*v2;
Rvs94 13:a6770307a5d2 67 double y = b0*v + b1*v1 + b2*v2;
Rvs94 12:69ab81cf5b7d 68 v2 = v1; v1 = v;
Rvs94 12:69ab81cf5b7d 69 return y;
Rvs94 12:69ab81cf5b7d 70 }
Rvs94 12:69ab81cf5b7d 71
Rvs94 12:69ab81cf5b7d 72
Rvs94 12:69ab81cf5b7d 73 // Reusable PID controller
Rvs94 12:69ab81cf5b7d 74 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
Rvs94 13:a6770307a5d2 75 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 12:69ab81cf5b7d 76 {
Rvs94 12:69ab81cf5b7d 77 // Derivative
Rvs94 12:69ab81cf5b7d 78 double e_der = (e-e_prev)/Ts;
Rvs94 12:69ab81cf5b7d 79 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 13:a6770307a5d2 80 e_prev = e;
Rvs94 12:69ab81cf5b7d 81 // Integral
Rvs94 12:69ab81cf5b7d 82 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 83 // PID
Rvs94 12:69ab81cf5b7d 84 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 9:774fc3c6a39e 85 }
Margreeth95 0:284ed397e046 86
Margreeth95 18:6f71bb91b8bd 87 // Motor2 control
Rvs94 11:0793a78109a2 88 void motor2_Controller()
Rvs94 9:774fc3c6a39e 89 {
Rvs94 15:7fbee317af2d 90 reference = m2_ref; // Setpoint
Margreeth95 18:6f71bb91b8bd 91 double pulses = Encoder.getPulses();
Margreeth95 19:9417d2011e8b 92 position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
Rvs94 13:a6770307a5d2 93 double P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,
Rvs94 12:69ab81cf5b7d 94 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 15:7fbee317af2d 95 motor2speed = abs(P2); // Speed control
Rvs94 15:7fbee317af2d 96 if(P2 > 0) // Direction control
Rvs94 9:774fc3c6a39e 97 {
Rvs94 9:774fc3c6a39e 98 motor2direction = 0;
Rvs94 9:774fc3c6a39e 99 }
Rvs94 9:774fc3c6a39e 100 else
Rvs94 9:774fc3c6a39e 101 {
Rvs94 9:774fc3c6a39e 102 motor2direction = 1;
Rvs94 9:774fc3c6a39e 103 }
Rvs94 20:f5091e29cd26 104
Rvs94 9:774fc3c6a39e 105 }
Rvs94 3:687729d7996e 106
Margreeth95 0:284ed397e046 107 int main()
Rvs94 9:774fc3c6a39e 108 {
Rvs94 20:f5091e29cd26 109 LedR.write(1);
Rvs94 20:f5091e29cd26 110 LedB.write(1);
Rvs94 20:f5091e29cd26 111 LedG.write(1);
Margreeth95 0:284ed397e046 112 pc.baud(115200);
Rvs94 3:687729d7996e 113 pc.printf("Tot aan loop werkt\n");
Rvs94 9:774fc3c6a39e 114
Margreeth95 0:284ed397e046 115 ScopeTime.attach_us(&ScopeSend, 10e4);
Rvs94 11:0793a78109a2 116 myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
Rvs94 20:f5091e29cd26 117 PrintTime.attach(&PCPrintf,0.1f);
Rvs94 9:774fc3c6a39e 118 while(true)
Rvs94 20:f5091e29cd26 119 {
Rvs94 20:f5091e29cd26 120 char c = pc.getc();
Rvs94 20:f5091e29cd26 121 if(c == 'e') //Ga 1 programma omhoog
Rvs94 20:f5091e29cd26 122 {
Rvs94 20:f5091e29cd26 123 count = count + 1;
Rvs94 20:f5091e29cd26 124 if(count > 2)
Rvs94 20:f5091e29cd26 125 {
Rvs94 20:f5091e29cd26 126 count = 2;
Rvs94 20:f5091e29cd26 127 }
Rvs94 20:f5091e29cd26 128
Rvs94 20:f5091e29cd26 129 }
Rvs94 20:f5091e29cd26 130 if(c == 'd') //Ga 1 programma omlaag
Rvs94 20:f5091e29cd26 131 {
Rvs94 20:f5091e29cd26 132 count = count - 1;
Rvs94 20:f5091e29cd26 133 if(count < 0)
Rvs94 20:f5091e29cd26 134 {
Rvs94 20:f5091e29cd26 135 count = 0;
Rvs94 20:f5091e29cd26 136 }
Rvs94 20:f5091e29cd26 137 }
Rvs94 20:f5091e29cd26 138 if(count == 0) //Motor 1 control
Rvs94 20:f5091e29cd26 139 {
Rvs94 20:f5091e29cd26 140
Rvs94 20:f5091e29cd26 141 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 142 LedG = 0;
Margreeth95 19:9417d2011e8b 143 if(c == 'r')
Margreeth95 19:9417d2011e8b 144 {
Margreeth95 19:9417d2011e8b 145 m2_ref = m2_ref + 5;
Margreeth95 19:9417d2011e8b 146 if (m2_ref > 90)
Margreeth95 19:9417d2011e8b 147 {
Margreeth95 19:9417d2011e8b 148 m2_ref = 90;
Margreeth95 19:9417d2011e8b 149 }
Margreeth95 19:9417d2011e8b 150 }
Margreeth95 19:9417d2011e8b 151 if(c == 'f')
Margreeth95 19:9417d2011e8b 152 {
Margreeth95 19:9417d2011e8b 153 m2_ref = m2_ref - 5;
Margreeth95 19:9417d2011e8b 154 if (m2_ref < -90)
Margreeth95 19:9417d2011e8b 155 {
Margreeth95 19:9417d2011e8b 156 m2_ref = -90;
Margreeth95 19:9417d2011e8b 157 }
Margreeth95 19:9417d2011e8b 158 }
Rvs94 20:f5091e29cd26 159 }
Rvs94 20:f5091e29cd26 160 if(count == 1) //Motor 2 control
Rvs94 20:f5091e29cd26 161 {
Rvs94 20:f5091e29cd26 162 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 163 LedR = 0;
Rvs94 20:f5091e29cd26 164 }
Rvs94 20:f5091e29cd26 165 if(count == 2) //Vuur mechanisme
Rvs94 20:f5091e29cd26 166 {
Rvs94 20:f5091e29cd26 167
Rvs94 20:f5091e29cd26 168 LedR = LedG = 1;
Rvs94 20:f5091e29cd26 169 LedB = 0;
Rvs94 20:f5091e29cd26 170 }
Margreeth95 0:284ed397e046 171 }
Rvs94 9:774fc3c6a39e 172
Margreeth95 0:284ed397e046 173 }