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@19:9417d2011e8b, 2015-10-02 (annotated)

Committer:
Margreeth95
Date:
Fri Oct 02 07:27:12 2015 +0000
Revision:
19:9417d2011e8b
Parent:
18:6f71bb91b8bd 
Ingebouwde switch die tussen de verschillende motoren kan gaan wisselen. Aantal graden is niet goed, pulses wordt wel getoond.

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