Rahmadi S. Pandia
board baru, pin baru
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni_25Mar
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 44:e23f6d8586c6
- Parent:
- 43:3807a95aa284
- Child:
- 45:8af41da7fcd6
--- a/main.cpp Sun Mar 12 06:56:53 2017 +0000
+++ b/main.cpp Sun Mar 26 03:32:56 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,21 +52,20 @@
#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 = 16.0, target_rpm2 = 16.0; // selisih 4 bagus, sama bagus
const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
-const float pwmPowerUp = 0.8;
+const float pwmPowerUp = 0.9;
const float pwmPowerDown = -0.9;
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 PIVOT = 0.17; // 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 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,18 +130,18 @@
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( 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);
-Motor motorL (PA_11, PA_6, PA_7);
-Motor motorR (PB_7, PA_14, PA_15);
+Motor motorDpn(PC_7, PC_13, PC_14); //(PB_9, PA_12, PC_5);
+Motor motorBlk(PB_9, PC_5, PA_12); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13);
+Motor motorL (PA_11, PA_6, PA_7);
+Motor motorR (PC_6, PB_5, PB_4); //(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 launcherBlk(PB_7, PA_14, PA_15); // pwm, fwd, rev
Motor powerScrew(PA_9, PA_4, PC_15); // pwm, fwd, rev
/* Deklarasi Penumatik Launcher */
@@ -153,6 +151,7 @@
//DigitalIn infraAtas(PC_9, PullUp);
DigitalIn limitTengah(PB_10, PullUp);
DigitalIn limitBawah(PC_8, PullUp);
+DigitalIn limitBawah1(PA_5, PullUp);
/*deklarasi PING ultrasonic*/
Ping pingAtas(PC_9);
@@ -241,43 +240,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 +473,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 +526,6 @@
case (8) :
{
// Maju
- //init_rpm();
motorR.speed(tuneR);
motorL.speed(-tuneL);
motorDpn.brake(1);
@@ -544,7 +535,6 @@
case (9) :
{
// Mundur
- //init_rpm();
motorR.speed(-tuneR);
motorL.speed(tuneL);
motorDpn.brake(1);
@@ -576,9 +566,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) :
@@ -610,7 +604,7 @@
if(ReloadOn){
if(isReload){
powerScrew.speed(pwmPowerDown);
- if(!limitBawah){
+ if((!limitBawah)||(limitBawah1)){
isReload = false;
ReloadOn = false;
}
@@ -620,12 +614,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.4);
+ ready = false;
}
else{
powerScrew.brake(1);
+ ready = true;
}
}
else{
@@ -694,6 +691,12 @@
{
launcherDpn.brake(1);
launcherBlk.brake(1);
+ sum_error = 0;
+ sum_error2 = 0;
+ current_error = 0;
+ current_error2 = 0;
+ last_error = 0;
+ last_error2 = 0;
}
}
@@ -754,13 +757,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);