![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
Diff: main.cpp
- Revision:
- 25:054d3048dd03
- Parent:
- 24:b3e632cc4533
- Child:
- 26:256160a1a82d
--- a/main.cpp Thu Feb 02 11:12:43 2017 +0000 +++ b/main.cpp Fri Feb 10 10:29:45 2017 +0000 @@ -46,13 +46,16 @@ /* Konstanta dan Variabel */ /***********************************************/ #define PI 3.14159265 -#define D_ENCODER 0.997 // Diameter Roda Encoder -#define D_ROBOT // Diameter Roda Robot +#define D_ENCODER 10 // Diameter Roda Encoder +#define D_ROBOT 80 // Diameter Roda Robot #define VMAX 0.3 // Kiri Kanan #define PIVOT 0.4 // Pivka, Pivki #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri +float Vt; + float k_enc = PI*D_ENCODER; +float k_robot = PI*D_ROBOT; float speedT = 0.2; float speedB = 0.43; @@ -63,11 +66,11 @@ float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03; /* Deklarasi encoder */ -encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan +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_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan -Motor motor2(PB_6, PB_12, PA_7); // pwm, fwd, rev, Motor Belakang +Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev, Motor Depan +Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang /* Deklarasi Motor Launcher */ //Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev @@ -91,7 +94,7 @@ float getTetha(); // Fungsi mendapatkan jarak Tetha /* Inisialisasi Pin TX-RX Joystik dan PC */ -joysticknucleo joystick(PA_9,PA_10); +joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); /* Variabel Stick */ @@ -101,6 +104,16 @@ /****************************************************/ /* Deklarasi Fungsi dan Procedure */ /****************************************************/ + +float pid(float Kp, float Ki, float Kd) +{ + int errorP; + + errorP = getTetha(); + + return (float)Kp*errorP; +} + int case_gerak(){ /**************************************************** ** Gerak Motor Base @@ -173,26 +186,32 @@ switch (case_ger) { case (1): { // Pivot Kanan - motor1.speed(-PIVOT); - motor2.speed(-PIVOT); + motor1.speed(PIVOT); + motor2.speed(PIVOT); + setCenter(); break; } case (2): { // Pivot Kiri - motor1.speed(PIVOT); - motor2.speed(PIVOT); + motor1.speed(-PIVOT); + motor2.speed(-PIVOT); + setCenter(); break; } case (3) : { // Kanan - motor1.speed(-VMAX-vpid); - motor2.speed(0.2+vpid); + //motor1.speed(-VMAX-vpid); + //motor2.speed(0.2+vpid); + motor1.speed(-0.365+pid(0.09,0,0)); + motor2.speed(0.46+pid(0.09,0,0)); break; } case (4) : { // Kiri - motor1.speed(VMAX-vpid); - motor2.speed(-0.2+vpid); + //motor1.speed(VMAX-vpid); + //motor2.speed(-0.2+vpid); + motor1.speed(0.365+pid(0.09,0,0));//belakang + motor2.speed(-0.46+pid(0.09,0,0));//depan break; } default : { @@ -209,30 +228,11 @@ float getTetha(){ /* Fungsi untuk mendapatkan nilai tetha */ - float busurKir; + float busurKir, tetha; busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc); - - return -(busurKir); -} - -void gotoXYT(float Ta){ -/* Fungsi untuk bergerak ke target */ - float vt; + tetha = busurKir/k_robot*360; - errT = Ta-getTetha(); - vt = kp_tetha*errT; - - if (vt>speedT) - { vt = speedT; } - else if (vt<-speedT) - { vt = -speedT; } - - if (((errT > 0.05) || (errT<-0.05))){ - vpid = vt; - } - else{ - vpid = 0; - } + return -(tetha); } void speedKalibrasiMotor(){ @@ -264,15 +264,13 @@ setCenter(); wait_ms(500); pc.printf("ready...."); - /* Posisi Awal */ - Tetha = 0; /* Untuk mendapatkan serial dari Arduino */ while(1) { // Interrupt Serial joystick.idle(); - + //pc.printf("enco : %d \n",encoderKiri.getPulses()); if(joystick.readable() ) { // Panggil fungsi pembacaan joystik joystick.baca_data(); @@ -290,6 +288,8 @@ } else { joystick.idle(); + //motor1.brake(1); + //motor2.brake(1); } } }