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

Committer:
Rvs94
Date:
Tue Sep 29 16:58:52 2015 +0000
Revision:
14:baebaef79aa6
Parent:
13:a6770307a5d2
Child:
15:7fbee317af2d
Aansturing met pc toegevoegd. Waardes voor Kp, Ki en Kd zijn nog niet gevonden, systeem reageert nog vrij instabiel.

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