KRAI 2017
terbaru
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_20Feb_malam
by 
KRAI 2017
Revision 43:5e347df94a26, committed 2017-03-09
- Comitter:
- Najib_irvani
- Date:
- Thu Mar 09 13:06:30 2017 +0000
- Parent:
- 42:6caf8bd5abbc
- Commit message:
- terbaru
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Feb 20 13:29:52 2017 +0000
+++ b/main.cpp Thu Mar 09 13:06:30 2017 +0000
@@ -1,7 +1,7 @@
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
-/* Last Update : 18 Februari 2017 */
+/* Last Update : 8 Maret 2017 */
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
@@ -57,7 +57,7 @@
// 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;
@@ -77,9 +77,9 @@
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.45; // Tunning PWM motor Depan
+float tuneBlk = 0.37; // Tunning PWM motor belakang
/* Variabel Encoder Bawah */
float errTetha, Tetha; // Variabel yang didapatkan encoder
@@ -189,17 +189,25 @@
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.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // kiri + pivot kiri
+ caseJoystick = 14;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // kiri + pivot kanan
+ caseJoystick = 15;
}
- else if (joystick.L3_click){
- // Target Pulse PID -- Motor Belakang
- caseJoystick = 9;
- }*/
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // kanan + pivot kiri
+ caseJoystick = 16;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // kanan + pivot kanan
+ caseJoystick = 17;
+ }
else if (joystick.silang_click){
// Pnemuatik ON
caseJoystick = 10;
@@ -280,21 +288,21 @@
case (3) :
{
// Kanan
- motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
- motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
+ //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);
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 + pidBase(0.009,0,0));
+ //motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
+ motorDpn.speed(tuneDpn+0.02);
+ motorBlk.speed(-tuneBlk);
break;
}
case (5) :
@@ -353,6 +361,34 @@
//powerScrew.speed(pwmPowerDown);
break;
}
+ case (14) :
+ {
+ // kiri + piv kiri
+ motorDpn.speed(tuneDpn+0.02);
+ motorBlk.brake(1);
+ break;
+ }
+ case (15) :
+ {
+ // kiri + pivkanan
+ motorDpn.brake(1);
+ motorBlk.speed(-tuneBlk);
+ break;
+ }
+ case (16) :
+ {
+ // kanan + pivkiri
+ motorDpn.brake(1);
+ motorBlk.speed(tuneBlk);
+ break;
+ }
+ case (17) :
+ {
+ // kanan + pivkanan
+ motorDpn.speed(-tuneDpn);
+ motorBlk.brake(1);
+ break;
+ }
default :
{
motorDpn.brake(1);
@@ -387,10 +423,10 @@
else if(!limitTengah){
isReload = true;
}
- else if((jarak_ping > 4) && !flag_Pneu){
+ else if((jarak_ping > 3.8) && !flag_Pneu){
powerScrew.speed(pwmPowerUp);
}
- else if((jarak_ping < 3.5 ) && !flag_Pneu) {
+ else if((jarak_ping < 3.3 ) && !flag_Pneu) {
powerScrew.speed(-0.1);
}
else{
@@ -512,7 +548,7 @@
aktuator();
launcher();
reloader();
- if ((millis()-previousMillis3 >= 370)&&(flag_Pneu)){
+ if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
pneumatik = 1;
flag_Pneu = false;
}
@@ -523,8 +559,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);
--- a/mbed.bld Mon Feb 20 13:29:52 2017 +0000 +++ b/mbed.bld Thu Mar 09 13:06:30 2017 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90 \ No newline at end of file