KRAI 2017
dear chio dan madi dan calman dan josh
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
DagonFly__RoadToJapan_13Mei_a
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 46:85169ae8659b
- Parent:
- 45:964ae71a30e3
- Child:
- 47:6cac4f1a3c8e
--- a/main.cpp Wed Apr 05 16:04:08 2017 +0000
+++ b/main.cpp Mon Apr 24 14:12:08 2017 +0000
@@ -2,7 +2,7 @@
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
-/* Last Update : 11 Maret 2017 */
+/* Last Update : 16 April 2017 */
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
@@ -53,38 +53,48 @@
#define D_ENCODER 10 // Diameter Roda Encoder
#define D_ROBOT 80 // Diameter Roda Robot
-// Variable Atas
+// Variable Atas
+// indek 2 untuk motor depan, 1 untuk motor belakang
double speed, speed2;
-const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
-double p,i,d;
-double p2,i2,d2;
-double last_error = 0, current_error, sum_error = 0;
-double last_error2 = 0, current_error2, sum_error2 = 0;
+const double maxSpeed = 0.95, minSpeed = 0.0, 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;
+double a1,b1,c1;
+double a2,b2,c2;
+double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
+double current_error2, previous_error2_1 = 0, previous_error2_2 = 0;
+double previous_speed1 = 0;
+double previous_speed2 = 0;
+
float rpm, rpm2;
-float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
-const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
+double target_rpm = 17.0, target_rpm2 = 17.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;
-float jarak_ping=0;
-
+double jarak_ping=0;
+double ping_target = 12;
+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_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 = 0.80; // Tunning PWM motor Depan
-float tuneBlk = 0.80; // Tunning PWM motor belakang
+float tuneDpn = 1.0; // Tunning PWM motor Depan
+float tuneBlk = 1.0; // Tunning PWM motor belakang
float tuneAksel = 0.6;
int aksel = 0;
float tuneAkselBlk = 0.4;
-float tuneR = 0.78;
-float tuneL = 0.72;
+float tuneR = 1.0;
+float tuneL = 1.0;
float serong = 0.4;
-float rasio = 1.4545;
+float rasio = 3.0;
+float t_new = 0.25;
/* variable tunning */
float tunning_L;
@@ -141,7 +151,7 @@
//Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13);
Motor motorBlk(PB_2, PB_15, PB_1);
Motor motorL (PB_9, PA_12, PA_6);
-Motor motorR (PB_8, PC_5, PC_6); //(PC_6, PB_4, PB_5);
+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
@@ -494,36 +504,36 @@
{
// Serong kanan maju
motorDpn.speed(-serong);
- motorL.speed(-serong);
+ motorL.speed(-serong-t_new);
motorBlk.speed(serong);
- motorR.speed(serong);
+ motorR.speed(serong+t_new);
break;
}
case (14) :
{
// Serong kiri maju
motorDpn.speed(serong);
- motorR.speed(serong);
+ motorR.speed(serong+t_new);
motorBlk.speed(-serong);
- motorL.speed(-serong);
+ motorL.speed(-serong-t_new);
break;
}
case (15) :
{
// Serong kanan mundur
motorDpn.speed(-serong);
- motorR.speed(-serong);
+ motorR.speed(-serong-t_new);
motorBlk.speed(serong);
- motorL.speed(serong);
+ motorL.speed(serong+t_new);
break;
}
case (16) :
{
// Serong kiri mundur
motorDpn.speed(serong);
- motorL.speed(serong);
+ motorL.speed(serong+t_new);
motorBlk.speed(-serong);
- motorR.speed(-serong);
+ motorR.speed(-serong-t_new);
break;
}
case (3) :
@@ -559,8 +569,8 @@
if (aksel>300)
tuneAksel = 0.9;
- motorR.speed(tuneAksel);
- motorL.speed(-tuneAksel);
+ motorR.speed(tuneAksel+t_new);
+ motorL.speed(-tuneAksel-t_new);
motorDpn.brake(1);
motorBlk.brake(1);
break;
@@ -572,8 +582,8 @@
if (aksel>300)
tuneAksel = 0.9;
- motorR.speed(-tuneAksel);
- motorL.speed(tuneAksel);
+ motorR.speed(-tuneAksel-t_new);
+ motorL.speed(tuneAksel+t_new);
motorDpn.brake(1);
motorBlk.brake(1);
break;
@@ -631,8 +641,8 @@
case (32) :
{
// select
- target_rpm2 = 10;
- target_rpm = 10;
+ target_rpm2 = 11;
+ target_rpm = 11;
init_rpm();
break;
}
@@ -676,18 +686,27 @@
else if(!limitTengah){
isReload = true;
}
- else if((jarak_ping > 6.5) && !flag_Pneu){
- powerScrew.speed(pwmPowerUp);
- ready = false;
+ else if(!flag_Pneu){
+ //pc.printf("%.2f\n", ping_pwm);
+ ping_current_error = (double) (ping_target-jarak_ping);
+
+ 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;
+
+ powerScrew.speed(ping_pwm);
+
+ ping_previous_error1 = ping_current_error;
}
- else if((jarak_ping < 6.0) && !flag_Pneu) {
- powerScrew.speed(-0.85);
+ if ((jarak_ping>(ping_target-3))&&(jarak_ping<(ping_target+3))){
+ ready = true;
+ }else{
ready = false;
}
- else{
- powerScrew.brake(1);
- ready = true;
- }
}
else{
powerScrew.brake(1);
@@ -702,16 +721,15 @@
currentMillis = millis();
currentMillis2 = millis();
- // PID Launcher Depan
- if (currentMillis-previousMillis>=12.5)
+ // PID Launcher Belakang
+ if (currentMillis-previousMillis>=Ts)
{
rpm = (float)encLauncherBlk.getPulses();
- current_error = target_rpm - rpm;
- sum_error = sum_error + current_error*12.5;
- p = current_error*kpA;
- d = (current_error-last_error)*kdA/12.5;
- i = sum_error*kiA;
- speed = p + d + i;
+ current_error1 = target_rpm - rpm;
+ a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts;
+ b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts;
+ c1 = kdA1/Ts;
+ speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2;
//init_speed();
if(speed > maxSpeed){
launcherBlk.speed(maxSpeed);
@@ -722,31 +740,35 @@
else {
launcherBlk.speed(speed);
}
- last_error = current_error;
+ previous_speed1 = speed;
+ previous_error1_2 = previous_error1_1;
+ previous_error1_1 = current_error1;
encLauncherBlk.reset();
- //pc.printf("%.04lf\n",rpm);
previousMillis = currentMillis;
+
}
- if (currentMillis2-previousMillis2>=12.5)
+ // PID Launcher Depan
+ if (currentMillis2-previousMillis2>=Ts)
{
rpm2 = (float)encLauncherDpn.getPulses();
current_error2 = target_rpm2 - rpm2;
- sum_error2 = sum_error2 + current_error2*12.5;
- p2 = current_error2*kpA;
- d2 = (current_error2-last_error2)*kdA/12.5;
- i2 = sum_error2*kiA;
- speed2 = p2 + d2 + i2;
+ a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts;
+ b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts;
+ c2 = kdA2/Ts;
+ speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2;
//init_speed();
if (speed2 > maxSpeed){
launcherDpn.speed(maxSpeed);
}
- else if ( speed < minSpeed){
+ else if ( speed2 < minSpeed){
launcherDpn.speed(minSpeed);
}
else{
launcherDpn.speed(speed2);
}
- last_error2 = current_error2;
+ previous_speed2 = speed2;
+ previous_error2_2 = previous_error2_1;
+ previous_error2_1 = current_error2;
encLauncherDpn.reset();
previousMillis2 = currentMillis2;
}
@@ -756,12 +778,13 @@
{
launcherDpn.brake(1);
launcherBlk.brake(1);
- sum_error = 0;
- sum_error2 = 0;
- current_error = 0;
- current_error2 = 0;
- last_error = 0;
- last_error2 = 0;
+
+ previous_error1_1 = 0;
+ previous_error1_2 = 0;
+ previous_error2_1 = 0;
+ previous_error2_2 = 0;
+ previous_speed1 = 0;
+ previous_speed2 = 0;
}
}
@@ -791,8 +814,8 @@
while(1)
{
// interupsi pembacaan PING setiap 30 ms
- if(millis() - previousMillis4 >= 5){ //30
- jarak_ping = (float)pingAtas.Read_cm()/2;
+ if(millis() - previousMillis4 >= 10){ //30
+ jarak_ping = (float)pingAtas.Read_cm();
pingAtas.Send();
previousMillis4 = millis();