KRAI 2017
Fix pisan inimah plis jangan revisi ultimate mantep
Dependencies: DigitDisplay Motor PID mbed millis
Fork of
DagonFly__RoadToJapan_15Mei
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 43:3807a95aa284
- Parent:
- 42:6caf8bd5abbc
- Child:
- 44:452c214d9cf5
--- a/main.cpp Mon Feb 20 13:29:52 2017 +0000
+++ b/main.cpp Sun Mar 12 06:56:53 2017 +0000
@@ -1,7 +1,8 @@
+
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
-/* Last Update : 18 Februari 2017 */
+/* Last Update : 11 Maret 2017 */
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
@@ -9,8 +10,8 @@
/* / \ Rode Depan Belakang: */
/* / 2 (Belakang) \ Omniwheel */
/* | | */
-/* | 3 (Encoder) 4 | Roda Kiri Kanan: */
-/* | | Fixed Wheel */
+/* | 3 (kiri) 4 (kanan) | Roda Kiri Kanan: */
+/* | | Omniwheel */
/* \ 1 (Depan) / */
/* \______________________/ Putaran berlawanan arah */
/* jarum jam positif */
@@ -57,13 +58,13 @@
// Variable Atas
double speed, speed2;
const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
+const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
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;
float rpm, rpm2;
-float target_rpm = 15.0, target_rpm2 = 17.0;
+float target_rpm = 8.0, target_rpm2 = 10.0;
const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
const float pwmPowerUp = 0.8;
@@ -77,9 +78,19 @@
float keliling_robot = PI*D_ROBOT;
float speedT = 0.2;
float vpid = 0;
-float PIVOT = 0.27; // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn = 0.35; // Tunning PWM motor Depan
-float tuneBlk = 0.3; // Tunning PWM motor belakang
+float PIVOT = 0.2; // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn = 0.62; // Tunning PWM motor Depan
+float tuneBlk = 0.62; // Tunning PWM motor belakang
+float tuneR = 0.72;
+float tuneL = 0.72;
+float serong = 0.65;
+float rasio = 1.4545;
+
+/* variable tunning */
+float tunning_L;
+float tunning_R;
+float tunning_Dpn;
+float tunning_Blk;
/* Variabel Encoder Bawah */
float errTetha, Tetha; // Variabel yang didapatkan encoder
@@ -125,13 +136,15 @@
encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
/* Deklarasi Motor Base */
-Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev
-Motor motorBlk(PB_6, PB_1, PB_12); // pwm, fwd, rev
+Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev
+Motor motorBlk(PB_6, PB_1, PB_12);
+Motor motorL (PA_11, PA_6, PA_7);
+Motor motorR (PB_7, PA_14, PA_15);
/* Deklarasi Motor Launcher */
Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev
Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev
-Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev
+Motor powerScrew(PA_9, PA_4, PC_15); // pwm, fwd, rev
/* Deklarasi Penumatik Launcher */
DigitalOut pneumatik(PB_3, PullUp);
@@ -162,13 +175,89 @@
//---------------------------------------------------//
int caseJoystick;
- if (!joystick.L1 && joystick.R1) {
+ if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Pivot Kanan
caseJoystick = 1;
}
- else if (!joystick.R1 && joystick.L1) {
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Pivot Kiri
caseJoystick = 2;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan + Rotasi kanan
+ caseJoystick = 17;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan + Rotasi kiri
+ caseJoystick = 18;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri + Rotasi kanan
+ caseJoystick = 19;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kanan + Rotasi kiri
+ caseJoystick = 20;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju + Rotasi kanan
+ caseJoystick = 21;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju + Rotasi kiri
+ caseJoystick = 22;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur + Rotasi kanan
+ caseJoystick = 23;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur + Rotasi kiri
+ caseJoystick = 24;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) {
+ // Kanan + segitiga
+ caseJoystick = 25;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) {
+ // Kiri + segitiga
+ caseJoystick = 26;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) {
+ // Kanan + lingkaran
+ caseJoystick = 27;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) {
+ // Kiri + lingkaran
+ caseJoystick = 28;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) {
+ // Kanan + kotak
+ caseJoystick = 29;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) {
+ // Kiri + kotak
+ caseJoystick = 30;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Serong kanan maju
+ caseJoystick = 13;
+ //pc.printf("bawah");
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Serong kiri maju
+ caseJoystick = 14;
+ //pc.printf("bawah");
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Serong kanan mundur
+ caseJoystick = 15;
+ //pc.printf("bawah");
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Serong kiri mundur
+ caseJoystick = 16;
+ //pc.printf("bawah");
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
@@ -179,6 +268,16 @@
// Kiri
caseJoystick = 4;
//pc.printf("kiri");
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Atas -- Maju
+ caseJoystick = 8;
+ //pc.printf("atas");
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Bawah -- Mundur
+ caseJoystick = 9;
+ //pc.printf("bawah");
}
else if (joystick.segitiga_click){
// Motor Launcher
@@ -189,33 +288,21 @@
caseJoystick = 6;
}
else if (joystick.L2_click){
- // Target Pulse PID -- Motor Depan
+ // Target Pulse PID -- Motor
caseJoystick = 7;
}
- /*else if (joystick.R3_click){
- // Target Pulse PID ++ Motor Belakang
- caseJoystick = 8;
- }
- else if (joystick.L3_click){
- // Target Pulse PID -- Motor Belakang
- caseJoystick = 9;
- }*/
else if (joystick.silang_click){
// Pnemuatik ON
caseJoystick = 10;
}
- else if ((joystick.atas)&&(!joystick.bawah)) {
+ else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
// Power Screw Up
caseJoystick = 11;
}
- else if ((!joystick.atas)&&(joystick.bawah)) {
+ else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
// Power Screw Down
caseJoystick = 12;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
- // Break
- caseJoystick = 13;
- }
return(caseJoystick);
}
@@ -266,7 +353,8 @@
// Pivot Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- setCenter();
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
break;
}
case (2):
@@ -274,27 +362,193 @@
// Pivot Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- setCenter();
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
+ break;
+ }
+ case (17):
+ {
+ // Kanan + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
+ break;
+ }
+ case (18):
+ {
+ // Kanan + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
+ break;
+ }
+ case (19):
+ {
+ // Kiri + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
+ break;
+ }
+ case (20):
+ {
+ // Kiri + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
+ break;
+ }
+ case (21):
+ {
+ // Maju + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
+ break;
+ }
+ case (22):
+ {
+ // Maju + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
+ break;
+ }
+ case (23):
+ {
+ // Mundur + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
+ break;
+ }
+ case (24):
+ {
+ // Mundur + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
+ break;
+ }
+ case (25):
+ {
+ // Kanan + segitiga
+ isLauncher = !isLauncher;
+ break;
+ }
+ case (26):
+ {
+ // Kiri + segitiga
+ isLauncher = !isLauncher;
+ break;
+ }
+ case (27):
+ {
+ // Kanan + lingkaran
+ ReloadOn = !ReloadOn;
+ isReload = false;
+ break;
+ }
+ case (28):
+ {
+ // Kiri + lingkaran
+ ReloadOn = !ReloadOn;
+ isReload = false;
+ break;
+ }
+ case (29):
+ {
+ // Kanan + kotak
+ ReloadOn = !ReloadOn;
+ isReload = true;
+ break;
+ }
+ case (30):
+ {
+ // Kiri + kotak
+ ReloadOn = !ReloadOn;
+ isReload = true;
+ break;
+ }
+ case (13) :
+ {
+ // Serong kanan maju
+ motorDpn.speed(-serong);
+ motorL.brake(-rasio*serong);
+ motorBlk.speed(serong);
+ motorR.brake(rasio*serong);
+ break;
+ }
+ case (14) :
+ {
+ // Serong kiri maju
+ motorDpn.speed(serong);
+ motorR.brake(rasio*serong);
+ motorBlk.speed(-serong);
+ motorL.brake(-rasio*serong);
+ break;
+ }
+ case (15) :
+ {
+ // Serong kanan mundur
+ motorR.brake(-rasio*serong);
+ motorBlk.speed(serong);
+ motorL.brake(rasio*serong);
+ break;
+ }
+ case (16) :
+ {
+ // Serong kiri mundur
+ motorDpn.speed(serong);
+ motorL.brake(rasio*serong);
+ motorBlk.speed(-serong);
+ motorR.brake(-rasio*serong);
break;
}
case (3) :
{
// Kanan
- motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
- motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
- //speedDpn = tuneDpn + pidBase(0.009,0,0)
- //speedBlk = tuneBlk + pidBase(0.009,0,0)
- //motorDpn.speed(-tuneDpn);
- //motorBlk.speed(tuneBlk);
+ motorDpn.speed(-tuneDpn);
+ motorBlk.speed(tuneBlk);
+ motorR.brake(1);
+ motorL.brake(1);
break;
}
case (4) :
{
// Kiri
- motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
- motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
- //motorDpn.speed(tuneDpn);
- //motorBlk.speed(-tuneBlk);
+ motorDpn.speed(tuneDpn);
+ motorBlk.speed(-tuneBlk);
+ motorR.brake(1);
+ motorL.brake(1);
+ break;
+ }
+ case (8) :
+ {
+ // Maju
+ //init_rpm();
+ motorR.speed(tuneR);
+ motorL.speed(-tuneL);
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ break;
+ }
+ case (9) :
+ {
+ // Mundur
+ //init_rpm();
+ motorR.speed(-tuneR);
+ motorL.speed(tuneL);
+ motorDpn.brake(1);
+ motorBlk.brake(1);
break;
}
case (5) :
@@ -319,18 +573,6 @@
init_rpm();
break;
}
- /*case (8) :
- {
- // Target Pulse PID ++ Motor Belakang=
- //init_rpm();
- break;
- }
- case (9) :
- {
- // Target Pulse PID -- Motor Belakang
- //init_rpm();
- break;
- }*/
case (10) :
{
// Pneumatik
@@ -342,34 +584,23 @@
case (11) :
{
// Power Screw Up
- //powerScrew.speed(pwmPowerUp);
ReloadOn = !ReloadOn;
- //powerScrew.speed(pwmPowerUp);
+ isReload = false;
break;
}
case (12) :
{
// Power Screw Down
- //powerScrew.speed(pwmPowerDown);
+ ReloadOn = !ReloadOn;
+ isReload = true;
break;
}
default :
{
motorDpn.brake(1);
motorBlk.brake(1);
- /* if(isReload){
- powerScrew.speed(pwmPowerDown);
- if(!limitBawah){
- isReload = false;
- ReloadOn = false;
- }
- }
- else if(!limitTengah){
- isReload = true;
- }
- else{
- powerScrew.brake(1);
- }*/
+ motorR.brake(1);
+ motorL.brake(1);
}
} // End Switch
}
@@ -387,10 +618,10 @@
else if(!limitTengah){
isReload = true;
}
- else if((jarak_ping > 4) && !flag_Pneu){
+ else if((jarak_ping > 6.5) && !flag_Pneu){
powerScrew.speed(pwmPowerUp);
}
- else if((jarak_ping < 3.5 ) && !flag_Pneu) {
+ else if((jarak_ping < 6 ) && !flag_Pneu) {
powerScrew.speed(-0.1);
}
else{
@@ -509,10 +740,11 @@
joystick.olah_data();
// Masuk ke case joystick
case_joy = case_joystick();
+ pc.printf("%d\n",case_joy);
aktuator();
launcher();
reloader();
- if ((millis()-previousMillis3 >= 370)&&(flag_Pneu)){
+ if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
pneumatik = 1;
flag_Pneu = false;
}
@@ -523,8 +755,14 @@
}
if(millis() - previousMillis5 >= 400){
- display.write(0,((int) target_rpm2-2) / 10);
- display.write(1,((int)target_rpm2-2) % 10);
+ //display.write(0,((int) target_rpm2-2) / 10);
+ //display.write(1,((int)target_rpm2-2) % 10);
+ //display.write(2, (int)target_rpm2 / 10);
+ //display.write(3, (int)target_rpm2 % 10);
+ //display.setColon(true);
+
+ display.write(0,((int) rpm2) / 10);
+ display.write(1,((int)rpm2) % 10);
display.write(2, (int)target_rpm2 / 10);
display.write(3, (int)target_rpm2 % 10);
display.setColon(true);