Casper Maas / Mbed 2 deprecated PID_controller

pid+sineinput

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@6:bbc9ce35e0a4, 2018-10-30 (annotated)

Committer:
cmaas
Date:
Tue Oct 30 06:35:29 2018 +0000
Revision:
6:bbc9ce35e0a4
Parent:
5:ea9f9f11d7c1 
Correct Kp, Ki, Kd values added

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 6:bbc9ce35e0a4 9 //#include "Math.h"
cmaas 0:447a347725bb 10
cmaas 1:e10ae03926e6 11 AnalogIn button2(A4);
cmaas 0:447a347725bb 12 PwmOut pwmpin1(D6);
cmaas 0:447a347725bb 13 DigitalOut directionpin1(D7);
cmaas 0:447a347725bb 14 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
cmaas 0:447a347725bb 15 Ticker ref_rot;
cmaas 2:2c32825bb327 16 Ticker show_counts;
cmaas 0:447a347725bb 17 Ticker Scope_Data;
cmaas 0:447a347725bb 18 MODSERIAL pc(USBTX, USBRX);
cmaas 3:fe052c67c840 19 HIDScope scope(2);
cmaas 0:447a347725bb 20
cmaas 0:447a347725bb 21 // INITIALIZING GLOBAl VALUES
cmaas 5:ea9f9f11d7c1 22
cmaas 5:ea9f9f11d7c1 23 double PI = 3.141592653589793238462;
cmaas 6:bbc9ce35e0a4 24 double Kp = 16; //200 , 50
cmaas 6:bbc9ce35e0a4 25 double Ki = 0; //1, 0.5
cmaas 6:bbc9ce35e0a4 26 double Kd = 4.5; //200, 10
cmaas 0:447a347725bb 27 double Ts = 0.1; // Sample time in seconds
cmaas 5:ea9f9f11d7c1 28 volatile double reference_rotation = PI; //define as radians
cmaas 0:447a347725bb 29 double motor_position;
cmaas 0:447a347725bb 30 bool AlwaysTrue;
cmaas 5:ea9f9f11d7c1 31
cmaas 0:447a347725bb 32
cmaas 0:447a347725bb 33 //----------- FUNCTIONS ----------------------
cmaas 0:447a347725bb 34
cmaas 0:447a347725bb 35
cmaas 0:447a347725bb 36 // PID CONTROLLER FUNCTION
cmaas 0:447a347725bb 37 double PID_controller(double error)
cmaas 0:447a347725bb 38 {
cmaas 2:2c32825bb327 39 static double error_integral = 0;
cmaas 2:2c32825bb327 40 static double error_prev = error; // initialization with this value only done once!
cmaas 2:2c32825bb327 41 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
cmaas 0:447a347725bb 42
cmaas 2:2c32825bb327 43 // Proportional part:
cmaas 2:2c32825bb327 44 double u_k = Kp * error;
cmaas 0:447a347725bb 45
cmaas 2:2c32825bb327 46 // Integral part
cmaas 2:2c32825bb327 47 error_integral = error_integral + error * Ts;
cmaas 2:2c32825bb327 48 double u_i = Ki * error_integral;
cmaas 0:447a347725bb 49
cmaas 2:2c32825bb327 50 // Derivative part
cmaas 2:2c32825bb327 51 double error_derivative = (error - error_prev)/Ts;
cmaas 2:2c32825bb327 52 double filtered_error_derivative = LowPassFilter.step(error_derivative);
cmaas 2:2c32825bb327 53 double u_d = Kd * filtered_error_derivative;
cmaas 2:2c32825bb327 54 error_prev = error;
cmaas 0:447a347725bb 55
cmaas 2:2c32825bb327 56 // Sum all parts and return it
cmaas 2:2c32825bb327 57 return u_k + u_i + u_d;
cmaas 0:447a347725bb 58 }
cmaas 0:447a347725bb 59
cmaas 0:447a347725bb 60
cmaas 0:447a347725bb 61 // DIRECTON AND SPEED CONTROL
cmaas 0:447a347725bb 62 void moter_control(double u)
cmaas 0:447a347725bb 63 {
cmaas 0:447a347725bb 64 directionpin1= u > 0.0f; //eithertrueor false
cmaas 2:2c32825bb327 65 pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
cmaas 0:447a347725bb 66 }
cmaas 0:447a347725bb 67
cmaas 0:447a347725bb 68
cmaas 0:447a347725bb 69 // CONTROLLING THE MOTOR
cmaas 0:447a347725bb 70 void Motor_mover()
cmaas 0:447a347725bb 71 {
cmaas 1:e10ae03926e6 72 double motor_position = encoder1.getPulses(); //output in counts
cmaas 1:e10ae03926e6 73 double error = reference_rotation - motor_position*(2*PI)/8400;
cmaas 0:447a347725bb 74 double u = PID_controller(error);
cmaas 0:447a347725bb 75 moter_control(u);
cmaas 0:447a347725bb 76 }
cmaas 0:447a347725bb 77
cmaas 0:447a347725bb 78 //PRINT TICKER
cmaas 0:447a347725bb 79 void PrintFlag()
cmaas 0:447a347725bb 80 {
cmaas 0:447a347725bb 81 AlwaysTrue = true;
cmaas 2:2c32825bb327 82 }
cmaas 3:fe052c67c840 83
cmaas 0:447a347725bb 84 // HIDSCOPE
cmaas 0:447a347725bb 85 void ScopeData()
cmaas 0:447a347725bb 86 {
cmaas 0:447a347725bb 87 double y = encoder1.getPulses();
cmaas 0:447a347725bb 88 scope.set(0, y);
cmaas 0:447a347725bb 89 scope.send();
cmaas 2:2c32825bb327 90 }
cmaas 3:fe052c67c840 91
cmaas 0:447a347725bb 92 //------------------ EXECUTION ---------------
cmaas 0:447a347725bb 93
cmaas 0:447a347725bb 94 int main()
cmaas 0:447a347725bb 95 {
cmaas 2:2c32825bb327 96 // INITIALIZING
cmaas 2:2c32825bb327 97
cmaas 2:2c32825bb327 98 pwmpin1.period_us(60);
cmaas 2:2c32825bb327 99 pc.printf("test");
cmaas 2:2c32825bb327 100 pc.baud(9600);
cmaas 2:2c32825bb327 101 //show_counts.attach(PrintFlag, 0.2);
cmaas 2:2c32825bb327 102 ref_rot.attach(Motor_mover, 0.01);
cmaas 6:bbc9ce35e0a4 103 Scope_Data.attach(ScopeData, 0.01);
cmaas 2:2c32825bb327 104
cmaas 2:2c32825bb327 105 // DEFININING VARIABLES
cmaas 2:2c32825bb327 106 //float reference;
cmaas 2:2c32825bb327 107 float foo[17];
cmaas 2:2c32825bb327 108 float length = 9.0f;
cmaas 2:2c32825bb327 109 int a;
cmaas 2:2c32825bb327 110 int q;
cmaas 2:2c32825bb327 111
cmaas 2:2c32825bb327 112 while (true) {
cmaas 2:2c32825bb327 113
cmaas 2:2c32825bb327 114 // EXECTURION IN MAIN
cmaas 2:2c32825bb327 115 // SIN ALS INPUT GEVEN
cmaas 2:2c32825bb327 116 if (button2 == false){
cmaas 2:2c32825bb327 117 wait (0.2f);
cmaas 2:2c32825bb327 118 for (q=0; q<4; q++){
cmaas 1:e10ae03926e6 119 float b = 8.0f;
cmaas 2:2c32825bb327 120 for (a=0; a<10; a++){
cmaas 2:2c32825bb327 121 foo[a] = 3.14f/length*(a);
cmaas 2:2c32825bb327 122 //printf("%f\n\r", foo[a]);
cmaas 2:2c32825bb327 123 reference_rotation = foo[a];
cmaas 6:bbc9ce35e0a4 124 wait(0.2f);
cmaas 2:2c32825bb327 125 }
cmaas 2:2c32825bb327 126 for(a=10; a<18; a++){
cmaas 2:2c32825bb327 127 foo[a] = 3.14f/length*b;
cmaas 2:2c32825bb327 128 //printf("%f\n\r", foo[a]);
cmaas 2:2c32825bb327 129 reference_rotation = foo[a];
cmaas 2:2c32825bb327 130 b = b-1.0f;
cmaas 6:bbc9ce35e0a4 131 wait(0.2f);
cmaas 2:2c32825bb327 132 }
cmaas 1:e10ae03926e6 133 }
cmaas 2:2c32825bb327 134
cmaas 6:bbc9ce35e0a4 135 reference_rotation = 0;
cmaas 2:2c32825bb327 136
cmaas 2:2c32825bb327 137
cmaas 2:2c32825bb327 138
cmaas 2:2c32825bb327 139 /* //IETS PRINTEN OP JE TERMINAL
cmaas 0:447a347725bb 140 PrintFlag();
cmaas 0:447a347725bb 141 while (AlwaysTrue){
cmaas 0:447a347725bb 142 float q = encoder1.getPulses();
cmaas 0:447a347725bb 143 pc.printf("%f \r\n", q);
cmaas 0:447a347725bb 144 AlwaysTrue = false;
cmaas 0:447a347725bb 145 wait(0.2f);
cmaas 0:447a347725bb 146 }
cmaas 0:447a347725bb 147 */
cmaas 1:e10ae03926e6 148
cmaas 0:447a347725bb 149
cmaas 2:2c32825bb327 150
cmaas 0:447a347725bb 151 }
cmaas 2:2c32825bb327 152
cmaas 2:2c32825bb327 153 }
cmaas 2:2c32825bb327 154 }