Casper Maas / Mbed 2 deprecated PID_controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@0:447a347725bb, 2018-10-26 (annotated)

Committer:
cmaas
Date:
Fri Oct 26 17:46:54 2018 +0000
Revision:
0:447a347725bb
Child:
1:e10ae03926e6
Basic versie. Verijst standaard input. PID werkt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmaas 0:447a347725bb 1
cmaas 0:447a347725bb 2 // ------------- INITIALIZING -----------------
cmaas 0:447a347725bb 3
cmaas 0:447a347725bb 4 #include "mbed.h"
cmaas 0:447a347725bb 5 #include "BiQuad.h"
cmaas 0:447a347725bb 6 #include "QEI.h"
cmaas 0:447a347725bb 7 #include "MODSERIAL.h"
cmaas 0:447a347725bb 8 #include "HIDScope.h"
cmaas 0:447a347725bb 9
cmaas 0:447a347725bb 10 PwmOut pwmpin1(D6);
cmaas 0:447a347725bb 11 DigitalOut directionpin1(D7);
cmaas 0:447a347725bb 12 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
cmaas 0:447a347725bb 13 Ticker ref_rot;
cmaas 0:447a347725bb 14 Ticker show_counts;
cmaas 0:447a347725bb 15 Ticker Scope_Data;
cmaas 0:447a347725bb 16 MODSERIAL pc(USBTX, USBRX);
cmaas 0:447a347725bb 17 HIDScope scope(2);
cmaas 0:447a347725bb 18
cmaas 0:447a347725bb 19 // INITIALIZING GLOBAl VALUES
cmaas 0:447a347725bb 20 double Kp = 50; //200
cmaas 0:447a347725bb 21 double Ki = 0.5; //1
cmaas 0:447a347725bb 22 double Kd = 10; //200
cmaas 0:447a347725bb 23 double Ts = 0.1; // Sample time in seconds
cmaas 0:447a347725bb 24 volatile double reference_rotation = 8400;
cmaas 0:447a347725bb 25 double motor_position;
cmaas 0:447a347725bb 26 bool AlwaysTrue;
cmaas 0:447a347725bb 27
cmaas 0:447a347725bb 28 //----------- FUNCTIONS ----------------------
cmaas 0:447a347725bb 29
cmaas 0:447a347725bb 30
cmaas 0:447a347725bb 31 // PID CONTROLLER FUNCTION
cmaas 0:447a347725bb 32 double PID_controller(double error)
cmaas 0:447a347725bb 33 {
cmaas 0:447a347725bb 34 static double error_integral = 0;
cmaas 0:447a347725bb 35 static double error_prev = error; // initialization with this value only done once!
cmaas 0:447a347725bb 36 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
cmaas 0:447a347725bb 37
cmaas 0:447a347725bb 38 // Proportional part:
cmaas 0:447a347725bb 39 double u_k = Kp * error;
cmaas 0:447a347725bb 40
cmaas 0:447a347725bb 41 // Integral part
cmaas 0:447a347725bb 42 error_integral = error_integral + error * Ts;
cmaas 0:447a347725bb 43 double u_i = Ki * error_integral;
cmaas 0:447a347725bb 44
cmaas 0:447a347725bb 45 // Derivative part
cmaas 0:447a347725bb 46 double error_derivative = (error - error_prev)/Ts;
cmaas 0:447a347725bb 47 double filtered_error_derivative = LowPassFilter.step(error_derivative);
cmaas 0:447a347725bb 48 double u_d = Kd * filtered_error_derivative;
cmaas 0:447a347725bb 49 error_prev = error;
cmaas 0:447a347725bb 50
cmaas 0:447a347725bb 51 // Sum all parts and return it
cmaas 0:447a347725bb 52 return u_k + u_i + u_d;
cmaas 0:447a347725bb 53 }
cmaas 0:447a347725bb 54
cmaas 0:447a347725bb 55
cmaas 0:447a347725bb 56 // DIRECTON AND SPEED CONTROL
cmaas 0:447a347725bb 57 void moter_control(double u)
cmaas 0:447a347725bb 58 {
cmaas 0:447a347725bb 59 directionpin1= u > 0.0f; //eithertrueor false
cmaas 0:447a347725bb 60 pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
cmaas 0:447a347725bb 61 }
cmaas 0:447a347725bb 62
cmaas 0:447a347725bb 63
cmaas 0:447a347725bb 64 // CONTROLLING THE MOTOR
cmaas 0:447a347725bb 65 void Motor_mover()
cmaas 0:447a347725bb 66 {
cmaas 0:447a347725bb 67 double motor_position = encoder1.getPulses();
cmaas 0:447a347725bb 68 double error = reference_rotation - motor_position;
cmaas 0:447a347725bb 69 double u = PID_controller(error);
cmaas 0:447a347725bb 70 moter_control(u);
cmaas 0:447a347725bb 71 }
cmaas 0:447a347725bb 72
cmaas 0:447a347725bb 73 //PRINT TICKER
cmaas 0:447a347725bb 74 void PrintFlag()
cmaas 0:447a347725bb 75 {
cmaas 0:447a347725bb 76 AlwaysTrue = true;
cmaas 0:447a347725bb 77 }
cmaas 0:447a347725bb 78
cmaas 0:447a347725bb 79 // HIDSCOPE
cmaas 0:447a347725bb 80 void ScopeData()
cmaas 0:447a347725bb 81 {
cmaas 0:447a347725bb 82 double y = encoder1.getPulses();
cmaas 0:447a347725bb 83 scope.set(0, y);
cmaas 0:447a347725bb 84 scope.send();
cmaas 0:447a347725bb 85 }
cmaas 0:447a347725bb 86
cmaas 0:447a347725bb 87 //------------------ EXECUTION ---------------
cmaas 0:447a347725bb 88
cmaas 0:447a347725bb 89 int main()
cmaas 0:447a347725bb 90 {
cmaas 0:447a347725bb 91 pwmpin1.period_us(60);
cmaas 0:447a347725bb 92 pc.printf("test");
cmaas 0:447a347725bb 93 pc.baud(9600);
cmaas 0:447a347725bb 94 //show_counts.attach(PrintFlag, 0.2);
cmaas 0:447a347725bb 95 ref_rot.attach(Motor_mover, 0.01);
cmaas 0:447a347725bb 96 Scope_Data.attach(ScopeData, 0.1);
cmaas 0:447a347725bb 97
cmaas 0:447a347725bb 98 while (true) {
cmaas 0:447a347725bb 99 /*
cmaas 0:447a347725bb 100 PrintFlag();
cmaas 0:447a347725bb 101 while (AlwaysTrue){
cmaas 0:447a347725bb 102 float q = encoder1.getPulses();
cmaas 0:447a347725bb 103 pc.printf("%f \r\n", q);
cmaas 0:447a347725bb 104 AlwaysTrue = false;
cmaas 0:447a347725bb 105 wait(0.2f);
cmaas 0:447a347725bb 106 }
cmaas 0:447a347725bb 107 */
cmaas 0:447a347725bb 108
cmaas 0:447a347725bb 109 }
cmaas 0:447a347725bb 110 }