Eki Liptiay / Mbed 2 deprecated PID_Controller

hode pid broertje

Dependencies:   HIDScope QEI biquadFilter mbed

main.cpp@1:3d92e7ff3dda, 2018-10-17 (annotated)

Committer:
ekiliptiay
Date:
Wed Oct 17 12:23:15 2018 +0000
Revision:
1:3d92e7ff3dda
Parent:
0:edcdd09960f7
Child:
2:93918cad51dd
With moving average;

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 0:edcdd09960f7 16 DigitalOut directionpin(D7);
ekiliptiay 0:edcdd09960f7 17 PwmOut pwmpin(D6);
ekiliptiay 0:edcdd09960f7 18 Serial pc(USBTX, USBRX); // tx, rx
ekiliptiay 0:edcdd09960f7 19
ekiliptiay 1:3d92e7ff3dda 20 const int CpR = 4200; //When the count is 4200, 1 revolution is made. (Using X2 encoding)
ekiliptiay 1:3d92e7ff3dda 21 const int MAlength = 100; // Set the length of the MA
ekiliptiay 1:3d92e7ff3dda 22 int MovingAverageArray[MAlength] = {pot1*CpR}; // Make an array where we can save 100 values to get an average value from our potentiometer.
ekiliptiay 1:3d92e7ff3dda 23 int count = 0; // Counter for the moving average array
ekiliptiay 1:3d92e7ff3dda 24
ekiliptiay 1:3d92e7ff3dda 25 void SetMotor(){
ekiliptiay 1:3d92e7ff3dda 26 int counts1 = -1*Encoder1.getPulses(); // Get counts from encoder 1 (motor 1). Notice the minus sign; CCW movement = positive counts.
ekiliptiay 1:3d92e7ff3dda 27 int DesiredCounts = pot1*CpR; // The potmeter sets the desired amount of counts.
ekiliptiay 1:3d92e7ff3dda 28 MovingAverageArray[count] = DesiredCounts;
ekiliptiay 1:3d92e7ff3dda 29 count++;
ekiliptiay 1:3d92e7ff3dda 30
ekiliptiay 1:3d92e7ff3dda 31 // define
ekiliptiay 1:3d92e7ff3dda 32 int AvgCounts = 0;
ekiliptiay 1:3d92e7ff3dda 33 int sum = 0;
ekiliptiay 1:3d92e7ff3dda 34 for (int i = 0; i < MAlength; ++i)
ekiliptiay 1:3d92e7ff3dda 35 {
ekiliptiay 1:3d92e7ff3dda 36 sum += MovingAverageArray[i];
ekiliptiay 1:3d92e7ff3dda 37 }
ekiliptiay 1:3d92e7ff3dda 38 AvgCounts = (sum)/MAlength; //or cast sum to double before division
ekiliptiay 1:3d92e7ff3dda 39
ekiliptiay 1:3d92e7ff3dda 40 if(count==MAlength){
ekiliptiay 1:3d92e7ff3dda 41 count = 0; //reset counts
ekiliptiay 0:edcdd09960f7 42 }
ekiliptiay 1:3d92e7ff3dda 43
ekiliptiay 1:3d92e7ff3dda 44 scope.set(0,counts1);
ekiliptiay 1:3d92e7ff3dda 45 scope.set(1,DesiredCounts);
ekiliptiay 1:3d92e7ff3dda 46 scope.set(2,AvgCounts);
ekiliptiay 1:3d92e7ff3dda 47 scope.send();
ekiliptiay 1:3d92e7ff3dda 48
ekiliptiay 1:3d92e7ff3dda 49 // we need the following: if amount of counts by the encoder =/= desiredcounts, turn on the motor until it is equal. if aoc = dc, do nothing.
ekiliptiay 1:3d92e7ff3dda 50 if(counts1 != DesiredCounts){
ekiliptiay 1:3d92e7ff3dda 51 // Check which direction we have to move; when DC > AOC, we have to move CW and vice versa.
ekiliptiay 1:3d92e7ff3dda 52 if(DesiredCounts>counts1){
ekiliptiay 1:3d92e7ff3dda 53 directionpin=1; // still need to check whether directionpin = 1 makes the motor rotate CW.
ekiliptiay 1:3d92e7ff3dda 54 }
ekiliptiay 1:3d92e7ff3dda 55 else{
ekiliptiay 1:3d92e7ff3dda 56 directionpin=0;
ekiliptiay 1:3d92e7ff3dda 57 }
ekiliptiay 1:3d92e7ff3dda 58 pwmpin = 1;
ekiliptiay 1:3d92e7ff3dda 59 pc.printf("Counts from potentiometer: %d. Average of 100 (counts): %d \n", count, AvgCounts);
ekiliptiay 1:3d92e7ff3dda 60 }
ekiliptiay 1:3d92e7ff3dda 61 else{
ekiliptiay 1:3d92e7ff3dda 62 pwmpin = 0; // when aoc = dc, the motor does not rotate.
ekiliptiay 1:3d92e7ff3dda 63 }
ekiliptiay 0:edcdd09960f7 64 }
ekiliptiay 0:edcdd09960f7 65
ekiliptiay 0:edcdd09960f7 66 int main() {
ekiliptiay 0:edcdd09960f7 67 pwmpin.period_us(60); // Set pwm period
ekiliptiay 0:edcdd09960f7 68 while (true) {
ekiliptiay 1:3d92e7ff3dda 69 SetMotor();
ekiliptiay 1:3d92e7ff3dda 70 wait(0.001f);
ekiliptiay 0:edcdd09960f7 71 }
ekiliptiay 0:edcdd09960f7 72 }