KRAI 2017
/
MainProgram_BaseBaru_fix_omni_5April_23-04
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_fix
by 
KRAI 2017
Revision 5:3aa203218306, committed 2016-11-22
- Comitter:
- rahmadirizki18
- Date:
- Tue Nov 22 15:30:31 2016 +0000
- Parent:
- 4:483c07cc22e1
- Child:
- 6:68293bed71ea
- Commit message:
- lintasan masih curve, balik ke posisi awal masih error besar
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Nov 22 15:30:31 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp Tue Nov 22 15:30:31 2016 +0000
@@ -0,0 +1,126 @@
+/********************************************************/
+/* Library untuk pembacaan Encoder */
+/* Adapsi dari QEI */
+/* */
+/* Encoder yang sudah dicoba : */
+/* 1. Autonics */
+/* 2. Encoder bawaan Motor */
+/* */
+/* ______________________ */
+/* |______Autonics______| */
+/* | Out A = Input 1 | */
+/* | Out B = Input 2 | */
+/* | 5V | */
+/* |_Gnd________________| */
+/* */
+/********************************************************/
+
+#include "mbed.h"
+
+#include "encoderKRAI.h"
+
+encoderKRAI::encoderKRAI(PinName channelA,
+ PinName channelB,
+ int pulsesPerRev,
+ Encoding encoding) : channelA_(channelA), channelB_(channelB)
+{
+ pulses_ = 0;
+ revolutions_ = 0;
+ pulsesPerRev_ = pulsesPerRev;
+ encoding_ = encoding;
+
+ //Workout what the current state is.
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+ prevState_ = currState_;
+
+ //X2 encoding uses interrupts on only channel A.
+ //X4 encoding uses interrupts on channel A,
+ //and on channel B.
+ channelA_.rise(this, &encoderKRAI::encode);
+ channelA_.fall(this, &encoderKRAI::encode);
+
+ //If we're using X4 encoding, then attach interrupts to channel B too.
+ if (encoding == X4_ENCODING) {
+ channelB_.rise(this, &encoderKRAI::encode);
+ channelB_.fall(this, &encoderKRAI::encode);
+ }
+}
+
+void encoderKRAI::reset(void) {
+
+ pulses_ = 0;
+ revolutions_ = 0;
+
+}
+
+/*int encoderKRAI::getCurrentState(void) {
+
+ return currState_;
+
+}*/
+
+int encoderKRAI::getPulses(void) {
+
+ return pulses_;
+
+}
+
+int encoderKRAI::getRevolutions(void) {
+
+ revolutions_ = pulses_ / pulsesPerRev_;
+ return revolutions_;
+
+}
+
+////////////////////////////////////////////////////////
+
+void encoderKRAI::encode(void) {
+
+ int change = 0;
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+
+ if (encoding_ == X2_ENCODING) {
+
+ //11->00->11->00 is counter clockwise rotation or "forward".
+ if ((prevState_ == 0x3 && currState_ == 0x0) ||
+ (prevState_ == 0x0 && currState_ == 0x3)) {
+
+ pulses_++;
+
+ }
+ //10->01->10->01 is clockwise rotation or "backward".
+ else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+ (prevState_ == 0x1 && currState_ == 0x2)) {
+
+ pulses_--;
+
+ }
+
+ } else if (encoding_ == X4_ENCODING) {
+
+ //Entered a new valid state.
+ if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+ //2 bit state. Right hand bit of prev XOR left hand bit of current
+ //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+ change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+ if (change == 0) {
+ change = -1;
+ }
+
+ pulses_ -= change;
+ }
+
+ }
+
+ prevState_ = currState_;
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h Tue Nov 22 15:30:31 2016 +0000
@@ -0,0 +1,106 @@
+#ifndef ENCODERKRAI_H
+#define ENCODERKRAI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID 0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class encoderKRAI {
+
+public:
+
+ typedef enum Encoding {
+
+ X2_ENCODING,
+ X4_ENCODING
+
+ } Encoding;
+
+ /** Membuat interface dari encoder
+ *
+ * @param inA DigitalIn, out A dari encoder
+ * @param inB DigitalIn, out B dari encoder
+ */
+ encoderKRAI(PinName channelA, PinName channelB, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+ /**
+ * Reset encoder.
+ *
+ * Menset pulse dan revolusi/putaran menjadi 0
+ */
+ void reset(void);
+
+ /**
+ * Membaca pulse yang didapat oleh encoder
+ *
+ * @return Nilai pulse yang telah dilalui.
+ */
+ int getPulses(void);
+
+ /**
+ * Membaca putaran yang didapat oleh encoder
+ *
+ * @return Nilai revolusi/putaran yang telah dilalui.
+ */
+ int getRevolutions(void);
+
+ /**
+ * Membaca pulse yang didapat oleh encoder
+ *
+ * @return Nilai pulse yang telah dilalui.
+ */
+ //int readDeltaPulses(void);
+
+ /**
+ * Membaca putaran yang didapat oleh encoder
+ *
+ * @return Nilai revolusi/putaran yang telah dilalui.
+ */
+ //int readDeltaRevolutions(void);
+
+private:
+
+ /**
+ * Menghitung pulse
+ *
+ * Digunakan setiap rising/falling edge baik channel A atau B
+ * Membaca putaran CW atau CCW => mengakibatkan pertambahan/pengurangan pulse
+ */
+ void encode(void);
+
+ /**
+ * Indeks setiap rising edge untuk menghitung putaran
+ * Nilai bertambah 1
+ */
+ //void index(void);
+
+ Encoding encoding_;
+
+ InterruptIn channelA_;
+ InterruptIn channelB_;
+ //InterruptIn index_;
+
+ int pulsesPerRev_;
+ int prevState_;
+ int currState_;
+
+ volatile int pulses_;
+ volatile int revolutions_;
+
+
+};
+
+#endif /* ENCODERKRAI_H */
\ No newline at end of file
--- a/main.cpp Tue Oct 25 13:21:57 2016 +0000
+++ b/main.cpp Tue Nov 22 15:30:31 2016 +0000
@@ -1,24 +1,23 @@
-/**
-Case Gerak
-1. cw
-2. ccw
-3. Maju
-4. Mundur
-5. Serong Atas Kanan
-6. Serong Bawah Kanan
-7. Serong Atas Kiri
-8. Serong Bawah Kiri
-9. Kanan
-10. Kiri
-11. Analog kiri base
-12. stop
+/****************************************************************************/
+/* PROGRAM UNTUK PID CLOSED LOOP */
+/* */
+/* - Digunakan encoder autonics */
+/* - Konfigurasi Motor dan Encoder sbb : */
+/* _________________ */
+/* | DEPAN | */
+/* | 1. e .2 | Angka ==> Motor */
+/* | ` ` | e ==> Encoder */
+/* | e e | */
+/* | . . | */
+/* | 4` e `3 | */
+/* |________________| */
+/* */
+/* SETTINGS (WAJIB!) : */
+/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
+/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
+/* */
+/****************************************************************************/
-Urutan motor 1234 searah jarum jam
-
-Source Code dari
-Bima Sahbani EL'12
-Fanny Achmad Hindrarta EL'12
-**/
#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"
@@ -29,23 +28,80 @@
#define vmaxanalog 0.9
#define ax 0.005
//#define koefperlambatan 0.8
+#include "encoderKRAI.h"
+
+#define pi 22/7
+#define diaEncoder 0.058
+#define PPR 1000
+#define diaRobot 0.64
+
+float K_enc = pi*diaEncoder;
+float K_robot = pi*diaRobot;
+
+float speed1=0.6;
+float speed2=0.6;
+float speed3=0.6;
+float speed4=0.6;
+
+float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
+//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 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);
// Deklarasi variabel motor
-Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
-Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
-Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
-Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
+Motor motor1(PB_8, PB_1 , PA_13); // pwm, fwd, rev
+Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
+Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev
+Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev
+
+// Deklarasi variabel posisi robot
+//robotPos posisi();
+
+// Inisialisasi Pin TX-RX Joystik dan PC
+//Serial pc(PA_0,PA_1);
+//Serial pc(USBTX,USBRX);
+// Deklarasi Variabel Global
+/*
+ * posX dan posY berdasarkan arah robot
+ * encoder Depan & Belakang sejajar sumbu Y
+ * encoder Kanan & Kiri sejajar sumbu X
+ */
+float jarak, posX, posY;
+//float keliling = pi*diameterRoda;
+
+void detect_encoder(float speed);
+void setCenter();
+float getY();
+float getX();
+float getTetha();
+
+
+
// Inisialisasi Pin TX-RX Joystik dan PC
joysticknucleo joystick(PA_0,PA_1);
Serial pc(USBTX,USBRX);
+// Posisi target
+float XT, YT, Tetha;
+
+//encoder variable
+float errX, errY, errT, Vt, Vx, Vy;
+float V1, V2, V3, V4;
+
//bool perlambatan=0;
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;
float jLX,jLY;
-double vcurr=0;
+double vcurr;
float x,y; // untuk analoghat kiri
float errorx=0,errory=0;
@@ -147,525 +203,269 @@
**/
+
// Pergerakan dari base
void aktuator()
{
- double koef;
- double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
// MOTOR
switch (case_ger)
{
case (1):
{
- if (pivka) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmaxpivot) {
- vcurr=vmaxpivot;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- }
- else {
- koef=1;
- }
-
- s1 = (float)(-0.5*koef*vcurr);
- s2 = (float)(-0.5*koef*vcurr);
- s3 = (float)(-0.5*koef*vcurr);
- s4 = (float)(-0.5*koef*vcurr);
-
+ Tetha = Tetha - 0.5;
pivka=true;
maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("pivKa\n");
+ pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
-
+
break;
}
case (2):
{
- if (pivki){
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmaxpivot) {
- vcurr=vmaxpivot;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 = (float)(0.5*koef*vcurr);
- s2 = (float)(0.5*koef*vcurr);
- s3 = (float)(0.5*koef*vcurr);
- s4 = (float)(0.5*koef*vcurr);
+ Tetha = Tetha + 0.5;
pivki=true;
maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("pivKi\n");
+ pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
-
+
break;
}
case (3):
{
- if (maju) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmax) {
- vcurr=vmax;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) { koef=0.5;}
- else {
- koef=1;
- }
-
- //Case s1 untuk mode L2 lebih lambat
- s1 = (float)(-1*koef*(vcurr+0.005));
-
- s2 = (float)(1.0*koef*vcurr);
- s3 = (float)(1.0*koef*vcurr);
- s4 = (float)(-1*koef*vcurr);
-
- //s1 =-0.8*koef*vcurr;
- //s2 =koef*vcurr;
- //s3 =-koef*vcurr;
- //s4 =koef*vcurr;
+ YT = YT + 0.01;
maju=true;
mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("maju\n");
+ pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
-
break;
}
case (4):
{
- if (mundur) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmax) {
- vcurr=vmax;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
- //Motor 4 telat mulai
- s1 = (float)(1*koef*(vcurr-0.008));
- s2 = (float)(-1*koef*(vcurr-0.005));
- s3 = (float)(-1*koef*(vcurr-0.005));
- s4 = (float)(1*koef*(vcurr+0.005));
+ YT = YT - 0.01;
mundur=true;
maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("mundur\n");
+ pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
break;
}
case (5) :
{
- if (saka) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmax) {
- vcurr=vmax;
- } if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 = (float)(-koef*vcurr);
- s2 = (float)(0); //koef*0.1*vcurr;
- s3 = (float)(koef*vcurr);
- s4 = (float)(0); //-koef*0.1*vcurr;
+ XT = XT + 0.01;
+ YT = YT + 0.01;
saka=true;
maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("saka\n");
+ pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- motor1.speed(s1);
- motor2.brake(1);
- //motor2.speed(s2);
- motor3.speed(s3);
- motor4.brake(1);
- //motor4.speed(s4);
break;
}
case (6) :
{
- if (sbka){
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmaxserong) {
- vcurr=vmaxserong;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 = (float)(0); //koef*0.1*vcurr;
- s2 = (float)(-koef*vcurr);
- s3 = (float)(0); //-koef*0.1*vcurr;
- s4 = (float)(koef*vcurr);
+ XT = XT + 0.01;
+ YT = YT - 0.01;
+
sbka=true;
maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("sbka\n");
+ pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- //motor1.speed(s1);
- motor1.brake(1);
- motor2.speed(s2);
- //motor3.speed(s3);
- motor3.brake(1);
- motor4.speed(s4);
break;
}
case (7) :
{
- if (saki) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmaxserong) {
- vcurr=vmaxserong;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 = (float)(0); //-koef*0.1*vcurr;
- s2 = (float)(koef*vcurr);
- s3 = (float)(0); //koef*0.1*vcurr;
- s4 = (float)(-koef*vcurr);
+ XT = XT - 0.01;
+ YT = YT + 0.01;
+
saki=true;
maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("saki\n");
+ pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- //motor1.speed(s1);
- motor1.brake(1);
- motor2.speed(s2);
- //motor3.speed(s3);
- motor3.brake(1);
- motor4.speed(s4);
break;
}
case (8) :
{
- if (sbki) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmaxserong) {
- vcurr=vmaxserong;
- }
+ XT = XT - 0.01;
+ YT = YT - 0.01;
+
+ pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 = (float)(koef*vcurr);
- s2 = (float)(0); //-koef*0.1*vcurr;
- s3 = (float)(-koef*vcurr);
- s4 = (float)(0); //koef*0.1*vcurr;
-
- sbki=true;
- maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-
- pc.printf("sbki\n");
-
- motor1.speed(s1);
- //motor2.speed(s2);
- motor2.brake(1);
- motor3.speed(s3);
- //motor4.speed(s4);
- motor4.brake(1);
break;
}
case (9) :
{
- if (kanan) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=0;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmax) {
- vcurr=vmax;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 =(float)(-1*koef*vcurr);
- s2 =(float)(-1.0*koef*vcurr);
- s3 =(float)(1*koef*vcurr);
- s4 =(float)(1.0*koef*vcurr);
-
+ XT = XT + 0.01;
kanan=true;
maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("Kanan\n");
+ pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
break;
}
case (10) :
{
- if (kiri) {
- if(vcurr<0.1) {
- vcurr=0.1;
- } else {
- vcurr+=ax;
- }
- //perlambatan=1;
- } else {
- //perlambatan=1;
- }
-
- if (vcurr>=vmax) {
- vcurr=vmax;
- }
-
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- } else {
- koef=1;
- }
-
- s1 =(float)(1*koef*vcurr);
- s2 =(float)(1*koef*vcurr);
- s3 =(float)(-1*koef*vcurr);
- s4 =(float)(-1.0*koef*vcurr);
+ XT = XT - 0.01;
kiri=true;
maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("Kiri\n");
+ pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
break;
}
case (11):
{
- if(joystick.R2==255 && joystick.L2==0) {
- koef=2;
- } else if (joystick.L2==255 && joystick.R2==0) {
- koef=0.5;
- }
- else {
- koef=1;
- }
-
- s1t = (vmaxanalog*(-x+y));
- s2t = (vmaxanalog*(-x-y));
- s3t = (vmaxanalog*(x-y));
- s4t = (vmaxanalog*(x+y));
-
- s1 = (float)(0.5*koef*s1t);
- s2 = (float)(0.5*koef*s2t);
- s3 = (float)(0.5*koef*s3t);
- s4 = (float)(0.5*koef*s4t);
-
-
+ XT = XT + 0.01*x;
+ YT = YT + 0.01*y;
analog=true;
maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- pc.printf("analog X =%.2f Y =%.2f \n ",x,y);
+ pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
break;
}
default :
{
- //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
- //if (maju && (vcurr>=0.5)) wait_ms(100);
- //else if (maju && (vcurr<0.5)) wait_ms(50);
- /*
- if(s1>0.2 || s1<-0.2 || s2>0.2 || s2<-0.2) {
- s1 = koefperlambatan * s1;
- s2 = koefperlambatan * s2;
- s3 = koefperlambatan * s3;
- s4 = koefperlambatan * s4;
-
- motor1.speed(s1);
- motor2.speed(s2);
- motor3.speed(s3);
- motor4.speed(s4);
-
-
- } else {
- */
- motor1.brake(1);
- motor2.brake(1);
- motor3.brake(1);
- motor4.brake(1);
- //}
-
+
maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
stop = true;
//s1 = 0;s2 =0; s3 =0; s4 =0;
- pc.printf("Stop\n");
+ // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
}
- }
+ }
+}
+
+//Begin Encoder
+
+void setCenter()
+{
+ encoderDepan.reset();
+ encoderBelakang.reset();
+ encoderKanan.reset();
+ encoderKiri.reset();
+}
+
+float getX()
+{
+ float jarakEncDpn, jarakEncBlk;
+ jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
+ jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
+ return (jarakEncDpn-jarakEncBlk)/2;
+}
+
+float getY()
+{
+ float jarakEncKir, jarakEncKan;
+ jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
+ jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
+ return (jarakEncKir-jarakEncKan)/2;
+}
+
+float getTetha()
+{
+ float busurDpn, busurBlk, busurKir, busurKan;
+ busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
+ busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
+ busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
+ busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
+
+ return -(busurDpn+busurBlk+busurKir+busurKan)/4;
}
+void gotoXYT(float xa, float ya, float Ta)
+{
+
+ errX = xa-getX();
+ Vx = KpX*errX;
+
+ errY = ya-getY();
+ Vy = KpY*errY;
+
+ errT = Ta-getTetha();
+ Vt = Kp_tetha*errT;
+
+ V1 = Vx+Vy-Vt;
+ V2 = Vx-Vy-Vt;
+ V3 = -Vx-Vy-Vt;
+ V4 = -Vx+Vy-Vt;
+
+ if (V1>speed1)
+ { V1 = speed1; }
+ else if (V1<-speed1)
+ { V1 = -speed1; }
+
+ if (V2>speed2)
+ { V2 = speed2; }
+ else if (V2<-speed2)
+ { V2 = -speed2; }
+
+ if (V3>speed3)
+ { V3 = speed3; }
+ else if (V3<-speed3)
+ { V3 = -speed3; }
+
+ if (V4>speed4)
+ { V4 = speed4; }
+ else if (V4<-speed4)
+ { V4 = -speed4; }
+
+ if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
+ {
+ motor1.speed(V1);
+ motor2.speed(V2);
+ motor3.speed(V3);
+ motor4.speed(V4);
+ // pc.printf("V1=%.2f \ ", V1);
+ }
+ else
+ {
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+ //_ms(1000);
+ }
+ //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT);
+ // wait_ms(10);
+
+}
+//End Encoder
+
int main (void)
{
// Set baud rate - 115200
joystick.setup();
pc.baud(115200);
+ wait_ms(1000);
+ setCenter();
+ wait_ms(500);
+
+ //Posisi Awal
+ XT = 0;
+ YT = 0;
+ Tetha = 0;
pc.printf("Ready...\n");
kalibrasi();
@@ -687,9 +487,19 @@
// analog switch mode
if (joystick.SELECT_click) {analog=!analog;}
//pc.printf(" X =%.2f Y =%.2f \n ",x,y);
+ if (joystick.silang) {
+ XT = 0;
+ YT = 0;
+ Tetha = 0;
+ pc.printf("x..\n");
+ }
+
+
} else {
joystick.idle();
}
+ gotoXYT(XT,YT,Tetha);
+
}
}