KRAI 2017
ping + display on
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_20Feb
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 22:4632f58461e0
- Parent:
- 21:da2f3d04468f
- Child:
- 23:023b522977b2
--- a/main.cpp Sat Jan 28 07:24:07 2017 +0000
+++ b/main.cpp Sat Jan 28 10:19:36 2017 +0000
@@ -3,15 +3,15 @@
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
-/* _________________ */
-/* | DEPAN | */
-/* | 1. e .2 | Angka ==> Motor */
-/* | ` ` | e ==> Encoder */
-/* | e e | */
-/* | . . | */
-/* | 4` e `3 | */
-/* |________________| */
-/* */
+/* ______________________ */
+/* / \ Rode Depan Belakang: */
+/* / 2 (Belakang) \ Omniwheel */
+/* | | */
+/* | 3 (Encoder) 4 | Roda Kiri Kanan: */
+/* | | Fixed Wheel */
+/* \ 1 (Depan) / */
+/* \______________________/ Putaran berlawanan arah */
+/* jarum jam positif */
/* SETTINGS (WAJIB!) : */
/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
@@ -21,14 +21,14 @@
/* Joystick */
/* Kanan => Posisi target x ditambah 'perpindahan' */
/* Kiri => Posisi target x dikurang 'perpindahan' */
-/* Atas => Posisi target y ditambah 0.01 */
-/* Bawah => Posisi target y dikurang 0.01 */
/* */
/* Tombol silang => Kembali keposisi Awal */
/* Tombol segitiga => Aktif motor Launcher */
/* Tombol lingkaran=> Aktif servo Launcher */
-/* Tombol L3 => PWM Launcher dikurangin */
-/* Tombol R3 => PWM Launcher ditambahin */
+/* Tombol L1 => Pivot Kiri */
+/* Tombol R1 => Pivot Kanan */
+/* Tombol L3 => PWM Launcher Belakang dikurangin */
+/* Tombol R3 => PWM Launcher Belakang ditambahin */
/* */
/* Bismillahirahmanirrahim */
/* Jagalah Kebersihan Kodingan */
@@ -44,36 +44,28 @@
/* Konstanta dan Variabel */
/***********************************************/
#define PI 3.14159265
-#define D_ENCODER 0.058
-#define D_ROBOT 0.64
-#define VMAX 0.3 // Maju, Mundur, Kiri Kanan
-#define SAMPING 0.3 // Saka, Saki, Sbka, Sbki
+#define D_ENCODER 0.997 // Diameter Roda Encoder
+#define D_ROBOT // Diameter Roda Robot
+#define VMAX 0.3 // Kiri Kanan
#define PIVOT 0.4 // Pivka, Pivki
#define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
float k_enc = PI*D_ENCODER;
-float k_robot = PI*D_ROBOT;
-float speed1 =0.6;
-float speed2 =0.6;
-float speed3 =0.6;
-float speed4 =0.6;
-float speedB =0.43;
-float speedL =0.4;
+float speedT = 0.2;
+float speedB = 0.43;
+float speedL = 0.4;
-float kpX=0.5, kpY=0.5, kp_tetha=0.03;
+float vpid = 0;
+
+float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03;
/* Deklarasi encoder */
-encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
-encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
-encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
/* Deklarasi Motor Base */
-Motor motor1(PB_7, PA_14, PA_15); // pwm, fwd, rev
-Motor motor2(PB_8, PA_13, PB_0); // pwm, fwd, rev
-Motor motor3(PB_9, PA_12, PC_5); // pwm, fwd, rev
-Motor motor4(PB_6, PB_1, PB_12); // pwm, fwd, rev
+Motor motor1(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Depan
+Motor motor2(PB_8, PA_13, PB_0); // pwm, fwd, rev, Motor Belakang
/* Deklarasi Motor Launcher */
Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev
@@ -90,15 +82,10 @@
**/
/* Variabel Encoder */
-float jarak, posX, posY; //
-float XT, YT, Tetha; // Jarak Target Robot
-float errX, errY, errT, Vt, Vx, Vy; // Variabel yang didapatkan encoder
-float v1, v2, v3, v4; // Variabel kecepatan motor dari encoder
+float errT, Tetha; // Variabel yang didapatkan encoder
/* Fungsi dan Procedur Encoder */
void setCenter(); // Fungsi reset agar robot di tengah
-float getY(); // Fungsi mendapatkan jarak Y
-float getX(); // Fungsi mendapatkan jarak X
float getTetha(); // Fungsi mendapatkan jarak Tetha
/* Inisialisasi Pin TX-RX Joystik dan PC */
@@ -106,27 +93,21 @@
/* Variabel Stick */
char case_ger;
-bool launcher = false, servoGo = false, manual = true;
-int caseSebelum;
+bool launcher = false, servoGo = false;
-/***********************************************/
-/* Deklarasi Fungsi dan Procedure */
-/***********************************************/
+/****************************************************/
+/* Deklarasi Fungsi dan Procedure */
+/****************************************************/
int case_gerak(){
-/*****************************************************
+/****************************************************/
** Gerak Motor Base
** Case 1 : Pivot kanan
** Case 2 : Pivot Kiri
- ** Case 3 : Maju
- ** Case 4 : Mundur
- ** Case 5 : Serong Atas Kanan
- ** Case 6 : Serong Bawah Kanan
- ** Case 7 : Serong Atas Kiri
- ** Case 8 : Serong Bawah Kiri
- ** Case 9 : Kanan
- ** Case 10 : Kiri
- ** Case 12 : break
+ ** Case 3 : Kanan
+ ** Case 4 : Kiri
+ ** Case 5 : Break
****************************************************/
+
int casegerak;
if (!joystick.L1 && joystick.R1) {
// Pivot Kanan
@@ -136,40 +117,17 @@
// Pivot Kiri
casegerak = 2;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
- // Maju
- casegerak = 3;
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
- // Mundur
- casegerak = 4;
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
- // Serong Atas Kanan
- casegerak = 5;
- }
- else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
- // Serong Bawah Kanan
- casegerak = 6;
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
- // Serong Atas Kiri
- casegerak = 7;
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
- // Serong Bawah Kiri
- casegerak = 8;
- }
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
- casegerak = 9;
+ casegerak = 3;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Kiri
- casegerak = 10;
+ casegerak = 4;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
- casegerak = 12;
+ // Break
+ casegerak = 5;
}
return(casegerak);
}
@@ -207,257 +165,90 @@
}
// MOTOR Bawah
- if (manual) {
- // Mode Manual
- switch (case_ger) {
+ switch (case_ger) {
case (1): {
// Pivot Kanan
motor1.speed(-PIVOT);
motor2.speed(-PIVOT);
- motor3.speed(-PIVOT);
- motor4.speed(-PIVOT);
break;
}
case (2): {
// Pivot Kiri
motor1.speed(PIVOT);
motor2.speed(PIVOT);
- motor3.speed(PIVOT);
- motor4.speed(PIVOT);
- break;
- }
- case (3): {
- // Maju
- motor1.speed(-VMAX);
- motor2.speed(VMAX);
- motor3.speed(VMAX);
- motor4.speed(-VMAX);
- break;
- }
- case (4): {
- // Mundur
- motor1.speed(VMAX);
- motor2.speed(-VMAX);
- motor3.speed(-VMAX);
- motor4.speed(VMAX);
- break;
- }
- case (5) : {
- // Samping Atas Kanan
- motor1.speed(-SAMPING);
- motor2.brake(1);
- motor3.speed(SAMPING);
- motor4.brake(1);
break;
- }
- case (6) : {
- // Samping Bawah Kanan
- motor1.brake(1);
- motor2.speed(-SAMPING);
- motor3.brake(1);
- motor4.speed(SAMPING);
- break;
- }
- case (7) : {
- // Samping Atas Kiri
- motor1.brake(1);
- motor2.speed(SAMPING);
- motor3.brake(1);
- motor4.speed(-SAMPING);
+ }
+ case (3) : {
+ // Kanan
+ motor1.speed(-VMAX-vpid);
+ motor2.speed(VMAX+vpid);
break;
- }
- case (8) : {
- // Samping Bawah Kiri
- motor1.speed(SAMPING);
- motor2.brake(1);
- motor3.speed(-SAMPING);
- motor4.brake(1);
- break;
- }
- case (9) : {
- // Kanan
- motor1.speed(-VMAX);
- motor2.speed(-VMAX);
- motor3.speed(VMAX);
- motor4.speed(VMAX);
- break;
- }
- case (10) : {
+ }
+ case (4) : {
// Kiri
- motor1.speed(VMAX);
- motor2.speed(VMAX);
- motor3.speed(-VMAX);
- motor4.speed(-VMAX);
+ motor1.speed(VMAX-vpid);
+ motor2.speed(-VMAX+vpid);
break;
}
default : {
- motor1.brake(1);
- motor2.brake(1);
- motor3.brake(1);
- motor4.brake(1);
+ motor1.brake(1);
+ motor2.brake(1);
}
- } // End Switch
- } else {
- // Mode Encoder
- switch (case_ger) {
- case (1):{
- Tetha = Tetha - 0.05;
- break;
- }
- case (2):{
- Tetha = Tetha + 0.05;
- break;
- }
- case (3):{
- YT = YT + 0.01;
- break;
- }
- case (4):{
- YT = YT - 0.01;
- break;
- }
- case (5) :{
- XT = XT + 0.01;
- YT = YT + 0.01;
- break;
- }
- case (6) :{
- XT = XT + 0.01;
- YT = YT - 0.01;
- break;
- }
- case (7) :{
- XT = XT - 0.01;
- YT = YT + 0.01;
- break;
- }
- case (8) :{
- XT = XT - 0.01;
- YT = YT - 0.01;
- break;
- }
- case (9) :{
- // Kanan
- if (case_ger != caseSebelum)
- XT = XT + PERPINDAHAN;
- break;
- }
- case (10) :{
- // Kiri
- if (case_ger != caseSebelum)
- XT = XT - PERPINDAHAN;
- break;
- }
- default :{}
- } // End of Switch
- caseSebelum = case_ger;
- }
+ } // End Switch
}
void setCenter(){
/* Fungsi untuk menentukan center dari robot */
- encoderDepan.reset();
- encoderBelakang.reset();
- encoderKanan.reset();
encoderKiri.reset();
}
-float getX(){
-/* Fungsi untuk mendapatkan jarak X */
- float jarakEncDpn, jarakEncBlk;
- jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc;
- jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc;
- return (jarakEncDpn-jarakEncBlk)/2;
-}
-
-float getY(){
-/* Fungsi untuk mendapatkan jarak Y */
- float jarakEncKir, jarakEncKan;
- jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc;
- jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc;
- return (jarakEncKir-jarakEncKan)/2;
-}
-
float getTetha(){
/* Fungsi untuk mendapatkan nilai tetha */
- float busurDpn, busurBlk, busurKir, busurKan;
- busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
- busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
- busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
- busurKan = ((encoderKanan.getPulses())/(float)(720.0)*k_enc)/k_robot*360.0;
+ float busurKir;
+ busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
- return -(busurDpn+busurBlk+busurKir+busurKan)/4;
+ return -(busurKir);
}
-void gotoXYT(float xa, float ya, float Ta){
-/* Fungsi untuk bergerat ke target */
- errX = xa-getX();
- Vx = kpX*errX;
-
- errY = ya-getY();
- Vy = kpY*errY;
+void gotoXYT(float Ta){
+/* Fungsi untuk bergerak ke target */
+ float vt;
errT = Ta-getTetha();
Vt = kp_tetha*errT;
- v1 = Vx+Vy-Vt;
- v2 = Vx-Vy-Vt;
- v3 = -Vx-Vy-Vt;
- v4 = -Vx+Vy-Vt;
-
- if (v1>speed1)
- { v1 = speed1; }
- else if (v1<-speed1)
- { v1 = -speed1; }
-
- if (v2>speed2)
- { v2 = speed2; }
- else if (v2<-speed2)
- { v2 = -speed2; }
+ if (vt>speed1)
+ { vt = speed1; }
+ else if (vt<-speed1)
+ { vt = -speed1; }
- if (v3>speed3)
- { v3 = speed3; }
- else if (v3<-speed3)
- { v3 = -speed3; }
-
- if (v4>speed4)
- { v4 = speed4; }
- else if (v4<-speed4)
- { v4 = -speed4; }
-
- if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))){
- motor1.speed(v1);
- motor2.speed(v2);
- motor3.speed(v3);
- motor4.speed(v4);
+ if ((((errT > 0.05) || (errT<-0.05))){
+ vpid = vt;
}
else{
- motor1.brake(1);
- motor2.brake(1);
- motor3.brake(1);
- motor4.brake(1);
+ vpid = 0;
}
}
void speedLauncher(){
/* Fungsi untuk speed launcher */
if (joystick.R3_click and speedL < 0.8){
- speedL = speedL + 0.01;
+ speedL = speedL + 0.01; // PWM++ Motor Belakang
}
if (joystick.L3_click and speedL > 0.1){
- speedL = speedL - 0.01;
+ speedL = speedL - 0.01; // PWM-- Motor Belakang
}
if (joystick.R2_click and speedB < 0.8 ){
- speedB = speedB + 0.01;
+ speedB = speedB + 0.01; // PWM++ Motor Depan
}
if (joystick.L2_click and speedB > 0.1 ){
- speedB = speedB - 0.01;
+ speedB = speedB - 0.01; // PWM-- Motor Depan
}
}
-/***********************************************/
-/* Main Function */
-/***********************************************/
+/*********************************************************/
+/* Main Function */
+/*********************************************************/
int main (void){
/* Set baud rate - 115200 */
@@ -467,8 +258,6 @@
wait_ms(500);
/* Posisi Awal */
- XT = 0;
- YT = 0;
Tetha = 0;
/* Untuk mendapatkan serial dari Arduino */
@@ -490,18 +279,10 @@
if (joystick.segitiga_click) launcher = !launcher;
if (joystick.lingkaran_click) servoGo = true;
- if (joystick.SELECT_click) manual = !manual;
- if (joystick.silang) {
- XT = 0;
- YT = 0;
- Tetha = 0;
- }
speedLauncher();
}
else {
joystick.idle();
}
- if (!manual)
- gotoXYT(XT,YT,Tetha);
}
}