Rick Kouwen / Mbed 2 deprecated template_biorobotics

LED toggle

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@6:81dda0c8eb71, 2019-10-07 (annotated)

Committer:
rick309
Date:
Mon Oct 07 13:11:37 2019 +0000
Revision:
6:81dda0c8eb71
Parent:
5:532b9a72e012 
PID-controller with HIDSCOPE extension

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
rick309 4:bb9bcbb22882 2 #include "HIDScope.h"
rick309 3:390b7ac07433 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
rick309 4:bb9bcbb22882 5 #include "BiQuad.h"
rick309 3:390b7ac07433 6 #include "FastPWM.h"
rick309 3:390b7ac07433 7 #include "math.h"
rick309 4:bb9bcbb22882 8
rick309 4:bb9bcbb22882 9 MODSERIAL pc(USBTX, USBRX);
rick309 4:bb9bcbb22882 10 QEI motor1 (D8, D9, NC, 32);
rick309 4:bb9bcbb22882 11 FastPWM motor1_pwm(D5);
rick309 4:bb9bcbb22882 12 DigitalOut motor1_dir(D4);
rick309 4:bb9bcbb22882 13 AnalogIn pod1(A0);
rick309 4:bb9bcbb22882 14 AnalogIn pod2(A1);
rick309 6:81dda0c8eb71 15
rick309 4:bb9bcbb22882 16 Ticker measure;
rick309 4:bb9bcbb22882 17
rick309 4:bb9bcbb22882 18 int m1_count;
rick309 4:bb9bcbb22882 19 float m1_angle;
rick309 4:bb9bcbb22882 20 float pi = 3.14;
rick309 4:bb9bcbb22882 21 float pod1_angle;
rick309 4:bb9bcbb22882 22 float motor1_er;
rick309 6:81dda0c8eb71 23 float pod2_tick;
rick309 6:81dda0c8eb71 24 float PWM_m1;
rick309 6:81dda0c8eb71 25
rick309 6:81dda0c8eb71 26 //PID variables
rick309 6:81dda0c8eb71 27 bool q = true;
rick309 6:81dda0c8eb71 28 float error_prev;
rick309 6:81dda0c8eb71 29 float Kp = 8;
rick309 6:81dda0c8eb71 30 float Ki = 1.02;
rick309 6:81dda0c8eb71 31 float Kd = 0.1;
rick309 6:81dda0c8eb71 32 float Ts = 0.01;
rick309 6:81dda0c8eb71 33 float u;
rick309 6:81dda0c8eb71 34 float u_p;
rick309 6:81dda0c8eb71 35 float u_i;
rick309 6:81dda0c8eb71 36 float u_d;
rick309 6:81dda0c8eb71 37
rick309 6:81dda0c8eb71 38 static double error_integral = 0;
rick309 6:81dda0c8eb71 39 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rick309 3:390b7ac07433 40
rick309 5:532b9a72e012 41 //initial parameters time and HIDSCOPE
rick309 5:532b9a72e012 42 volatile float theoretical_time = 0;
rick309 5:532b9a72e012 43 static int frequency_theoretical_time = 20;
rick309 5:532b9a72e012 44 static float duration_theoretical_time = (1/(float)frequency_theoretical_time);
rick309 5:532b9a72e012 45 Ticker sample_timer; //HIDSCOPE ticker, deze is langzamer dan en in periode met measure,
rick309 5:532b9a72e012 46 //deze frequentie is frequency_time
rick309 5:532b9a72e012 47 HIDScope scope( 2 ); //2 ingangen
rick309 5:532b9a72e012 48
rick309 5:532b9a72e012 49 /** Sample function
rick309 5:532b9a72e012 50 * this function samples the emg and sends it to HIDScope
rick309 5:532b9a72e012 51 **/
rick309 5:532b9a72e012 52 void sample()
rick309 5:532b9a72e012 53 {
rick309 5:532b9a72e012 54 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
rick309 5:532b9a72e012 55 scope.set(0, m1_count );
rick309 5:532b9a72e012 56 scope.set(1, theoretical_time);
rick309 6:81dda0c8eb71 57 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
rick309 5:532b9a72e012 58 * Ensure that enough channels are available (HIDScope scope( 2 ))
rick309 5:532b9a72e012 59 * Finally, send all channels to the PC at once */
rick309 5:532b9a72e012 60 scope.send();
rick309 5:532b9a72e012 61 /* To indicate that the function is working, the LED is toggled */
rick309 5:532b9a72e012 62 //led = !led; suppressed for now
rick309 5:532b9a72e012 63 }
rick309 5:532b9a72e012 64
rick309 5:532b9a72e012 65 /** Theoretical time function
rick309 5:532b9a72e012 66 *This function calculates time based on sample numbers and frequency
rick309 5:532b9a72e012 67 **/
rick309 5:532b9a72e012 68 void theoretical_time_fun(){
rick309 5:532b9a72e012 69 theoretical_time = theoretical_time + duration_theoretical_time; //increment by 1 second
rick309 5:532b9a72e012 70 }
rick309 5:532b9a72e012 71 void StepResponse(){
rick309 5:532b9a72e012 72 theoretical_time_fun();
rick309 5:532b9a72e012 73 sample();
rick309 5:532b9a72e012 74 }
rick309 5:532b9a72e012 75
rick309 4:bb9bcbb22882 76 void getCounts(){
rick309 4:bb9bcbb22882 77 m1_count = motor1.getPulses();
rick309 4:bb9bcbb22882 78 m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
rick309 4:bb9bcbb22882 79 pod1_angle = (float)pod1 * 2.0f * pi;
rick309 6:81dda0c8eb71 80 pod2_tick = (float)pod2;
rick309 4:bb9bcbb22882 81 }
rick309 4:bb9bcbb22882 82
rick309 4:bb9bcbb22882 83 void error(void){
rick309 4:bb9bcbb22882 84 motor1_er = pod1_angle - m1_angle;
rick309 4:bb9bcbb22882 85 }
rick309 6:81dda0c8eb71 86
rick309 6:81dda0c8eb71 87 void PID_control(void){
rick309 6:81dda0c8eb71 88 //P action
rick309 6:81dda0c8eb71 89 u_p = Kp * fabs(motor1_er);
rick309 6:81dda0c8eb71 90
rick309 6:81dda0c8eb71 91 //I action
rick309 6:81dda0c8eb71 92 error_integral = error_integral + motor1_er * Ts;
rick309 6:81dda0c8eb71 93 u_i = Ki * error_integral;
rick309 6:81dda0c8eb71 94
rick309 6:81dda0c8eb71 95 //D action
rick309 6:81dda0c8eb71 96 if(q){
rick309 6:81dda0c8eb71 97 error_prev = motor1_er;
rick309 6:81dda0c8eb71 98 q=false;
rick309 6:81dda0c8eb71 99 }
rick309 6:81dda0c8eb71 100
rick309 6:81dda0c8eb71 101 float error_derivative = (motor1_er - error_prev)/Ts;
rick309 6:81dda0c8eb71 102 float filtered_error_derivative = LowPassFilter.step(error_derivative);
rick309 6:81dda0c8eb71 103 u_d = Kd * filtered_error_derivative;
rick309 6:81dda0c8eb71 104 error_prev = motor1_er;
rick309 6:81dda0c8eb71 105
rick309 6:81dda0c8eb71 106 u = u_p + u_i + u_d;
rick309 6:81dda0c8eb71 107
rick309 6:81dda0c8eb71 108 PWM_m1 = fabs(motor1_er * u);
rick309 3:390b7ac07433 109 }
rick309 6:81dda0c8eb71 110
rick309 4:bb9bcbb22882 111 void control(){
rick309 6:81dda0c8eb71 112 if(fabs(motor1_er) < 0.005f){
rick309 4:bb9bcbb22882 113 motor1_pwm.write(0);
rick309 4:bb9bcbb22882 114 }else if(motor1_er < 0.0f){
rick309 6:81dda0c8eb71 115 motor1_pwm.write(PWM_m1);
rick309 4:bb9bcbb22882 116 motor1_dir = 1;
rick309 4:bb9bcbb22882 117 }else if(motor1_er > 0.0f){
rick309 6:81dda0c8eb71 118 motor1_pwm.write(PWM_m1);
rick309 4:bb9bcbb22882 119 motor1_dir = 0;
rick309 4:bb9bcbb22882 120 }
rick309 3:390b7ac07433 121 }
rick309 4:bb9bcbb22882 122
rick309 4:bb9bcbb22882 123 void MeasureandControl(){
rick309 4:bb9bcbb22882 124 getCounts();
rick309 4:bb9bcbb22882 125 error();
rick309 4:bb9bcbb22882 126 control();
rick309 6:81dda0c8eb71 127 PID_control();
rick309 3:390b7ac07433 128 }
RobertoO 0:67c50348f842 129
rick309 4:bb9bcbb22882 130 int main()
rick309 4:bb9bcbb22882 131 {
rick309 4:bb9bcbb22882 132 pc.baud(115200);
rick309 4:bb9bcbb22882 133 pc.printf("\r\nStarting up...\r\n\r\n");
rick309 4:bb9bcbb22882 134 motor1_pwm.period(1.0/10000);
rick309 4:bb9bcbb22882 135 motor1_dir = 0;
rick309 4:bb9bcbb22882 136 measure.attach(MeasureandControl,0.01);
rick309 5:532b9a72e012 137 sample_timer.attach(StepResponse,duration_theoretical_time);
rick309 4:bb9bcbb22882 138 while(true){
rick309 6:81dda0c8eb71 139 pc.printf("\r\nMotor angle: %f Pod angle: %f Error: %f \r\n u = %f , u_d = %f",m1_angle,pod1_angle,motor1_er,u,u_d);
rick309 4:bb9bcbb22882 140 wait(0.8);
rick309 3:390b7ac07433 141 }
rick309 4:bb9bcbb22882 142 }