KRAI 2018
Source Code Robot Lama
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
DagonFly__RoadToJapan_15Mei_Ultimate
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 44:452c214d9cf5
- Parent:
- 43:3807a95aa284
- Child:
- 45:964ae71a30e3
--- a/main.cpp Sun Mar 12 06:56:53 2017 +0000
+++ b/main.cpp Wed Apr 05 12:54:03 2017 +0000
@@ -25,15 +25,14 @@
/* Kanan => */
/* Kiri => */
/* */
-/* Tombol silang => Power Screw Aktif */
+/* Tombol silang => Pneumatik aktif */
/* Tombol segitiga => Aktif motor Launcher */
-/* Tombol lingkaran => Aktif Pneumatik Launcher */
+/* Tombol lingkaran => Reloader naik */
+/* Tombol kotak => Reloader turun */
/* Tombol L1 => Pivot Kiri */
/* Tombol R1 => Pivot Kanan */
-/* Tombol L3 => PWM Motor Belakang Dikurangi */
-/* Tombol R3 => PWM Motor Belakang Ditambah */
-/* Tombol L2 => PWM Motor Depan Dikurangi */
-/* Tombol R2 => PWM Motor Depan Ditambah */
+/* Tombol L2 => Kurang PWM Motor Launcher */
+/* Tombol R2 => Tambah PWM Motor Launcher */
/* */
/* Bismillahirahmanirrahim */
/* Jagalah Kebersihan Kodingan */
@@ -53,22 +52,21 @@
#define PI 3.14159265
#define D_ENCODER 10 // Diameter Roda Encoder
#define D_ROBOT 80 // Diameter Roda Robot
-#define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
// Variable Atas
double speed, speed2;
const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
+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;
float rpm, rpm2;
-float target_rpm = 8.0, target_rpm2 = 10.0;
+float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
-const float pwmPowerUp = 0.8;
-const float pwmPowerDown = -0.9;
+const float pwmPowerUp = 1.0;
+const float pwmPowerDown = -1.0;
float jarak_ping=0;
@@ -77,13 +75,12 @@
float keliling_enc = PI*D_ENCODER;
float keliling_robot = PI*D_ROBOT;
float speedT = 0.2;
-float vpid = 0;
-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 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 tuneR = 0.78;
float tuneL = 0.72;
-float serong = 0.65;
+float serong = 0.4;
float rasio = 1.4545;
/* variable tunning */
@@ -111,6 +108,7 @@
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = false;
+bool ready = false;
/*****************************************************/
/* Definisi Prosedur, Fungsi dan Setting Pinout */
@@ -132,33 +130,35 @@
encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
/* Deklarasi Encoder Launcher */
-encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherBlk( PC_12, PD_2, 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);
-Motor motorL (PA_11, PA_6, PA_7);
-Motor motorR (PB_7, PA_14, PA_15);
+Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
+//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);
/* 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(PA_9, PA_4, PC_15); // pwm, fwd, rev
+Motor launcherDpn(PA_5,PB_12,PA_11); // 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(PB_3, PullUp);
+DigitalOut pneumatik(PA_4, PullUp);
/*Dekalrasi Limit Switch */
//DigitalIn infraAtas(PC_9, PullUp);
-DigitalIn limitTengah(PB_10, PullUp);
-DigitalIn limitBawah(PC_8, PullUp);
+DigitalIn limitTengah(PA_9, PullUp);
+DigitalIn limitBawah(PC_7, PullUp);
+DigitalIn limitBawah1(PA_7, PullUp);
/*deklarasi PING ultrasonic*/
-Ping pingAtas(PC_9);
+Ping pingAtas(PC_15);
/*Deklarasi Display*/
-DigitDisplay display (D15, D4);
+DigitDisplay display (PA_8, PC_8);
/****************************************************/
/* Deklarasi Fungsi dan Procedure */
@@ -183,6 +183,18 @@
// Pivot Kiri
caseJoystick = 2;
}
+ else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
+ // tambah rpm dengan nilai tertentu
+ caseJoystick = 31;
+ }
+ else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
+ // kurangi rpm dengan nilai tertentu
+ caseJoystick = 32;
+ }
+ else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
+ // kurangi rpm dengan nilai tertentu
+ caseJoystick = 33;
+ }
else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan + Rotasi kanan
caseJoystick = 17;
@@ -241,43 +253,35 @@
}
else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Serong kanan maju
- caseJoystick = 13;
- //pc.printf("bawah");
+ caseJoystick = 13;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Serong kiri maju
- caseJoystick = 14;
- //pc.printf("bawah");
+ caseJoystick = 14;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Serong kanan mundur
- caseJoystick = 15;
- //pc.printf("bawah");
+ caseJoystick = 15;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Serong kiri mundur
- caseJoystick = 16;
- //pc.printf("bawah");
+ caseJoystick = 16;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
caseJoystick = 3;
- //pc.printf("kanan");
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// 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");
+ caseJoystick = 8;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Bawah -- Mundur
- caseJoystick = 9;
- //pc.printf("bawah");
+ caseJoystick = 9;
}
else if (joystick.segitiga_click){
// Motor Launcher
@@ -482,35 +486,36 @@
{
// Serong kanan maju
motorDpn.speed(-serong);
- motorL.brake(-rasio*serong);
+ motorL.speed(-serong);
motorBlk.speed(serong);
- motorR.brake(rasio*serong);
+ motorR.speed(serong);
break;
}
case (14) :
{
// Serong kiri maju
motorDpn.speed(serong);
- motorR.brake(rasio*serong);
+ motorR.speed(serong);
motorBlk.speed(-serong);
- motorL.brake(-rasio*serong);
+ motorL.speed(-serong);
break;
}
case (15) :
{
// Serong kanan mundur
- motorR.brake(-rasio*serong);
+ motorDpn.speed(-serong);
+ motorR.speed(-serong);
motorBlk.speed(serong);
- motorL.brake(rasio*serong);
+ motorL.speed(serong);
break;
}
case (16) :
{
// Serong kiri mundur
motorDpn.speed(serong);
- motorL.brake(rasio*serong);
+ motorL.speed(serong);
motorBlk.speed(-serong);
- motorR.brake(-rasio*serong);
+ motorR.speed(-serong);
break;
}
case (3) :
@@ -534,7 +539,6 @@
case (8) :
{
// Maju
- //init_rpm();
motorR.speed(tuneR);
motorL.speed(-tuneL);
motorDpn.brake(1);
@@ -544,7 +548,6 @@
case (9) :
{
// Mundur
- //init_rpm();
motorR.speed(-tuneR);
motorL.speed(tuneL);
motorDpn.brake(1);
@@ -576,9 +579,13 @@
case (10) :
{
// Pneumatik
- pneumatik = 0;
- previousMillis3 = millis();
- flag_Pneu = true;
+ if (ready)
+ {
+ pneumatik = 0;
+ previousMillis3 = millis();
+ flag_Pneu = true;
+ ready = false;
+ }
break;
}
case (11) :
@@ -588,6 +595,30 @@
isReload = false;
break;
}
+ case (31) :
+ {
+ // start
+ target_rpm2 = 22;
+ target_rpm = 22;
+ init_rpm();
+ break;
+ }
+ case (32) :
+ {
+ // select
+ target_rpm2 = 10;
+ target_rpm = 10;
+ init_rpm();
+ break;
+ }
+ case (33) :
+ {
+ // R3
+ target_rpm2 = 17;
+ target_rpm = 17;
+ init_rpm();
+ break;
+ }
case (12) :
{
// Power Screw Down
@@ -610,7 +641,7 @@
if(ReloadOn){
if(isReload){
powerScrew.speed(pwmPowerDown);
- if(!limitBawah){
+ if((!limitBawah)||(!limitBawah1)){
isReload = false;
ReloadOn = false;
}
@@ -620,12 +651,15 @@
}
else if((jarak_ping > 6.5) && !flag_Pneu){
powerScrew.speed(pwmPowerUp);
+ ready = false;
}
- else if((jarak_ping < 6 ) && !flag_Pneu) {
- powerScrew.speed(-0.1);
+ else if((jarak_ping < 6.0) && !flag_Pneu) {
+ powerScrew.speed(-0.85);
+ ready = false;
}
else{
powerScrew.brake(1);
+ ready = true;
}
}
else{
@@ -646,10 +680,10 @@
{
rpm = (float)encLauncherBlk.getPulses();
current_error = target_rpm - rpm;
- sum_error = sum_error + current_error;
+ sum_error = sum_error + current_error*12.5;
p = current_error*kpA;
d = (current_error-last_error)*kdA/12.5;
- i = sum_error*kiA*12.5;
+ i = sum_error*kiA;
speed = p + d + i;
//init_speed();
if(speed > maxSpeed){
@@ -670,10 +704,10 @@
{
rpm2 = (float)encLauncherDpn.getPulses();
current_error2 = target_rpm2 - rpm2;
- sum_error2 = sum_error2 + current_error2;
+ sum_error2 = sum_error2 + current_error2*12.5;
p2 = current_error2*kpA;
d2 = (current_error2-last_error2)*kdA/12.5;
- i2 = sum_error2*kiA*12.5;
+ i2 = sum_error2*kiA;
speed2 = p2 + d2 + i2;
//init_speed();
if (speed2 > maxSpeed){
@@ -689,11 +723,18 @@
encLauncherDpn.reset();
previousMillis2 = currentMillis2;
}
+ pc.printf("%.2f\t%.2f\n",rpm,rpm2);
}
else
{
launcherDpn.brake(1);
launcherBlk.brake(1);
+ sum_error = 0;
+ sum_error2 = 0;
+ current_error = 0;
+ current_error2 = 0;
+ last_error = 0;
+ last_error2 = 0;
}
}
@@ -740,7 +781,7 @@
joystick.olah_data();
// Masuk ke case joystick
case_joy = case_joystick();
- pc.printf("%d\n",case_joy);
+ //pc.printf("%d\n",case_joy);
aktuator();
launcher();
reloader();
@@ -754,13 +795,8 @@
joystick.idle();
}
- if(millis() - previousMillis5 >= 400){
- //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);
-
+ if(millis() - previousMillis5 >= 400)
+ {
display.write(0,((int) rpm2) / 10);
display.write(1,((int)rpm2) % 10);
display.write(2, (int)target_rpm2 / 10);