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@15:7fbee317af2d, 2015-09-29 (annotated)

Committer:
Rvs94
Date:
Tue Sep 29 17:15:46 2015 +0000
Revision:
15:7fbee317af2d
Parent:
14:baebaef79aa6
Child:
16:5b729bd56155
Printf verplaatst, nu hebben we directere feedback in putty! nu begint de zoektocht naar de goede gain control vars.

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
Margreeth95 0:284ed397e046 8 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
Margreeth95 0:284ed397e046 9 PwmOut motor2speed(D5);
Rvs94 9:774fc3c6a39e 10 AnalogIn potmeter2(A5);
Rvs94 5:455773cf460b 11 QEI Encoder(D3, D2, NC, 128);
Rvs94 1:48aba8d5610a 12 HIDScope scope(3);
Rvs94 12:69ab81cf5b7d 13
Margreeth95 0:284ed397e046 14 Ticker ScopeTime;
Rvs94 9:774fc3c6a39e 15 Ticker myControllerTicker;
Rvs94 7:67b50d4fb03c 16
Rvs94 10:80fe931a71e4 17 double reference;
Rvs94 10:80fe931a71e4 18 double position;
Rvs94 15:7fbee317af2d 19 double m2_ref = 0;
Rvs94 2:099da0fc31b6 20
Margreeth95 0:284ed397e046 21 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 22 {
Margreeth95 0:284ed397e046 23 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 24 scope.set(1, motor2speed.read());
Rvs94 15:7fbee317af2d 25 scope.set(2, position);
Rvs94 4:0d4aff8b57b3 26
Margreeth95 0:284ed397e046 27 scope.send();
Rvs94 1:48aba8d5610a 28
Margreeth95 0:284ed397e046 29 }
Rvs94 12:69ab81cf5b7d 30 // Sample time (motor2-step)
Rvs94 12:69ab81cf5b7d 31 const double m2_Ts = 0.01;
Rvs94 12:69ab81cf5b7d 32
Rvs94 9:774fc3c6a39e 33 // Controller gain
Rvs94 14:baebaef79aa6 34 const double m2_Kp = 0.07,m2_Ki = 0.005, m2_Kd = 0.001;
Rvs94 13:a6770307a5d2 35 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 12:69ab81cf5b7d 36
Rvs94 12:69ab81cf5b7d 37 //Derivative filter coeffs
Rvs94 13:a6770307a5d2 38 const double BiGain = 0.016955;
Rvs94 13:a6770307a5d2 39 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 12:69ab81cf5b7d 40 // Filter variables
Rvs94 12:69ab81cf5b7d 41 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 10:80fe931a71e4 42
Rvs94 12:69ab81cf5b7d 43 // Biquad filter
Rvs94 12:69ab81cf5b7d 44 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 45 {
Rvs94 13:a6770307a5d2 46 double v = u - a1*v1 - a2*v2;
Rvs94 13:a6770307a5d2 47 double y = b0*v + b1*v1 + b2*v2;
Rvs94 12:69ab81cf5b7d 48 v2 = v1; v1 = v;
Rvs94 12:69ab81cf5b7d 49 return y;
Rvs94 12:69ab81cf5b7d 50 }
Rvs94 12:69ab81cf5b7d 51
Rvs94 12:69ab81cf5b7d 52
Rvs94 12:69ab81cf5b7d 53 // Reusable PID controller
Rvs94 12:69ab81cf5b7d 54 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 55 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 12:69ab81cf5b7d 56 {
Rvs94 12:69ab81cf5b7d 57 // Derivative
Rvs94 12:69ab81cf5b7d 58 double e_der = (e-e_prev)/Ts;
Rvs94 12:69ab81cf5b7d 59 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 13:a6770307a5d2 60 e_prev = e;
Rvs94 12:69ab81cf5b7d 61 // Integral
Rvs94 12:69ab81cf5b7d 62 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 63 // PID
Rvs94 12:69ab81cf5b7d 64 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 9:774fc3c6a39e 65 }
Margreeth95 0:284ed397e046 66
Rvs94 12:69ab81cf5b7d 67 // Motor control
Rvs94 11:0793a78109a2 68 void motor2_Controller()
Rvs94 9:774fc3c6a39e 69 {
Rvs94 15:7fbee317af2d 70 reference = m2_ref; // Setpoint
Rvs94 10:80fe931a71e4 71 position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
Rvs94 13:a6770307a5d2 72 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 73 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 15:7fbee317af2d 74 motor2speed = abs(P2); // Speed control
Rvs94 15:7fbee317af2d 75 if(P2 > 0) // Direction control
Rvs94 9:774fc3c6a39e 76 {
Rvs94 9:774fc3c6a39e 77 motor2direction = 0;
Rvs94 9:774fc3c6a39e 78 }
Rvs94 9:774fc3c6a39e 79 else
Rvs94 9:774fc3c6a39e 80 {
Rvs94 9:774fc3c6a39e 81 motor2direction = 1;
Rvs94 9:774fc3c6a39e 82 }
Rvs94 15:7fbee317af2d 83 pc.printf("position = %f aantal degs = %f \n",reference,position);
Rvs94 9:774fc3c6a39e 84 }
Rvs94 3:687729d7996e 85
Margreeth95 0:284ed397e046 86 int main()
Rvs94 9:774fc3c6a39e 87 {
Margreeth95 0:284ed397e046 88 pc.baud(115200);
Rvs94 3:687729d7996e 89 pc.printf("Tot aan loop werkt\n");
Rvs94 9:774fc3c6a39e 90
Margreeth95 0:284ed397e046 91 ScopeTime.attach_us(&ScopeSend, 10e4);
Rvs94 11:0793a78109a2 92 myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
Rvs94 9:774fc3c6a39e 93 while(true)
Margreeth95 0:284ed397e046 94 {
Rvs94 14:baebaef79aa6 95 char c = pc.getc();
Rvs94 14:baebaef79aa6 96 if(c == 'r')
Rvs94 14:baebaef79aa6 97 {
Rvs94 14:baebaef79aa6 98 m2_ref = m2_ref + 5;
Rvs94 14:baebaef79aa6 99 }
Rvs94 14:baebaef79aa6 100 if(c == 'f')
Rvs94 14:baebaef79aa6 101 {
Rvs94 14:baebaef79aa6 102 m2_ref = m2_ref - 5;
Rvs94 14:baebaef79aa6 103 }
Margreeth95 0:284ed397e046 104 }
Rvs94 9:774fc3c6a39e 105
Margreeth95 0:284ed397e046 106 }