Marchio Kevin
Buat agip
Dependencies: Motor_1 encoderKRAI mbed millis
Fork of
Robo_Taker_Nasional_2018
by 
KRAI 2018
Diff: main.cpp
- Revision:
- 6:bb7e29420efd
- Parent:
- 5:4a70c53d7f86
- Child:
- 7:8d8eb4676356
--- a/main.cpp Thu Mar 08 08:45:09 2018 +0000
+++ b/main.cpp Thu Mar 08 09:24:57 2018 +0000
@@ -1,3 +1,16 @@
+////////////////////////////////////////////////////////////////////////////////
+// Robo Taker 2018 //
+// -------------------------------------------------------------------------- //
+// Todo List: //
+// - Perhalus Gerakan //
+// - Pasang Mode Joystick Kabel/BT //
+// - Program Pneumatic //
+// - Tata code yang ada, Lengkapi dengan Komentar (Penjelasan) //
+// Gerakan Robot: //
+// - Arah -> Gerak //
+// - Silang -> Pneumatic //
+// - Start -> Ubah Mode //
+////////////////////////////////////////////////////////////////////////////////
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
@@ -5,17 +18,19 @@
#include "pinList.h"
#include "millis.h"
+////////////////////////////////////////////////////////////////////////////////
+// Konstanta Program //
+////////////////////////////////////////////////////////////////////////////////
#define PI 3.141592653593
#define RAD_TO_DEG 57.2957795131
#define MAX_W_SPEED 15000 //max angular speed of robot
-
#define TOLERANCET 0.8 //theta tolerance
#define PULSE_TO_JARAK 0.581776 //kll roda / pulses
#define L 298.0 //roda to center of robot
#define TS 2.0 //time sampling
#define LIMITPWM 0.4 //limit pwm motor
-//Konstanta PID Sudut
+//Konstanta PID Theta
#define KP_W 1.0
#define KI_W 0.0065
#define KD_W 175
@@ -23,10 +38,14 @@
#define MOTOR_LIMIT_MAX 1
#define MOTOR_LIMIT_MIN -1
+// Set 1 untuk aktifkan fitur pc.print
#define DEBUG 1
+////////////////////////////////////////////////////////////////////////////////
+// Object Program //
+////////////////////////////////////////////////////////////////////////////////
// Serial
-Serial pc(USBTX,USBRX);
+Serial pc(USBTX, USBRX, 115200);
joysticknucleo stick(PIN_TX, PIN_RX);
// Pneumatik
@@ -42,52 +61,45 @@
Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
-// Fungsi dan Prosedur
+////////////////////////////////////////////////////////////////////////////////
+// Deklarasi Prosedure dan Fungsi //
+////////////////////////////////////////////////////////////////////////////////
void gerakMotor();
-void hitungPID(float theta_s);
+void hitungPID(double theta_s);
void hitungParameter();
void printPulse();
void case_gerak();
-float compute_Alpha(float x_s, float y_s, float x, float y,float theta);
-// Variable-variable
+////////////////////////////////////////////////////////////////////////////////
+// Variable-variable //
+////////////////////////////////////////////////////////////////////////////////
int joystick;
-float pulse_A=0;
-float pulse_B=0;
-float pulse_C=0;
-float Vr = 0;
-float Vr_max = 0;
-float Vw = 0;
-float a = 0;
-float w = 0;
-float x =0;
-float x_s = 0;
-float y =0;
-float y_s = 0;
-float x_prev=0;
-float y_prev=0;
-float theta_s = 0;
-float theta = 0;
-float theta_prev = 0;
-float theta_error_prev = 0;
-float sum_theta_error = 0;
-float theta_error;
-unsigned long last_mt_print, last_mt_pid, last_mt_aksel, last_mt_desel, last_mt_rotasi;
+double pulse_A=0, pulse_B=0, pulse_C=0;
+double Vr = 0, Vr_max = 0;
+double Vw = 0;
+double a = 0;
+double w = 0;
+double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0;
+double theta=0, theta_s=0;
+double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0;
+unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
+unsigned long last_mt_aksel, last_mt_desel;
+bool modeauto = 1;
bool print_pulse = 0;
-bool modeauto = 1;
+////////////////////////////////////////////////////////////////////////////////
+// Main Program //
+////////////////////////////////////////////////////////////////////////////////
int main(){
encoder_A.reset();
encoder_B.reset();
encoder_C.reset();
- pc.baud(115200);
+ pneumatik = 0;
+ startMillis();
stick.setup();
stick.idle();
- pneumatik = 0;
- startMillis();
while(1){
- // do nothing
if(stick.readable() ) {
// Panggil fungsi pembacaan joystik
stick.baca_data();
@@ -95,19 +107,26 @@
stick.olah_data();
// Ambil data joystick
case_gerak();
-
+
+ // Pengatur Gerak Motor
gerakMotor();
+
+ // Penghitungan Theta, X, dan Y
if (millis() - last_mt_pid > TS){
hitungParameter();
last_mt_pid = millis();
}
+
+ // Program PID
if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
hitungPID(theta_s);
}
if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
if(modeauto) Vw = 0;
}
- if (millis() - last_mt_print > TS+5){
+
+ // Fungsi Serial Print
+ if (millis() - last_mt_print > TS*10){
if (print_pulse && DEBUG)
printPulse();
last_mt_print = millis();
@@ -116,6 +135,10 @@
}
}
+
+////////////////////////////////////////////////////////////////////////////////
+// Prosedure dan Fungsi //
+////////////////////////////////////////////////////////////////////////////////
void hitungParameter(){
pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
@@ -135,8 +158,8 @@
encoder_B.reset();
encoder_C.reset();
}
-void hitungPID(float theta_s){
- //theta_s = theta_s/RAD_TO_DEG;
+
+void hitungPID(double theta_s){
//menghitung error jarak x,y terhaadap xs,ys
theta_error = theta_s - (theta*RAD_TO_DEG);
sum_theta_error += theta_error;
@@ -147,6 +170,7 @@
//update
theta_error_prev = theta_error;
+
//saturasi vw
if (Vw > 0.2){
Vw = 0.2;
@@ -158,19 +182,24 @@
void gerakMotor(){
if ((Vw == 0) && (Vr_max == 0)){
+ // Deselerasi
if (Vr >= 0.05){
if (millis() - last_mt_desel > 70){
Vr -= 0.1;
last_mt_desel = millis();
}
- } else {
+ } // Rem
+ else {
motor1.brake(BRAKE_HIGH);
motor2.brake(BRAKE_HIGH);
motor3.brake(BRAKE_HIGH);
+
+ // Pengaturan variable saat berhenti
print_pulse = 0;
Vr = 0;
}
} else {
+ // Akselerasi
if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){
if (Vr < 0.275)
Vr = 0.275;
@@ -178,8 +207,11 @@
Vr += 0.05;
last_mt_aksel = millis();
}
+ // Limit
if (Vr > Vr_max && Vr_max >= 0.000)
Vr = Vr_max;
+
+ // Control Motor
motor1.speed((-1*Vr*cos(a) + Vw));
motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw));
motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw));
@@ -188,18 +220,22 @@
}
void printPulse(){
- pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);
-}
-
-float compute_Alpha(float x_s, float y_s, float x, float y,float theta){
-//fungsi untuk menghitung alpha sebagai arah gerak robot
- float temp = atan((y_s - y)/(x_s - x)) - theta;
-
- if (x_s < x) return temp + PI;
- else return temp;
+ pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);
+ //pc.printf("%.2d\t%.2d\t%.2d\n", pulse_A, pulse_B, pulse_C);
}
void case_gerak(){
+ // Pengubah Mode
+ if(stick.START_click){
+ modeauto = !modeauto;
+ theta = 0.0;
+ theta_prev = 0.0;
+ theta_s = 0;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ }
+
// Rotasi
if(modeauto){
if (!stick.L1 && stick.R1){ // Pivot Kanan
@@ -220,8 +256,7 @@
theta_error = 0;
Vw = -0.2;
}
- }
- else if(!modeauto){
+ } else if(!modeauto){
if (!stick.L1 && stick.R1) // Pivot Kanan
Vw = 0.3;
else if (!stick.R1 && stick.L1) // Pivot Kiri
@@ -230,70 +265,47 @@
Vw = 0.0;
}
- if(stick.START_click){
- modeauto = !modeauto;
- theta = 0.0;
- theta_prev = 0.0;
- theta_s = 0;
- theta_error_prev = 0;
- sum_theta_error = 0;
- theta_error = 0;
- }
-
// Linier
if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
a = -90/RAD_TO_DEG; // Maju
Vr_max = 0.8;
- // x_s = 0;// Maju
- // y_s = 10000;
}
else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
a = 90/RAD_TO_DEG; // Mundur
Vr_max = 0.8;
- // x_s = 0; // Mundur
- // y_s = -10000;
}
else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
a = -135/RAD_TO_DEG; // Serong Atas Kanan
Vr_max = 0.7;
- // x_s = 50000; // Maju+Kanan
- // y_s = 50000;
}
else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
a = 135/RAD_TO_DEG; // Serong Bawah Kanan
Vr_max = 0.7;
- // x_s = 50000; // Mundur+Kanan
- // y_s = -50000;
}
else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
a = -45/RAD_TO_DEG; // Serong Atas Kiri
Vr_max = 0.7;
- // x_s = 50000; // Maju+Kiri
- // y_s = -50000;
}
else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
a = 45/RAD_TO_DEG; // Serong Bawah Kiri
Vr_max = 0.7;
- // x_s = -50000; // Mundur+Kiri
- // y_s = -50000;
}
else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
a = 180/RAD_TO_DEG; // Kanan
Vr_max = 0.5;
- // x_s = 50000; // Kanan
- // y_s = 0;
}
else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
a = 0/RAD_TO_DEG; // Kiri
Vr_max = 0.5;
- // x_s = -50000; // Kiri
- // y_s = 0;
}
else {
Vr_max = 0;
- //a = 0;
}
+ // Control Pneumatic
if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
- pneumatik = !pneumatik; // Silang = Toggle pneumatik
+ pneumatik = !pneumatik;
+ if ((!stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(stick.lingkaran))
+ pneumatik_2 = !pneumatik_2;
+
}
\ No newline at end of file