KRAI 2016
library baru
Dependencies: Motor PS_PAD mbed
Diff: main.cpp
- Revision:
- 0:e58595ad773c
- Child:
- 1:0c9e4edadb8f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Feb 05 18:15:10 2016 +0000
@@ -0,0 +1,373 @@
+
+#include "mbed.h"
+#include "PS_PAD.h"
+#include "Motor.h"
+
+PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //mosi,miso,clk,ss
+Serial pc(USBTX,USBRX);
+Serial com(A0,A1);
+
+#define kec_max 0.9 // konstanta kecepatan maksimum
+#define kec_pivot 0.8 // konstanta kecepatan pivot maksimum
+#define ax 0.1 // konstanta perceptan
+#define kec_gripper_naik 0.85
+#define kec_gripper_turun 0.15
+//Deklarasi pin motor
+//Motor NAME (PinName pwm, PinName fwd, PinName rev):
+
+ Motor gripper(PB_6, PB_8, PB_9);
+ Motor motor3(PC_6, PC_8, PC_9);
+ Motor motor2(PB_3, PB_5, PB_4);
+ Motor motor1(PA_8, PA_9, PC_7);
+
+bool maju=false;
+bool mundur=false;
+bool pivki=false;
+bool pivka=false;
+bool naik = false;
+bool turun = false;
+bool stop = true;
+char case_jalan, prev_case_jalan;
+
+
+// variabel kecepatan motor
+double kec_motor;
+
+void prosedurmotor()
+{
+ double koef;
+ double s1=0,s2=0;
+ //Kondisi Motor
+ switch (case_jalan)
+ {
+ case (1): // pivot kanan
+ {
+ if (pivka)
+ {
+
+ if(kec_motor<0.1)
+ {
+ kec_motor=0.1;
+ }
+ else
+ {
+ kec_motor+=ax;
+ }
+
+ //perlambatan=0;
+ }
+ else
+ {
+ //perlambatan=1;
+ }
+ if (kec_motor>=kec_pivot)
+ {
+ kec_motor=kec_pivot;
+ }
+ //mode lambat dan cepat pada tombol R2 dan L2
+ if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
+ {
+ koef=2;
+ }
+ else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
+ {
+ koef=0.5;
+ }
+ else
+ {
+ koef=1;
+ }
+
+ s1 = (float)(-0.5*koef*kec_motor);
+ s2 = (float)(0.5*koef*kec_motor);
+
+ pivka=true;
+ maju=mundur=pivki=naik=turun=false;
+
+ pc.printf("\rPivot Kanan Cuy\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ gripper.brake(1);
+
+ break;
+ }
+ case (2): // pivot kiri
+ {
+ if (pivki)
+ {
+ if(kec_motor<0.1)
+ {
+ kec_motor=0.1;
+ }
+ else
+ {
+ kec_motor+=ax;
+ }
+ //perlambatan=0;
+ }
+ else
+ {
+ //perlambatan=1;
+ }
+
+ if (kec_motor>=kec_pivot)
+ {
+ kec_motor=kec_pivot;
+ }
+ //mode lambat dan cepat pada tombol R2 dan L2
+ if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
+ {
+ koef=2;
+ } else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
+ {
+ koef=0.5;
+ }
+ else
+ {
+ koef=1;
+ }
+
+ s1 = (float)( 0.5*koef*kec_motor);
+ s2 = (float)(-0.5*koef*kec_motor);
+
+
+ pivki=true;
+ maju=mundur=pivka=naik=turun=false;
+
+ pc.printf("\rPivot Kiri Cuy\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ gripper.brake(1);
+
+
+ break;
+ }
+ case (3): // maju
+ {
+ if (maju)
+ {
+ if(kec_motor<0.1)
+ {
+ kec_motor=0.1;
+ }
+ else
+ {
+ kec_motor+=ax;
+ }
+ //perlambatan=0;
+ }
+ else
+ {
+ //perlambatan=1;
+ }
+
+ if (kec_motor>=kec_max)
+ {
+ kec_motor=kec_max;
+ }
+ //mode lambat dan cepat pada tombol R2 dan L2
+ if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
+ {
+ koef=2;
+ }
+ else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
+ {
+ koef=0.5;
+ }
+ else
+ {
+ koef=1;
+ }
+
+ //Case s1 untuk mode L2 lebih lambat
+ s1 = (float)(1*koef*(kec_motor));
+ s2 = (float)(1*koef*(kec_motor));
+
+
+ maju=true;
+ mundur=pivka=pivki=naik=turun=false;
+
+ pc.printf("\rMaju Cuy\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ gripper.brake(1);
+
+
+ break;
+ }
+ case (4): // mundur
+ {
+ if (mundur)
+ {
+ if(kec_motor<0.1)
+ {
+ kec_motor=0.1;
+ }
+ else
+ {
+ kec_motor+=ax;
+ }
+ //perlambatan=0;
+ }
+ else
+ {
+ //perlambatan=1;
+ }
+
+ if (kec_motor>=kec_max)
+ {
+ kec_motor=kec_max;
+ }
+ //mode lambat dan cepat pada tombol R2 dan L2
+ if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
+ {
+ koef=2;
+ }
+ else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
+ {
+ koef=0.5;
+ }
+ else
+ {
+ koef=1;
+ }
+ //Motor 4 telat mulai
+ s1 = (float)(-1*koef*(kec_motor));
+ s2 = (float)(-1*koef*(kec_motor));
+
+
+ mundur=true;
+ maju=pivka=pivki=naik=turun=false;
+
+ pc.printf("\rMundur Cuy\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ gripper.brake(1);
+
+
+ break;
+ }
+ case (5) : //naik
+ {
+ //motor1.brake(1);
+ //motor2.brake(1);
+ gripper.speed(-kec_gripper_naik);
+ break;
+ }
+ case (6) : //turun
+ {
+ //motor1.brake(1);
+ //motor2.brake(1);
+ gripper.speed(kec_gripper_turun);
+ break;
+ }
+
+ //TAMBAHAN POWER
+ case (7) : //turun
+ {
+ //motor1.brake(1);
+ //motor2.brake(1);
+ com.putc('1');
+ break;
+ }
+ case (8) : //turun
+ {
+ //motor1.brake(1);
+ //motor2.brake(1);
+ com.putc('2');
+ break;
+ }
+
+
+ default :
+ {
+
+ motor1.brake(1);
+ motor2.brake(1);
+ gripper.brake(1);
+
+ //}
+
+ maju=mundur=pivki=pivka=false;
+ stop = true;
+
+ //s1 = 0;s2 =0;
+
+ pc.printf("\rStop\n");
+ }
+ }
+}
+
+int mode_jalan ()
+{
+ int jalan_ke;
+ if ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0))
+ {
+ // Pivot Kanan
+ jalan_ke = 1;
+ }
+ else if ((ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==1))
+ {
+ // Pivot Kiri
+ jalan_ke = 2;
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0))
+ {
+ // Maju
+ jalan_ke = 3;
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==1))
+ {
+ // Mundur
+ jalan_ke = 4;
+ }
+ else if((ps2.read(PS_PAD::PAD_X)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0))
+ {
+ jalan_ke = 5;
+ }
+ else if((ps2.read(PS_PAD::PAD_X)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1))
+ {
+ jalan_ke = 6;
+ }
+
+ //tambahan power
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0))
+ {
+ jalan_ke = 7;
+ }
+ else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==0))
+ {
+ jalan_ke = 8;
+ }
+ else
+ {
+ jalan_ke = 0;
+ }
+ return(jalan_ke);
+}
+
+int main ()
+{
+ // Set baud rate - 115200
+ pc.baud(115200);
+ com.baud(9600);
+ pc.printf("Ready...\n");
+ ps2.init();
+
+ while (1)
+ {
+ ps2.poll();
+ case_jalan= mode_jalan();
+ if (case_jalan != prev_case_jalan)
+ {
+ kec_motor=0;
+ }
+
+ prosedurmotor();
+ prev_case_jalan = mode_jalan();
+ }
+}
\ No newline at end of file