Eki Liptiay / Mbed 2 deprecated PID_Controller

hode pid broertje

Dependencies:   HIDScope QEI biquadFilter mbed

main.cpp@3:659998b2bee1, 2018-10-18 (annotated)

Committer:
ekiliptiay
Date:
Thu Oct 18 14:16:49 2018 +0000
Revision:
3:659998b2bee1
Parent:
2:93918cad51dd
Child:
4:6dec99ee29e6
P Controller working

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 1:3d92e7ff3dda 5
ekiliptiay 1:3d92e7ff3dda 6 HIDScope scope(3);
ekiliptiay 0:edcdd09960f7 7
ekiliptiay 0:edcdd09960f7 8 AnalogIn pot1(A2); // Controls potentiometer 1, which controls motor 1
ekiliptiay 1:3d92e7ff3dda 9 AnalogIn pot2(A3); // Controls potentiometer 2, which is used to control Kp (position gain)
ekiliptiay 0:edcdd09960f7 10 InterruptIn button1(D0);
ekiliptiay 1:3d92e7ff3dda 11
ekiliptiay 1:3d92e7ff3dda 12 InterruptIn motor1A(D13); // Encoder 1a
ekiliptiay 1:3d92e7ff3dda 13 InterruptIn motor1B(D12); // Encoder 1b
ekiliptiay 1:3d92e7ff3dda 14 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 15
ekiliptiay 3:659998b2bee1 16 DigitalOut directionpin1(D7);
ekiliptiay 3:659998b2bee1 17 PwmOut pwmpin1(D6);
ekiliptiay 0:edcdd09960f7 18 Serial pc(USBTX, USBRX); // tx, rx
ekiliptiay 0:edcdd09960f7 19
ekiliptiay 3:659998b2bee1 20 Ticker Motor1;
ekiliptiay 1:3d92e7ff3dda 21
ekiliptiay 3:659998b2bee1 22 double GetReferencePosition(){
ekiliptiay 3:659998b2bee1 23 // Returns the wanted reference position
ekiliptiay 3:659998b2bee1 24 const int CpR = 4200; //Counts Per Revolution; when the counts i 4200, 1 revolution has been made. (Using X2 encoding)
ekiliptiay 3:659998b2bee1 25 double ReferencePosition = CpR*pot1; // Returns a value between 0 and 4200, the wanted rotation.
ekiliptiay 3:659998b2bee1 26 return ReferencePosition;
ekiliptiay 3:659998b2bee1 27 }
ekiliptiay 3:659998b2bee1 28
ekiliptiay 3:659998b2bee1 29 double GetActualPosition(){
ekiliptiay 3:659998b2bee1 30 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 31 return ActualPosition;
ekiliptiay 3:659998b2bee1 32 }
ekiliptiay 3:659998b2bee1 33
ekiliptiay 3:659998b2bee1 34 double P_controller(double error){
ekiliptiay 3:659998b2bee1 35 double Kp = pot2*17+1;
ekiliptiay 3:659998b2bee1 36 double u_k = Kp*error; // Proportional part
ekiliptiay 3:659998b2bee1 37
ekiliptiay 3:659998b2bee1 38 // Sum all parts and return it
ekiliptiay 3:659998b2bee1 39 return u_k;
ekiliptiay 0:edcdd09960f7 40 }
ekiliptiay 0:edcdd09960f7 41
ekiliptiay 3:659998b2bee1 42 void SetMotor1(double motorValue){
ekiliptiay 3:659998b2bee1 43 // Given -1<=motorValue<=1, this sets the PWM and direction
ekiliptiay 3:659998b2bee1 44 // bits for motor 1. Positive value makes motor rotating
ekiliptiay 3:659998b2bee1 45 // clockwise. motorValues outside range are truncated to
ekiliptiay 3:659998b2bee1 46 // within range
ekiliptiay 3:659998b2bee1 47 if (motorValue >=0) directionpin1=1;
ekiliptiay 3:659998b2bee1 48 else directionpin1=0;
ekiliptiay 3:659998b2bee1 49 if (fabs(motorValue)>1) pwmpin1 = 1;
ekiliptiay 3:659998b2bee1 50 else pwmpin1 = fabs(motorValue);
ekiliptiay 3:659998b2bee1 51 }
ekiliptiay 3:659998b2bee1 52
ekiliptiay 3:659998b2bee1 53 void ScopeData(){
ekiliptiay 3:659998b2bee1 54 scope.set(0,pot1*4200); // Wanted amount of counts
ekiliptiay 3:659998b2bee1 55 scope.set(1,-Encoder1.getPulses()); // Amount of counts of motor 1
ekiliptiay 3:659998b2bee1 56 scope.set(2,pwmpin1); // Current pwm-value send to motor 1
ekiliptiay 3:659998b2bee1 57 scope.send(); // send what's in scope memory to PC
ekiliptiay 3:659998b2bee1 58 }
ekiliptiay 3:659998b2bee1 59
ekiliptiay 3:659998b2bee1 60 void MeasureAndControl(void){
ekiliptiay 3:659998b2bee1 61 // This function determines the desired velocity, measures the
ekiliptiay 3:659998b2bee1 62 // actual velocity, and controls the motor with
ekiliptiay 3:659998b2bee1 63 // a simple Feedback controller. Call this from a Ticker.
ekiliptiay 3:659998b2bee1 64 float ReferencePosition = GetReferencePosition();
ekiliptiay 3:659998b2bee1 65 float ActualPosition = GetActualPosition();
ekiliptiay 3:659998b2bee1 66 float motorValue = P_controller(ReferencePosition - ActualPosition);
ekiliptiay 3:659998b2bee1 67 SetMotor1(motorValue);
ekiliptiay 3:659998b2bee1 68 ScopeData();
ekiliptiay 3:659998b2bee1 69 }
ekiliptiay 3:659998b2bee1 70
ekiliptiay 3:659998b2bee1 71 int main(){
ekiliptiay 3:659998b2bee1 72 pwmpin1.period_us(60);
ekiliptiay 3:659998b2bee1 73 // Motor1.attach(&MeasureAndControl, 0.01);
ekiliptiay 3:659998b2bee1 74 while(1){
ekiliptiay 3:659998b2bee1 75 float ReferencePosition = GetReferencePosition();
ekiliptiay 3:659998b2bee1 76 float ActualPosition = GetActualPosition();
ekiliptiay 3:659998b2bee1 77 float motorValue = P_controller(ReferencePosition - ActualPosition);
ekiliptiay 3:659998b2bee1 78 SetMotor1(motorValue);
ekiliptiay 3:659998b2bee1 79 ScopeData();
ekiliptiay 3:659998b2bee1 80 wait(0.01f);
ekiliptiay 3:659998b2bee1 81 }
ekiliptiay 3:659998b2bee1 82 }