Marijke Zondag / Mbed 2 deprecated Project_script_encoder

encoder

Dependencies:   MODSERIAL mbed HIDScope biquadFilter

Fork of Project_script by Marijke Zondag

main.cpp@24:b9dd6cf5c366, 2018-10-30 (annotated)

Committer:
MarijkeZondag
Date:
Tue Oct 30 15:02:26 2018 +0000
Revision:
24:b9dd6cf5c366
Parent:
23:c027e5c57cc8
Child:
25:c719346df3cd
PID input, zonder motoraansturing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
MarijkeZondag 24:b9dd6cf5c366 3 #include "BiQuad.h"
vsluiter 0:c8f15874531b 4
MarijkeZondag 9:c722418997b5 5 AnalogIn potmetervalue1 (A1);
MarijkeZondag 9:c722418997b5 6 AnalogIn potmetervalue2 (A2);
MarijkeZondag 23:c027e5c57cc8 7
MarijkeZondag 9:c722418997b5 8 DigitalIn button2 (D10); //Let op, is deze niet bezet?
MarijkeZondag 23:c027e5c57cc8 9
MarijkeZondag 9:c722418997b5 10 InterruptIn encoderA (D9);
MarijkeZondag 9:c722418997b5 11 InterruptIn encoderB (D8);
MarijkeZondag 6:f4bbb73f3989 12
MarijkeZondag 23:c027e5c57cc8 13 DigitalOut directionpin1 (D7);
MarijkeZondag 23:c027e5c57cc8 14 DigitalOut directionpin2 (D4);
MarijkeZondag 23:c027e5c57cc8 15 PwmOut pwmpin1 (D6);
MarijkeZondag 23:c027e5c57cc8 16 PwmOut pwmpin2 (D5);
MarijkeZondag 9:c722418997b5 17
MarijkeZondag 6:f4bbb73f3989 18
vsluiter 0:c8f15874531b 19 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 20
MarijkeZondag 9:c722418997b5 21
MarijkeZondag 9:c722418997b5 22 //Global variables
MarijkeZondag 24:b9dd6cf5c366 23 int encoder = 0; //Start value encoder
MarijkeZondag 24:b9dd6cf5c366 24 int T = 0.002f; //Ticker period
MarijkeZondag 24:b9dd6cf5c366 25
MarijkeZondag 24:b9dd6cf5c366 26 double Kp = 17.5;
MarijkeZondag 24:b9dd6cf5c366 27 double Ki = 1.02;
MarijkeZondag 24:b9dd6cf5c366 28 double Kd = 23.2;
MarijkeZondag 24:b9dd6cf5c366 29
MarijkeZondag 24:b9dd6cf5c366 30 double err = 0;
MarijkeZondag 24:b9dd6cf5c366 31
MarijkeZondag 24:b9dd6cf5c366 32 //Tickers
MarijkeZondag 24:b9dd6cf5c366 33 Ticker function_tick;
MarijkeZondag 8:895d941a5910 34
MarijkeZondag 9:c722418997b5 35
MarijkeZondag 9:c722418997b5 36 //Functions
MarijkeZondag 8:895d941a5910 37 void encoderA_rise()
MarijkeZondag 8:895d941a5910 38 {
MarijkeZondag 8:895d941a5910 39 if(encoderB==false)
MarijkeZondag 8:895d941a5910 40 {
MarijkeZondag 8:895d941a5910 41 encoder++;
MarijkeZondag 8:895d941a5910 42 }
MarijkeZondag 8:895d941a5910 43 else
MarijkeZondag 8:895d941a5910 44 {
MarijkeZondag 8:895d941a5910 45 encoder--;
MarijkeZondag 8:895d941a5910 46 }
MarijkeZondag 8:895d941a5910 47 }
MarijkeZondag 8:895d941a5910 48
MarijkeZondag 8:895d941a5910 49 void encoderA_fall()
MarijkeZondag 8:895d941a5910 50 {
MarijkeZondag 8:895d941a5910 51 if(encoderB==true)
MarijkeZondag 8:895d941a5910 52 {
MarijkeZondag 8:895d941a5910 53 encoder++;
MarijkeZondag 8:895d941a5910 54 }
MarijkeZondag 8:895d941a5910 55 else
MarijkeZondag 8:895d941a5910 56 {
MarijkeZondag 8:895d941a5910 57 encoder--;
MarijkeZondag 8:895d941a5910 58 }
MarijkeZondag 8:895d941a5910 59 }
MarijkeZondag 8:895d941a5910 60
MarijkeZondag 8:895d941a5910 61 void encoderB_rise()
MarijkeZondag 8:895d941a5910 62 {
MarijkeZondag 8:895d941a5910 63 if(encoderA==true)
MarijkeZondag 8:895d941a5910 64 {
MarijkeZondag 8:895d941a5910 65 encoder++;
MarijkeZondag 8:895d941a5910 66 }
MarijkeZondag 8:895d941a5910 67 else
MarijkeZondag 8:895d941a5910 68 {
MarijkeZondag 8:895d941a5910 69 encoder--;
MarijkeZondag 8:895d941a5910 70 }
MarijkeZondag 8:895d941a5910 71 }
MarijkeZondag 8:895d941a5910 72
MarijkeZondag 8:895d941a5910 73 void encoderB_fall()
MarijkeZondag 8:895d941a5910 74 {
MarijkeZondag 8:895d941a5910 75 if(encoderA==false)
MarijkeZondag 8:895d941a5910 76 {
MarijkeZondag 8:895d941a5910 77 encoder++;
MarijkeZondag 8:895d941a5910 78 }
MarijkeZondag 8:895d941a5910 79 else
MarijkeZondag 8:895d941a5910 80 {
MarijkeZondag 8:895d941a5910 81 encoder--;
MarijkeZondag 8:895d941a5910 82 }
MarijkeZondag 8:895d941a5910 83 }
MarijkeZondag 8:895d941a5910 84
MarijkeZondag 24:b9dd6cf5c366 85 void encoder_count()
MarijkeZondag 24:b9dd6cf5c366 86 {
MarijkeZondag 24:b9dd6cf5c366 87 encoderA.rise(&encoderA_rise);
MarijkeZondag 24:b9dd6cf5c366 88 encoderA.fall(&encoderA_fall);
MarijkeZondag 24:b9dd6cf5c366 89 encoderB.rise(&encoderB_rise);
MarijkeZondag 24:b9dd6cf5c366 90 encoderB.fall(&encoderB_fall);
MarijkeZondag 24:b9dd6cf5c366 91 }
MarijkeZondag 24:b9dd6cf5c366 92
MarijkeZondag 24:b9dd6cf5c366 93 double PID_controller()
MarijkeZondag 24:b9dd6cf5c366 94 {
MarijkeZondag 24:b9dd6cf5c366 95 static double err_integral = 0;
MarijkeZondag 24:b9dd6cf5c366 96 static double err_prev = err; // initialization with this value only done once!
MarijkeZondag 24:b9dd6cf5c366 97
MarijkeZondag 24:b9dd6cf5c366 98 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 24:b9dd6cf5c366 99
MarijkeZondag 24:b9dd6cf5c366 100 // Proportional part:
MarijkeZondag 24:b9dd6cf5c366 101 double u_k = Kp * err;
MarijkeZondag 24:b9dd6cf5c366 102
MarijkeZondag 24:b9dd6cf5c366 103 // Integral part
MarijkeZondag 24:b9dd6cf5c366 104 err_integral = err_integral + err * T;
MarijkeZondag 24:b9dd6cf5c366 105 double u_i = Ki * err_integral;
MarijkeZondag 24:b9dd6cf5c366 106
MarijkeZondag 24:b9dd6cf5c366 107 // Derivative part
MarijkeZondag 24:b9dd6cf5c366 108 double err_derivative = (err - err_prev)/T;
MarijkeZondag 24:b9dd6cf5c366 109 double filtered_err_derivative = LowPassFilter.step(err_derivative);
MarijkeZondag 24:b9dd6cf5c366 110 double u_d = Kd * filtered_err_derivative;
MarijkeZondag 24:b9dd6cf5c366 111 err_prev = err;
MarijkeZondag 24:b9dd6cf5c366 112
MarijkeZondag 24:b9dd6cf5c366 113 // Sum all parts and return it
MarijkeZondag 24:b9dd6cf5c366 114 return u_k + u_i + u_d;
MarijkeZondag 24:b9dd6cf5c366 115 }
MarijkeZondag 24:b9dd6cf5c366 116
MarijkeZondag 24:b9dd6cf5c366 117 void call_all_functions()
MarijkeZondag 24:b9dd6cf5c366 118 {
MarijkeZondag 24:b9dd6cf5c366 119 PID_controller();
MarijkeZondag 24:b9dd6cf5c366 120 encoder_count();
MarijkeZondag 24:b9dd6cf5c366 121 }
MarijkeZondag 8:895d941a5910 122
MarijkeZondag 9:c722418997b5 123 // Main function start.
MarijkeZondag 8:895d941a5910 124
vsluiter 0:c8f15874531b 125 int main()
MarijkeZondag 3:a3ad58758696 126 {
vsluiter 0:c8f15874531b 127 pc.baud(115200);
MarijkeZondag 6:f4bbb73f3989 128 pc.printf("hello\n\r");
MarijkeZondag 6:f4bbb73f3989 129 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 6:f4bbb73f3989 130
MarijkeZondag 24:b9dd6cf5c366 131 function_tick.attach(&call_all_functions,T);
MarijkeZondag 24:b9dd6cf5c366 132
vsluiter 0:c8f15874531b 133
MarijkeZondag 2:2f4444f1504d 134 while (true)
MarijkeZondag 2:2f4444f1504d 135 {
MarijkeZondag 7:f32005d13749 136
MarijkeZondag 24:b9dd6cf5c366 137
MarijkeZondag 9:c722418997b5 138
vsluiter 0:c8f15874531b 139 }
vsluiter 0:c8f15874531b 140 }
MarijkeZondag 4:fbea05c8fe2b 141