KRAI 2017 / Mbed 2 deprecated Test_PID2

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

Dependencies:   Motor mbed millis

Fork of Test_PID by KRAI 2017

main.cpp@4:8e6dfc416ecf, 2017-02-09 (annotated)

Committer:
Joshua23
Date:
Thu Feb 09 14:34:10 2017 +0000
Revision:
4:8e6dfc416ecf
Parent:
3:e4cbba0706b0 
coba

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 4:8e6dfc416ecf 19 Motor motor1(PA_8, PC_2, PC_1); // pwm, fwd, rev
Joshua23 0:f439078cfdde 20
Joshua23 0:f439078cfdde 21 // Variabel PID
Joshua23 4:8e6dfc416ecf 22 int errorP = 0, last_errorD = 0;
Joshua23 4:8e6dfc416ecf 23 float pwm = 0, 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 4:8e6dfc416ecf 28 int desired_rpm = 50;
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 4:8e6dfc416ecf 36 DigitalIn encoder0PinA(PB_14);
Joshua23 4:8e6dfc416ecf 37 DigitalIn encoder0PinB(PB_13);
Joshua23 4:8e6dfc416ecf 38 Serial pc(USBTX,USBRX);
Joshua23 4:8e6dfc416ecf 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 4:8e6dfc416ecf 45 Kp = 0.000000033; //0.000000037
Joshua23 4:8e6dfc416ecf 46 Kd = 0.000000297; //0.000000270
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 4:8e6dfc416ecf 61
Joshua23 0:f439078cfdde 62 motor1.speed(pwm);
Joshua23 0:f439078cfdde 63 pwm_last = pwm;
Joshua23 0:f439078cfdde 64
Joshua23 4:8e6dfc416ecf 65 //motor1.speed(0.8);
Joshua23 0:f439078cfdde 66 unsigned long int currentMillis = millis();
Joshua23 0:f439078cfdde 67 // int counter;
Joshua23 0:f439078cfdde 68 // counter=currentMillis/100;
Joshua23 0:f439078cfdde 69
Joshua23 2:89d69eae8917 70 if (currentMillis-previousMillis==250)
Joshua23 0:f439078cfdde 71 {
Joshua23 1:accc8b66773b 72 previousMillis = currentMillis;
Joshua23 1:accc8b66773b 73 rpm = encoder0Pos;
Joshua23 4:8e6dfc416ecf 74 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 75 encoder0Pos = 0;
Joshua23 1:accc8b66773b 76
Joshua23 1:accc8b66773b 77 last_errorD = errorP;
Joshua23 0:f439078cfdde 78 }
Joshua23 0:f439078cfdde 79
Joshua23 0:f439078cfdde 80 else
Joshua23 0:f439078cfdde 81 {
Joshua23 0:f439078cfdde 82 n = encoder0PinA;
Joshua23 0:f439078cfdde 83 if ((!encoder0PinALast) && (n))
Joshua23 0:f439078cfdde 84 {
Joshua23 0:f439078cfdde 85 if (!encoder0PinB)
Joshua23 0:f439078cfdde 86 {
Joshua23 0:f439078cfdde 87 encoder0Pos--;
Joshua23 0:f439078cfdde 88 } else {
Joshua23 0:f439078cfdde 89 encoder0Pos++;
Joshua23 0:f439078cfdde 90 }
Joshua23 4:8e6dfc416ecf 91 //pc.printf ("%d \n",encoder0Pos);
Joshua23 0:f439078cfdde 92 }
Joshua23 0:f439078cfdde 93 encoder0PinALast = n;
Joshua23 0:f439078cfdde 94 }
Joshua23 0:f439078cfdde 95 }
Joshua23 0:f439078cfdde 96 }