KRAI 2017 / Mbed 2 deprecated Tes_Encoder_Bawaan

Silahkan dibaca ^_^

Dependencies:   Motor mbed millis PID

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?

UserRevisionLine numberNew 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 }