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}
main.cpp@3:545fb5899097, 2016-10-07 (annotated)
- Committer:
- rayandrew
- Date:
- Fri Oct 07 13:39:44 2016 +0000
- Revision:
- 3:545fb5899097
- Parent:
- 2:7565fb31357d
first version of pid with encoder as its feedback
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gustavaditya | 0:24cb158c4064 | 1 | #include "mbed.h" |
gustavaditya | 0:24cb158c4064 | 2 | #include "millis.h" |
Fathoni17 | 1:8d4fab4e069c | 3 | #include "Motor.h" |
rayandrew | 3:545fb5899097 | 4 | #include "PID.h" |
gustavaditya | 0:24cb158c4064 | 5 | |
rayandrew | 3:545fb5899097 | 6 | int val; |
gustavaditya | 0:24cb158c4064 | 7 | |
gustavaditya | 0:24cb158c4064 | 8 | int encoder0Pos = 0; |
gustavaditya | 0:24cb158c4064 | 9 | int encoder0PinALast = 0; |
gustavaditya | 0:24cb158c4064 | 10 | int n = 0; |
rayandrew | 3:545fb5899097 | 11 | float rpm, rpms=0; |
rayandrew | 3:545fb5899097 | 12 | float speed; |
rayandrew | 3:545fb5899097 | 13 | float speeds; |
rayandrew | 3:545fb5899097 | 14 | float maxSpeed = 0.6; |
rayandrew | 3:545fb5899097 | 15 | float minSpeed = 0.1; |
rayandrew | 3:545fb5899097 | 16 | float speedR; |
rayandrew | 3:545fb5899097 | 17 | float pid; |
rayandrew | 3:545fb5899097 | 18 | |
gustavaditya | 0:24cb158c4064 | 19 | int start = 1; |
rayandrew | 3:545fb5899097 | 20 | |
rayandrew | 3:545fb5899097 | 21 | float rpmStandard = 40; // tentuin rpm yang diinginkan |
gustavaditya | 0:24cb158c4064 | 22 | unsigned long int previousMillis = 0; |
rayandrew | 3:545fb5899097 | 23 | |
rayandrew | 3:545fb5899097 | 24 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 25 | // inisialisasi encoder |
rayandrew | 3:545fb5899097 | 26 | DigitalIn encoder0PinA(D7); |
rayandrew | 3:545fb5899097 | 27 | DigitalIn encoder0PinB(D8); |
rayandrew | 3:545fb5899097 | 28 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 29 | |
rayandrew | 3:545fb5899097 | 30 | // ----------------------------- |
rayandrew | 3:545fb5899097 | 31 | // Inisialisasi PID |
rayandrew | 3:545fb5899097 | 32 | // PID(KP,TD,TI,INTERVAL PID) |
rayandrew | 3:545fb5899097 | 33 | PID PID(0.985,0.0,0.0,0.001); |
rayandrew | 3:545fb5899097 | 34 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 35 | |
rayandrew | 3:545fb5899097 | 36 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 37 | // INISIALISASI SERIAL |
rayandrew | 3:545fb5899097 | 38 | Serial pc(USBTX,USBRX); |
rayandrew | 3:545fb5899097 | 39 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 40 | |
rayandrew | 3:545fb5899097 | 41 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 42 | // INISIALISASI MORTOR |
rayandrew | 3:545fb5899097 | 43 | Motor motor1(D3,D2,D4); |
rayandrew | 3:545fb5899097 | 44 | // ---------------------------- |
rayandrew | 3:545fb5899097 | 45 | |
rayandrew | 3:545fb5899097 | 46 | |
rayandrew | 3:545fb5899097 | 47 | /* |
rayandrew | 3:545fb5899097 | 48 | init_speed() untuk mencegah pwm kelebihan batas |
rayandrew | 3:545fb5899097 | 49 | batas ditentukan oleh variabel global maxSpeed |
rayandrew | 3:545fb5899097 | 50 | ubah sesuai keinginan |
rayandrew | 3:545fb5899097 | 51 | */ |
rayandrew | 3:545fb5899097 | 52 | void init_speed (){ |
rayandrew | 3:545fb5899097 | 53 | if (speed>maxSpeed){ |
rayandrew | 3:545fb5899097 | 54 | speed = maxSpeed; |
rayandrew | 3:545fb5899097 | 55 | } |
rayandrew | 3:545fb5899097 | 56 | |
rayandrew | 3:545fb5899097 | 57 | if (speed<minSpeed){ |
rayandrew | 3:545fb5899097 | 58 | speed = minSpeed; |
rayandrew | 3:545fb5899097 | 59 | } |
rayandrew | 3:545fb5899097 | 60 | } |
rayandrew | 3:545fb5899097 | 61 | |
rayandrew | 3:545fb5899097 | 62 | /* |
rayandrew | 3:545fb5899097 | 63 | PID_ENCODER(float speed) |
rayandrew | 3:545fb5899097 | 64 | speed == pwm |
rayandrew | 3:545fb5899097 | 65 | pv merupakan process value dari keadaan nyata |
rayandrew | 3:545fb5899097 | 66 | --- 0 untuk keadaan rpm == rpmStandard |
rayandrew | 3:545fb5899097 | 67 | --- -1 untuk keadaan rpm < rpmStandard |
rayandrew | 3:545fb5899097 | 68 | --- 1 untuk keadaan rpm > rpmStandard |
rayandrew | 3:545fb5899097 | 69 | */ |
rayandrew | 3:545fb5899097 | 70 | void PID_ENCODER(float speed) { |
rayandrew | 3:545fb5899097 | 71 | int pv; |
rayandrew | 3:545fb5899097 | 72 | if(rpm == rpmStandard) { |
rayandrew | 3:545fb5899097 | 73 | pv = 0; |
rayandrew | 3:545fb5899097 | 74 | } |
rayandrew | 3:545fb5899097 | 75 | else if(rpm < rpmStandard) { |
rayandrew | 3:545fb5899097 | 76 | pv = -1; |
rayandrew | 3:545fb5899097 | 77 | } else { |
rayandrew | 3:545fb5899097 | 78 | pv = 1; |
rayandrew | 3:545fb5899097 | 79 | } |
rayandrew | 3:545fb5899097 | 80 | |
rayandrew | 3:545fb5899097 | 81 | PID.setProcessValue(pv); |
rayandrew | 3:545fb5899097 | 82 | PID.setSetPoint(0); |
rayandrew | 3:545fb5899097 | 83 | pid = PID.compute(); |
rayandrew | 3:545fb5899097 | 84 | speedR = speed + PID.compute(); // compute |
rayandrew | 3:545fb5899097 | 85 | if(speedR < minSpeed) { |
rayandrew | 3:545fb5899097 | 86 | speedR = minSpeed; |
rayandrew | 3:545fb5899097 | 87 | } |
rayandrew | 3:545fb5899097 | 88 | motor1.speed(speedR); // change speed of motor 1 |
rayandrew | 3:545fb5899097 | 89 | } |
Fathoni17 | 1:8d4fab4e069c | 90 | |
gustavaditya | 0:24cb158c4064 | 91 | int main(void) { |
rayandrew | 3:545fb5899097 | 92 | pc.baud(9600); // set pc_baud |
rayandrew | 3:545fb5899097 | 93 | PID.setInputLimits(-1,1); // input limits pv dari -1 sampai 1 yaitu -1,0,1 |
rayandrew | 3:545fb5899097 | 94 | PID.setOutputLimits(0,0.2); |
rayandrew | 3:545fb5899097 | 95 | // output limits adalah batasan pwm max. karna pwm max diset oleh variabel maxSpeed, jadi ubah2 output limits sesuai batasan |
rayandrew | 3:545fb5899097 | 96 | // 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 |
rayandrew | 3:545fb5899097 | 97 | PID.setMode(1.0); // set pid jadi full auto |
rayandrew | 3:545fb5899097 | 98 | PID.setBias(0.0); // default |
rayandrew | 3:545fb5899097 | 99 | PID.reset(); // default |
Fathoni17 | 1:8d4fab4e069c | 100 | |
Fathoni17 | 1:8d4fab4e069c | 101 | |
rayandrew | 3:545fb5899097 | 102 | |
gustavaditya | 0:24cb158c4064 | 103 | startMillis(); |
rayandrew | 3:545fb5899097 | 104 | speed = 0.3; // tentuin speed sesuai keinginan |
Fathoni17 | 1:8d4fab4e069c | 105 | |
gustavaditya | 0:24cb158c4064 | 106 | while(1){ |
rayandrew | 3:545fb5899097 | 107 | speeds = speed; |
rayandrew | 3:545fb5899097 | 108 | init_speed(); |
rayandrew | 3:545fb5899097 | 109 | PID_ENCODER(speeds); |
rayandrew | 3:545fb5899097 | 110 | |
rayandrew | 3:545fb5899097 | 111 | pc.printf("pwm = %.02f\t pid = %.02f \t rp_loop = %.0f\n",speedR, pid, rpm); // output ke serial komputer |
gustavaditya | 0:24cb158c4064 | 112 | |
rayandrew | 3:545fb5899097 | 113 | unsigned long int currentMillis = millis(); |
gustavaditya | 0:24cb158c4064 | 114 | |
rayandrew | 3:545fb5899097 | 115 | n = encoder0PinA; |
rayandrew | 3:545fb5899097 | 116 | if ((!encoder0PinALast) && (n)) |
rayandrew | 3:545fb5899097 | 117 | { |
rayandrew | 3:545fb5899097 | 118 | if (!encoder0PinB) |
rayandrew | 3:545fb5899097 | 119 | { |
rayandrew | 3:545fb5899097 | 120 | encoder0Pos--; |
rayandrew | 3:545fb5899097 | 121 | } else { |
rayandrew | 3:545fb5899097 | 122 | encoder0Pos++; |
rayandrew | 3:545fb5899097 | 123 | } |
rayandrew | 3:545fb5899097 | 124 | } |
rayandrew | 3:545fb5899097 | 125 | encoder0PinALast = n; |
rayandrew | 3:545fb5899097 | 126 | |
rayandrew | 3:545fb5899097 | 127 | rpm = 120.0/7.0*encoder0Pos; // hitung rpm |
rayandrew | 3:545fb5899097 | 128 | //rpms = rpms + rpm; |
rayandrew | 3:545fb5899097 | 129 | |
Fathoni17 | 1:8d4fab4e069c | 130 | if (currentMillis-previousMillis>=500) |
gustavaditya | 0:24cb158c4064 | 131 | { |
gustavaditya | 0:24cb158c4064 | 132 | previousMillis = currentMillis; |
rayandrew | 3:545fb5899097 | 133 | |
rayandrew | 3:545fb5899097 | 134 | //pc.printf ("rpms = %.0f \n",rpms); |
rayandrew | 3:545fb5899097 | 135 | rpms = 0; |
gustavaditya | 0:24cb158c4064 | 136 | encoder0Pos = 0; |
gustavaditya | 0:24cb158c4064 | 137 | } |
gustavaditya | 0:24cb158c4064 | 138 | } |
rayandrew | 3:545fb5899097 | 139 | |
gustavaditya | 0:24cb158c4064 | 140 | } |