![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ping + display on
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of MainProgram_BaseBaru_20Feb by
Diff: main.cpp
- Revision:
- 23:023b522977b2
- Parent:
- 22:4632f58461e0
- Child:
- 24:b3e632cc4533
diff -r 4632f58461e0 -r 023b522977b2 main.cpp --- a/main.cpp Sat Jan 28 10:19:36 2017 +0000 +++ b/main.cpp Wed Feb 01 15:00:08 2017 +0000 @@ -64,16 +64,16 @@ encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan /* Deklarasi Motor Base */ -Motor motor1(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Depan -Motor motor2(PB_8, PA_13, PB_0); // pwm, fwd, rev, Motor Belakang +Motor motor1(PB_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan +Motor motor2(PB_6, PB_12, PA_7); // pwm, fwd, rev, Motor Belakang /* Deklarasi Motor Launcher */ -Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev -Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev +//Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev +//Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev /* Deklarasi Servo Launcher */ -Servo servoS(PB_2); -Servo servoB(PA_5); +//Servo servoS(PB_2); +//Servo servoB(PA_5); /** * posX dan posY berdasarkan arah robot @@ -89,7 +89,8 @@ float getTetha(); // Fungsi mendapatkan jarak Tetha /* Inisialisasi Pin TX-RX Joystik dan PC */ -joysticknucleo joystick(PA_0,PA_1); +joysticknucleo joystick(PA_9,PA_10); +Serial pc(USBTX,USBRX); /* Variabel Stick */ char case_ger; @@ -99,7 +100,7 @@ /* Deklarasi Fungsi dan Procedure */ /****************************************************/ int case_gerak(){ -/****************************************************/ +/**************************************************** ** Gerak Motor Base ** Case 1 : Pivot kanan ** Case 2 : Pivot Kiri @@ -120,10 +121,12 @@ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { // Kanan casegerak = 3; + pc.printf("kanan"); } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri - casegerak = 4; + casegerak = 4; + pc.printf("kiri"); } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Break @@ -135,7 +138,7 @@ void aktuator(){ /* Fungsi untuk menggerakkan servo */ // Servo - if (servoGo){ +/* if (servoGo){ servoS.position(20); wait_ms(500); servoS.position(-28); @@ -163,7 +166,7 @@ motorld.speed(0); motorlb.speed(0); } - +*/ // MOTOR Bawah switch (case_ger) { case (1): { @@ -215,14 +218,14 @@ float vt; errT = Ta-getTetha(); - Vt = kp_tetha*errT; + vt = kp_tetha*errT; - if (vt>speed1) - { vt = speed1; } - else if (vt<-speed1) - { vt = -speed1; } + if (vt>speedT) + { vt = speedT; } + else if (vt<-speedT) + { vt = -speedT; } - if ((((errT > 0.05) || (errT<-0.05))){ + if (((errT > 0.05) || (errT<-0.05))){ vpid = vt; } else{ @@ -253,10 +256,11 @@ int main (void){ /* Set baud rate - 115200 */ joystick.setup(); + pc.baud(115200); wait_ms(1000); setCenter(); wait_ms(500); - + pc.printf("ready...."); /* Posisi Awal */ Tetha = 0;