Eki Liptiay / Mbed 2 deprecated PID_Controller

hode pid broertje

Dependencies:   HIDScope QEI biquadFilter mbed

main.cpp@2:93918cad51dd, 2018-10-17 (annotated)

Committer:
ekiliptiay
Date:
Wed Oct 17 12:23:50 2018 +0000
Revision:
2:93918cad51dd
Parent:
1:3d92e7ff3dda
Child:
3:659998b2bee1
without 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
ekiliptiay 1:3d92e7ff3dda 29 scope.set(0,counts1);
ekiliptiay 1:3d92e7ff3dda 30 scope.set(1,DesiredCounts);
ekiliptiay 1:3d92e7ff3dda 31 scope.send();
ekiliptiay 1:3d92e7ff3dda 32
ekiliptiay 1:3d92e7ff3dda 33 // 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 34 if(counts1 != DesiredCounts){
ekiliptiay 1:3d92e7ff3dda 35 // Check which direction we have to move; when DC > AOC, we have to move CW and vice versa.
ekiliptiay 1:3d92e7ff3dda 36 if(DesiredCounts>counts1){
ekiliptiay 1:3d92e7ff3dda 37 directionpin=1; // still need to check whether directionpin = 1 makes the motor rotate CW.
ekiliptiay 1:3d92e7ff3dda 38 }
ekiliptiay 1:3d92e7ff3dda 39 else{
ekiliptiay 1:3d92e7ff3dda 40 directionpin=0;
ekiliptiay 1:3d92e7ff3dda 41 }
ekiliptiay 1:3d92e7ff3dda 42 pwmpin = 1;
ekiliptiay 1:3d92e7ff3dda 43 pc.printf("Counts from potentiometer: %d. Average of 100 (counts): %d \n", count, AvgCounts);
ekiliptiay 1:3d92e7ff3dda 44 }
ekiliptiay 1:3d92e7ff3dda 45 else{
ekiliptiay 1:3d92e7ff3dda 46 pwmpin = 0; // when aoc = dc, the motor does not rotate.
ekiliptiay 1:3d92e7ff3dda 47 }
ekiliptiay 0:edcdd09960f7 48 }
ekiliptiay 0:edcdd09960f7 49
ekiliptiay 0:edcdd09960f7 50 int main() {
ekiliptiay 0:edcdd09960f7 51 pwmpin.period_us(60); // Set pwm period
ekiliptiay 0:edcdd09960f7 52 while (true) {
ekiliptiay 1:3d92e7ff3dda 53 SetMotor();
ekiliptiay 1:3d92e7ff3dda 54 wait(0.001f);
ekiliptiay 0:edcdd09960f7 55 }
ekiliptiay 0:edcdd09960f7 56 }