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@18:6f71bb91b8bd, 2015-10-01 (annotated)

Committer:
Margreeth95
Date:
Thu Oct 01 15:46:34 2015 +0000
Revision:
18:6f71bb91b8bd
Parent:
17:9b667e6e1290
Child:
19:9417d2011e8b
Motor draait binnen gestelde grenzen (+/- 90 graden) richting de referentie. Aantal graden dat berekent wordt niet in overeenstemming met werkelijkheid, ligt mogelijk aan uitgelezen pulses (altijd 0).

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;
Margreeth95 18:6f71bb91b8bd 40
Rvs94 12:69ab81cf5b7d 41 // Filter variables
Rvs94 12:69ab81cf5b7d 42 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 10:80fe931a71e4 43
Rvs94 12:69ab81cf5b7d 44 // Biquad filter
Rvs94 12:69ab81cf5b7d 45 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 46 {
Rvs94 13:a6770307a5d2 47 double v = u - a1*v1 - a2*v2;
Rvs94 13:a6770307a5d2 48 double y = b0*v + b1*v1 + b2*v2;
Rvs94 12:69ab81cf5b7d 49 v2 = v1; v1 = v;
Rvs94 12:69ab81cf5b7d 50 return y;
Rvs94 12:69ab81cf5b7d 51 }
Rvs94 12:69ab81cf5b7d 52
Rvs94 12:69ab81cf5b7d 53
Rvs94 12:69ab81cf5b7d 54 // Reusable PID controller
Rvs94 12:69ab81cf5b7d 55 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 56 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 12:69ab81cf5b7d 57 {
Rvs94 12:69ab81cf5b7d 58 // Derivative
Rvs94 12:69ab81cf5b7d 59 double e_der = (e-e_prev)/Ts;
Rvs94 12:69ab81cf5b7d 60 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 13:a6770307a5d2 61 e_prev = e;
Rvs94 12:69ab81cf5b7d 62 // Integral
Rvs94 12:69ab81cf5b7d 63 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 64 // PID
Rvs94 12:69ab81cf5b7d 65 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 9:774fc3c6a39e 66 }
Margreeth95 0:284ed397e046 67
Margreeth95 18:6f71bb91b8bd 68 // Motor2 control
Rvs94 11:0793a78109a2 69 void motor2_Controller()
Rvs94 9:774fc3c6a39e 70 {
Rvs94 15:7fbee317af2d 71 reference = m2_ref; // Setpoint
Margreeth95 18:6f71bb91b8bd 72 double pulses = Encoder.getPulses();
Margreeth95 18:6f71bb91b8bd 73 position = pulses*360/(0.5*128*131); // Aantal Degs
Rvs94 13:a6770307a5d2 74 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 75 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 15:7fbee317af2d 76 motor2speed = abs(P2); // Speed control
Rvs94 15:7fbee317af2d 77 if(P2 > 0) // Direction control
Rvs94 9:774fc3c6a39e 78 {
Rvs94 9:774fc3c6a39e 79 motor2direction = 0;
Rvs94 9:774fc3c6a39e 80 }
Rvs94 9:774fc3c6a39e 81 else
Rvs94 9:774fc3c6a39e 82 {
Rvs94 9:774fc3c6a39e 83 motor2direction = 1;
Rvs94 9:774fc3c6a39e 84 }
Margreeth95 18:6f71bb91b8bd 85 pc.printf("position = %f aantal degs = %f, pulses = %d\n",reference,position, pulses);
Rvs94 9:774fc3c6a39e 86 }
Rvs94 3:687729d7996e 87
Margreeth95 0:284ed397e046 88 int main()
Rvs94 9:774fc3c6a39e 89 {
Margreeth95 0:284ed397e046 90 pc.baud(115200);
Rvs94 3:687729d7996e 91 pc.printf("Tot aan loop werkt\n");
Rvs94 9:774fc3c6a39e 92
Margreeth95 0:284ed397e046 93 ScopeTime.attach_us(&ScopeSend, 10e4);
Rvs94 11:0793a78109a2 94 myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
Rvs94 9:774fc3c6a39e 95 while(true)
Margreeth95 0:284ed397e046 96 {
Rvs94 14:baebaef79aa6 97 char c = pc.getc();
Rvs94 14:baebaef79aa6 98 if(c == 'r')
Rvs94 14:baebaef79aa6 99 {
Rvs94 14:baebaef79aa6 100 m2_ref = m2_ref + 5;
Margreeth95 17:9b667e6e1290 101 if (m2_ref > 90)
Margreeth95 17:9b667e6e1290 102 {
Margreeth95 17:9b667e6e1290 103 m2_ref = 90;
Margreeth95 17:9b667e6e1290 104 }
Rvs94 14:baebaef79aa6 105 }
Rvs94 14:baebaef79aa6 106 if(c == 'f')
Rvs94 14:baebaef79aa6 107 {
Rvs94 14:baebaef79aa6 108 m2_ref = m2_ref - 5;
Margreeth95 17:9b667e6e1290 109 if (m2_ref < -90)
Margreeth95 17:9b667e6e1290 110 {
Margreeth95 17:9b667e6e1290 111 m2_ref = -90;
Margreeth95 17:9b667e6e1290 112 }
Rvs94 14:baebaef79aa6 113 }
Margreeth95 0:284ed397e046 114 }
Rvs94 9:774fc3c6a39e 115
Margreeth95 0:284ed397e046 116 }