Eki Liptiay / Mbed 2 deprecated PID_Controller

hode pid broertje

Dependencies:   HIDScope QEI biquadFilter mbed

main.cpp@4:6dec99ee29e6, 2018-11-01 (annotated)

Committer:
ekiliptiay
Date:
Thu Nov 01 13:16:41 2018 +0000
Revision:
4:6dec99ee29e6
Parent:
3:659998b2bee1 
rew; brsr

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ekiliptiay 0:edcdd09960f7 1 #include "mbed.h"
ekiliptiay 1:3d92e7ff3dda 2 #include "math.h"
ekiliptiay 1:3d92e7ff3dda 3 #include "QEI.h"
ekiliptiay 1:3d92e7ff3dda 4 #include "HIDScope.h"
ekiliptiay 4:6dec99ee29e6 5 #include "BiQuad.h"
ekiliptiay 1:3d92e7ff3dda 6
ekiliptiay 1:3d92e7ff3dda 7 HIDScope scope(3);
ekiliptiay 0:edcdd09960f7 8
ekiliptiay 4:6dec99ee29e6 9 AnalogIn pot1(A2); // Controls potentiometer 1, which controls the reference position
ekiliptiay 1:3d92e7ff3dda 10
ekiliptiay 1:3d92e7ff3dda 11 InterruptIn motor1A(D13); // Encoder 1a
ekiliptiay 1:3d92e7ff3dda 12 InterruptIn motor1B(D12); // Encoder 1b
ekiliptiay 1:3d92e7ff3dda 13 QEI Encoder1(D12,D13,NC,64); // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
ekiliptiay 1:3d92e7ff3dda 14
ekiliptiay 3:659998b2bee1 15 DigitalOut directionpin1(D7);
ekiliptiay 3:659998b2bee1 16 PwmOut pwmpin1(D6);
ekiliptiay 0:edcdd09960f7 17 Serial pc(USBTX, USBRX); // tx, rx
ekiliptiay 0:edcdd09960f7 18
ekiliptiay 3:659998b2bee1 19 Ticker Motor1;
ekiliptiay 1:3d92e7ff3dda 20
ekiliptiay 3:659998b2bee1 21 double GetReferencePosition(){
ekiliptiay 3:659998b2bee1 22 // Returns the wanted reference position
ekiliptiay 4:6dec99ee29e6 23 const int CpR = 4200; //Counts Per Revolution; when the counts 4200, 1 revolution has been made. (Using X2 encoding)
ekiliptiay 3:659998b2bee1 24 double ReferencePosition = CpR*pot1; // Returns a value between 0 and 4200, the wanted rotation.
ekiliptiay 3:659998b2bee1 25 return ReferencePosition;
ekiliptiay 3:659998b2bee1 26 }
ekiliptiay 3:659998b2bee1 27
ekiliptiay 3:659998b2bee1 28 double GetActualPosition(){
ekiliptiay 4:6dec99ee29e6 29 double ActualPosition = Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
ekiliptiay 3:659998b2bee1 30 return ActualPosition;
ekiliptiay 3:659998b2bee1 31 }
ekiliptiay 4:6dec99ee29e6 32
ekiliptiay 4:6dec99ee29e6 33 double PID_controller(double error){
ekiliptiay 4:6dec99ee29e6 34 static double error_integral = 0;
ekiliptiay 4:6dec99ee29e6 35 static double error_prev = error; // initialization with this value only done once!
ekiliptiay 4:6dec99ee29e6 36 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
ekiliptiay 4:6dec99ee29e6 37 double Ts = 0.01;
ekiliptiay 3:659998b2bee1 38
ekiliptiay 4:6dec99ee29e6 39 // Proportional part
ekiliptiay 4:6dec99ee29e6 40
ekiliptiay 4:6dec99ee29e6 41 double Kp = 1;
ekiliptiay 4:6dec99ee29e6 42 double u_k = Kp*error;
ekiliptiay 4:6dec99ee29e6 43
ekiliptiay 4:6dec99ee29e6 44 // Integral part
ekiliptiay 4:6dec99ee29e6 45 double Ki = 0.06;
ekiliptiay 4:6dec99ee29e6 46 error_integral = error_integral + error * Ts;
ekiliptiay 4:6dec99ee29e6 47 double u_i = Ki * error_integral;
ekiliptiay 3:659998b2bee1 48
ekiliptiay 4:6dec99ee29e6 49 // Derivative part
ekiliptiay 4:6dec99ee29e6 50 double Kd = 0.2;
ekiliptiay 4:6dec99ee29e6 51 double error_derivative = (error - error_prev)/Ts;
ekiliptiay 4:6dec99ee29e6 52 double filtered_error_derivative = LowPassFilter.step(error_derivative);
ekiliptiay 4:6dec99ee29e6 53 double u_d = Kd * filtered_error_derivative;
ekiliptiay 4:6dec99ee29e6 54 error_prev = error;
ekiliptiay 3:659998b2bee1 55 // Sum all parts and return it
ekiliptiay 4:6dec99ee29e6 56 return u_k + u_i + u_d;
ekiliptiay 0:edcdd09960f7 57 }
ekiliptiay 0:edcdd09960f7 58
ekiliptiay 3:659998b2bee1 59 void SetMotor1(double motorValue){
ekiliptiay 3:659998b2bee1 60 // Given -1<=motorValue<=1, this sets the PWM and direction
ekiliptiay 3:659998b2bee1 61 // bits for motor 1. Positive value makes motor rotating
ekiliptiay 3:659998b2bee1 62 // clockwise. motorValues outside range are truncated to
ekiliptiay 3:659998b2bee1 63 // within range
ekiliptiay 3:659998b2bee1 64 if (motorValue >=0) directionpin1=1;
ekiliptiay 3:659998b2bee1 65 else directionpin1=0;
ekiliptiay 3:659998b2bee1 66 if (fabs(motorValue)>1) pwmpin1 = 1;
ekiliptiay 3:659998b2bee1 67 else pwmpin1 = fabs(motorValue);
ekiliptiay 3:659998b2bee1 68 }
ekiliptiay 3:659998b2bee1 69
ekiliptiay 4:6dec99ee29e6 70 void ScopeData(double motorValue){
ekiliptiay 4:6dec99ee29e6 71 scope.set(0,GetReferencePosition()); // Wanted amount of counts
ekiliptiay 3:659998b2bee1 72 scope.set(1,-Encoder1.getPulses()); // Amount of counts of motor 1
ekiliptiay 4:6dec99ee29e6 73 scope.set(2,motorValue);
ekiliptiay 3:659998b2bee1 74 scope.send(); // send what's in scope memory to PC
ekiliptiay 3:659998b2bee1 75 }
ekiliptiay 3:659998b2bee1 76
ekiliptiay 3:659998b2bee1 77 void MeasureAndControl(void){
ekiliptiay 3:659998b2bee1 78 // This function determines the desired velocity, measures the
ekiliptiay 3:659998b2bee1 79 // actual velocity, and controls the motor with
ekiliptiay 3:659998b2bee1 80 // a simple Feedback controller. Call this from a Ticker.
ekiliptiay 3:659998b2bee1 81 float ReferencePosition = GetReferencePosition();
ekiliptiay 3:659998b2bee1 82 float ActualPosition = GetActualPosition();
ekiliptiay 4:6dec99ee29e6 83 float motorValue = PID_controller(ReferencePosition - ActualPosition);
ekiliptiay 3:659998b2bee1 84 SetMotor1(motorValue);
ekiliptiay 4:6dec99ee29e6 85 ScopeData(motorValue);
ekiliptiay 3:659998b2bee1 86 }
ekiliptiay 3:659998b2bee1 87
ekiliptiay 3:659998b2bee1 88 int main(){
ekiliptiay 3:659998b2bee1 89 pwmpin1.period_us(60);
ekiliptiay 4:6dec99ee29e6 90 // Motor1.attach(&MeasureAndControl, 0.01);
ekiliptiay 3:659998b2bee1 91 while(1){
ekiliptiay 3:659998b2bee1 92 float ReferencePosition = GetReferencePosition();
ekiliptiay 3:659998b2bee1 93 float ActualPosition = GetActualPosition();
ekiliptiay 4:6dec99ee29e6 94 float motorValue = PID_controller(ReferencePosition - ActualPosition);
ekiliptiay 3:659998b2bee1 95 SetMotor1(motorValue);
ekiliptiay 4:6dec99ee29e6 96 ScopeData(motorValue);
ekiliptiay 3:659998b2bee1 97 wait(0.01f);
ekiliptiay 3:659998b2bee1 98 }
ekiliptiay 3:659998b2bee1 99 }