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@21:cd7eb62183da, 2015-10-02 (annotated)

Committer:
Rvs94
Date:
Fri Oct 02 18:15:24 2015 +0000
Revision:
21:cd7eb62183da
Parent:
20:f5091e29cd26
Child:
23:6b7695ffadcb
Go flags toegevoegd, systeem werkt totaal niet. Motor gaat trillen en ledjes gaan willekeurig knipperen

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
Rvs94 21:cd7eb62183da 21 Ticker t1, t2, t3, t4;
Rvs94 21:cd7eb62183da 22 volatile bool fn1_go = false, fn2_go = false, fn3_go = false, fn4_go = false;
Rvs94 21:cd7eb62183da 23 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
Rvs94 21:cd7eb62183da 24 void fn2_activate(){ fn2_go = true; };
Rvs94 21:cd7eb62183da 25 void fn3_activate(){ fn3_go = true; };
Rvs94 21:cd7eb62183da 26 void fn4_activate(){ fn4_go = true; };
Rvs94 7:67b50d4fb03c 27
Rvs94 20:f5091e29cd26 28 //Startwaarden
Rvs94 10:80fe931a71e4 29 double reference;
Rvs94 10:80fe931a71e4 30 double position;
Rvs94 15:7fbee317af2d 31 double m2_ref = 0;
Margreeth95 19:9417d2011e8b 32 int count = 0;
Rvs94 2:099da0fc31b6 33
Rvs94 20:f5091e29cd26 34 //Sample time (motor2-step)
Rvs94 20:f5091e29cd26 35 const double m2_Ts = 0.01;
Rvs94 20:f5091e29cd26 36
Rvs94 20:f5091e29cd26 37 //Controller gain Motor 2
Rvs94 20:f5091e29cd26 38 const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
Rvs94 20:f5091e29cd26 39 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 20:f5091e29cd26 40
Rvs94 20:f5091e29cd26 41 //Derivative filter coeffs Motor 2
Rvs94 20:f5091e29cd26 42 const double BiGain = 0.016955;
Rvs94 20:f5091e29cd26 43 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 44
Rvs94 20:f5091e29cd26 45 // Filter variables
Rvs94 20:f5091e29cd26 46 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 20:f5091e29cd26 47
Rvs94 20:f5091e29cd26 48
Rvs94 20:f5091e29cd26 49 //HIDScope
Margreeth95 0:284ed397e046 50 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 51 {
Margreeth95 0:284ed397e046 52 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 53 scope.set(1, motor2speed.read());
Rvs94 15:7fbee317af2d 54 scope.set(2, position);
Rvs94 4:0d4aff8b57b3 55
Margreeth95 0:284ed397e046 56 scope.send();
Rvs94 1:48aba8d5610a 57
Margreeth95 0:284ed397e046 58 }
Rvs94 12:69ab81cf5b7d 59
Rvs94 20:f5091e29cd26 60 // PC Printf
Rvs94 20:f5091e29cd26 61 void PCPrintf()
Rvs94 20:f5091e29cd26 62 {
Rvs94 20:f5091e29cd26 63 pc.printf("position = %f aantal degs = %f, count = %i\n",reference,position,count);
Rvs94 20:f5091e29cd26 64 }
Rvs94 10:80fe931a71e4 65
Rvs94 12:69ab81cf5b7d 66 // Biquad filter
Rvs94 12:69ab81cf5b7d 67 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 68 {
Rvs94 13:a6770307a5d2 69 double v = u - a1*v1 - a2*v2;
Rvs94 13:a6770307a5d2 70 double y = b0*v + b1*v1 + b2*v2;
Rvs94 12:69ab81cf5b7d 71 v2 = v1; v1 = v;
Rvs94 12:69ab81cf5b7d 72 return y;
Rvs94 12:69ab81cf5b7d 73 }
Rvs94 12:69ab81cf5b7d 74
Rvs94 12:69ab81cf5b7d 75
Rvs94 12:69ab81cf5b7d 76 // Reusable PID controller
Rvs94 12:69ab81cf5b7d 77 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 78 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 12:69ab81cf5b7d 79 {
Rvs94 12:69ab81cf5b7d 80 // Derivative
Rvs94 12:69ab81cf5b7d 81 double e_der = (e-e_prev)/Ts;
Rvs94 12:69ab81cf5b7d 82 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 13:a6770307a5d2 83 e_prev = e;
Rvs94 12:69ab81cf5b7d 84 // Integral
Rvs94 12:69ab81cf5b7d 85 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 86 // PID
Rvs94 12:69ab81cf5b7d 87 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 9:774fc3c6a39e 88 }
Margreeth95 0:284ed397e046 89
Margreeth95 18:6f71bb91b8bd 90 // Motor2 control
Rvs94 11:0793a78109a2 91 void motor2_Controller()
Rvs94 9:774fc3c6a39e 92 {
Rvs94 15:7fbee317af2d 93 reference = m2_ref; // Setpoint
Margreeth95 19:9417d2011e8b 94 position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
Rvs94 13:a6770307a5d2 95 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 96 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 15:7fbee317af2d 97 motor2speed = abs(P2); // Speed control
Rvs94 15:7fbee317af2d 98 if(P2 > 0) // Direction control
Rvs94 9:774fc3c6a39e 99 {
Rvs94 9:774fc3c6a39e 100 motor2direction = 0;
Rvs94 9:774fc3c6a39e 101 }
Rvs94 9:774fc3c6a39e 102 else
Rvs94 9:774fc3c6a39e 103 {
Rvs94 9:774fc3c6a39e 104 motor2direction = 1;
Rvs94 9:774fc3c6a39e 105 }
Rvs94 20:f5091e29cd26 106
Rvs94 9:774fc3c6a39e 107 }
Rvs94 3:687729d7996e 108
Margreeth95 0:284ed397e046 109 int main()
Rvs94 9:774fc3c6a39e 110 {
Rvs94 20:f5091e29cd26 111 LedR.write(1);
Rvs94 20:f5091e29cd26 112 LedB.write(1);
Rvs94 20:f5091e29cd26 113 LedG.write(1);
Margreeth95 0:284ed397e046 114 pc.baud(115200);
Rvs94 3:687729d7996e 115 pc.printf("Tot aan loop werkt\n");
Rvs94 21:cd7eb62183da 116
Rvs94 9:774fc3c6a39e 117
Rvs94 21:cd7eb62183da 118 t1.attach( &fn1_activate, 0.01f); // Function@100hz
Rvs94 21:cd7eb62183da 119 t2.attach( &fn2_activate, 0.001f); // Function@ 1khz
Rvs94 21:cd7eb62183da 120 t3.attach( &fn3_activate, 0.2f); // Function@ 5hz
Rvs94 21:cd7eb62183da 121 t4.attach( &fn3_activate, 0.02f); // Function@ 50hz
Rvs94 21:cd7eb62183da 122
Rvs94 9:774fc3c6a39e 123 while(true)
Rvs94 20:f5091e29cd26 124 {
Rvs94 20:f5091e29cd26 125 char c = pc.getc();
Rvs94 21:cd7eb62183da 126 if(fn1_go)
Rvs94 21:cd7eb62183da 127 {
Rvs94 21:cd7eb62183da 128 fn1_go = false;
Rvs94 21:cd7eb62183da 129 motor2_Controller();
Rvs94 21:cd7eb62183da 130 }
Rvs94 21:cd7eb62183da 131 if(fn2_go)
Rvs94 21:cd7eb62183da 132 {
Rvs94 21:cd7eb62183da 133 fn2_go = false;
Rvs94 21:cd7eb62183da 134 ScopeSend();
Rvs94 21:cd7eb62183da 135 }
Rvs94 21:cd7eb62183da 136 if(fn3_go)
Rvs94 21:cd7eb62183da 137 {
Rvs94 21:cd7eb62183da 138 fn3_go = false;
Rvs94 21:cd7eb62183da 139 PCPrintf();
Rvs94 21:cd7eb62183da 140 }
Rvs94 21:cd7eb62183da 141
Rvs94 20:f5091e29cd26 142 if(c == 'e') //Ga 1 programma omhoog
Rvs94 20:f5091e29cd26 143 {
Rvs94 21:cd7eb62183da 144 count = count + 1;
Rvs94 20:f5091e29cd26 145 if(count > 2)
Rvs94 21:cd7eb62183da 146 {
Rvs94 21:cd7eb62183da 147 count = 2;
Rvs94 21:cd7eb62183da 148 }
Rvs94 20:f5091e29cd26 149 }
Rvs94 21:cd7eb62183da 150
Rvs94 20:f5091e29cd26 151 if(c == 'd') //Ga 1 programma omlaag
Rvs94 20:f5091e29cd26 152 {
Rvs94 20:f5091e29cd26 153 count = count - 1;
Rvs94 20:f5091e29cd26 154 if(count < 0)
Rvs94 21:cd7eb62183da 155 {
Rvs94 21:cd7eb62183da 156 count = 0;
Rvs94 21:cd7eb62183da 157 }
Rvs94 21:cd7eb62183da 158 }
Rvs94 21:cd7eb62183da 159
Rvs94 20:f5091e29cd26 160 if(count == 0) //Motor 1 control
Rvs94 20:f5091e29cd26 161 {
Rvs94 20:f5091e29cd26 162
Rvs94 20:f5091e29cd26 163 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 164 LedG = 0;
Margreeth95 19:9417d2011e8b 165 if(c == 'r')
Margreeth95 19:9417d2011e8b 166 {
Margreeth95 19:9417d2011e8b 167 m2_ref = m2_ref + 5;
Margreeth95 19:9417d2011e8b 168 if (m2_ref > 90)
Margreeth95 19:9417d2011e8b 169 {
Margreeth95 19:9417d2011e8b 170 m2_ref = 90;
Margreeth95 19:9417d2011e8b 171 }
Margreeth95 19:9417d2011e8b 172 }
Margreeth95 19:9417d2011e8b 173 if(c == 'f')
Margreeth95 19:9417d2011e8b 174 {
Margreeth95 19:9417d2011e8b 175 m2_ref = m2_ref - 5;
Margreeth95 19:9417d2011e8b 176 if (m2_ref < -90)
Margreeth95 19:9417d2011e8b 177 {
Margreeth95 19:9417d2011e8b 178 m2_ref = -90;
Margreeth95 19:9417d2011e8b 179 }
Margreeth95 19:9417d2011e8b 180 }
Rvs94 20:f5091e29cd26 181 }
Rvs94 20:f5091e29cd26 182 if(count == 1) //Motor 2 control
Rvs94 20:f5091e29cd26 183 {
Rvs94 20:f5091e29cd26 184 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 185 LedR = 0;
Rvs94 20:f5091e29cd26 186 }
Rvs94 20:f5091e29cd26 187 if(count == 2) //Vuur mechanisme
Rvs94 20:f5091e29cd26 188 {
Rvs94 20:f5091e29cd26 189
Rvs94 20:f5091e29cd26 190 LedR = LedG = 1;
Rvs94 20:f5091e29cd26 191 LedB = 0;
Rvs94 20:f5091e29cd26 192 }
Margreeth95 0:284ed397e046 193 }
Rvs94 9:774fc3c6a39e 194
Margreeth95 0:284ed397e046 195 }