KRAI 2017
/
MainProgram_BaseBaru_fix_omni_11April
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni_5April_23-04
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 38:3ef6754bd8d8
- Parent:
- 37:67d54563af90
- Child:
- 39:11358f3f61ff
diff -r 67d54563af90 -r 3ef6754bd8d8 main.cpp
--- a/main.cpp Tue Feb 14 17:11:20 2017 +0000
+++ b/main.cpp Fri Feb 17 13:05:02 2017 +0000
@@ -1,7 +1,7 @@
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
-/* Last Update : 14 Februari 2017 */
+/* Last Update : 18 Februari 2017 */
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
@@ -54,18 +54,19 @@
#define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
// Variable Atas
-double speed, speed2, maxSpeed = 0.95, minSpeed = 0;
-double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
+double speed, speed2;
+const double maxSpeed = 0.95;
+const double kpA=0.6757, kdA=0.6757, 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, target_rpm = 8.0;
-float rpm2, target_rpm2 = 10.0;
-float maxRPM = 40, minRPM = 0;
-float pwmPowerUp = 0.75;
-float pwmPowerDown = -0.80;
-bool henti=false, hentis=false;
+float rpm, rpm2;
+float target_rpm = 12.0, target_rpm2 = 16.0;
+const float maxRPM = 27, minRPM = 0; // Limit 25 atau 27
+
+const float pwmPowerUp = 0.78;
+const float pwmPowerDown = -0.9;
// Variable Bawah
float Vt;
@@ -93,7 +94,6 @@
bool isLauncher = false;
bool isReload = false;
bool ReloadOn = false;
-int flagLauncher = 1;
bool flag_Pneu = false;
/*****************************************************/
@@ -170,26 +170,26 @@
caseJoystick = 4;
//pc.printf("kiri");
}
- else if (joystick.segitiga_click && flagLauncher){
+ else if (joystick.segitiga_click){
// Motor Launcher
caseJoystick = 5;
}
- else if (joystick.R2_click && (target_rpm2 < 40)){
+ else if (joystick.R2_click){
// Target Pulse PID ++ Motor Depan
caseJoystick = 6;
}
- else if (joystick.L2_click && (target_rpm2 > 1)){
+ else if (joystick.L2_click){
// Target Pulse PID -- Motor Depan
caseJoystick = 7;
}
- else if (joystick.R3_click && (target_rpm < 40 )){
+ /*else if (joystick.R3_click){
// Target Pulse PID ++ Motor Belakang
caseJoystick = 8;
}
- else if (joystick.L3_click && (target_rpm > 1)){
+ else if (joystick.L3_click){
// Target Pulse PID -- Motor Belakang
caseJoystick = 9;
- }
+ }*/
else if (joystick.silang_click){
// Pnemuatik ON
caseJoystick = 10;
@@ -227,7 +227,7 @@
errorP = 0;
return (float)Kp*errorP;
}
-
+/*
void init_speed (){
if (speed>maxSpeed){
speed = maxSpeed;
@@ -241,7 +241,7 @@
if (speed2<minSpeed){
speed2 = minSpeed;
}
-}
+}*/
void setCenter(){
// Fungsi untuk menentukan center dari robot
@@ -249,8 +249,8 @@
}
void init_rpm (){
- if (target_rpm>maxRPM){
- target_rpm = maxRPM;
+ if (target_rpm>maxRPM-2){
+ target_rpm = maxRPM-2;
}
if (target_rpm<minRPM){
target_rpm = minRPM;
@@ -258,8 +258,8 @@
if (target_rpm2>maxRPM){
target_rpm2 = maxRPM;
}
- if (target_rpm2<minRPM){
- target_rpm2 = minRPM;
+ if (target_rpm2<minRPM+2){
+ target_rpm2 = minRPM+2;
}
}
@@ -312,6 +312,7 @@
{
// Target Pulse PID ++ Motor Depan
target_rpm2 = target_rpm2++;
+ target_rpm = target_rpm++;
init_rpm();
break;
}
@@ -319,23 +320,22 @@
{
// Target Pulse PID -- Motor Depan
target_rpm2 = target_rpm2--;
+ target_rpm = target_rpm--;
init_rpm();
break;
}
- case (8) :
+ /*case (8) :
{
// Target Pulse PID ++ Motor Belakang=
- target_rpm = target_rpm++;
- init_rpm();
+ //init_rpm();
break;
}
case (9) :
{
// Target Pulse PID -- Motor Belakang
- target_rpm = target_rpm--;
- init_rpm();
+ //init_rpm();
break;
- }
+ }*/
case (10) :
{
// Pneumatik
@@ -414,33 +414,43 @@
currentMillis2 = millis();
// PID Launcher Depan
- if (currentMillis-previousMillis>=10)
+ if (currentMillis-previousMillis>=12.5)
{
rpm = (float)encLauncherBlk.getPulses();
current_error = target_rpm - rpm;
sum_error = sum_error + current_error;
p = current_error*kpA;
- d = (current_error-last_error)*kdA/10.0;
- i = sum_error*kiA*10.0;
+ d = (current_error-last_error)*kdA/12.5;
+ i = sum_error*kiA*12.5;
speed = p + d + i;
- init_speed();
- launcherBlk.speed(speed);
+ //init_speed();
+ if(speed > maxSpeed){
+ launcherBlk.speed(maxSpeed);
+ }
+ else {
+ launcherBlk.speed(speed);
+ }
last_error = current_error;
encLauncherBlk.reset();
//pc.printf("%.04lf\n",rpm);
previousMillis = currentMillis;
}
- if (currentMillis2-previousMillis2>=10)
+ if (currentMillis2-previousMillis2>=12.5)
{
rpm2 = (float)encLauncherDpn.getPulses();
current_error2 = target_rpm2 - rpm2;
sum_error2 = sum_error2 + current_error2;
p2 = current_error2*kpA;
- d2 = (current_error2-last_error2)*kdA/10.0;
- i2 = sum_error2*kiA*10.0;
+ d2 = (current_error2-last_error2)*kdA/12.5;
+ i2 = sum_error2*kiA*12.5;
speed2 = p2 + d2 + i2;
- init_speed();
- launcherDpn.speed(speed2);
+ //init_speed();
+ if (speed2 > maxSpeed){
+ launcherDpn.speed(maxSpeed);
+ }
+ else{
+ launcherDpn.speed(speed2);
+ }
last_error2 = current_error2;
encLauncherDpn.reset();
previousMillis2 = currentMillis2;
@@ -482,7 +492,7 @@
aktuator();
launcher();
reloader();
- if ((millis()-previousMillis3 >= 350)&&(flag_Pneu)){
+ if ((millis()-previousMillis3 >= 425)&&(flag_Pneu)){
pneumatik = 1;
flag_Pneu = false;
}