KRAI 2017 / Mbed 2 deprecated Test_PID

PID Kontrol atas, tanpa library. Baru P dan D, tapi D error

Dependencies:   Motor mbed millis

main.cpp@2:89d69eae8917, 2016-10-18 (annotated)

Committer:
Joshua23
Date:
Tue Oct 18 11:56:30 2016 +0000
Revision:
2:89d69eae8917
Parent:
1:accc8b66773b
Child:
3:e4cbba0706b0
Rev 18 Oct 2016

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joshua23 0:f439078cfdde 1 #include "mbed.h"
Joshua23 0:f439078cfdde 2 #include "millis.h"
Joshua23 0:f439078cfdde 3 #include "Motor.h"
Joshua23 0:f439078cfdde 4
Joshua23 0:f439078cfdde 5
Joshua23 0:f439078cfdde 6 int val;
Joshua23 0:f439078cfdde 7
Joshua23 0:f439078cfdde 8 // Variable Declaration
Joshua23 0:f439078cfdde 9
Joshua23 0:f439078cfdde 10 //Variabel Encoder
Joshua23 0:f439078cfdde 11 int encoder0Pos = 0;
Joshua23 0:f439078cfdde 12 int encoder0PinALast = 0;
Joshua23 0:f439078cfdde 13 int n = 0;
Joshua23 0:f439078cfdde 14 float rpm;
Joshua23 0:f439078cfdde 15 int start = 1;
Joshua23 0:f439078cfdde 16 unsigned long int previousMillis = 0;
Joshua23 0:f439078cfdde 17
Joshua23 0:f439078cfdde 18 //Variabel Motor
Joshua23 0:f439078cfdde 19 Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev
Joshua23 0:f439078cfdde 20
Joshua23 0:f439078cfdde 21 // Variabel PID
Joshua23 0:f439078cfdde 22 int errorP, last_errorD;
Joshua23 0:f439078cfdde 23 float pwm, pwm_last;
Joshua23 0:f439078cfdde 24 float P, Kp;
Joshua23 0:f439078cfdde 25 float D, Kd;
Joshua23 0:f439078cfdde 26
Joshua23 0:f439078cfdde 27 // Variabel Sabeb
Joshua23 2:89d69eae8917 28 int desired_rpm = 125;
Joshua23 0:f439078cfdde 29
Joshua23 0:f439078cfdde 30 // Test Variabel
Joshua23 0:f439078cfdde 31 int selisiherror;
Joshua23 0:f439078cfdde 32
Joshua23 0:f439078cfdde 33 // Main Program
Joshua23 0:f439078cfdde 34 int main(void) {
Joshua23 0:f439078cfdde 35
Joshua23 0:f439078cfdde 36 DigitalIn encoder0PinA(PA_5);
Joshua23 0:f439078cfdde 37 DigitalIn encoder0PinB(PA_6);
Joshua23 0:f439078cfdde 38 Serial pc(USBTX,USBRX);
Joshua23 0:f439078cfdde 39 pc.baud(9600);
Joshua23 0:f439078cfdde 40 startMillis();
Joshua23 0:f439078cfdde 41
Joshua23 0:f439078cfdde 42 // Deklarasi awal PID
Joshua23 0:f439078cfdde 43 last_errorD = 0;
Joshua23 0:f439078cfdde 44 pwm_last = 0;
Joshua23 2:89d69eae8917 45 Kp = 0.00000007;
Joshua23 2:89d69eae8917 46 Kd = 0.000000007;
Joshua23 0:f439078cfdde 47
Joshua23 0:f439078cfdde 48 while(1){
Joshua23 0:f439078cfdde 49
Joshua23 0:f439078cfdde 50 errorP = desired_rpm-rpm;
Joshua23 0:f439078cfdde 51 P = errorP*Kp;
Joshua23 0:f439078cfdde 52 D = Kd*(errorP-last_errorD);
Joshua23 0:f439078cfdde 53 selisiherror = (errorP-last_errorD);
Joshua23 0:f439078cfdde 54
Joshua23 0:f439078cfdde 55 pwm = P + D + pwm_last;
Joshua23 0:f439078cfdde 56
Joshua23 0:f439078cfdde 57 if (pwm >= 1) {
Joshua23 0:f439078cfdde 58 pwm = 1;
Joshua23 0:f439078cfdde 59 }
Joshua23 0:f439078cfdde 60
Joshua23 0:f439078cfdde 61 motor1.speed(pwm);
Joshua23 0:f439078cfdde 62 pwm_last = pwm;
Joshua23 0:f439078cfdde 63
Joshua23 0:f439078cfdde 64 unsigned long int currentMillis = millis();
Joshua23 0:f439078cfdde 65 // int counter;
Joshua23 0:f439078cfdde 66 // counter=currentMillis/100;
Joshua23 0:f439078cfdde 67
Joshua23 2:89d69eae8917 68 if (currentMillis-previousMillis==250)
Joshua23 0:f439078cfdde 69 {
Joshua23 1:accc8b66773b 70 previousMillis = currentMillis;
Joshua23 1:accc8b66773b 71 rpm = encoder0Pos;
Joshua23 1:accc8b66773b 72 pc.printf ("pwm = %.5f | rpm = %.0f | P = %.10f | D = %.10f | errorP = %.d | last_errorD = %.d \n",pwm,rpm,P,D, errorP, last_errorD);
Joshua23 1:accc8b66773b 73 encoder0Pos = 0;
Joshua23 1:accc8b66773b 74
Joshua23 1:accc8b66773b 75 last_errorD = errorP;
Joshua23 0:f439078cfdde 76 }
Joshua23 0:f439078cfdde 77
Joshua23 0:f439078cfdde 78 else
Joshua23 0:f439078cfdde 79 {
Joshua23 0:f439078cfdde 80 n = encoder0PinA;
Joshua23 0:f439078cfdde 81 if ((!encoder0PinALast) && (n))
Joshua23 0:f439078cfdde 82 {
Joshua23 0:f439078cfdde 83 if (!encoder0PinB)
Joshua23 0:f439078cfdde 84 {
Joshua23 0:f439078cfdde 85 encoder0Pos--;
Joshua23 0:f439078cfdde 86 } else {
Joshua23 0:f439078cfdde 87 encoder0Pos++;
Joshua23 0:f439078cfdde 88 }
Joshua23 0:f439078cfdde 89 //pc.printf ("%d \n",encoder0Pos);
Joshua23 0:f439078cfdde 90 }
Joshua23 0:f439078cfdde 91 encoder0PinALast = n;
Joshua23 0:f439078cfdde 92 }
Joshua23 0:f439078cfdde 93 }
Joshua23 0:f439078cfdde 94 }