main.cpp

00001 /**
00002  * Drive a robot forwards or backwards by using a PID controller to vary
00003  * the PWM signal to H-bridges connected to the motors to attempt to maintain
00004  * a constant velocity.
00005  */
00006 #include "mbed.h"
00007 #include "Motor.h"
00008 #include "QEI.h"
00009 #include "PID.h"
00010 
00011 Timer t;
00012 Timer t2;
00013 
00014 Serial pc(USBTX, USBRX); // tx, rx
00015 
00016 
00017 //Motor DepanKanan(PA_0, PB_0, PA_4); //Depan Kiri
00018 Motor DepanKanan(PA_9, PB_10 , PA_8); //DepanKanan
00019 //Motor DepanKanan(PC_8, PC_5, PA_12); //Belakang Kanan
00020 //Motor DepanKanan(PA_1, PC_0, PC_1); //Belakang Kiri
00021 
00022 //QEI DepanKananQEI(PC_10, PC_12, NC, 986); //Depan Kiri
00023 QEI DepanKananQEI(PC_11, PD_2, NC, 624); //Depan Kanan
00024 //QEI DepanKananQEI(PB_1, PB_15, NC, 986); //Belakang Kanan
00025 //QEI DepanKananQEI(PA_15, PB_7, NC, 986);// Belakang Kiri
00026 
00027 //25  Mei 2022
00028 PID DepanKananPID(0.96, 0.1, 0.000001, 0.02);
00029 //PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02);
00030 //PID BelakangKiriPID(0.496, 0.0565, 0.000000000001, 0.02); Belakang Kiri
00031 //PID BelakangKananPID(0.5, 0.022, 0.00000000001, 0.02);
00032 
00033 //2022
00034 //PID DepanKananPID(0.65, 0.1, 0.0000000001, 0.02);  //Kc, Ti, Td belakang kiri
00035 
00036 
00037 
00038 //PID DepanKananPID(0.8, 0.1, 0.0000000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 1
00039 //PID DepanKananPID(0.7, 0.1, 0.0000000000005, 0.02);  //Kc, Ti, Td MOTOR baru nomor 2
00040 //PID DepanKananPID(0.8, 0.1, 0.00000002, 0.02);  //Kc, Ti, Td MOTOR baru nomor 3
00041 
00042 
00043 
00044 //PID DepanKananPID(0.495, 0.1, 0.00000035, 0.09); //Kc, Ti, Td, RATE
00045 
00046 
00047 int main() {
00048     //int DepanKananPulses  = 0; 
00049     pc.baud(9600);
00050     DepanKanan.period(0.01f);    
00051 
00052     DepanKananPID.setInputLimits(0, 3000);//Input  units: counts per second.
00053     DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
00054     DepanKananPID.setMode(AUTO_MODE);
00055 
00056     int DepanKananPulses      = 0; //How far the left wheel has travelled.
00057     int DepanKananPrevPulses  = 0; //The previous reading of how far the left wheel
00058     //has travelled.
00059     float DepanKananVelocity  = 0.0; //The velocity of the left wheel in pulses per
00060     //second.
00061   
00062 
00063     wait(3); //Wait a few seconds before we start moving.
00064 
00065     
00066     DepanKananPID.setSetPoint(1000);
00067 
00068     t.start();
00069     while (t.read_ms() <= 3000) {
00070         
00071         /*DepanKanan.speed(0.5f);
00072         
00073         DepanKananPulses = DepanKananQEI.getPulses();
00074         
00075         pc.printf("\n\r %d " , DepanKananPulses);*/
00076         t2.start();
00077         
00078 
00079         DepanKananPulses = DepanKananQEI.getPulses();
00080         DepanKananVelocity = ((double)DepanKananPulses - (double)DepanKananPrevPulses) / 0.02;
00081         DepanKananPrevPulses = DepanKananPulses;
00082         //DepanKananVelocity = (DepanKananVelocity/210.0f)*60.0f;
00083         
00084         DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
00085 
00086         //----------//
00087         DepanKanan.speed(DepanKananPID.compute());
00088 
00089         
00090         pc.printf("\n %f " ,DepanKananVelocity );
00091     
00092         
00093         while (t2.read_ms() <= 20) {
00094         
00095         }
00096         
00097         t2.reset();
00098 
00099     }
00100     t.reset();
00101     DepanKanan.brake();
00102 
00103 }