KRTMI_Titanium2022 / Mbed 2 deprecated Sampling_Kecepatan

Kodingan untuk sampling kecepatan motor

Dependencies:   Motor PID QEI mbed

main.cpp@0:99de7bb1501c, 2022-10-24 (annotated)

Committer:
photonicbabe
Date:
Mon Oct 24 12:59:51 2022 +0000
Revision:
0:99de7bb1501c
Kodingan Sampling Kecepatan

Who changed what in which revision?

UserRevisionLine numberNew contents of line
photonicbabe 0:99de7bb1501c 1 /**
photonicbabe 0:99de7bb1501c 2 * Drive a robot forwards or backwards by using a PID controller to vary
photonicbabe 0:99de7bb1501c 3 * the PWM signal to H-bridges connected to the motors to attempt to maintain
photonicbabe 0:99de7bb1501c 4 * a constant velocity.
photonicbabe 0:99de7bb1501c 5 */
photonicbabe 0:99de7bb1501c 6 #include "mbed.h"
photonicbabe 0:99de7bb1501c 7 #include "Motor.h"
photonicbabe 0:99de7bb1501c 8 #include "QEI.h"
photonicbabe 0:99de7bb1501c 9 #include "PID.h"
photonicbabe 0:99de7bb1501c 10
photonicbabe 0:99de7bb1501c 11 Timer t;
photonicbabe 0:99de7bb1501c 12 Timer t2;
photonicbabe 0:99de7bb1501c 13
photonicbabe 0:99de7bb1501c 14 Serial pc(USBTX, USBRX); // tx, rx
photonicbabe 0:99de7bb1501c 15
photonicbabe 0:99de7bb1501c 16
photonicbabe 0:99de7bb1501c 17 //Motor DepanKanan(PA_0, PB_0, PA_4); //Depan Kiri
photonicbabe 0:99de7bb1501c 18 Motor DepanKanan(PA_9, PB_10 , PA_8); //DepanKanan
photonicbabe 0:99de7bb1501c 19 //Motor DepanKanan(PC_8, PC_5, PA_12); //Belakang Kanan
photonicbabe 0:99de7bb1501c 20 //Motor DepanKanan(PA_1, PC_0, PC_1); //Belakang Kiri
photonicbabe 0:99de7bb1501c 21
photonicbabe 0:99de7bb1501c 22 //QEI DepanKananQEI(PC_10, PC_12, NC, 986); //Depan Kiri
photonicbabe 0:99de7bb1501c 23 QEI DepanKananQEI(PC_11, PD_2, NC, 624); //Depan Kanan
photonicbabe 0:99de7bb1501c 24 //QEI DepanKananQEI(PB_1, PB_15, NC, 986); //Belakang Kanan
photonicbabe 0:99de7bb1501c 25 //QEI DepanKananQEI(PA_15, PB_7, NC, 986);// Belakang Kiri
photonicbabe 0:99de7bb1501c 26
photonicbabe 0:99de7bb1501c 27 //25 Mei 2022
photonicbabe 0:99de7bb1501c 28 PID DepanKananPID(0.96, 0.1, 0.000001, 0.02);
photonicbabe 0:99de7bb1501c 29 //PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02);
photonicbabe 0:99de7bb1501c 30 //PID BelakangKiriPID(0.496, 0.0565, 0.000000000001, 0.02); Belakang Kiri
photonicbabe 0:99de7bb1501c 31 //PID BelakangKananPID(0.5, 0.022, 0.00000000001, 0.02);
photonicbabe 0:99de7bb1501c 32
photonicbabe 0:99de7bb1501c 33 //2022
photonicbabe 0:99de7bb1501c 34 //PID DepanKananPID(0.65, 0.1, 0.0000000001, 0.02); //Kc, Ti, Td belakang kiri
photonicbabe 0:99de7bb1501c 35
photonicbabe 0:99de7bb1501c 36
photonicbabe 0:99de7bb1501c 37
photonicbabe 0:99de7bb1501c 38 //PID DepanKananPID(0.8, 0.1, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 1
photonicbabe 0:99de7bb1501c 39 //PID DepanKananPID(0.7, 0.1, 0.0000000000005, 0.02); //Kc, Ti, Td MOTOR baru nomor 2
photonicbabe 0:99de7bb1501c 40 //PID DepanKananPID(0.8, 0.1, 0.00000002, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
photonicbabe 0:99de7bb1501c 41
photonicbabe 0:99de7bb1501c 42
photonicbabe 0:99de7bb1501c 43
photonicbabe 0:99de7bb1501c 44 //PID DepanKananPID(0.495, 0.1, 0.00000035, 0.09); //Kc, Ti, Td, RATE
photonicbabe 0:99de7bb1501c 45
photonicbabe 0:99de7bb1501c 46
photonicbabe 0:99de7bb1501c 47 int main() {
photonicbabe 0:99de7bb1501c 48 //int DepanKananPulses = 0;
photonicbabe 0:99de7bb1501c 49 pc.baud(9600);
photonicbabe 0:99de7bb1501c 50 DepanKanan.period(0.01f);
photonicbabe 0:99de7bb1501c 51
photonicbabe 0:99de7bb1501c 52 DepanKananPID.setInputLimits(0, 3000);//Input units: counts per second.
photonicbabe 0:99de7bb1501c 53 DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
photonicbabe 0:99de7bb1501c 54 DepanKananPID.setMode(AUTO_MODE);
photonicbabe 0:99de7bb1501c 55
photonicbabe 0:99de7bb1501c 56 int DepanKananPulses = 0; //How far the left wheel has travelled.
photonicbabe 0:99de7bb1501c 57 int DepanKananPrevPulses = 0; //The previous reading of how far the left wheel
photonicbabe 0:99de7bb1501c 58 //has travelled.
photonicbabe 0:99de7bb1501c 59 float DepanKananVelocity = 0.0; //The velocity of the left wheel in pulses per
photonicbabe 0:99de7bb1501c 60 //second.
photonicbabe 0:99de7bb1501c 61
photonicbabe 0:99de7bb1501c 62
photonicbabe 0:99de7bb1501c 63 wait(3); //Wait a few seconds before we start moving.
photonicbabe 0:99de7bb1501c 64
photonicbabe 0:99de7bb1501c 65
photonicbabe 0:99de7bb1501c 66 DepanKananPID.setSetPoint(1000);
photonicbabe 0:99de7bb1501c 67
photonicbabe 0:99de7bb1501c 68 t.start();
photonicbabe 0:99de7bb1501c 69 while (t.read_ms() <= 3000) {
photonicbabe 0:99de7bb1501c 70
photonicbabe 0:99de7bb1501c 71 /*DepanKanan.speed(0.5f);
photonicbabe 0:99de7bb1501c 72
photonicbabe 0:99de7bb1501c 73 DepanKananPulses = DepanKananQEI.getPulses();
photonicbabe 0:99de7bb1501c 74
photonicbabe 0:99de7bb1501c 75 pc.printf("\n\r %d " , DepanKananPulses);*/
photonicbabe 0:99de7bb1501c 76 t2.start();
photonicbabe 0:99de7bb1501c 77
photonicbabe 0:99de7bb1501c 78
photonicbabe 0:99de7bb1501c 79 DepanKananPulses = DepanKananQEI.getPulses();
photonicbabe 0:99de7bb1501c 80 DepanKananVelocity = ((double)DepanKananPulses - (double)DepanKananPrevPulses) / 0.02;
photonicbabe 0:99de7bb1501c 81 DepanKananPrevPulses = DepanKananPulses;
photonicbabe 0:99de7bb1501c 82 //DepanKananVelocity = (DepanKananVelocity/210.0f)*60.0f;
photonicbabe 0:99de7bb1501c 83
photonicbabe 0:99de7bb1501c 84 DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
photonicbabe 0:99de7bb1501c 85
photonicbabe 0:99de7bb1501c 86 //----------//
photonicbabe 0:99de7bb1501c 87 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:99de7bb1501c 88
photonicbabe 0:99de7bb1501c 89
photonicbabe 0:99de7bb1501c 90 pc.printf("\n %f " ,DepanKananVelocity );
photonicbabe 0:99de7bb1501c 91
photonicbabe 0:99de7bb1501c 92
photonicbabe 0:99de7bb1501c 93 while (t2.read_ms() <= 20) {
photonicbabe 0:99de7bb1501c 94
photonicbabe 0:99de7bb1501c 95 }
photonicbabe 0:99de7bb1501c 96
photonicbabe 0:99de7bb1501c 97 t2.reset();
photonicbabe 0:99de7bb1501c 98
photonicbabe 0:99de7bb1501c 99 }
photonicbabe 0:99de7bb1501c 100 t.reset();
photonicbabe 0:99de7bb1501c 101 DepanKanan.brake();
photonicbabe 0:99de7bb1501c 102
photonicbabe 0:99de7bb1501c 103 }