KRAI 2017
hari ini
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
DagonFly__RoadToJapan_11Mei
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 47:6cac4f1a3c8e
- Parent:
- 46:85169ae8659b
- Child:
- 48:3006386dc8df
--- a/main.cpp Mon Apr 24 14:12:08 2017 +0000
+++ b/main.cpp Sat May 13 05:47:10 2017 +0000
@@ -1,8 +1,8 @@
-
+/*tuning motor baru untuk konstanta pid baru */
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
-/* Last Update : 16 April 2017 */
+/* Last Update : 16 April 2017 */
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
@@ -56,9 +56,9 @@
// Variable Atas
// indek 2 untuk motor depan, 1 untuk motor belakang
double speed, speed2;
-const double maxSpeed = 0.95, minSpeed = 0.0, Ts = 12.5;
+const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5;
const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
-const double kpA2=0.1293, kdA2=1.007, kiA2=0.0002986;
+const double kpA2=0.13978, kdA2=0.9222, kiA2=0.00038636;
double a1,b1,c1;
double a2,b2,c2;
double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
@@ -67,23 +67,19 @@
double previous_speed2 = 0;
float rpm, rpm2;
-double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
+double target_rpm = 18.0, target_rpm2 = 18.0; // selisih 4 bagus, sama bagus
const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27
const float pwmPowerUp = 1.0;
const float pwmPowerDown = -1.0;
double jarak_ping=0;
-double ping_target = 12;
+double ping_target = 14;
double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.13, ping_Kd =-0.049, ping_Ts=10;
+double ping_Kp = -0.2747, ping_Kd =0, ping_Ts=10;
double ping_pwm, ping_previous_pwm = 0;
// Variable Bawah
-float Vt;
-float keliling_enc = PI*D_ENCODER;
-float keliling_robot = PI*D_ROBOT;
-float speedT = 0.2;
float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri
float tuneDpn = 1.0; // Tunning PWM motor Depan
float tuneBlk = 1.0; // Tunning PWM motor belakang
@@ -93,8 +89,8 @@
float tuneR = 1.0;
float tuneL = 1.0;
float serong = 0.4;
-float rasio = 3.0;
-float t_new = 0.25;
+float rasio = 1.4545;
+float t_new = 0.1;
/* variable tunning */
float tunning_L;
@@ -102,9 +98,6 @@
float tunning_Dpn;
float tunning_Blk;
-/* Variabel Encoder Bawah */
-float errTetha, Tetha; // Variabel yang didapatkan encoder
-
/* Deklarasi Variable Millis */
unsigned long int previousMillis = 0; // PID launcher
unsigned long int currentMillis;
@@ -121,6 +114,7 @@
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = false;
+bool flag_paku = false;
bool ready = false;
/*****************************************************/
@@ -132,8 +126,6 @@
void aktuator(); // Pergerakan aktuator berdasarkan case joystick
int case_joystick(); // Mendapatkan case dari joystick
//void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick
-void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
-float getTetha(); // Fungsi mendapatkan error Tetha
/* Inisialisasi Pin TX-RX Joystik dan PC */
joysticknucleo joystick(PA_0,PA_1);
@@ -154,12 +146,13 @@
Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5);
/* Deklarasi Motor Launcher */
-Motor launcherDpn(PA_5,PB_12,PA_11); // pwm ,fwd, rev
+Motor launcherDpn(PA_5,PA_11,PB_12); // pwm ,fwd, rev
Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev
Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev
/* Deklarasi Penumatik Launcher */
DigitalOut pneumatik(PA_4, PullUp);
+DigitalOut pneu_paku(PC_2, PullUp);
/*Dekalrasi Limit Switch */
//DigitalIn infraAtas(PC_9, PullUp);
@@ -320,6 +313,10 @@
// Power Screw Down
caseJoystick = 12;
}
+ else if (joystick.L3){
+ // Paku Bumi ON/OFF
+ caseJoystick = 34;
+ }
else
{
tuneAksel = 0.6;
@@ -329,28 +326,6 @@
return(caseJoystick);
}
-float getTetha(){
-// Fungsi untuk mendapatkan nilai tetha
- float busur, tetha;
- busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
- tetha = busur/keliling_robot*360;
-
- return -(tetha);
-}
-
-float pidBase(float Kp, float Ki, float Kd)
-{
- int errorP;
- errorP = getTetha();
- if (errorP<3.5 && errorP>(-3.5))
- errorP = 0;
- return (float)Kp*errorP;
-}
-
-void setCenter(){
-// Fungsi untuk menentukan center dari robot
- encoderBase.reset();
-}
void init_rpm (){
if (target_rpm>maxRPM-2){
@@ -375,8 +350,8 @@
// Pivot Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT);
- motorL.speed(-rasio*PIVOT);
+ motorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
break;
}
case (2):
@@ -384,8 +359,8 @@
// Pivot Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT);
- motorL.speed(rasio*PIVOT);
+ motorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
break;
}
case (17):
@@ -393,8 +368,8 @@
// Kanan + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT);
- motorL.speed(-rasio*PIVOT);
+ motorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
break;
}
case (18):
@@ -402,8 +377,8 @@
// Kanan + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT);
- motorL.speed(rasio*PIVOT);
+ motorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
break;
}
case (19):
@@ -411,8 +386,8 @@
// Kiri + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT);
- motorL.speed(-rasio*PIVOT);
+ motorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
break;
}
case (20):
@@ -420,8 +395,8 @@
// Kiri + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT);
- motorL.speed(rasio*PIVOT);
+ motorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
break;
}
case (21):
@@ -429,8 +404,8 @@
// Maju + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT);
- motorL.speed(-rasio*PIVOT);
+ motorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
break;
}
case (22):
@@ -438,8 +413,8 @@
// Maju + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT);
- motorL.speed(rasio*PIVOT);
+ motorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
break;
}
case (23):
@@ -447,8 +422,8 @@
// Mundur + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT);
- motorL.speed(-rasio*PIVOT);
+ motorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
break;
}
case (24):
@@ -456,8 +431,8 @@
// Mundur + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT);
- motorL.speed(rasio*PIVOT);
+ motorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
break;
}
case (25):
@@ -569,8 +544,8 @@
if (aksel>300)
tuneAksel = 0.9;
- motorR.speed(tuneAksel+t_new);
- motorL.speed(-tuneAksel-t_new);
+ motorR.speed(tuneAksel);
+ motorL.speed(-tuneAksel);
motorDpn.brake(1);
motorBlk.brake(1);
break;
@@ -582,8 +557,8 @@
if (aksel>300)
tuneAksel = 0.9;
- motorR.speed(-tuneAksel-t_new);
- motorL.speed(tuneAksel+t_new);
+ motorR.speed(-tuneAksel);
+ motorL.speed(tuneAksel);
motorDpn.brake(1);
motorBlk.brake(1);
break;
@@ -633,8 +608,8 @@
case (31) :
{
// start
- target_rpm2 = 22;
- target_rpm = 22;
+ target_rpm2 = 23;
+ target_rpm = 23;
init_rpm();
break;
}
@@ -649,8 +624,8 @@
case (33) :
{
// R3
- target_rpm2 = 17;
- target_rpm = 17;
+ target_rpm2 = 18;
+ target_rpm = 18;
init_rpm();
break;
}
@@ -661,6 +636,15 @@
isReload = true;
break;
}
+ case (34) :{
+ pneu_paku = !pneu_paku;
+ wait_ms(50);
+ if (pneu_paku == 1){
+ PIVOT = 0.17;
+ }else{
+ PIVOT = 0.8;
+ }
+ }
default :
{
tuneAksel = 0.6;
@@ -678,6 +662,7 @@
if(ReloadOn){
if(isReload){
powerScrew.speed(pwmPowerDown);
+ pc.printf("%.2f\n", jarak_ping);
if((!limitBawah)||(!limitBawah1)){
isReload = false;
ReloadOn = false;
@@ -693,16 +678,12 @@
ping_sum_error += ping_current_error*ping_Ts;
ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;
- if (ping_pwm>1) ping_pwm=1;
- if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5;
- if (ping_pwm<-0.049 && ping_pwm>-0.5) ping_pwm = -0.5;
- if (ping_pwm<-1) ping_pwm=-1;
-
+ pc.printf("%.2f\n", jarak_ping);
powerScrew.speed(ping_pwm);
ping_previous_error1 = ping_current_error;
}
- if ((jarak_ping>(ping_target-3))&&(jarak_ping<(ping_target+3))){
+ if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
ready = true;
}else{
ready = false;
@@ -772,7 +753,7 @@
encLauncherDpn.reset();
previousMillis2 = currentMillis2;
}
- pc.printf("%.2f\t%.2f\n",rpm,rpm2);
+ //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
}
else
{
@@ -802,8 +783,6 @@
// initializing encoder
pneumatik =1;
- setCenter();
-
wait_ms(500);
//initializing PING