Hester van de Ven / Mbed 2 deprecated PIDcontroller

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@5:3eace29679bc, 2019-10-23 (annotated)

Committer:
HestervdVen
Date:
Wed Oct 23 20:29:00 2019 +0000
Revision:
5:3eace29679bc
Parent:
4:f6862a157db9
Child:
6:a177cc1af618
Full code, however not working well. Don't quite know what the problem is

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 Ticker EncTicker; // Ticker for encoder
HestervdVen 5:3eace29679bc 15
HestervdVen 4:f6862a157db9 16 DigitalIn but_1(D1); //D1 to BUT1
HestervdVen 4:f6862a157db9 17 DigitalOut msignal_1(D6); //Signal to motor controller
HestervdVen 4:f6862a157db9 18 DigitalOut direction_1(D7); //ouput pin for direction of rotation
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 5:3eace29679bc 21 QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING);
HestervdVen 2:29daa03e4f62 22
HestervdVen 4:f6862a157db9 23 //These seemed the best values after trying them out and using wikipedia's info
HestervdVen 4:f6862a157db9 24 const float k_p_1 = 1;
HestervdVen 4:f6862a157db9 25 const float k_i_1 = 1;
HestervdVen 4:f6862a157db9 26 const float k_d_1 = 0.1;
HestervdVen 4:f6862a157db9 27 const float t_s = 0.0025; //sample time in sec; same for both motors
HestervdVen 2:29daa03e4f62 28
HestervdVen 2:29daa03e4f62 29 //inputs for LPF; still to fill in!
HestervdVen 2:29daa03e4f62 30 float b1;
HestervdVen 2:29daa03e4f62 31 float b2;
HestervdVen 2:29daa03e4f62 32 float b3;
HestervdVen 2:29daa03e4f62 33 float b4;
HestervdVen 2:29daa03e4f62 34 float b5;
HestervdVen 2:29daa03e4f62 35
HestervdVen 5:3eace29679bc 36 float outcome_1; //what comes out of controller
HestervdVen 5:3eace29679bc 37 float setpoint_1; //desired outcome
HestervdVen 5:3eace29679bc 38 const float to_rads =(2*M_PI); //2*Pi
HestervdVen 5:3eace29679bc 39 const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor)
HestervdVen 5:3eace29679bc 40 const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count)
HestervdVen 5:3eace29679bc 41 volatile float currentAngle_1 = 0; //initialize angle of motor_1 in radians
HestervdVen 5:3eace29679bc 42 //volatile float psiabs_1 = 0; //absolute value of psi_1
HestervdVen 5:3eace29679bc 43 volatile int counts_1 = 0; // counts of Encoder_1
HestervdVen 5:3eace29679bc 44 bool dir_1; //true = CCW, false = CW
HestervdVen 4:f6862a157db9 45 bool moving_1 = false;
RobertoO 0:67c50348f842 46
HestervdVen 5:3eace29679bc 47 //gets desired angle from keyboard
HestervdVen 4:f6862a157db9 48 float getSetpoint_1(){
HestervdVen 5:3eace29679bc 49 float a; //setpoint
HestervdVen 3:750afb8580dd 50 cin >> a; //input on keyboard
HestervdVen 2:29daa03e4f62 51 return a;
HestervdVen 2:29daa03e4f62 52 }
HestervdVen 2:29daa03e4f62 53
HestervdVen 5:3eace29679bc 54 //calculates error that goes into controller
HestervdVen 4:f6862a157db9 55 float getError_1(){
HestervdVen 5:3eace29679bc 56 float errorSum_1 = setpoint_1 - currentAngle_1;
HestervdVen 4:f6862a157db9 57 return errorSum_1;
HestervdVen 4:f6862a157db9 58 }
HestervdVen 4:f6862a157db9 59
HestervdVen 5:3eace29679bc 60 //checks if the motor is turning and in what direction
HestervdVen 5:3eace29679bc 61 void checkMovement_1(){
HestervdVen 4:f6862a157db9 62 if (outcome_1 >= 1){
HestervdVen 5:3eace29679bc 63 dir_1 = true; //CCW rotation
HestervdVen 4:f6862a157db9 64 moving_1 = true;
HestervdVen 5:3eace29679bc 65 //pc.printf("Positive movement\r\n");
HestervdVen 4:f6862a157db9 66 }
HestervdVen 4:f6862a157db9 67 else if (outcome_1 <= -1){
HestervdVen 5:3eace29679bc 68 dir_1 = false; //CW rotation
HestervdVen 4:f6862a157db9 69 moving_1 = true;
HestervdVen 5:3eace29679bc 70 //pc.printf("Negative movement\r\n");
HestervdVen 4:f6862a157db9 71 }
HestervdVen 4:f6862a157db9 72 else{
HestervdVen 4:f6862a157db9 73 direction_1 = 0;
HestervdVen 4:f6862a157db9 74 moving_1 = false;
HestervdVen 5:3eace29679bc 75 //pc.printf("No movement\r\n");
HestervdVen 4:f6862a157db9 76 }
HestervdVen 4:f6862a157db9 77 }
HestervdVen 2:29daa03e4f62 78
HestervdVen 5:3eace29679bc 79 //sends signals to the motor
HestervdVen 5:3eace29679bc 80 void motor_1 (){
HestervdVen 4:f6862a157db9 81 if(moving_1){
HestervdVen 4:f6862a157db9 82 msignal_1 = 1; // Turn motor on
HestervdVen 4:f6862a157db9 83 }
HestervdVen 4:f6862a157db9 84 else{
HestervdVen 4:f6862a157db9 85 msignal_1 = 0; // Turn motor off
HestervdVen 4:f6862a157db9 86 }
HestervdVen 4:f6862a157db9 87 }
HestervdVen 4:f6862a157db9 88
HestervdVen 4:f6862a157db9 89
HestervdVen 5:3eace29679bc 90 //calculates the angle of motor 1
HestervdVen 5:3eace29679bc 91 float angleEncoder_1(){
HestervdVen 5:3eace29679bc 92 currentAngle_1 = ShaftResolution * counts_1;
HestervdVen 5:3eace29679bc 93 pc.printf("Angle of motor_1: %f radians\r\n",currentAngle_1);
HestervdVen 5:3eace29679bc 94 return currentAngle_1;
HestervdVen 5:3eace29679bc 95 }
HestervdVen 5:3eace29679bc 96
HestervdVen 5:3eace29679bc 97
HestervdVen 2:29daa03e4f62 98 /*PID controller code
HestervdVen 2:29daa03e4f62 99 Proportional part general formula: u_k = k_p * e
HestervdVen 2:29daa03e4f62 100 Integral part general formula: u_i = k_i * integral e dt
HestervdVen 2:29daa03e4f62 101 Derivative part general formula: u_d = k_d * derivative e */
HestervdVen 2:29daa03e4f62 102
HestervdVen 4:f6862a157db9 103 float PIDControl_1(float error){
HestervdVen 2:29daa03e4f62 104 static float error_integral = 0;
HestervdVen 2:29daa03e4f62 105 static float error_prev = error;
HestervdVen 2:29daa03e4f62 106 static BiQuad LPF (b1, b2, b3, b4, b5);
HestervdVen 2:29daa03e4f62 107
HestervdVen 2:29daa03e4f62 108 //proportional
HestervdVen 4:f6862a157db9 109 float u_k_1 = k_p_1 * error;
HestervdVen 2:29daa03e4f62 110
HestervdVen 2:29daa03e4f62 111 //integral
HestervdVen 2:29daa03e4f62 112 error_integral = error_integral + error * t_s;
HestervdVen 4:f6862a157db9 113 float u_i_1 = k_i_1 * error_integral;
HestervdVen 2:29daa03e4f62 114
HestervdVen 2:29daa03e4f62 115 //differential
HestervdVen 2:29daa03e4f62 116 float error_derivative = (error - error_prev) / t_s;
HestervdVen 2:29daa03e4f62 117 // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
HestervdVen 4:f6862a157db9 118 // float u_d_1 = k_d_1 * filtered_error;
HestervdVen 4:f6862a157db9 119 float u_d_1 = k_d_1 * error_derivative; //this is very sensitive to noise, hence the LPF above
HestervdVen 2:29daa03e4f62 120 error_prev = error;
HestervdVen 2:29daa03e4f62 121
HestervdVen 4:f6862a157db9 122 return u_k_1 + u_i_1 + u_d_1;
HestervdVen 2:29daa03e4f62 123 }
HestervdVen 2:29daa03e4f62 124
HestervdVen 4:f6862a157db9 125
HestervdVen 4:f6862a157db9 126 int main(){
HestervdVen 5:3eace29679bc 127 pc.baud(SERIAL_BAUD);
HestervdVen 5:3eace29679bc 128 // startmain:
HestervdVen 5:3eace29679bc 129 pc.printf("--------\r\nWelcome!\r\n--------\r\n");
HestervdVen 4:f6862a157db9 130
RobertoO 0:67c50348f842 131
HestervdVen 4:f6862a157db9 132 while(true){
HestervdVen 5:3eace29679bc 133 pc.printf("Please input your desired angle.\r\n");
HestervdVen 5:3eace29679bc 134 setpoint_1 = getSetpoint_1();
HestervdVen 5:3eace29679bc 135 pc.printf("Setpoint = %f\r\n", setpoint_1);
HestervdVen 5:3eace29679bc 136 float err_1 = getError_1();
HestervdVen 5:3eace29679bc 137 pc.printf("Error = %f\r\n",err_1);
HestervdVen 4:f6862a157db9 138 outcome_1 = PIDControl_1(err_1);
HestervdVen 5:3eace29679bc 139 //pc.printf("Outcome = %f\r\n",outcome_1);
HestervdVen 4:f6862a157db9 140 direction_1 = dir_1;
HestervdVen 4:f6862a157db9 141 checkMovement_1();
HestervdVen 4:f6862a157db9 142 motor_1();
HestervdVen 5:3eace29679bc 143 counts_1 = Encoder_1.getPulses();
HestervdVen 5:3eace29679bc 144 angleEncoder_1();
HestervdVen 5:3eace29679bc 145 pc.printf("-------\r\n-------\r\n");
HestervdVen 5:3eace29679bc 146 wait(0.5);
HestervdVen 2:29daa03e4f62 147
HestervdVen 2:29daa03e4f62 148
HestervdVen 4:f6862a157db9 149 //only works if error is constantly put in:
HestervdVen 4:f6862a157db9 150 // if (but_1==0){
HestervdVen 4:f6862a157db9 151 // goto startmain; //allows to input multiple numbers after each other
HestervdVen 4:f6862a157db9 152 // }
HestervdVen 3:750afb8580dd 153
HestervdVen 3:750afb8580dd 154
HestervdVen 4:f6862a157db9 155 }
RobertoO 0:67c50348f842 156 }