Casper Maas / Mbed 2 deprecated PID_controller

pid+sineinput

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@3:fe052c67c840, 2018-10-29 (annotated)

Committer:
cmaas
Date:
Mon Oct 29 10:46:46 2018 +0000
Revision:
3:fe052c67c840
Parent:
2:2c32825bb327
Child:
4:fc5889cf69f9
Volledig werken signaal met sin als input signaal

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