Diff: main.cpp

Revision:

0:a6918b57d3fa

Child:

1:5e0cb74f950e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 17 15:43:07 2017 +0000
@@ -0,0 +1,122 @@
+/**
+ *          THROWER ROBOT
+ * Bismillahirahmanirrahim
+ *
+ * Updated :
+ *  -Perhitungan PID masih error
+ *  -encoder kanan sama encoder kiri belum kebaca. ntah kenapa
+ *
+ * To do List :
+ *  -Coba ODOMETRY !!
+ *
+ * Last Edited by : Calmantara
+ **/
+ 
+//LIBRARY
+#include "mbed.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "millis.h"
+
+/***************************
+ *  Konstanta dan Variabel *
+ ***************************/
+
+//Deklarasi Motor
+Motor motor_depan(PB_6, PB_13, PA_10);  //pwm, fwd, rev
+Motor motor_kanan(PB_8, PA_9, PA_5);    //pwm, fwd, rev
+Motor motor_kiri(PB_9, PA_12, PA_11);   //pwm, fwd, rev
+
+//Variabel Motor
+float pwm_kanan = 0.3;
+float pwm_kiri = -0.3;
+float limit_speed = 0.7;
+
+//Variabel Serial
+Serial pc(USBTX, USBRX);    //Deklarasi serial pc TX RX
+
+//Deklarasi Rotary Encoder
+encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
+encoderKRAI encMotor_kanan(PC_5, PC_4, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
+encoderKRAI encMotor_kiri(PC_7, PC_6, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
+
+//Variabel Encoder
+float pulses_depan;    //variabel untuk membaca nilai encoder motor depan
+float pulses_kanan;    //variabel untuk membaca nilai encoder motor kanan
+float pulses_kiri;     //variabel untuk membaca nilai encoder motor kiri
+
+//Target nilai encoder
+float target_depan = 0;
+
+//Variabel PID_1
+unsigned long int current_millis;    //variabel untuk mendapatkan millis yang berjalan
+unsigned long int prev_millis1;      //variabel untuk mendapatkan millis sebelumnya
+unsigned short time_sampling = 12;   //variabel untuk time sampling
+float max_speed = 0.4;
+float min_speed = -0.4;
+float kp_1 = 0.5;                    //variabel konstanta PID
+float current_error1, prev_error1;
+float speed, prev_speed;
+
+//Variabel Motor Berhenti
+unsigned long int prev_millis2;      //variabel untuk mendapatkan millis sebelumnya
+
+/***************************
+ *  Main Function          *
+ ***************************/
+
+int main(){
+
+//Inisiasi Serial
+    pc.baud(115200);
+
+//Start Millis
+    //wait_ms(5000);
+    startMillis();
+
+//Looping Program
+    while(1){
+        current_millis = millis();
+        /* Speed Motor */
+        if(pwm_kanan > limit_speed){
+            pwm_kanan = limit_speed;
+            }
+        else if(pwm_kanan < -limit_speed){
+            pwm_kanan = -limit_speed;
+            } 
+        if(pwm_kiri > limit_speed){
+            pwm_kiri = limit_speed;
+            }
+        else if(pwm_kiri < -limit_speed){
+            pwm_kiri = -limit_speed;
+            } 
+        
+        motor_kanan.speed(pwm_kanan);
+        motor_kiri.speed(pwm_kiri);
+        
+        //pc.printf("%f\t%f\t%f\n", pulses_depan, pulses_kanan, pulses_kiri);
+        
+        if(current_millis - prev_millis1 >= time_sampling){
+        /* masuk ke perhitungan ketika sudah pada time sampling */
+            pulses_depan = (float) encMotor_depan.getRevolutions();  //Mengambil pulses encoder
+            current_error1 = target_depan - pulses_depan;
+            speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1;
+            /* Kondisi untuk menambahkan speed */
+            if(speed > max_speed){
+                pwm_kiri = pwm_kiri+max_speed;
+                pwm_kanan = pwm_kanan - max_speed;
+            }
+            else if ( speed < min_speed){
+                pwm_kiri = pwm_kiri-min_speed;
+                pwm_kanan = pwm_kanan + min_speed;
+            }
+            else {
+                pwm_kiri = pwm_kiri + speed;
+                pwm_kanan = pwm_kanan - speed;
+            }
+            prev_speed = speed;
+            prev_error1 = current_error1;
+            prev_millis1 = current_millis;
+            }
+    }
+} /* end of main fuction */
\ No newline at end of file