library baru
Dependencies: Motor PS_PAD mbed
main.cpp
- Committer:
- rizqicahyo
- Date:
- 2016-02-09
- Revision:
- 1:0c9e4edadb8f
- Parent:
- 0:e58595ad773c
- Child:
- 2:35c396166e13
File content as of revision 1:0c9e4edadb8f:
#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.speed(0.01); motor2.brake(0.01); //com.putc('1'); //break; } case (8) : //turun { motor1.speed(0.01); motor2.speed(0.01); //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(); } }