Biorobotics Project / Mbed 2 deprecated Project_motor

PID en biquad filter implemented and outcommented

Dependencies:   Motor_with_encoder MODSERIAL mbed QEI

Fork of Tutorial_lecture5_pin_pot_motor by Biorobotics Project

main.cpp@7:986a0c5fba0e, 2017-10-11 (annotated)

Committer:
EvB
Date:
Wed Oct 11 06:57:28 2017 +0000
Revision:
7:986a0c5fba0e
Parent:
6:b89ca8add6ee 
Changed state of speed calculation of encoder to 'true'

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vera1 0:bc949f735b8d 1 #include "mbed.h"
vera1 0:bc949f735b8d 2 #include "MODSERIAL.h"
vera1 0:bc949f735b8d 3 #include "encoder.h"
EvB 7:986a0c5fba0e 4 #include "QEI.h"
vera1 0:bc949f735b8d 5 // this program can turn the motor clockwise or counterclockwise depending on the value p or n typed into the terminal. with 's' you can stop the motor
vera1 0:bc949f735b8d 6
vera1 0:bc949f735b8d 7 MODSERIAL pc(USBTX,USBRX);
vera1 0:bc949f735b8d 8 PwmOut motorspeed(D5);
vera1 0:bc949f735b8d 9 DigitalOut motorposition(D4);
vera1 0:bc949f735b8d 10 DigitalOut led1(D8);
vera1 1:e0f6cdefcd6e 11 DigitalOut led2(D11);
vera1 0:bc949f735b8d 12 AnalogIn pot(A1);
vera1 2:80177e90c1e6 13 DigitalIn button1(D13);
vera1 2:80177e90c1e6 14 DigitalIn button2(D12);
vera1 0:bc949f735b8d 15 Ticker potmeterreadout;
vera1 1:e0f6cdefcd6e 16 Ticker encoderreadout;
EvB 7:986a0c5fba0e 17 Encoder motor1(PTD0,PTC4, true); // motor 1 is from encoder, otherwise it would be a pain in the ass to change all that code to motorencoder
EvB 7:986a0c5fba0e 18 //QEI motor1(PTD0, PTC4, NC, 64, QEI::X4_ENCODING); // maybe the speed readout works when using X4 decoding... But this may break the code...
vera1 0:bc949f735b8d 19
vera1 3:4520d0ca4e56 20 float PwmPeriod = 0.0001f;
MarkHeimgartner 5:05d09e921b5d 21
vera1 6:b89ca8add6ee 22 const double M1_TS = 0.01f; // timestep
vera1 6:b89ca8add6ee 23 const double M1_KP = 0.216, M1_KI = 1.8, M1_KD = 0.0; // controller gains for motor 1
vera1 3:4520d0ca4e56 24 double m1_err_int = 0, m1_prev_err = 0; // initiallize errors
vera1 3:4520d0ca4e56 25 const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0; // derivative filter coefficients
vera1 3:4520d0ca4e56 26 double m1_f_v1 = 0.0, m1_f_v2 = 0.0; // filter variables
vera1 6:b89ca8add6ee 27 /*
vera1 3:4520d0ca4e56 28 // biquad filter for emg signals
vera1 3:4520d0ca4e56 29 double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0, const double b1, const double b2){
vera1 3:4520d0ca4e56 30 double v = u - a1*v1 - a2*v2;
vera1 3:4520d0ca4e56 31 double y = b0*v + b1*v1 + b2*v2;
vera1 3:4520d0ca4e56 32 v2 = v1;
vera1 3:4520d0ca4e56 33 v1 = v;
vera1 3:4520d0ca4e56 34 return y;
vera1 3:4520d0ca4e56 35 }
vera1 6:b89ca8add6ee 36 */
vera1 3:4520d0ca4e56 37 // PID controller function
vera1 3:4520d0ca4e56 38 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, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
vera1 3:4520d0ca4e56 39 double e_der = (e - e_prev)/Ts; // derivative
vera1 6:b89ca8add6ee 40 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
vera1 3:4520d0ca4e56 41 e_prev = e;
vera1 3:4520d0ca4e56 42 e_int = e_int + Ts*e; // integral
vera1 6:b89ca8add6ee 43 return Kp*e + Ki*e_int; //PID controller
vera1 3:4520d0ca4e56 44 }
vera1 3:4520d0ca4e56 45
vera1 6:b89ca8add6ee 46
vera1 3:4520d0ca4e56 47
vera1 6:b89ca8add6ee 48 volatile double potvalue = 0.0;
vera1 1:e0f6cdefcd6e 49 volatile float position = 0.0;
vera1 0:bc949f735b8d 50 void readpot(){
vera1 6:b89ca8add6ee 51 potvalue = pot.read()*6.28;
MarkHeimgartner 4:abdc2d24d0bc 52 position = motor1.getPosition()/4200.00*6.28;
MarkHeimgartner 4:abdc2d24d0bc 53 pc.printf("pos: %f, speed %f reference velocity = %.2f\r\n",position, motor1.getSpeed(), potvalue);
vera1 6:b89ca8add6ee 54 //motorspeed = potvalue;
vera1 6:b89ca8add6ee 55 float motorPID = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
vera1 6:b89ca8add6ee 56 motorspeed = motorPID;
vera1 0:bc949f735b8d 57 }
vera1 3:4520d0ca4e56 58 // output PID controller is not yet tested.
vera1 3:4520d0ca4e56 59
vera1 0:bc949f735b8d 60 int main()
vera1 1:e0f6cdefcd6e 61 {
vera1 0:bc949f735b8d 62
vera1 0:bc949f735b8d 63 pc.baud(9600);
vera1 0:bc949f735b8d 64 potmeterreadout.attach(readpot,0.2f);
vera1 3:4520d0ca4e56 65 motorspeed.period(PwmPeriod);
vera1 0:bc949f735b8d 66 //float motorspeed = 0.0f;
vera1 0:bc949f735b8d 67 while (true) {
vera1 0:bc949f735b8d 68
vera1 2:80177e90c1e6 69
vera1 1:e0f6cdefcd6e 70 //pc.printf("reference velocity = %.2f\r\n", potvalue);
vera1 0:bc949f735b8d 71
vera1 0:bc949f735b8d 72
vera1 0:bc949f735b8d 73 if ((button2 == 1)&&(button1 == 0)) {
vera1 0:bc949f735b8d 74
vera1 0:bc949f735b8d 75 motorposition = 0; // motor turns anticlockwise
vera1 0:bc949f735b8d 76 led2 = 0;
vera1 0:bc949f735b8d 77
vera1 0:bc949f735b8d 78 }
vera1 0:bc949f735b8d 79 if ((button2 ==0)&&(button1 ==1)){
vera1 0:bc949f735b8d 80
vera1 0:bc949f735b8d 81 motorposition = 1; // motor turns anticlockwise
vera1 0:bc949f735b8d 82 led2 = 1;
vera1 0:bc949f735b8d 83 }
vera1 0:bc949f735b8d 84 //}
vera1 0:bc949f735b8d 85
vera1 0:bc949f735b8d 86 }
vera1 0:bc949f735b8d 87 }
vera1 0:bc949f735b8d 88