Joshua Gunawan
Tambahan codingan servo, Cek pin servo terlebih dahulu
Dependencies: Motor PID Joystic_OrdoV3 mbed
Fork of
Joystick_OrdoV3
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 0:9072e932503c
- Child:
- 1:56bd3e8f38c5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu May 28 14:50:05 2015 +0000
@@ -0,0 +1,659 @@
+/**
+Base 4 Nasional
+
+Case Gerak
+1. Pivot Kanan
+2. Pivot Kiri
+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. Stop
+
+
+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(PA_15, PA_13, PA_14); // pwm, fwd, rev
+Motor motor2(PA_0, PC_14, PC_15); // pwm, fwd, rev
+Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
+Motor motor4(PC_6, PC_9, PC_8); // pwm, fwd, rev
+
+// Deklarasi Register Pneumatik
+DigitalOut pneumatik1(PC_11);
+DigitalOut pneumatik2(PD_2);
+
+// Deklarasi Timer Pneumatik
+Timer timer_pneu;
+
+// Inisialisasi Pin TX-RX Joystik dan PC
+joysticknucleo joystick(PA_11,PA_12);
+//Serial pc(USBTX,USBRX);
+
+//bool perlambatan=0;
+char case_ger;
+bool maju=false,mundur=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,pivki=false,pivka=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false;
+bool stop = true;
+bool t1, t2, pneu1, pneu2;
+int delay_pneumatik;
+double vcurr;
+
+
+int case_gerak()
+{
+ int casegerak;
+ if (!joystick.L1 && joystick.R1) {
+ // Pivot Kanan
+ casegerak = 1;
+ } else if (!joystick.R1 && joystick.L1) {
+ // Pivot Kiri
+ casegerak = 2;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>110 && joystick.LX<190) && (joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) ) {
+ // Maju
+ casegerak = 3;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>90 && joystick.LX<190) && (joystick.LY>=200) )|| ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))) {
+ // Mundur
+ casegerak = 4;
+ } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)))) {
+ // Serong Atas Kanan
+ casegerak = 5;
+ } else if(((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))))) {
+ // Serong Bawah Kanan
+ casegerak = 6;
+ } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {
+ // Serong Atas Kiri
+ casegerak = 7;
+ } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {
+ // Serong Bawah Kiri
+ casegerak = 8;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>=210) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) ) {
+ // Kanan
+ casegerak = 9;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX<=50) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))) {
+ // Kiri
+ casegerak = 10;
+ } else {
+ // Stop
+ casegerak = 11;
+ }
+
+ if(joystick.silang_click && t1==0 && t2==0) {
+ pneu1 = 1;
+ } else {
+ pneu1 = 0;
+ }
+
+ if(joystick.kotak_click && t1==0 && t2==0) {
+ pneu2 = 1;
+ } else {
+ pneu2 = 0;
+ }
+ 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 : Break
+
+**/
+void aktuator()
+{
+ double koef;
+ double s1=0,s2=0,s3=0,s4=0;
+
+ // PNEUMATIK
+ if(t1==1) {
+ if(timer_pneu.read_ms() - delay_pneumatik > 800) {
+ pneumatik1 = 1;
+ t1=0;
+ }
+ }
+ if(t2==1) {
+ if(timer_pneu.read_ms() - delay_pneumatik > 800) {
+ pneumatik2 = 1;
+ t2=0;
+ }
+ }
+
+ if (pneu1 == 1 || pneu2==1) {
+ timer_pneu.reset();
+ delay_pneumatik = timer_pneu.read_ms();
+ if(pneu1 == 1) {
+ pneumatik1 = 0;
+ t1 = 1;
+ pneu1 = 0;
+ } else if(pneu2 == 1) {
+ pneumatik2 = 0;
+ t2 = 1;
+ pneu2 = 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=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=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;
+ }
+
+ s1 = (float)(-1*koef*vcurr);
+ 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=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;
+ }
+
+ s1 = (float)(1*koef*vcurr);
+ s2 = (float)(-1*koef*vcurr);
+ s3 = (float)(-1*koef*vcurr);
+ s4 = (float)(1*koef*vcurr);
+
+ mundur=true;
+ maju=kiri=kanan=saka=saki=sbka=sbki=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=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=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=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=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=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=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;
+ }
+ 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=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ stop = true;
+
+ //s1 = 0;s2 =0; s3 =0; s4 =0;
+
+ //pc.printf("Stop\n");
+ }
+ }
+}
+
+
+int main (void)
+{
+ // Set baud rate - 115200
+ joystick.setup();
+ //pc.baud(115200);
+ //pc.printf("Ready...\n");
+ timer_pneu.start();
+ pneumatik1=1;
+ pneumatik2=1;
+ t1=0;
+ t2=0;
+ 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();
+
+ } else {
+ joystick.idle();
+
+ }
+ }
+}