Casper Maas / Mbed 2 deprecated PID_controller

pid+sineinput

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@1:e10ae03926e6, 2018-10-29 (annotated)

Committer:
cmaas
Date:
Mon Oct 29 07:24:56 2018 +0000
Revision:
1:e10ae03926e6
Parent:
0:447a347725bb
Child:
2:2c32825bb327
add sine. input in rasians

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