Margreeth de Breij / Mbed 2 deprecated frdm_Motor_V2_2

Definitieve motorscript in aanbouw

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_2 by Robert Schulte

main.cpp@17:9b667e6e1290, 2015-09-30 (annotated)

Committer:
Margreeth95
Date:
Wed Sep 30 15:20:14 2015 +0000
Revision:
17:9b667e6e1290
Parent:
16:5b729bd56155
Child:
18:6f71bb91b8bd
Motor draait naar referentiepositie, die nooit buiten de gestelde grenzen mag gaan

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 16:5b729bd56155 34 const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
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;
Margreeth95 17:9b667e6e1290 99 if (m2_ref > 90)
Margreeth95 17:9b667e6e1290 100 {
Margreeth95 17:9b667e6e1290 101 m2_ref = 90;
Margreeth95 17:9b667e6e1290 102 }
Rvs94 14:baebaef79aa6 103 }
Rvs94 14:baebaef79aa6 104 if(c == 'f')
Rvs94 14:baebaef79aa6 105 {
Rvs94 14:baebaef79aa6 106 m2_ref = m2_ref - 5;
Margreeth95 17:9b667e6e1290 107 if (m2_ref < -90)
Margreeth95 17:9b667e6e1290 108 {
Margreeth95 17:9b667e6e1290 109 m2_ref = -90;
Margreeth95 17:9b667e6e1290 110 }
Rvs94 14:baebaef79aa6 111 }
Margreeth95 0:284ed397e046 112 }
Rvs94 9:774fc3c6a39e 113
Margreeth95 0:284ed397e046 114 }