pid with encoder as its feedback
Dependencies: Motor PID mbed millis
Fork of Tes_Encoder_Bawaan by
PID using PID library of mbed
depedencies = Library : {PID, mbed, motor, milis}
Diff: main.cpp
- Revision:
- 3:545fb5899097
- Parent:
- 2:7565fb31357d
diff -r 7565fb31357d -r 545fb5899097 main.cpp --- a/main.cpp Fri Sep 23 12:56:22 2016 +0000 +++ b/main.cpp Fri Oct 07 13:39:44 2016 +0000 @@ -1,61 +1,140 @@ #include "mbed.h" #include "millis.h" #include "Motor.h" +#include "PID.h" -int val; + int val; int encoder0Pos = 0; int encoder0PinALast = 0; int n = 0; - float rpm; + float rpm, rpms=0; + float speed; + float speeds; + float maxSpeed = 0.6; + float minSpeed = 0.1; + float speedR; + float pid; + int start = 1; + + float rpmStandard = 40; // tentuin rpm yang diinginkan unsigned long int previousMillis = 0; - //float pwmRead=0.00; - - Motor motor1(D11,D13,D12); - AnalogIn pwmIn(A0); + +// ---------------------------- +// inisialisasi encoder +DigitalIn encoder0PinA(D7); +DigitalIn encoder0PinB(D8); +// ---------------------------- + +// ----------------------------- +// Inisialisasi PID +// PID(KP,TD,TI,INTERVAL PID) +PID PID(0.985,0.0,0.0,0.001); +// ---------------------------- + +// ---------------------------- +// INISIALISASI SERIAL +Serial pc(USBTX,USBRX); +// ---------------------------- + +// ---------------------------- +// INISIALISASI MORTOR +Motor motor1(D3,D2,D4); +// ---------------------------- + + +/* + init_speed() untuk mencegah pwm kelebihan batas + batas ditentukan oleh variabel global maxSpeed + ubah sesuai keinginan +*/ +void init_speed (){ + if (speed>maxSpeed){ + speed = maxSpeed; + } + + if (speed<minSpeed){ + speed = minSpeed; + } +} + +/* + PID_ENCODER(float speed) + speed == pwm + pv merupakan process value dari keadaan nyata + --- 0 untuk keadaan rpm == rpmStandard + --- -1 untuk keadaan rpm < rpmStandard + --- 1 untuk keadaan rpm > rpmStandard +*/ +void PID_ENCODER(float speed) { + int pv; + if(rpm == rpmStandard) { + pv = 0; + } + else if(rpm < rpmStandard) { + pv = -1; + } else { + pv = 1; + } + + PID.setProcessValue(pv); + PID.setSetPoint(0); + pid = PID.compute(); + speedR = speed + PID.compute(); // compute + if(speedR < minSpeed) { + speedR = minSpeed; + } + motor1.speed(speedR); // change speed of motor 1 +} int main(void) { + pc.baud(9600); // set pc_baud + PID.setInputLimits(-1,1); // input limits pv dari -1 sampai 1 yaitu -1,0,1 + PID.setOutputLimits(0,0.2); + // output limits adalah batasan pwm max. karna pwm max diset oleh variabel maxSpeed, jadi ubah2 output limits sesuai batasan + // contoh jika input speed 0.2 dan batasan speed maksimum adalah 0.5, maka setoutputlimits menjadi (0,0.3); karna 0.2 + 0.3 = 0.5 + PID.setMode(1.0); // set pid jadi full auto + PID.setBias(0.0); // default + PID.reset(); // default - DigitalIn encoder0PinA(D3); - DigitalIn encoder0PinB(D4); - Serial pc(USBTX,USBRX); + startMillis(); + speed = 0.3; // tentuin speed sesuai keinginan while(1){ + speeds = speed; + init_speed(); + PID_ENCODER(speeds); + + pc.printf("pwm = %.02f\t pid = %.02f \t rp_loop = %.0f\n",speedR, pid, rpm); // output ke serial komputer -// pwmRead = pwmIn.read(); - motor1.speed(pwmIn.read()); - pc.printf("pwm = %.02f",pwmIn.read()); + unsigned long int currentMillis = millis(); - unsigned long int currentMillis = millis(); -// int counter; -// counter=currentMillis/100; - + n = encoder0PinA; + if ((!encoder0PinALast) && (n)) + { + if (!encoder0PinB) + { + encoder0Pos--; + } else { + encoder0Pos++; + } + } + encoder0PinALast = n; + + rpm = 120.0/7.0*encoder0Pos; // hitung rpm + //rpms = rpms + rpm; + if (currentMillis-previousMillis>=500) { previousMillis = currentMillis; - rpm = 120.0/7.0*encoder0Pos; - pc.printf ("Dalam 500 ms = %d ",encoder0Pos); - pc.printf ("rpm = %.0f \n",rpm); + + //pc.printf ("rpms = %.0f \n",rpms); + rpms = 0; encoder0Pos = 0; } - - else - { - n = encoder0PinA; - if ((!encoder0PinALast) && (n)) - { - if (!encoder0PinB) - { - encoder0Pos--; - } else { - encoder0Pos++; - } - //pc.printf ("%d \n",encoder0Pos); - } - encoder0PinALast = n; - } } + } \ No newline at end of file