KRAI 2017
/
Joystick_OrdoV6_2
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: Motor PID Joystick_OrdoV5 mbed
Fork of
Joystick_OrdoV6_1
by 
KRAI 2017
main.cpp
- Committer:
- rahmadirizki18
- Date:
- 2016-10-16
- Revision:
- 3:1287fccc11be
- Parent:
- 1:56bd3e8f38c5
- Child:
- 4:483c07cc22e1
File content as of revision 3:1287fccc11be:
/**
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
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"
#define vmax 1
#define vmaxserong 0.9
#define vmaxpivot 0.7
#define ax 0.005
//#define koefperlambatan 0.8
// Deklarasi variabel motor
Motor motor1(PB_6, PA_7 , PB_12); // pwm, fwd, rev
Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
Motor motor3(PB_7, PA_15, PA_14); // pwm, fwd, rev
Motor motor4(PB_8, PB_1 , PA_13); // pwm, fwd, rev
// Inisialisasi Pin TX-RX Joystik dan PC
joysticknucleo joystick(PA_0,PA_1);
Serial pc(USBTX,USBRX);
//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;
int jLX,jLY;
double vcurr=0;
float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi
float errorx=0,errory=0;
// Fungsi mapping 0-255 ke -1 sampai 1
float mapping(int x)
{
float hasil;
hasil = ((x+1)/128)-1;
return (hasil);
}
// simpan data analog
void baca_analog()
{
jLX = joystick.LX;
jLY = joystick.LY;
// Pembacaan nilai Y terbalik
x = mapping(jLX);
y = -mapping(jLY);
// Setelah di kalibrasi
xk = x + errorx;
yk = y + errory;
}
// Gerak dari Motor base
int case_gerak()
{
int casegerak;
baca_analog();
if (!joystick.L1 && joystick.R1) {
// Pivot Kanan
casegerak = 1;
} else if (!joystick.R1 && joystick.L1) {
// Pivot Kiri
casegerak = 2;
} else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Maju
casegerak = 3;
} else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Mundur
casegerak = 4;
} else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
// Serong Atas Kanan
casegerak = 5;
} else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
// Serong Bawah Kanan
casegerak = 6;
} else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
// Serong Atas Kiri
casegerak = 7;
} else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
// Serong Bawah Kiri
casegerak = 8;
} else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
casegerak = 9;
} else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Kiri
casegerak = 10;
} else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// analog switch mode
if (joystick.SELECT_click) {analog=!analog;}
// case gerak analog on off
if (analog){
casegerak = 11;
} else {
casegerak = 12;
}
}
return(casegerak);
}
/**
** Case 1 : Pivot kanan
** Case 2 : Pivot Kiri
** Case 3 : Maju
** Case 4 : Mundur
** Case 5 : Serong Atas Kanan
** Case 6 : Serong Bawah Kanan
** Case 7 : Serong Atas Kiri
** Case 8 : Serong Bawah Kiri
** Case 9 : Kanan
** Case 10 : Kiri
** Case 11 : Analog
** Case 11 : break
**/
// Pergerakan dari base
void aktuator()
{
double koef;
double s1=0,s2=0,s3=0,s4=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);
pivka=true;
maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("pivKa\n");
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);
pivki=true;
maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("pivKi\n");
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;
maju=true;
mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("maju\n");
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));
mundur=true;
maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("mundur\n");
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;
saka=true;
maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("saka\n");
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);
sbka=true;
maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("sbka\n");
//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);
saki=true;
maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("saki\n");
//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;
}
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);
kanan=true;
maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("Kanan\n");
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);
kiri=true;
maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("Kiri\n");
motor1.speed(s1);
motor2.speed(s2);
motor3.speed(s3);
motor4.speed(s4);
break;
}
case (11):
{
if (analog) {
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)(0.5*koef*vcurr*((-xk)+(yk)));
s2 = (float)(0.5*koef*vcurr*((-xk)+(-yk)));
s3 = (float)(0.5*koef*vcurr*((xk)+(-yk)));
s4 = (float)(0.5*koef*vcurr*((xk)+(yk)));
analog=true;
maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
pc.printf("analog x= %d y= %d /n ",x,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");
}
}
}
// Kalibrasi tombol analog kiri
void kalibrasi()
{
errorx = 0-x;
errory = 0-y;
}
int main (void)
{
// Set baud rate - 115200
joystick.setup();
pc.baud(115200);
pc.printf("Ready...\n");
while(1)
{
// Interrupt Serial
joystick.idle();
if(joystick.readable() ) {
// Panggil fungsi pembacaan joystik
joystick.baca_data();
// Panggil fungsi pengolahan data joystik
joystick.olah_data();
//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();
//kalibrasi
if (joystick.START_click){
kalibrasi();
}
} else {
joystick.idle();
}
}
}