Marchio Kevin
Buat agip
Dependencies: Motor_1 encoderKRAI mbed millis
Fork of
Robo_Taker_Nasional_2018
by 
KRAI 2018
Diff: main.cpp
- Revision:
- 5:4a70c53d7f86
- Parent:
- 3:b1403fcdaeb1
- Child:
- 6:bb7e29420efd
--- a/main.cpp Thu Mar 01 11:02:03 2018 +0000
+++ b/main.cpp Thu Mar 08 08:45:09 2018 +0000
@@ -18,7 +18,7 @@
//Konstanta PID Sudut
#define KP_W 1.0
#define KI_W 0.0065
-#define KD_W 125
+#define KD_W 175
#define MOTOR_LIMIT_MAX 1
#define MOTOR_LIMIT_MIN -1
@@ -45,6 +45,7 @@
// Fungsi dan Prosedur
void gerakMotor();
void hitungPID(float theta_s);
+void hitungParameter();
void printPulse();
void case_gerak();
float compute_Alpha(float x_s, float y_s, float x, float y,float theta);
@@ -55,6 +56,7 @@
float pulse_B=0;
float pulse_C=0;
float Vr = 0;
+float Vr_max = 0;
float Vw = 0;
float a = 0;
float w = 0;
@@ -70,9 +72,9 @@
float theta_error_prev = 0;
float sum_theta_error = 0;
float theta_error;
-unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
+unsigned long last_mt_print, last_mt_pid, last_mt_aksel, last_mt_desel, last_mt_rotasi;
bool print_pulse = 0;
-bool modeauto = 0;
+bool modeauto = 1;
int main(){
encoder_A.reset();
@@ -95,30 +97,14 @@
case_gerak();
gerakMotor();
-
- if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
+ if (millis() - last_mt_pid > TS){
+ hitungParameter();
+ last_mt_pid = millis();
+ }
+ if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
hitungPID(theta_s);
- last_mt_pid = millis();
}
if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
- pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
- pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
- pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
-
- //Compute value
- theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
- x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
- y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
-
- //Update value
- theta_prev = theta;
- x_prev = x;
- y_prev = y;
-
- encoder_A.reset();
- encoder_B.reset();
- encoder_C.reset();
-
if(modeauto) Vw = 0;
}
if (millis() - last_mt_print > TS+5){
@@ -130,7 +116,7 @@
}
}
-void hitungPID(float theta_s){
+void hitungParameter(){
pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
@@ -148,7 +134,8 @@
encoder_A.reset();
encoder_B.reset();
encoder_C.reset();
-
+}
+void hitungPID(float theta_s){
//theta_s = theta_s/RAD_TO_DEG;
//menghitung error jarak x,y terhaadap xs,ys
theta_error = theta_s - (theta*RAD_TO_DEG);
@@ -170,13 +157,29 @@
}
void gerakMotor(){
- if ((Vw == 0) && (Vr == 0)){
- motor1.brake(BRAKE_HIGH);
- motor2.brake(BRAKE_HIGH);
- motor3.brake(BRAKE_HIGH);
- print_pulse = 0;
+ if ((Vw == 0) && (Vr_max == 0)){
+ if (Vr >= 0.05){
+ if (millis() - last_mt_desel > 70){
+ Vr -= 0.1;
+ last_mt_desel = millis();
+ }
+ } else {
+ motor1.brake(BRAKE_HIGH);
+ motor2.brake(BRAKE_HIGH);
+ motor3.brake(BRAKE_HIGH);
+ print_pulse = 0;
+ Vr = 0;
+ }
} else {
- //a = compute_Alpha(x_s, y_s, x, y, theta);
+ if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){
+ if (Vr < 0.275)
+ Vr = 0.275;
+ else
+ Vr += 0.05;
+ last_mt_aksel = millis();
+ }
+ if (Vr > Vr_max && Vr_max >= 0.000)
+ Vr = Vr_max;
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));
@@ -199,11 +202,24 @@
void case_gerak(){
// Rotasi
if(modeauto){
- if (!stick.L1 && stick.R1) // Pivot Kanan
- theta_s += 0.15;
- else if (!stick.R1 && stick.L1) // Pivot Kiri
- theta_s -= 0.15;
-
+ if (!stick.L1 && stick.R1){ // Pivot Kanan
+ theta = 0.0;
+ theta_prev = 0.0;
+ theta_s = 0;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ Vw = 0.2;
+ }
+ else if (!stick.R1 && stick.L1){ // Pivot Kiri
+ theta = 0.0;
+ theta_prev = 0.0;
+ theta_s = 0;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ Vw = -0.2;
+ }
}
else if(!modeauto){
if (!stick.L1 && stick.R1) // Pivot Kanan
@@ -221,54 +237,61 @@
theta_s = 0;
theta_error_prev = 0;
sum_theta_error = 0;
- theta_error;
+ theta_error = 0;
}
// Linier
- Vr = 0.5;
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 = 0;
- a = 0;
+ Vr_max = 0;
+ //a = 0;
}
if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))