KRTMI_Titanium2022
Pid remote satu motor dan koordinat
Dependencies: Motor PID QEI mbed
Revision 0:20c5e7e5c862, committed 2022-10-04
- Comitter:
- photonicbabe
- Date:
- Tue Oct 04 10:07:50 2022 +0000
- Commit message:
- PID remote 1 motor + koordinat
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Tue Oct 04 10:07:50 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Motor/#f265e441bcd9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Oct 04 10:07:50 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Tue Oct 04 10:07:50 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 04 10:07:50 2022 +0000
@@ -0,0 +1,146 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "QEI.h"
+#include "PID.h"
+
+Serial pc(USBTX, USBRX);
+Serial device(PB_6, PA_10);
+
+Motor DepanKanan(PB_9, PC_5, PA_12);
+QEI DepanKananQEI(PB_8, PC_9, NC, 624);
+PID DepanKananPID(1.2, 0.1, 0.00000001, 0.02);
+
+char nilai, huruf;
+int DepanKananPulses = 0; //How far the left wheel has travelled.
+int DepanKananPrevPulses = 0; //The previous reading of how far the left wheel
+ //has travelled.
+int DepanKananCount, d_pulse_dk;
+float DepanKananVelocity = 0.0; //The velocity of the left wheel in pulses per
+float Koordinat;
+int Resolusi = 624;
+float pi = 3.14;
+float diameter = 5.8;
+Timer t_jalan;
+
+void kosongkan()
+{
+ DepanKananQEI.reset();
+ DepanKananPulses = 0;
+ DepanKananPrevPulses = 0;
+ DepanKananVelocity = 0.0;
+}
+
+void proses_kecepatan()
+{
+ DepanKananPulses = DepanKananQEI.getPulses();
+ d_pulse_dk = DepanKananPulses - DepanKananPrevPulses;
+ DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02;
+ DepanKananPrevPulses = DepanKananPulses;
+ DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
+}
+
+void cari_koordinat()
+{
+ Koordinat = (DepanKananCount/Resolusi)*pi*diameter;
+}
+
+void fungsiBluetooth(void)
+{
+ if(device.readable())
+ {
+ nilai = device.getc();
+ if (nilai == 'S')
+ {
+ huruf = 'S';
+ }
+ else if (nilai == 'F')
+ {
+ huruf = 'F';
+ }
+ else if(nilai =='B')
+ {
+ huruf = 'B';
+ }
+ else if(nilai =='L')
+ {
+ huruf = 'L';
+ }
+ else if(nilai =='R')
+ {
+ huruf = 'R';
+ }
+ else
+ {
+ huruf = 'M';
+ }
+ }
+
+}
+
+void Get_Count()
+{
+ DepanKananCount = DepanKananCount + DepanKananPulses;
+}
+
+int main()
+{
+ pc.baud(38400);
+ device.baud(9600);
+ device.attach(&fungsiBluetooth, Serial::RxIrq);
+ DepanKanan.period(0.01f);
+
+ DepanKananPID.setInputLimits(0, 2220);//Input units: counts per second.
+ DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
+ DepanKananPID.setMode(AUTO_MODE);
+
+ DepanKananPID.setSetPoint(1000);
+ wait(3);
+
+ while(1)
+ {
+ if(huruf == 'F')
+ {
+ kosongkan();
+ while(huruf == 'F')
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ DepanKanan.speed(DepanKananPID.compute());
+ while(t_jalan.read_ms()<=20)
+ {
+ }
+ t_jalan.reset();
+ DepanKananCount = DepanKananCount + d_pulse_dk;
+ cari_koordinat();
+ pc.printf("Koordinat = %f\n", Koordinat);
+ }
+ DepanKanan.brake();
+ }
+
+ else if(huruf == 'B')
+ {
+ kosongkan();
+ while(huruf == 'B')
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ DepanKanan.speed(DepanKananPID.compute()*-1);
+ while(t_jalan.read_ms()<=20)
+ {
+ }
+ t_jalan.reset();
+ DepanKananCount = DepanKananCount + d_pulse_dk;
+ cari_koordinat();
+ pc.printf("\n\r Koordinat: %f", Koordinat);
+ }
+ DepanKanan.brake();
+ }
+
+ else
+ {
+ pc.printf("%c\n", huruf);
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 04 10:07:50 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file