![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
taker KRAI 2018
Dependencies: mbed encoderKRTMI Motornew millis
Revision 0:dddc43348c25, committed 2019-02-24
- Comitter:
- SalbiFaza
- Date:
- Sun Feb 24 09:47:02 2019 +0000
- Commit message:
- bismillah
Changed in this revision
diff -r 000000000000 -r dddc43348c25 JoystickPS3.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JoystickPS3.h Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,214 @@ +#ifndef MBED_H +#include "mbed.h" +#endif + +#ifndef JoystickPS3__serialDEFAULT_BAUD +#define JoystickPS3__serialDEFAULT_BAUD 115200 +#endif + +//Serial debug(USBTX,USBRX); + +namespace JoystickPS3 { + +class joysticknucleo { +public: + joysticknucleo(PinName tx, PinName rx) : _serial(tx, rx) + { + + } + +// Deklarasi variabel tombol analog + unsigned char LX, LY, RX, RY, R2, L2; + + unsigned char button; + unsigned char RL; + unsigned char button_click; + unsigned char RL_click; + + void setup(){ + _serial.baud(JoystickPS3__serialDEFAULT_BAUD); + // debug.baud(9600); + } + + /*********************************************************************************************/ + /** **/ + /** FUNGSI PEMBACAAN DATA **/ + /** - Data yang diterima dari Serial Arduino berbentuk 8-bit **/ + /** - Data yang diterima diolah menjadi boolean / 1-bit untuk data tombol button dan RL **/ + /** karena data yang digunakan adalah 1-bit (true/false) **/ + /** - Untuk analog data yang diterima tidak diolah karena rentang data yang dikirimkan **/ + /** memiliki rentang 0-255 / 8-bit, dan data yang akan digunakan adalah data 8-bit **/ + /** **/ + /** |------|-------|-------|------|-------|--------|-----------|----------| **/ + /** Bit Ke | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/ + /** |------|-------|-------|------|-------|--------|-----------|----------| **/ + /** Data | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga | **/ + /** |------|-------|-------|------|-------|--------|-----------|----------| **/ + /** **/ + /** - Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam **/ + /** variabel "RL" **/ + /** - Urutan data pada variabel "RL" dan "RL_click" adalah **/ + /** sebagai berikut **/ + /** **/ + /** |----|--------|-------|----|----|----|----| **/ + /** Bit Ke | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/ + /** |----|--------|-------|----|----|----|----| **/ + /** Data | PS | SELECT | START | L3 | L1 | R3 | R1 | **/ + /** |----|--------|-------|----|----|----|----| **/ + /** **/ + /*********************************************************************************************/ + + void olah_data() + { + // Pengolahan data dari data "button" + segitiga = (bool)((button >> 0) & 0x1); + lingkaran = (bool)((button >> 1) & 0x1); + silang = (bool)((button >> 2) & 0x1); + kotak = (bool)((button >> 3) & 0x1); + atas = (bool)((button >> 4) & 0x1); + kanan = (bool)((button >> 5) & 0x1); + bawah = (bool)((button >> 6) & 0x1); + kiri = (bool)((button >> 7) & 0x1); + + // Pengolahan data dari data "RL" + R1 = (bool)((RL >> 0) & 0x1); + R3 = (bool)((RL >> 1) & 0x1); + L1 = (bool)((RL >> 2) & 0x1); + L3 = (bool)((RL >> 3) & 0x1); + START = (bool)((RL >> 4) & 0x1); + SELECT = (bool)((RL >> 5) & 0x1); + PS = (bool)((RL >> 6) & 0x1); + + // R2 click dan L2 click + if (R2 > 100) { + if ( R2sebelum) { R2_click = false; + } else { R2_click = true;} + R2sebelum = true; + }else { + R2sebelum = false; + R2_click = false; + } + if (L2 > 100) { + if ( L2sebelum) { L2_click = false; + } else { L2_click = true;} + L2sebelum = true; + }else { L2sebelum = false; + L2_click = false; + } + + segitiga_click = (bool)((button_click >> 0) & 0x1); + lingkaran_click = (bool)((button_click >> 1) & 0x1); + silang_click = (bool)((button_click >> 2) & 0x1); + kotak_click = (bool)((button_click >> 3) & 0x1); + atas_click = (bool)((button_click >> 4) & 0x1); + kanan_click = (bool)((button_click >> 5) & 0x1); + bawah_click = (bool)((button_click >> 6) & 0x1); + kiri_click = (bool)((button_click >> 7) & 0x1); + + // Pengolahan data dari data "RL" + R1_click = (bool)((RL_click >> 0) & 0x1); + R3_click = (bool)((RL_click >> 1) & 0x1); + L1_click = (bool)((RL_click >> 2) & 0x1); + L3_click = (bool)((RL_click >> 3) & 0x1); + START_click = (bool)((RL_click >> 4) & 0x1); + SELECT_click = (bool)((RL_click >> 5) & 0x1); + PS_click = (bool)((RL_click >> 6) & 0x1); + } + + /*********************************************************************************************/ + /** **/ + /** FUNGSI IDLE **/ + /** - Fungsi dijalankan saat Arduino mengirimkan data yang merupakan **/ + /** kondisi PS3 Disconnected **/ + /** - Fungsi membuat semua data joystik bernilai 0 **/ + /** **/ + /*********************************************************************************************/ + + void idle(){ + // Set 0 + button = 0; + RL = 0; + button_click = 0; + RL_click = 0; + R2_click =0; + L2_click =0; + R2 = 0; + L2 = 0; + RX = 0; + RY = 0; + LX = 0; + LY = 0; + + } + + /*********************************************************************************************/ + /** **/ + /** FUNGSI PEMBACAAN DATA **/ + /** - Fungsi pembacaan data yang dikirim dari arduino **/ + /** - Data yang dikirim dari arduino merupakan paket data dengan format pengiriman **/ + /** **/ + /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/ + /** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY | **/ + /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/ + /** **/ + /** |------|------| **/ + /** | 0x88 | 0x09 | **/ + /** |------|------| **/ + /** **/ + /** - Jika urutan data yang diterima seperti tabel diatas, maka data tersebut akan **/ + /** diolah untuk input ke aktuator **/ + /** **/ + /*********************************************************************************************/ + + void baca_data() + { + // Interrupt Serial + if(_serial.readable()&&(_serial.getc()==0x88)) { + // Pembacaan data dilakukan jika data awal yang diterima adalah 0x88 kemudian 0x08 + if(_serial.getc()==0x08){ + // Proses Pembacaan Data + button = _serial.getc(); + RL = _serial.getc(); + button_click = _serial.getc(); + RL_click = _serial.getc(); + R2 = _serial.getc(); + L2 = _serial.getc(); + RX = _serial.getc(); + RY = _serial.getc(); + LX = _serial.getc(); + LY = _serial.getc(); + } else if(_serial.getc()==0x09) { + // PS3 Disconnected + idle(); + } else { + idle(); } + // Indikator - Print data pada monitor PC + // debug.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",button, RL, button_click, RL_click, R2, L2, RX, RY, LX, LY); + } + } + + + int readable(){ + return _serial.readable(); + } + +public: + // Deklarasi variabel tombol joystik + bool kiri, kanan, atas, bawah; + bool segitiga, lingkaran, kotak, silang; + bool L1, R1, L3, R3, START, SELECT, PS; + + bool kiri_click, kanan_click, atas_click, bawah_click; + bool segitiga_click, lingkaran_click, kotak_click, silang_click; + bool L1_click, R1_click, L3_click, R3_click, R2_click, L2_click; + bool R2sebelum,L2sebelum; + bool START_click, SELECT_click, PS_click; + +protected: + virtual int _getc(){return _serial.getc();} + Serial _serial; +}; + +}; + +using namespace JoystickPS3;
diff -r 000000000000 -r dddc43348c25 Motor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/KRAI-2018/code/Motornew/#1d0887244f8b
diff -r 000000000000 -r dddc43348c25 encoderKRTMI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/encoderKRTMI.lib Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/KRTMI-2019/code/encoderKRTMI/#fd29d4db8c40
diff -r 000000000000 -r dddc43348c25 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,504 @@ + +//////////////////////////////////////////////////////////////////////////////// +// Robo Taker (Semi-Otomatis) 2018 // +// -------------------------------------------------------------------------- // +// Gerakan Robot: // +// - Arah -> Gerak // +// - Silang -> Buka atau Tutup Gripper A // +// - Lingkaran -> Buka atau Tutup Gripper B // +// - Kotak -> Extend atau Shrink Slider A // +// - Segitigas -> Extend atau Shrink Slider B // +// - Start -> Ubah Mode // +//////////////////////////////////////////////////////////////////////////////// +#include "mbed.h" +#include "Motor.h" +#include "encoderKRTMI.h" +#include "JoystickPS3.h" +#include "pinList.h" +#include "millis.h" + +//////////////////////////////////////////////////////////////////////////////// +// Konstanta Program // +//////////////////////////////////////////////////////////////////////////////// +#define PI 3.141592653593 +#define RAD_TO_DEG 57.2957795131 +#define MAX_W_SPEED 15000 //max angular speed of robot +#define TOLERANCET 0.8 //theta tolerance +#define PULSE_TO_JARAK 0.7416027 //0.7656027 //0.581776 //kll roda / pulses +#define L 298.0 //roda to center of robot +#define TS 2.0 //time sampling +#define LIMITPWM 0.4 //limit pwm motor + +//Konstanta PID Theta +#define KP_W 1.8 +#define KI_W 0 +#define KD_W 140 + +#define MOTOR_LIMIT_MAX 1 +#define MOTOR_LIMIT_MIN -1 + +// Set 1 untuk aktifkan fitur pc.print +#define DEBUG 1 + +//////////////////////////////////////////////////////////////////////////////// +// Object Program // +//////////////////////////////////////////////////////////////////////////////// +// Serial +Serial pc(USBTX, USBRX, 115200); +joysticknucleo stick(PIN_TX, PIN_RX); + +// Pneumatik +DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9}; + +// Encoder +encoderKRTMI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING); +encoderKRTMI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING); +encoderKRTMI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING); + +// Motor +Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A); +Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B); +Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C); + +//////////////////////////////////////////////////////////////////////////////// +// Deklarasi Prosedure dan Fungsi // +//////////////////////////////////////////////////////////////////////////////// +void gerakMotor(); +void hitungPID(double theta_s); +void hitungParameter(); +void printPulse(); +void case_gerak(); +void motor_Stop(); +void motor_ForceStop(); +void gerak_Pneumatic(); +void case_pneumatic(); + +//////////////////////////////////////////////////////////////////////////////// +// Variable-variable // +//////////////////////////////////////////////////////////////////////////////// +int joystick; +int pn = 0; +int pn2 = 0; +int pn3 = 0; +int pn_state = 0; +double pulse_A=0, pulse_B=0, pulse_C=0; +double Vr = 0, Vr_max = 0; +double Vw = 0; +double a = 0; +double w = 0; +double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0; +double theta=0, theta_s=0; +double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0; +unsigned long last_mt_print,last_mt_print2, last_mt_pid, last_mt_rotasi; +unsigned long last_mt_aksel, last_mt_desel,last1; +bool modeauto = 1; +bool print_pulse = 0; +int brake_state=0; +double Vmax = 0.4; +double Vw_max = 0.3; +int force=0; +int count = 0; +int countX = 0; +int countKotak = 0; +int countCircle = 0; +int countSegitiga=0; + +//////////////////////////////////////////////////////////////////////////////// +// Main Program // +//////////////////////////////////////////////////////////////////////////////// +int main(){ + stick.setup(); + stick.idle(); + + while(1){ + if(stick.readable() ) { + // Panggil fungsi pembacaan joystik + stick.baca_data(); + // Panggil fungsi pengolahan data joystik + stick.olah_data(); + if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){ + + pc.printf("tidak ada input\n"); + } else { + pc.printf("ada input\n"); + } + + + } + + }/* + while(1){ + //doing nothing + + pulse_A = encoder_C.getPulses(); + pc.printf("%.2f \n", pulse_A); + if (pulse_A>10000) + motor3.speed(0.0); + else + motor3.speed(-0.9); + Thread::wait(2); + }*/ +} + +//////////////////////////////////////////////////////////////////////////////// +// Prosedure dan Fungsi // +//////////////////////////////////////////////////////////////////////////////// +void hitungParameter(){ + pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK; + pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK; + pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK; + + //Compute value + x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev); + y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev); + theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L); + + //Update value + x_prev = x; + y_prev = y; + theta_prev = theta; + + encoder_A.reset(); + encoder_B.reset(); + encoder_C.reset(); +} + +void hitungPID(double theta_s){ + //menghitung error theta + theta_error = theta_s - (theta*RAD_TO_DEG); + sum_theta_error += theta_error; + + //kalkulasi PID Theta + w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS; + Vw += (w*L/MAX_W_SPEED)*LIMITPWM; + + //update + theta_error_prev = theta_error; + + //Limit Kecepatan Vw (kec rotasi) + if (Vw > Vw_max){ + Vw = Vw_max; + } + else if ( Vw < -1*Vw_max){ + Vw = -1*Vw_max; + } +} + +void gerakMotor(){ + // Berhenti jika tidak maju, mundur, kiri, kanan, atau pivot + if (brake_state == 1){ + motor_ForceStop(); + //motor_Stop(); + } else { + if (count>50){ + if ((millis() - last_mt_aksel > 100) && Vr < Vr_max){ + if (Vr < 0.39){ + Vr = 0.39; + } + else if ( (Vr >= 0.39)&& (Vr<=0.45)){ + Vr+= 0.05; + } else if ((Vr>0.45) && (Vr<1.00) ){ + Vr+=0.12; + } else { + Vr = 1.2; + } + last_mt_aksel = millis(); + } + } else { + Vr=0.42; + } + + // Limit + if (Vr > Vr_max && Vr_max >= 0.000){ + Vr = Vr_max; + } + + // Control Motor + motor1.speed((Vr*cos(a) - Vw)); + motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) - Vw)); + motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) - Vw)); + print_pulse = 1; + } + +} + +void printPulse(){ + pc.printf("%d \n", pn); + //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, x, y); + //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, x, y, Vmax, modeauto); +} + +void motor_Stop(){ +//brake biasa + motor1.speed(0); + motor2.speed(0); + motor2.speed(0); +} + +void motor_ForceStop(){ +//FORCEBRAKE + motor1.brake(BRAKE_HIGH); + motor2.brake(BRAKE_HIGH); + motor3.brake(BRAKE_HIGH); +} + +void case_gerak(){ + if(stick.SELECT_click){ + //reset semua variable + pneumatik[0]=1; //gripA + pneumatik[1]=1; //gripB + pneumatik[2]=1; //sliderA + pneumatik[3]=1; //sliderB + pn=0; + pn2=0; + pn3=0; + modeauto = 0; + theta = 0.0; + theta_prev = 0.0; + theta_s = 0; + theta_error_prev = 0; + sum_theta_error = 0; + theta_error = 0; + } + + // Rotasi + if (!stick.L1 && stick.R1){ // Pivot Kanan + //theta = 0.0; + //theta_prev = 0.0; + theta_s = theta*RAD_TO_DEG; + theta_error_prev = 0; + sum_theta_error = 0; + theta_error = 0; + if (Vr_max==0) + Vw = 0.25; + else + Vw = 0.17; + } + else if (!stick.R1 && stick.L1){ // Pivot Kiri + //theta = 0.0; + //theta_prev = 0.0; + theta_s = theta*RAD_TO_DEG; + theta_error_prev = 0; + sum_theta_error = 0; + theta_error = 0; + if (Vr_max==0) + Vw = -0.25; + else + Vw = -0.17; + } + + + if (stick.R2){ + // Mode Sprint + Vmax=1.25; + } else + Vmax=0.83; + + + if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){ + count=0; + brake_state=1; + theta = 0.0; + theta_prev = 0.0; + theta_s = 0.0; + theta_error_prev = 0; + sum_theta_error = 0; + theta_error = 0; + }else{ + if (count<200) + count++; + else + count=500; + brake_state=0; + } + // Linier + + if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){ + //L2 : serong kiri bawah + pivot kiri (mundur saat kasih SC + a = (-22/RAD_TO_DEG); // mundur saat setelah pengambilan + Vr_max = 0.84; + Vw=-0.19; + } + else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){ + a = (5/RAD_TO_DEG); // L2+atas : mundur saat setelah pengambilan + Vr_max = 0.87; + Vw=-0.05; + } + else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ + a = (90/RAD_TO_DEG); // Maju + Vr_max = Vmax; + } + else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ + a = (-96/RAD_TO_DEG); // Mundur + Vr_max = Vmax; + } + else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ + a = (135/RAD_TO_DEG); // Serong Atas Kanan + Vr_max = Vmax; + } + else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ + a = (-135/RAD_TO_DEG); // Serong Bawah Kanan + Vr_max = Vmax; + } + else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ + a = (45/RAD_TO_DEG); // Serong Atas Kiri + Vr_max = Vmax; + } + else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ + a = (-45/RAD_TO_DEG); // Serong Bawah Kiri + Vr_max = Vmax; + } + else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){ + a = (180/RAD_TO_DEG); // Kanan + Vr_max = 0.8; + } + else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){ + a = (0/RAD_TO_DEG); // Kiri + Vr_max = 0.8; + } + else { + Vr_max = 0; + } + //} +} +void case_pneumatic(){ + // Control Pneumatic + /** + * pneumatik[0] = Gripper A + * pneumatik[1] = Gripper B + * pneumatik[3] = Slider A + * pneumatik[4] = Slider B + **/ + if((!stick.silang) && (!stick.kotak) && (!stick.lingkaran) && (!stick.segitiga)) { + // reset state pneumatik + countX=0; + countCircle=0; + countSegitiga=0; + countKotak=0; + } + if ((stick.silang && countX<50)) { + //SILANG : sekuensial tiap tangan gripper + countX++; + } + + if (countX>0 && countX<100) { + //state button X, lengan Kiri - lengakn Kanan + countX=150; + if (pn<4 && pn>=0) + pn++; + else + pn=0; + + if(pn==0 || pn==22){ + pneumatik[0]=1;//gripA + pneumatik[1]=1;//gripB + pneumatik[2]=1;//sliderA + pneumatik[3]=1;//sliderB + } else if (pn==1){ + pneumatik[2] = 0; + } else if (pn==2){ + pneumatik[0] = 0; + wait_ms(50); + pneumatik[2] = 1; + } else if (pn==3){ + pneumatik[3]=0; + } else if (pn==4){ + pneumatik[1] = 0; + wait_ms(50); + pneumatik[3] = 1; + } + pn2=0; + pn3=0; + } + + if ((stick.kotak) && countKotak<50){ + //KOTAK : maju 2 glider + countKotak++; + } + + if (countKotak>5 && countKotak<100){ + //state button KOTAK + countKotak=250; + pn=0; + pn3=0; + if (pn2<2 && pn2>=0) + pn2++; + else + pn2=0; + + if(pn2==0){ + pneumatik[0]=1; + pneumatik[1]=1; + pneumatik[2]=1; + pneumatik[3]=1; + } else if(pn2==1){ + pneumatik[2]=0; + pneumatik[3]=0; + } else if(pn2==2){ + pneumatik[0]=0; + pneumatik[1]=0; + wait_ms(50); + pneumatik[2]=1; + pneumatik[3]=1; + } + + } + + if ((stick.segitiga) && countSegitiga<50) + //SEGITIGA : buka tutup kedua gripper + countSegitiga++; + if (countSegitiga>4 && countSegitiga<100) { + //state button SEGITIGA + countSegitiga=250; + if ( (pneumatik[0]==0 && pneumatik[1]==1) || (pneumatik[0]==1 && pneumatik[1]==0) ){ + pneumatik[0]=1; + pneumatik[1]=1; + pneumatik[2]=1; + pneumatik[3]=1; + } else { + pneumatik[0]=!pneumatik[0]; + pneumatik[1]=!pneumatik[1]; + pneumatik[2]=1; + pneumatik[3]=1; + } + pn=0; + pn2=0; + pn3=0; + } + + if ((stick.lingkaran) && countCircle<50){ + //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran + countCircle++; + } + + if (countCircle>4 && countCircle<100){ + //LINGKARAN : lengan kanan - lengan kiri + countCircle=250; + pn=0; + pn2=0; + if (pn3<4 && pn3>=0) + pn3++; + else + pn3=0; + if(pn3==0 || pn3==22){ + pneumatik[0]=1;//gripA + pneumatik[1]=1;//gripB + pneumatik[2]=1;//sliderA + pneumatik[3]=1;//sliderB + } else if (pn3==1){ + pneumatik[3] = 0; + } else if (pn3==2){ + pneumatik[1] = 0; + wait_ms(50); + pneumatik[3] = 1; + } else if (pn3==3){ + pneumatik[2]=0; + } else if (pn3==4){ + pneumatik[0] = 0; + wait_ms(50); + pneumatik[2] = 1; + } + pn2=0; + } + +}
diff -r 000000000000 -r dddc43348c25 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r 000000000000 -r dddc43348c25 millis.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/millis.lib Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/DFRobot/code/millis/#736e6cc31bcd
diff -r 000000000000 -r dddc43348c25 pinList.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pinList.h Sun Feb 24 09:47:02 2019 +0000 @@ -0,0 +1,38 @@ +// Pin Motor +// Motor A +#define PIN_PWM_A PB_6 +#define PIN_FWD_A PB_13 +#define PIN_REV_A PA_10 +// Motor B +#define PIN_PWM_B PB_8 +#define PIN_FWD_B PA_5 +#define PIN_REV_B PA_9 +// Motor C +#define PIN_PWM_C PB_9 +#define PIN_FWD_C PA_12 +#define PIN_REV_C PA_11 + +// Pin UART +// Arduino +#define PIN_TX PA_0 +#define PIN_RX PA_1 + +// Pin Encoder +#define PIN_A_CHANNEL_A PC_0 +#define PIN_A_CHANNEL_B PC_1 +#define PIN_B_CHANNEL_A PC_6 +#define PIN_B_CHANNEL_B PC_7 +#define PIN_C_CHANNEL_A PC_5 +#define PIN_C_CHANNEL_B PC_4 + +// Pin pneumatik +#define PIN_PNEUMATIK_1 PC_11 +#define PIN_PNEUMATIK_2 PA_15 +#define PIN_PNEUMATIK_3 PC_14 +#define PIN_PNEUMATIK_4 PH_0 +#define PIN_PNEUMATIK_5 PH_1 +#define PIN_PNEUMATIK_6 PA_4 +#define PIN_PNEUMATIK_7 PC_15 +#define PIN_PNEUMATIK_8 PC_13 +#define PIN_PNEUMATIK_9 PA_14 +#define PIN_PNEUMATIK_10 PC_10