Isro Syaeful Iman
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 9:e2c3175936fb
- Parent:
- 8:4a536a26ee07
- Child:
- 10:e550e5daed74
diff -r 4a536a26ee07 -r e2c3175936fb main.cpp
--- a/main.cpp Sun Oct 22 03:32:02 2017 +0000
+++ b/main.cpp Tue Oct 24 03:53:45 2017 +0000
@@ -3,10 +3,9 @@
* Bismillahirahmanirrahim
*
* To do List :
- * - TAMBAHKAN ENC DEPAN BIAR GA GERAK UNTUK MAJU
* - Cari konstanta Kanan Kiri
- * - Buat library odometry
- * - Rapihkan kodingan :)
+ * - Cobda library Odometry
+ *
* - JAGA SEMANGAT DAN KESEHATAN KITA !!! :)
*
* UPDATED : 22/10
@@ -22,24 +21,27 @@
#include "encoderKRAI.h"
#include "millis.h"
#include "pidKRAI.h"
+#include "odometryKRAI.h"
/***************************
* Konstanta dan Variabel *
***************************/
#define PI 3.141592
-#define DRODA 100
+#define DRODA 100.0
//Variabel Robot
-float kll_roda = (float) PI*DRODA; //variabel keliling robot
+double kll_roda = PI*DRODA; //variabel keliling robot
//Variabel Program dan Planning
unsigned short in_program = 1; //variabel untuk masuk ke looping program
unsigned short state = 0;
//Primitive Function
-void limitMotor(); //procedure untuk mengecek limit pwm motor
-void pidMotor(int x); //procedure untuk arah gerak motor
-float getY(float y1, float y2); //fungsi untuk menghitung jarak Y
+void limitMotor(); //procedure untuk mengecek limit pwm motor
+void pidMotor(int x); //procedure untuk arah gerak motor
+void getValue(); //procedure untuk menghitung pulses dan rev
+double getY(float y1, float y2); //fungsi untuk menghitung jarak Y
+//double getX(float x1, float x2, float x3); //fungsi untuk menghitung jarak X
//Deklarasi Motor
Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev
@@ -65,22 +67,21 @@
float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan
float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri
-float delta_kaki; //variabel untuk membaca nilai delta kanan kiri
-float delta_semua; //variabel untuk membaca nilai delta semua encoder
+float d_tetha; //variabel untuk membaca nilai delta kanan kiri
float rev_depan; //variabel untuk membaca nilai putaran roda depan
float rev_kanan; //variabel untuk membaca nilai putaran roda kanan
float rev_kiri; //variabel untuk membaca nilai putaran roda kiri
//Target nilai encoder
-float tn_y = 0; //target untuk jumlah rotary encoder maju atau mundur
-float tn_x = 0; //target untuk jumlah rotary encoder kanan atau kiri
+float tn_tetha = 0; //target untuk nilai tetha robot
//Variabel PID
unsigned short ts_base = 12; //variabel untuk time sampling
float speed_roty; //variabel untuk penambahan pwm PID rotasi y
float speed_disy; //variabel untuk penambahan pwm PID distance y
float limit_y = 0.5;
+
float speed_rotx; //variabel untuk penambahan pwm PID rotasi x
float speed_disx; //variabel untuk penambahan pwm PID distance x
@@ -92,16 +93,15 @@
pidKRAI pidDisX(); //kp ki kd ts
//Variabel cari jarak
-float target_y[100] = {-5000.0, -5000.0}; //variabel yang digunakan untuk target jarak y
-float jarak_y; //variabel untuk increment jarak y
+float target_y[100] = {3000.0, -5000.0}; //variabel yang digunakan untuk target jarak y
+double jarak_y; //variabel untuk increment jarak y
float target_x[100]; //variabel yang digunakan untuk target jarak x
-float jarak_x; //variabel untuk increment jarak x
+double jarak_x; //variabel untuk increment jarak x
//Variabel Waktu
unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan
unsigned long int prev_millis1; //variabel untuk masuk ke perhitungan PID
-unsigned long int prev_millis2; //variabel untuk matikan motor
/***************************
* Main Function *
@@ -124,10 +124,11 @@
while(in_program == 1){
//current_millis = millis();
if((jarak_y <= target_y[state]-(float)10.0)||(jarak_y >= target_y[state]+(float)10.0)){
+ /* masuk ke case sumbu Y */
current_millis = millis();
if(current_millis - prev_millis1 >= 12){
pidMotor(0);
- pc.printf("%.2f %.2f\n", jarak_y, delta_kaki);
+ pc.printf("%.2f %.2f\n", jarak_y, d_tetha);
}
/* Speed Motor */
@@ -135,23 +136,28 @@
motor_kanan.speed(pwm_kanan);
motor_kiri.speed(pwm_kiri);
}
+ else if((jarak_x <= target_x[state]-(float)10.0)||(jarak_x >= target_x[state]+(float)10.0)){
+ /* masuk ke case sumbu X */
+ }
else{
+ pwm_depan = 0.0;
pwm_kanan = 0.0;
pwm_kiri = 0.0;
+ motor_depan.brake(1);
motor_kanan.brake(1);
motor_kiri.brake(1);
- }
- pulses_kanan = (float) encMotor_kanan.getPulses();
- pulses_kiri = (float) encMotor_kiri.getPulses();
- rev_kanan = (float) pulses_kanan/540;
- rev_kiri = (float) pulses_kiri/540;
+ }
+
+ /* mengambil data encoder motor */
+ getValue();
/* masuk ke perhitungan parameter */
jarak_y += getY(rev_kanan, rev_kiri);
- delta_kaki = pulses_kanan + pulses_kiri;
+ d_tetha = pulses_kanan + pulses_kiri + pulses_depan;
+ encMotor_depan.reset();
encMotor_kanan.reset();
encMotor_kiri.reset();
- pc.printf("%.2f %.2f\n", jarak_y, delta_kaki);
+ pc.printf("%.2f %.2f\n", jarak_y, d_tetha);
}
} /* end of main program */
}
@@ -189,57 +195,71 @@
**/
if(x==0){
/* masuk ke perhitungan ketika sudah pada time sampling */
- pulses_kanan = (float) encMotor_kanan.getPulses();
- pulses_kiri = (float) encMotor_kiri.getPulses();
- rev_kanan = (float) pulses_kanan/540;
- rev_kiri = (float) pulses_kiri/540;
+ /* mengambil data encoder motor */
+ getValue();
/* masuk ke perhitungan parameter */
jarak_y += getY(rev_kanan, rev_kiri);
- delta_kaki = pulses_kanan + pulses_kiri;
+ d_tetha = pulses_kanan + pulses_kiri + pulses_depan;
- speed_roty = pidRotY.pidCount(tn_y, delta_kaki, limit_y);
+ speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_y);
speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_y);
+
/* Kondisi untuk menambahkan speed */
pwm_kiri = -speed_disy + speed_roty;
pwm_kanan = speed_disy + speed_roty;
+ pwm_depan = speed_roty;
prev_millis1 = current_millis;
+ encMotor_depan.reset();
encMotor_kanan.reset();
encMotor_kiri.reset();
}
else if(x==1){
/* masuk ke perhitungan ketika sudah pada time sampling */
- pulses_kanan = (float) encMotor_kanan.getPulses();
- pulses_kiri = (float) encMotor_kiri.getPulses();
- pulses_depan = (float) encMotor_depan.getPulses();
- rev_kanan = (float) pulses_kanan/540;
- rev_kiri = (float) pulses_kiri/540;
- rev_depan = (float) pulses_depan/540;
+ /* mengambil data encoder motor */
+ getValue();
/* masuk ke perhitungan parameter */
//jarak_x += getX(rev_kanan+rev_kiri, rev_depan);
- delta_semua = pulses_kanan + pulses_kiri + pulses_depan;
+ d_tetha = pulses_kanan + pulses_kiri + pulses_depan;
- //speed_rotx = pidRotY.pidCount(tn_x, delta_semua);
- //speed_disx = pidDisY.pidCount(target_y[0], jarak_y);
+ //speed_rotx = pidRotX.pidCount(tn_x, delta_semua);
+ //speed_disx = pidDisX.pidCount(target_x[0], jarak_x);
+
/* Kondisi untuk menambahkan speed */
pwm_kiri = speed_disx + speed_rotx;
pwm_kanan = speed_disx + speed_rotx;
pwm_depan = -(speed_disx + speed_rotx);
prev_millis1 = current_millis;
+ encMotor_depan.reset();
encMotor_kanan.reset();
encMotor_kiri.reset();
- encMotor_depan.reset();
}
}/* end of pidMotor */
-float getY(float y1, float y2){
+void getValue(){
+/*
+ * Procedure yang digunakan untuk menghitung value pulses encoder
+ * dan revolusi dari motor
+ */
+ pulses_depan = (float)encMotor_depan.getPulses();
+ pulses_kanan = (float) encMotor_kanan.getPulses();
+ pulses_kiri = (float) encMotor_kiri.getPulses();
+ rev_depan = (float) pulses_kanan/540;
+ rev_kanan = (float) pulses_kanan/540;
+ rev_kiri = (float) pulses_kiri/540;
+ }/* end of getValue */
+
+double getY(float y1, float y2){
/* fungsi untuk menghitung nilai Y dari roda kanan kiri */
- float jarak_y1 =(float) 1.732*(kll_roda*y1)/2;
- float jarak_y2 =(float) 1.732*(kll_roda*y2)/2;
+ double jarak_y1 = 1.732*(kll_roda*y1)/2;
+ double jarak_y2 = 1.732*(kll_roda*y2)/2;
- return (jarak_y1 - jarak_y2)*2;
+ return (jarak_y1 - jarak_y2);
}/* end of getY */
+/*float getX(float x1, float x2, float x3){
+
+ }*/