Rick Bisschop / Mbed 2 deprecated motor_v3

groep 6

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_2 by Margreeth de Breij

main.cpp@20:84cc373e6fbb, 2016-11-01 (annotated)

Committer:
RickB
Date:
Tue Nov 01 11:24:52 2016 +0000
Revision:
20:84cc373e6fbb
Parent:
19:9417d2011e8b
Child:
21:9e6461109547
6

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);
RickB 20:84cc373e6fbb 12 DigitalOut motor2direction(D7); //D6 en D7 zijn motor 2 (op het motorshield)
RickB 20:84cc373e6fbb 13 PwmOut motor2speed(D6);
RickB 20:84cc373e6fbb 14 AnalogIn potmeter2(A1);
RickB 20:84cc373e6fbb 15 QEI Encoder(D12, D13, NC, 64, QEI::X4_ENCODING);
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
RickB 20:84cc373e6fbb 21 int q = 0 ;
RickB 20:84cc373e6fbb 22 double P2 ;
RickB 20:84cc373e6fbb 23 double m_ref = 0 ;
RickB 20:84cc373e6fbb 24 double reference = 0 ;
RickB 20:84cc373e6fbb 25 double position ;
RickB 20:84cc373e6fbb 26 double h_m = 0 ;
RickB 20:84cc373e6fbb 27 int count = 0 ;
RickB 20:84cc373e6fbb 28 int f_motor = 100 ;
RickB 20:84cc373e6fbb 29 int t =0 ;
RickB 20:84cc373e6fbb 30 int x1 = -1 ;
RickB 20:84cc373e6fbb 31 int x2 = -1 ;
Rvs94 2:099da0fc31b6 32
Margreeth95 0:284ed397e046 33 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 34 {
Margreeth95 0:284ed397e046 35 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 36 scope.set(1, motor2speed.read());
Rvs94 15:7fbee317af2d 37 scope.set(2, position);
Rvs94 4:0d4aff8b57b3 38
Margreeth95 0:284ed397e046 39 scope.send();
Rvs94 1:48aba8d5610a 40
Margreeth95 0:284ed397e046 41 }
Rvs94 12:69ab81cf5b7d 42 // Sample time (motor2-step)
Rvs94 12:69ab81cf5b7d 43 const double m2_Ts = 0.01;
Rvs94 12:69ab81cf5b7d 44
Rvs94 9:774fc3c6a39e 45 // Controller gain
Rvs94 16:5b729bd56155 46 const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
Rvs94 13:a6770307a5d2 47 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 12:69ab81cf5b7d 48
Rvs94 12:69ab81cf5b7d 49 //Derivative filter coeffs
Rvs94 13:a6770307a5d2 50 const double BiGain = 0.016955;
Rvs94 13:a6770307a5d2 51 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 52
Rvs94 12:69ab81cf5b7d 53 // Filter variables
Rvs94 12:69ab81cf5b7d 54 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 10:80fe931a71e4 55
Rvs94 12:69ab81cf5b7d 56 // Biquad filter
Rvs94 12:69ab81cf5b7d 57 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 58 {
Rvs94 13:a6770307a5d2 59 double v = u - a1*v1 - a2*v2;
Rvs94 13:a6770307a5d2 60 double y = b0*v + b1*v1 + b2*v2;
Rvs94 12:69ab81cf5b7d 61 v2 = v1; v1 = v;
Rvs94 12:69ab81cf5b7d 62 return y;
Rvs94 12:69ab81cf5b7d 63 }
Rvs94 12:69ab81cf5b7d 64
Rvs94 12:69ab81cf5b7d 65 // Reusable PID controller
Rvs94 12:69ab81cf5b7d 66 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 67 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 12:69ab81cf5b7d 68 {
Rvs94 12:69ab81cf5b7d 69 // Derivative
Rvs94 12:69ab81cf5b7d 70 double e_der = (e-e_prev)/Ts;
Rvs94 12:69ab81cf5b7d 71 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 13:a6770307a5d2 72 e_prev = e;
Rvs94 12:69ab81cf5b7d 73 // Integral
Rvs94 12:69ab81cf5b7d 74 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 75 // PID
Rvs94 12:69ab81cf5b7d 76 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 9:774fc3c6a39e 77 }
Margreeth95 0:284ed397e046 78
RickB 20:84cc373e6fbb 79 void m2_ref()
RickB 20:84cc373e6fbb 80 {
RickB 20:84cc373e6fbb 81 double pi = 3.14 ;
RickB 20:84cc373e6fbb 82 double t_m = 1 ;
RickB 20:84cc373e6fbb 83 double start1 ;
RickB 20:84cc373e6fbb 84 double start2 ;
RickB 20:84cc373e6fbb 85
RickB 20:84cc373e6fbb 86 switch (q)
RickB 20:84cc373e6fbb 87 {
RickB 20:84cc373e6fbb 88 case 1 : // clockwise
RickB 20:84cc373e6fbb 89
RickB 20:84cc373e6fbb 90 start1 = x1*h_m - (x2+1)*h_m ;
RickB 20:84cc373e6fbb 91 if (t >= 0 & t <= f_motor) {
RickB 20:84cc373e6fbb 92 reference = start1 + (h_m* t_m *t)*1/f_motor - h_m/(2*pi)*sin(2*pi/t_m*t/f_motor) ;
RickB 20:84cc373e6fbb 93 t++ ; }
RickB 20:84cc373e6fbb 94 else if (t > f_motor){
RickB 20:84cc373e6fbb 95 motor2speed.write(0.0) ; }
RickB 20:84cc373e6fbb 96
RickB 20:84cc373e6fbb 97 pc.printf("\r\n t = %i reference1 = %d pos1 = %d ",t,reference,position) ;
RickB 20:84cc373e6fbb 98 break ;
RickB 20:84cc373e6fbb 99
RickB 20:84cc373e6fbb 100 case 2 : // counterclockwise
RickB 20:84cc373e6fbb 101
RickB 20:84cc373e6fbb 102 start2 = (x1+1)*h_m - x2*h_m ;
RickB 20:84cc373e6fbb 103 if (t >= 0 & t <= f_motor) {
RickB 20:84cc373e6fbb 104 reference = start2 - ((h_m* t_m *t)*1/f_motor - h_m/(2*pi)*sin(2*pi/t_m*t/f_motor)) ;
RickB 20:84cc373e6fbb 105 t++ ; }
RickB 20:84cc373e6fbb 106 else if (t > f_motor){
RickB 20:84cc373e6fbb 107 motor2speed.write(0.0) ; }
RickB 20:84cc373e6fbb 108
RickB 20:84cc373e6fbb 109 pc.printf("\r\n start1 = %d reference2 = %d pos2 = %d ",start1,reference,position) ;
RickB 20:84cc373e6fbb 110 break ;
RickB 20:84cc373e6fbb 111 }
RickB 20:84cc373e6fbb 112 }
RickB 20:84cc373e6fbb 113
Margreeth95 18:6f71bb91b8bd 114 // Motor2 control
Rvs94 11:0793a78109a2 115 void motor2_Controller()
Rvs94 9:774fc3c6a39e 116 {
RickB 20:84cc373e6fbb 117 m2_ref() ;
RickB 20:84cc373e6fbb 118 // Setpoint
Margreeth95 18:6f71bb91b8bd 119 double pulses = Encoder.getPulses();
RickB 20:84cc373e6fbb 120 position = pulses*360/(64*131.25); // Aantal Degs
RickB 20:84cc373e6fbb 121 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 122 m2_f_b0, m2_f_b1, m2_f_b2);
RickB 20:84cc373e6fbb 123
RickB 20:84cc373e6fbb 124 if (P2 > 0.6)
RickB 20:84cc373e6fbb 125 { P2 = 0.6 ; }
RickB 20:84cc373e6fbb 126 else if (P2 < 0.6)
RickB 20:84cc373e6fbb 127 { P2 = -0.6 ; }
RickB 20:84cc373e6fbb 128
RickB 20:84cc373e6fbb 129 const int frequency_pwm = 20000 ;
RickB 20:84cc373e6fbb 130 motor2speed.period(1/frequency_pwm) ;
RickB 20:84cc373e6fbb 131 motor2speed = abs(P2); // Speed control
RickB 20:84cc373e6fbb 132
RickB 20:84cc373e6fbb 133 if (P2 > 0) // Direction control
Rvs94 9:774fc3c6a39e 134 {
Rvs94 9:774fc3c6a39e 135 motor2direction = 0;
Rvs94 9:774fc3c6a39e 136 }
RickB 20:84cc373e6fbb 137 else if (P2 < 0)
Rvs94 9:774fc3c6a39e 138 {
Rvs94 9:774fc3c6a39e 139 motor2direction = 1;
Rvs94 9:774fc3c6a39e 140 }
Rvs94 9:774fc3c6a39e 141 }
Rvs94 3:687729d7996e 142
Margreeth95 0:284ed397e046 143 int main()
Rvs94 9:774fc3c6a39e 144 {
Margreeth95 0:284ed397e046 145 pc.baud(115200);
RickB 20:84cc373e6fbb 146
RickB 20:84cc373e6fbb 147 ScopeTime.attach_us(&ScopeSend, 10e4);
RickB 20:84cc373e6fbb 148 myControllerTicker.attach( &motor2_Controller, 0.01); // 100 Hz
Rvs94 9:774fc3c6a39e 149
Rvs94 9:774fc3c6a39e 150 while(true)
RickB 20:84cc373e6fbb 151 {
RickB 20:84cc373e6fbb 152 char c = pc.getc();
RickB 20:84cc373e6fbb 153 if(c == 'r')
RickB 20:84cc373e6fbb 154 {
RickB 20:84cc373e6fbb 155 q = 1 ; // case 1
RickB 20:84cc373e6fbb 156 t = 0 ;
RickB 20:84cc373e6fbb 157 h_m = 3 ;
RickB 20:84cc373e6fbb 158 x1++ ;
RickB 20:84cc373e6fbb 159 }
RickB 20:84cc373e6fbb 160 if(c == 'f')
RickB 20:84cc373e6fbb 161 {
RickB 20:84cc373e6fbb 162 q = 2 ; // case 2
RickB 20:84cc373e6fbb 163 t = 0 ;
RickB 20:84cc373e6fbb 164 h_m = 3 ;
RickB 20:84cc373e6fbb 165 x2++ ;
RickB 20:84cc373e6fbb 166 }
Margreeth95 0:284ed397e046 167 }
Margreeth95 0:284ed397e046 168 }