Hester van de Ven / Mbed 2 deprecated PIDcontroller

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@4:f6862a157db9, 2019-10-23 (annotated)

Committer:
HestervdVen
Date:
Wed Oct 23 14:47:48 2019 +0000
Revision:
4:f6862a157db9
Parent:
3:750afb8580dd
Child:
5:3eace29679bc
Motor is aanstuurbaar door error in te typen. Reageert op CW en CCW rotatie.

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 2:29daa03e4f62 10
HestervdVen 2:29daa03e4f62 11 Serial pc(USBTX, USBRX);
HestervdVen 4:f6862a157db9 12 DigitalIn but_1(D1); //D1 to BUT1
HestervdVen 4:f6862a157db9 13 DigitalOut msignal_1(D6); //Signal to motor controller
HestervdVen 4:f6862a157db9 14 DigitalOut direction_1(D7); //ouput pin for direction of rotation
HestervdVen 2:29daa03e4f62 15
HestervdVen 4:f6862a157db9 16 //These seemed the best values after trying them out and using wikipedia's info
HestervdVen 4:f6862a157db9 17 const float k_p_1 = 1;
HestervdVen 4:f6862a157db9 18 const float k_i_1 = 1;
HestervdVen 4:f6862a157db9 19 const float k_d_1 = 0.1;
HestervdVen 4:f6862a157db9 20 const float t_s = 0.0025; //sample time in sec; same for both motors
HestervdVen 2:29daa03e4f62 21
HestervdVen 2:29daa03e4f62 22 //inputs for LPF; still to fill in!
HestervdVen 2:29daa03e4f62 23 float b1;
HestervdVen 2:29daa03e4f62 24 float b2;
HestervdVen 2:29daa03e4f62 25 float b3;
HestervdVen 2:29daa03e4f62 26 float b4;
HestervdVen 2:29daa03e4f62 27 float b5;
HestervdVen 2:29daa03e4f62 28
HestervdVen 4:f6862a157db9 29 float outcome_1 = 0;
HestervdVen 4:f6862a157db9 30 float setpoint_1;
HestervdVen 4:f6862a157db9 31 bool dir_1;
HestervdVen 4:f6862a157db9 32 bool moving_1 = false;
RobertoO 0:67c50348f842 33
HestervdVen 4:f6862a157db9 34 float getSetpoint_1(){
HestervdVen 2:29daa03e4f62 35 float a; //error
HestervdVen 3:750afb8580dd 36 cin >> a; //input on keyboard
HestervdVen 2:29daa03e4f62 37 return a;
HestervdVen 2:29daa03e4f62 38 }
HestervdVen 2:29daa03e4f62 39
HestervdVen 4:f6862a157db9 40 float getError_1(){
HestervdVen 4:f6862a157db9 41 float errorSum_1 = setpoint_1 - outcome_1;
HestervdVen 4:f6862a157db9 42 return errorSum_1;
HestervdVen 4:f6862a157db9 43 }
HestervdVen 4:f6862a157db9 44
HestervdVen 4:f6862a157db9 45 float checkMovement_1(){
HestervdVen 4:f6862a157db9 46 if (outcome_1 >= 1){
HestervdVen 4:f6862a157db9 47 dir_1 = true; //CCW
HestervdVen 4:f6862a157db9 48 moving_1 = true;
HestervdVen 4:f6862a157db9 49 pc.printf("Positive movement\r\n");
HestervdVen 4:f6862a157db9 50 }
HestervdVen 4:f6862a157db9 51 else if (outcome_1 <= -1){
HestervdVen 4:f6862a157db9 52 dir_1 = false; //CW
HestervdVen 4:f6862a157db9 53 moving_1 = true;
HestervdVen 4:f6862a157db9 54 pc.printf("Negative movement\r\n");
HestervdVen 4:f6862a157db9 55 }
HestervdVen 4:f6862a157db9 56 else{
HestervdVen 4:f6862a157db9 57 direction_1 = 0;
HestervdVen 4:f6862a157db9 58 moving_1 = false;
HestervdVen 4:f6862a157db9 59 pc.printf("No movement\r\n");
HestervdVen 4:f6862a157db9 60 }
HestervdVen 4:f6862a157db9 61 }
HestervdVen 2:29daa03e4f62 62
HestervdVen 4:f6862a157db9 63 void motor_1 (){ //runs the motor
HestervdVen 4:f6862a157db9 64
HestervdVen 4:f6862a157db9 65 if(moving_1){
HestervdVen 4:f6862a157db9 66 msignal_1 = 1; // Turn motor on
HestervdVen 4:f6862a157db9 67 }
HestervdVen 4:f6862a157db9 68 else{
HestervdVen 4:f6862a157db9 69 msignal_1 = 0; // Turn motor off
HestervdVen 4:f6862a157db9 70 }
HestervdVen 4:f6862a157db9 71
HestervdVen 4:f6862a157db9 72 }
HestervdVen 4:f6862a157db9 73
HestervdVen 4:f6862a157db9 74
HestervdVen 2:29daa03e4f62 75 /*PID controller code
HestervdVen 2:29daa03e4f62 76 Proportional part general formula: u_k = k_p * e
HestervdVen 2:29daa03e4f62 77 Integral part general formula: u_i = k_i * integral e dt
HestervdVen 2:29daa03e4f62 78 Derivative part general formula: u_d = k_d * derivative e */
HestervdVen 2:29daa03e4f62 79
HestervdVen 4:f6862a157db9 80 float PIDControl_1(float error){
HestervdVen 2:29daa03e4f62 81 static float error_integral = 0;
HestervdVen 2:29daa03e4f62 82 static float error_prev = error;
HestervdVen 2:29daa03e4f62 83 static BiQuad LPF (b1, b2, b3, b4, b5);
HestervdVen 2:29daa03e4f62 84
HestervdVen 2:29daa03e4f62 85 //proportional
HestervdVen 4:f6862a157db9 86 float u_k_1 = k_p_1 * error;
HestervdVen 2:29daa03e4f62 87
HestervdVen 2:29daa03e4f62 88 //integral
HestervdVen 2:29daa03e4f62 89 error_integral = error_integral + error * t_s;
HestervdVen 4:f6862a157db9 90 float u_i_1 = k_i_1 * error_integral;
HestervdVen 2:29daa03e4f62 91
HestervdVen 2:29daa03e4f62 92 //differential
HestervdVen 2:29daa03e4f62 93 float error_derivative = (error - error_prev) / t_s;
HestervdVen 2:29daa03e4f62 94 // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
HestervdVen 4:f6862a157db9 95 // float u_d_1 = k_d_1 * filtered_error;
HestervdVen 4:f6862a157db9 96 float u_d_1 = k_d_1 * error_derivative; //this is very sensitive to noise, hence the LPF above
HestervdVen 2:29daa03e4f62 97 error_prev = error;
HestervdVen 2:29daa03e4f62 98
HestervdVen 4:f6862a157db9 99 return u_k_1 + u_i_1 + u_d_1;
HestervdVen 2:29daa03e4f62 100 }
HestervdVen 2:29daa03e4f62 101
HestervdVen 4:f6862a157db9 102
HestervdVen 4:f6862a157db9 103 int main(){
RobertoO 0:67c50348f842 104 pc.baud(115200);
HestervdVen 4:f6862a157db9 105 startmain:
HestervdVen 2:29daa03e4f62 106 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
HestervdVen 4:f6862a157db9 107
RobertoO 0:67c50348f842 108
HestervdVen 4:f6862a157db9 109 while(true){
HestervdVen 2:29daa03e4f62 110 pc.printf("Please input your error.\r\n");
HestervdVen 4:f6862a157db9 111 //setpoint_1 = getSetpoint_1();
HestervdVen 4:f6862a157db9 112 //float err_1 = getError_1();
HestervdVen 4:f6862a157db9 113 float err_1 = getSetpoint_1();
HestervdVen 4:f6862a157db9 114 pc.printf("Your error is: %f\r\n",err_1);
HestervdVen 2:29daa03e4f62 115 pc.printf("-------\r\n-------\r\n");
HestervdVen 2:29daa03e4f62 116 wait(0.5);
HestervdVen 4:f6862a157db9 117 outcome_1 = PIDControl_1(err_1);
HestervdVen 4:f6862a157db9 118 pc.printf("The outcome is %f\r\n",outcome_1);
HestervdVen 4:f6862a157db9 119 direction_1 = dir_1;
HestervdVen 4:f6862a157db9 120 checkMovement_1();
HestervdVen 4:f6862a157db9 121 motor_1();
HestervdVen 2:29daa03e4f62 122
HestervdVen 2:29daa03e4f62 123
HestervdVen 4:f6862a157db9 124 //only works if error is constantly put in:
HestervdVen 4:f6862a157db9 125 // if (but_1==0){
HestervdVen 4:f6862a157db9 126 // goto startmain; //allows to input multiple numbers after each other
HestervdVen 4:f6862a157db9 127 // }
HestervdVen 3:750afb8580dd 128
HestervdVen 3:750afb8580dd 129
HestervdVen 4:f6862a157db9 130 }
RobertoO 0:67c50348f842 131 }