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@0:f439078cfdde, 2016-10-12 (annotated)

Committer:
Joshua23
Date:
Wed Oct 12 10:56:42 2016 +0000
Revision:
0:f439078cfdde
Child:
1:accc8b66773b
PID Kontrol Atas, Tanpa Library, Baru P dan D, Tapi D error

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 0:f439078cfdde 28 int desired_rpm = 450;
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 0:f439078cfdde 45 Kp = 0.000000007;
Joshua23 0:f439078cfdde 46 Kd = 0.00007;
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 0:f439078cfdde 68 if (currentMillis-previousMillis==500)
Joshua23 0:f439078cfdde 69 {
Joshua23 0:f439078cfdde 70 previousMillis = currentMillis;
Joshua23 0:f439078cfdde 71 rpm = encoder0Pos;
Joshua23 0:f439078cfdde 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 0:f439078cfdde 73 encoder0Pos = 0;
Joshua23 0:f439078cfdde 74 }
Joshua23 0:f439078cfdde 75
Joshua23 0:f439078cfdde 76 else
Joshua23 0:f439078cfdde 77 {
Joshua23 0:f439078cfdde 78 n = encoder0PinA;
Joshua23 0:f439078cfdde 79 if ((!encoder0PinALast) && (n))
Joshua23 0:f439078cfdde 80 {
Joshua23 0:f439078cfdde 81 if (!encoder0PinB)
Joshua23 0:f439078cfdde 82 {
Joshua23 0:f439078cfdde 83 encoder0Pos--;
Joshua23 0:f439078cfdde 84 } else {
Joshua23 0:f439078cfdde 85 encoder0Pos++;
Joshua23 0:f439078cfdde 86 }
Joshua23 0:f439078cfdde 87 //pc.printf ("%d \n",encoder0Pos);
Joshua23 0:f439078cfdde 88 }
Joshua23 0:f439078cfdde 89 encoder0PinALast = n;
Joshua23 0:f439078cfdde 90 }
Joshua23 0:f439078cfdde 91
Joshua23 0:f439078cfdde 92 last_errorD = errorP;
Joshua23 0:f439078cfdde 93 }
Joshua23 0:f439078cfdde 94 }