![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Update 12 Maret 2017. Tambahan kombinasi tombol
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of MainProgram_BaseBaru_20Feb_malam by
Diff: main.cpp
- Revision:
- 6:68293bed71ea
- Parent:
- 5:3aa203218306
- Child:
- 7:d138c56dab20
diff -r 3aa203218306 -r 68293bed71ea main.cpp --- a/main.cpp Tue Nov 22 15:30:31 2016 +0000 +++ b/main.cpp Sun Nov 27 09:38:37 2016 +0000 @@ -17,16 +17,25 @@ /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ /* */ /****************************************************************************/ +/* */ +/* Joystick */ +/* kanan => posisi target x ditambah 0.01 */ +/* kiri => posisi target x dikurang 0.01 */ +/* atas => posisi target y ditambah 0.01 */ +/* bawah => posisi target y dikurang 0.01 */ +/* */ +/* Tombol silang => Kembali keposisi Awal */ +/* Tombol segitiga => Aktif motor Launcher */ +/* Tombol kotak => Aktif servo Launcher */ +/* */ +/****************************************************************************/ + #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" +#include "Servo.h" -#define vmax 1 -#define vmaxserong 0.9 -#define vmaxpivot 0.7 -#define vmaxanalog 0.9 -#define ax 0.005 //#define koefperlambatan 0.8 #include "encoderKRAI.h" @@ -34,6 +43,8 @@ #define diaEncoder 0.058 #define PPR 1000 #define diaRobot 0.64 +#define pinservo1 PC_9 +#define pinservo2 PC_8 float K_enc = pi*diaEncoder; float K_robot = pi*diaRobot; @@ -44,13 +55,15 @@ float speed4=0.6; float KpX=0.5, KpY=0.5, Kp_tetha=0.03; + +Timer waktu; //float jarakGlobalY; // Deklarasi variabel PID //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling) // Deklarasi variabel encoder -encoderKRAI encoderDepan( D3, D4, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan +encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING); encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); @@ -61,6 +74,14 @@ Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev +//Motor Atas +Motor motorld(PA_8, PB_0 ,PC_15); // pwm, fwd, rev +Motor motorlb(PA_11, PA_6 ,PA_5); // pwm, fwd, rev + +//Servo Atas +Servo servoS(pinservo1); +Servo servoB(pinservo2); + // Deklarasi variabel posisi robot //robotPos posisi(); @@ -100,6 +121,7 @@ char case_ger; bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false; bool stop = true; +bool Launcher = false,ServoGo = false; float jLX,jLY; double vcurr; float x,y; // untuk analoghat kiri @@ -207,8 +229,37 @@ // Pergerakan dari base void aktuator() { + //Servo + /*if (ServoGo){ + servoS.position(90); + servoB.position(-90); + if (waktu.read_ms()>=15){ + pc.printf("Servo samping..."); + }else if ( waktu.read()<10) { + // Delay + servoB.position(90); + pc.printf("Servo belakang..."); + } else { + // Delay + ServoGo = false; + } + + }else{ + servoS.position(90); + servoB.position(0); - // MOTOR + }*/ + + // Motor Atas + if (Launcher) { + motorld.speed(0.2); + motorlb.speed(0.2); + }else{ + motorld.speed(0); + motorlb.speed(0); + } + + // MOTOR Bawah switch (case_ger) { case (1): @@ -458,7 +509,7 @@ // Set baud rate - 115200 joystick.setup(); pc.baud(115200); - wait_ms(1000); + wait_ms(1000); setCenter(); wait_ms(500); @@ -468,7 +519,9 @@ Tetha = 0; pc.printf("Ready...\n"); kalibrasi(); - + //servoS.position(90); + //servoB.position(0); + waktu.start(); while(1) { // Interrupt Serial @@ -481,20 +534,26 @@ //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY); case_ger = case_gerak(); aktuator(); + + pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read()); + //kalibrasi if (joystick.START){ kalibrasi();} // analog switch mode - if (joystick.SELECT_click) {analog=!analog;} - //pc.printf(" X =%.2f Y =%.2f \n ",x,y); + if (joystick.SELECT_click) {analog = !analog;} + if (joystick.segitiga_click) {Launcher = !Launcher;} + if (joystick.lingkaran_click){ + ServoGo = true; + waktu.reset(); + } if (joystick.silang) { XT = 0; YT = 0; Tetha = 0; pc.printf("x..\n"); } - - + } else { joystick.idle();