Hester van de Ven / Mbed 2 deprecated PIDcontroller

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@8:31276b85a9a1, 2019-10-30 (annotated)

Committer:
HestervdVen
Date:
Wed Oct 30 11:35:03 2019 +0000
Revision:
8:31276b85a9a1
Parent:
7:14c0c10a0090 
Kp, Ki en Kd getweaked door trial and error.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
HestervdVen 2:29daa03e4f62 3 #include "BiQuad.h"
HestervdVen 2:29daa03e4f62 4 #include <iostream>
HestervdVen 2:29daa03e4f62 5 #include "FastPWM.h"
HestervdVen 2:29daa03e4f62 6 #include "QEI.h"
HestervdVen 2:29daa03e4f62 7 #include <math.h>
HestervdVen 2:29daa03e4f62 8 //#include "HIDScope.h"
HestervdVen 2:29daa03e4f62 9
HestervdVen 5:3eace29679bc 10 #define SERIAL_BAUD 115200
HestervdVen 5:3eace29679bc 11 #define M_PI acos(-1.0)
HestervdVen 2:29daa03e4f62 12
HestervdVen 2:29daa03e4f62 13 Serial pc(USBTX, USBRX);
HestervdVen 5:3eace29679bc 14
HestervdVen 6:a177cc1af618 15 PwmOut msignal_1(D6); //Signal to motor controller
HestervdVen 7:14c0c10a0090 16 PwmOut msignal_2(D5);
HestervdVen 4:f6862a157db9 17 DigitalOut direction_1(D7); //ouput pin for direction of rotation
HestervdVen 7:14c0c10a0090 18 DigitalOut direction_2(D4);
HestervdVen 5:3eace29679bc 19 DigitalIn enc_1a(D10); //D10 to ENC1 A
HestervdVen 5:3eace29679bc 20 DigitalIn enc_1b(D9); //D9 to ENC1 B
HestervdVen 8:31276b85a9a1 21 DigitalIn enc_2a(D2); //D11 to ENC2 A
HestervdVen 8:31276b85a9a1 22 DigitalIn enc_2b(D1); //D12 to ENC2 B
HestervdVen 5:3eace29679bc 23 QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING);
HestervdVen 8:31276b85a9a1 24 QEI Encoder_2(D2,D1,NC,64,QEI::X4_ENCODING);
HestervdVen 2:29daa03e4f62 25
HestervdVen 4:f6862a157db9 26 //These seemed the best values after trying them out and using wikipedia's info
HestervdVen 6:a177cc1af618 27 const float kp_1 = 1;
HestervdVen 6:a177cc1af618 28 const float ki_1 = 1;
HestervdVen 6:a177cc1af618 29 const float kd_1 = 0.1;
HestervdVen 6:a177cc1af618 30 const float kp_2 = 1;
HestervdVen 6:a177cc1af618 31 const float ki_2 = 1;
HestervdVen 6:a177cc1af618 32 const float kd_2 = 0.1;
HestervdVen 4:f6862a157db9 33 const float t_s = 0.0025; //sample time in sec; same for both motors
HestervdVen 2:29daa03e4f62 34
HestervdVen 2:29daa03e4f62 35
HestervdVen 8:31276b85a9a1 36 volatile float currentAngle_1 = 0;
HestervdVen 8:31276b85a9a1 37 volatile float currentAngle_2 = 0;
HestervdVen 8:31276b85a9a1 38 float error_prev_1 = 0;
HestervdVen 8:31276b85a9a1 39 float error_prev_2 = 0;
HestervdVen 8:31276b85a9a1 40 float error_integral_1 = 0;
HestervdVen 8:31276b85a9a1 41 float error_integral_2 = 0;
HestervdVen 6:a177cc1af618 42 const float period = 10.0; //sets period to 10ms
HestervdVen 5:3eace29679bc 43 const float to_rads =(2*M_PI); //2*Pi
HestervdVen 5:3eace29679bc 44 const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor)
HestervdVen 5:3eace29679bc 45 const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count)
HestervdVen 6:a177cc1af618 46 volatile float width;
HestervdVen 8:31276b85a9a1 47 volatile float width_percent = 0.5; //higher value is higher motor speed
HestervdVen 6:a177cc1af618 48 volatile float on_time_1;
HestervdVen 6:a177cc1af618 49 volatile float off_time_1;
HestervdVen 8:31276b85a9a1 50 volatile float counts_1 = 0; // counts of Encoder_1; THIS HAS TO BE A FLOAT TO WORK
HestervdVen 8:31276b85a9a1 51 volatile float counts_2 = 0; // counts of Encoder_2
HestervdVen 5:3eace29679bc 52 bool dir_1; //true = CCW, false = CW
HestervdVen 6:a177cc1af618 53 bool dir_2; //true = CCW, false = CW
HestervdVen 4:f6862a157db9 54 bool moving_1 = false;
HestervdVen 6:a177cc1af618 55 bool moving_2 = false;
RobertoO 0:67c50348f842 56
HestervdVen 5:3eace29679bc 57 //gets desired angle from keyboard
HestervdVen 4:f6862a157db9 58 float getSetpoint_1(){
HestervdVen 6:a177cc1af618 59 float setpoint1; //setpoint
HestervdVen 6:a177cc1af618 60 cin >> setpoint1; //input on keyboard
HestervdVen 6:a177cc1af618 61 return setpoint1;
HestervdVen 6:a177cc1af618 62 }
HestervdVen 8:31276b85a9a1 63
HestervdVen 6:a177cc1af618 64 float getSetpoint_2(){
HestervdVen 6:a177cc1af618 65 float setpoint2; //setpoint
HestervdVen 6:a177cc1af618 66 cin >> setpoint2; //input on keyboard
HestervdVen 6:a177cc1af618 67 return setpoint2;
HestervdVen 2:29daa03e4f62 68 }
HestervdVen 8:31276b85a9a1 69
HestervdVen 5:3eace29679bc 70 //calculates error that goes into controller
HestervdVen 6:a177cc1af618 71 float getError(float setpoint, float angle){
HestervdVen 6:a177cc1af618 72 float errorSum = setpoint - angle;
HestervdVen 6:a177cc1af618 73 return errorSum;
HestervdVen 4:f6862a157db9 74 }
HestervdVen 4:f6862a157db9 75
HestervdVen 5:3eace29679bc 76 //checks if the motor is turning and in what direction
HestervdVen 6:a177cc1af618 77 void checkMovement_1(double outcome){
HestervdVen 6:a177cc1af618 78 if (outcome >= 0.1){
HestervdVen 5:3eace29679bc 79 dir_1 = true; //CCW rotation
HestervdVen 4:f6862a157db9 80 moving_1 = true;
HestervdVen 5:3eace29679bc 81 //pc.printf("Positive movement\r\n");
HestervdVen 4:f6862a157db9 82 }
HestervdVen 6:a177cc1af618 83 else if (outcome <= -0.1){
HestervdVen 5:3eace29679bc 84 dir_1 = false; //CW rotation
HestervdVen 4:f6862a157db9 85 moving_1 = true;
HestervdVen 5:3eace29679bc 86 //pc.printf("Negative movement\r\n");
HestervdVen 4:f6862a157db9 87 }
HestervdVen 4:f6862a157db9 88 else{
HestervdVen 8:31276b85a9a1 89 //direction_1 = 0;
HestervdVen 4:f6862a157db9 90 moving_1 = false;
HestervdVen 5:3eace29679bc 91 //pc.printf("No movement\r\n");
HestervdVen 4:f6862a157db9 92 }
HestervdVen 4:f6862a157db9 93 }
HestervdVen 2:29daa03e4f62 94
HestervdVen 6:a177cc1af618 95
HestervdVen 6:a177cc1af618 96 //checks if the motor is turning and in what direction
HestervdVen 6:a177cc1af618 97 void checkMovement_2(double outcome){
HestervdVen 6:a177cc1af618 98 if (outcome >= 0.1){
HestervdVen 8:31276b85a9a1 99 dir_2 = false; //CCW rotation
HestervdVen 6:a177cc1af618 100 moving_2 = true;
HestervdVen 6:a177cc1af618 101 //pc.printf("Positive movement\r\n");
HestervdVen 6:a177cc1af618 102 }
HestervdVen 6:a177cc1af618 103 else if (outcome <= -0.1){
HestervdVen 8:31276b85a9a1 104 dir_2 = true; //CW rotation
HestervdVen 6:a177cc1af618 105 moving_2 = true;
HestervdVen 6:a177cc1af618 106 //pc.printf("Negative movement\r\n");
HestervdVen 6:a177cc1af618 107 }
HestervdVen 6:a177cc1af618 108 else{
HestervdVen 8:31276b85a9a1 109 // direction_2 = 0;
HestervdVen 6:a177cc1af618 110 moving_2 = false;
HestervdVen 6:a177cc1af618 111 //pc.printf("No movement\r\n");
HestervdVen 6:a177cc1af618 112 }
HestervdVen 6:a177cc1af618 113 }
HestervdVen 8:31276b85a9a1 114
HestervdVen 6:a177cc1af618 115 void PWM_Motor (void) //Calculates on and off times; higher PWM = higher avarage voltage
HestervdVen 6:a177cc1af618 116 {
HestervdVen 6:a177cc1af618 117 width = period * width_percent;
HestervdVen 6:a177cc1af618 118 on_time_1 = width;
HestervdVen 6:a177cc1af618 119 off_time_1 = period - width;
HestervdVen 6:a177cc1af618 120 }
HestervdVen 6:a177cc1af618 121
HestervdVen 5:3eace29679bc 122 //sends signals to the motor
HestervdVen 5:3eace29679bc 123 void motor_1 (){
HestervdVen 4:f6862a157db9 124 if(moving_1){
HestervdVen 4:f6862a157db9 125 msignal_1 = 1; // Turn motor on
HestervdVen 6:a177cc1af618 126 wait_ms(on_time_1); //According to calculated PWM
HestervdVen 6:a177cc1af618 127 msignal_1 = 0; // Turn motor off
HestervdVen 6:a177cc1af618 128 wait_ms(off_time_1); //According to calculated PWM
HestervdVen 6:a177cc1af618 129 }
HestervdVen 6:a177cc1af618 130 else{
HestervdVen 6:a177cc1af618 131 msignal_1 = 0; // Turn motor off
HestervdVen 6:a177cc1af618 132 }
HestervdVen 6:a177cc1af618 133 }
HestervdVen 6:a177cc1af618 134
HestervdVen 6:a177cc1af618 135 void motor_2 (){
HestervdVen 6:a177cc1af618 136 if(moving_2){
HestervdVen 7:14c0c10a0090 137 msignal_2 = 1; // Turn motor on
HestervdVen 6:a177cc1af618 138 wait_ms(on_time_1); //According to calculated PWM
HestervdVen 7:14c0c10a0090 139 msignal_2 = 0; // Turn motor off
HestervdVen 6:a177cc1af618 140 wait_ms(off_time_1); //According to calculated PWM
HestervdVen 4:f6862a157db9 141 }
HestervdVen 4:f6862a157db9 142 else{
HestervdVen 7:14c0c10a0090 143 msignal_2 = 0; // Turn motor off
HestervdVen 4:f6862a157db9 144 }
HestervdVen 4:f6862a157db9 145 }
HestervdVen 4:f6862a157db9 146
HestervdVen 4:f6862a157db9 147
HestervdVen 5:3eace29679bc 148 //calculates the angle of motor 1
HestervdVen 6:a177cc1af618 149 float angleEncoder(float counts){
HestervdVen 6:a177cc1af618 150 float currentAngle = ShaftResolution * counts;
HestervdVen 8:31276b85a9a1 151 pc.printf("Angle of motor: %f radians\r\n",currentAngle);
HestervdVen 6:a177cc1af618 152 return currentAngle;
HestervdVen 5:3eace29679bc 153 }
HestervdVen 5:3eace29679bc 154
HestervdVen 5:3eace29679bc 155
HestervdVen 2:29daa03e4f62 156 /*PID controller code
HestervdVen 2:29daa03e4f62 157 Proportional part general formula: u_k = k_p * e
HestervdVen 2:29daa03e4f62 158 Integral part general formula: u_i = k_i * integral e dt
HestervdVen 2:29daa03e4f62 159 Derivative part general formula: u_d = k_d * derivative e */
HestervdVen 2:29daa03e4f62 160
HestervdVen 8:31276b85a9a1 161 float PIDControl_1(float error, float kp, float ki, float kd){
HestervdVen 8:31276b85a9a1 162 //static float error_integral_1 = 0;
HestervdVen 8:31276b85a9a1 163 // static float error_prev_1 = error;
HestervdVen 8:31276b85a9a1 164 static BiQuad LPF1 (0.0675, 0.1349, 0.0675, -1.1430, 0.4128);
HestervdVen 2:29daa03e4f62 165
HestervdVen 2:29daa03e4f62 166 //proportional
HestervdVen 6:a177cc1af618 167 float u_k = kp * error;
HestervdVen 2:29daa03e4f62 168
HestervdVen 2:29daa03e4f62 169 //integral
HestervdVen 8:31276b85a9a1 170 error_integral_1 = error_integral_1 + error * t_s;
HestervdVen 8:31276b85a9a1 171 float u_i = ki * error_integral_1;
HestervdVen 2:29daa03e4f62 172
HestervdVen 2:29daa03e4f62 173 //differential
HestervdVen 8:31276b85a9a1 174 float error_derivative_1 = (error - error_prev_1) / t_s;
HestervdVen 8:31276b85a9a1 175 float filtered_error = LPF1.step(error_derivative_1); //LPF with function of BiQuad library
HestervdVen 8:31276b85a9a1 176 float u_d = kd * filtered_error;
HestervdVen 8:31276b85a9a1 177 error_prev_1 = error;
HestervdVen 2:29daa03e4f62 178
HestervdVen 6:a177cc1af618 179 return u_k + u_i + u_d;
HestervdVen 2:29daa03e4f62 180 }
HestervdVen 2:29daa03e4f62 181
HestervdVen 8:31276b85a9a1 182 float PIDControl_2(float error, float kp, float ki, float kd){
HestervdVen 8:31276b85a9a1 183 //static float error_integral_2 = 0;
HestervdVen 8:31276b85a9a1 184 //static float error_prev_2 = error;
HestervdVen 8:31276b85a9a1 185 //static BiQuad LPF2 (0.0675, 0.1349, 0.0675, -1.1430, 0.4128);
HestervdVen 4:f6862a157db9 186
HestervdVen 8:31276b85a9a1 187 //proportional
HestervdVen 8:31276b85a9a1 188 float u_k = kp * error;
HestervdVen 8:31276b85a9a1 189
HestervdVen 8:31276b85a9a1 190 //integral
HestervdVen 8:31276b85a9a1 191 error_integral_2 = error_integral_2 + error * t_s;
HestervdVen 8:31276b85a9a1 192 float u_i = ki * error_integral_2;
HestervdVen 8:31276b85a9a1 193
HestervdVen 8:31276b85a9a1 194
HestervdVen 8:31276b85a9a1 195 //differential
HestervdVen 8:31276b85a9a1 196 float error_derivative_2 = (error - error_prev_2) / t_s;
HestervdVen 8:31276b85a9a1 197 //float filtered_error = LPF2.step(error_derivative_2); //LPF with function of BiQuad library
HestervdVen 8:31276b85a9a1 198 //float u_d = kd * filtered_error;
HestervdVen 8:31276b85a9a1 199 float u_d = kd * error_derivative_2;
HestervdVen 8:31276b85a9a1 200 error_prev_2 = error;
HestervdVen 8:31276b85a9a1 201
HestervdVen 8:31276b85a9a1 202 return u_k + u_i + u_d;
HestervdVen 8:31276b85a9a1 203 }
HestervdVen 8:31276b85a9a1 204
HestervdVen 4:f6862a157db9 205 int main(){
HestervdVen 5:3eace29679bc 206 pc.baud(SERIAL_BAUD);
HestervdVen 8:31276b85a9a1 207
HestervdVen 6:a177cc1af618 208 // startmain:
HestervdVen 5:3eace29679bc 209 pc.printf("--------\r\nWelcome!\r\n--------\r\n");
HestervdVen 8:31276b85a9a1 210 //pc.printf("Please input Setpoint 1\r\n");
HestervdVen 8:31276b85a9a1 211 //float setpoint_1 = getSetpoint_1();
HestervdVen 8:31276b85a9a1 212 //pc.printf("Setpoint 1= %f\r\n", setpoint_1);
HestervdVen 7:14c0c10a0090 213 pc.printf("Please input Setpoint 2\r\n");
HestervdVen 6:a177cc1af618 214 float setpoint_2 = getSetpoint_2();
HestervdVen 7:14c0c10a0090 215 pc.printf("Setpoint 2= %f\r\n", setpoint_2);
HestervdVen 6:a177cc1af618 216 PWM_Motor();
RobertoO 0:67c50348f842 217
HestervdVen 4:f6862a157db9 218 while(true){
HestervdVen 8:31276b85a9a1 219 /*
HestervdVen 6:a177cc1af618 220 float err_1 = getError(setpoint_1, currentAngle_1);
HestervdVen 8:31276b85a9a1 221
HestervdVen 6:a177cc1af618 222 pc.printf("Error 1 = %f\r\n",err_1);
HestervdVen 8:31276b85a9a1 223
HestervdVen 8:31276b85a9a1 224 float outcome_1 = PIDControl_1(err_1, kp_1, ki_1, kd_1);
HestervdVen 8:31276b85a9a1 225
HestervdVen 6:a177cc1af618 226 pc.printf("Outcome 1 = %f\r\n",outcome_1);
HestervdVen 8:31276b85a9a1 227
HestervdVen 4:f6862a157db9 228 direction_1 = dir_1;
HestervdVen 8:31276b85a9a1 229
HestervdVen 6:a177cc1af618 230 checkMovement_1(outcome_1);
HestervdVen 8:31276b85a9a1 231
HestervdVen 4:f6862a157db9 232 motor_1();
HestervdVen 8:31276b85a9a1 233
HestervdVen 5:3eace29679bc 234 counts_1 = Encoder_1.getPulses();
HestervdVen 8:31276b85a9a1 235
HestervdVen 6:a177cc1af618 236 currentAngle_1 = angleEncoder(counts_1);
HestervdVen 8:31276b85a9a1 237
HestervdVen 8:31276b85a9a1 238 pc.printf("Counts1 = %f\r\n", counts_1);
HestervdVen 8:31276b85a9a1 239
HestervdVen 8:31276b85a9a1 240 */
HestervdVen 8:31276b85a9a1 241
HestervdVen 8:31276b85a9a1 242 float err_2 = getError(setpoint_2, currentAngle_2);
HestervdVen 8:31276b85a9a1 243 pc.printf("Error 2 = %f\r\n",err_2);
HestervdVen 8:31276b85a9a1 244 float outcome_2 = PIDControl_2(err_2, kp_2, ki_2, kd_2);
HestervdVen 8:31276b85a9a1 245 pc.printf("Outcome 2 = %f\r\n",outcome_2);
HestervdVen 8:31276b85a9a1 246 direction_2 = dir_2;
HestervdVen 8:31276b85a9a1 247 checkMovement_2(outcome_2);
HestervdVen 8:31276b85a9a1 248 motor_2();
HestervdVen 8:31276b85a9a1 249 counts_2 = Encoder_2.getPulses();
HestervdVen 8:31276b85a9a1 250 currentAngle_2 = angleEncoder(counts_2);
HestervdVen 8:31276b85a9a1 251
HestervdVen 8:31276b85a9a1 252 pc.printf("Counts2 = %f\r\n", counts_2);
HestervdVen 5:3eace29679bc 253 pc.printf("-------\r\n-------\r\n");
HestervdVen 2:29daa03e4f62 254
HestervdVen 2:29daa03e4f62 255
HestervdVen 4:f6862a157db9 256 }
RobertoO 0:67c50348f842 257 }