![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Kode Taker 2018
Dependencies: mbed PS_PAD encoderKRAI Motor_1 millis
Diff: main.cpp
- Revision:
- 0:8c6f29487664
- Child:
- 1:1dc7c9cb4f8c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jul 03 05:45:18 2018 +0000 @@ -0,0 +1,524 @@ + +//////////////////////////////////////////////////////////////////////////////// +// 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 "encoderKRAI.h" +#include "JoystickPS3.h" +#include "PS_PAD.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); + +//joystick PS2 +PS_PAD ps2(PB_15,PB_14, PB_10, PB_12); //(mosi, miso, sck, ss) +//PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //PB_13 udah dipake pin forward motor-_- + +// Pneumatik +DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9}; + +// Encoder +encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING); +encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING); +encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::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 countSegitiga = 0; +int countCircle = 0; +int dudu; + +//////////////////////////////////////////////////////////////////////////////// +// Main Program // +//////////////////////////////////////////////////////////////////////////////// +int main(){ + encoder_A.reset(); + encoder_B.reset(); + encoder_C.reset(); + + pneumatik[0]=0; //gripA + pneumatik[1]=0; //gripB + pneumatik[2]=1; //sliderA + pneumatik[3]=1; //sliderB + + startMillis(); + //stick.setup(); + //stick.idle(); + + ps2.init(); + + + while(1){ + ps2.poll(); + if (millis() - last_mt_pid > TS){ + last_mt_pid = millis(); + hitungParameter(); + if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) ){ + hitungPID(theta_s); + } + //if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET ){ +// Vw = 0; +// } + if (DEBUG==1) + printPulse(); + } + //gerakMotor(); + + //if (millis() - last_mt_print2 > TS){ +// hitungParameter(); +// // printPulse(); +// pc.printf("%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, theta_s, a,modeauto); +// last_mt_print2 = millis(); +// } + //if(stick.readable() ) { + // Panggil fungsi pembacaan joystik + //stick.baca_data(); + // Panggil fungsi pengolahan data joystik + //stick.olah_data(); + // Ambil data joystick + case_gerak(); + case_pneumatic(); + + // Pengatur Gerak Motor + gerakMotor(); + + // Program PID + if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET ){ + Vw = 0; + } + + // Fungsi Serial Print + if (millis() - last_mt_print > TS*10){ + if (print_pulse && DEBUG) + printPulse(); + last_mt_print = millis(); + } + //} + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// 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); + //theta2 = theta_prev2 + (pulse_A + pulse_C + pulse_B)/(3.0*L); + + //Update value + x_prev = x; + y_prev = y; + theta_prev = theta; + //theta_prev2 = theta2; + + encoder_A.reset(); + encoder_B.reset(); + encoder_C.reset(); +} + +void hitungPID(double theta_s){ + //menghitung error jarak x,y terhaadap xs,ys + 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 vw + if (Vw > Vw_max){ + Vw = Vw_max; + } + else if ( Vw < -1*Vw_max){ + Vw = -1*Vw_max; + } +} + +void gerakMotor(){ + // Berhent 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 > 60) && Vr < Vr_max){ + if (Vr < 0.35){ + Vr = 0.35; + } + else if ( (Vr >= 0.35)&& (Vr<=0.45)){ + Vr+= 0.025; + } else if ((Vr>0.45) && (Vr<0.7) ){ + Vr+=0.1; + } else if ((Vr>=0.7) && (Vr<1.00)) { + Vr+=0.15; + } else + Vr = 1.00; + last_mt_aksel = millis(); + } + } else { + Vr=0.4; + } + + // 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("Up=%d \t Down=%d \t Left=%d \t Right=%d \t\n", (ps2.read(PS_PAD::PAD_TOP)), (ps2.read(PS_PAD::PAD_BOTTOM)), (ps2.read(PS_PAD::PAD_LEFT)), (ps2.read(PS_PAD::PAD_RIGHT))); + //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(){ + motor1.speed(0); + motor2.speed(0); + motor2.speed(0); +} + +void motor_ForceStop(){ + motor1.brake(BRAKE_HIGH); + motor2.brake(BRAKE_HIGH); + motor3.brake(BRAKE_HIGH); +} + +void gerak_Pneumatic(){ + if (pn_state==0){ + pneumatik[0] = 1; + pneumatik[1] = 1; + pneumatik[2] = 1; + pneumatik[3] = 1; + } else if (pn_state==1){ + pneumatik[0] = 1; + pneumatik[1] = 1; + pneumatik[2] = 0; + pneumatik[3] = 1; + } else if (pn_state==2){ + pneumatik[0] = 0; + pneumatik[1] = 1; + wait_ms(100); + pneumatik[2] = 1; + pneumatik[3] = 1; + } else if (pn_state==3){ + pneumatik[0] = 0; + pneumatik[1] = 1; + pneumatik[2] = 1; + pneumatik[3] = 0; + } else if (pn_state==4){ + pneumatik[0] = 0; + pneumatik[1] = 0; + wait_ms(100); + pneumatik[2] = 1; + pneumatik[3] = 1; + } +} +void case_gerak(){ + if(ps2.read(PS_PAD::PAD_SELECT)==1){ + pneumatik[0]=1; //gripA + pneumatik[1]=1; //gripB + pneumatik[2]=1; //sliderA + pneumatik[3]=1; //sliderB + pn=0; + pn2=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 ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ // 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.2; + else + Vw = 0.2; + } + else if ((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ // 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.2; + else + Vw = -0.2; + } + + if (ps2.read(PS_PAD::PAD_R2)==1){ + // Mode Sprint + Vmax=1.0; + } else { + Vmax=0.8; + } + + if ( (ps2.read(PS_PAD::PAD_TOP)==0)&&(ps2.read(PS_PAD::PAD_BOTTOM)==0)&&(ps2.read(PS_PAD::PAD_RIGHT)==0)&&(ps2.read(PS_PAD::PAD_LEFT)==0)&&(ps2.read(PS_PAD::PAD_L1)==0)&&(ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_R2)==0)){ + count=0; + brake_state=1; + }else{ + if (count<200) + count++; + else + count=500; + brake_state=0; + } + // Linier + if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){ + a = (-90/RAD_TO_DEG); // Maju + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){ + a = (90/RAD_TO_DEG); // Mundur + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){ + a = (-45/RAD_TO_DEG); // Serong Atas Kanan + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){ + a = (45/RAD_TO_DEG); // Serong Bawah Kanan + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ + a = (-135/RAD_TO_DEG); // Serong Atas Kiri + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){ + a = (135/RAD_TO_DEG); // Serong Bawah Kiri + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){ + a = (0/RAD_TO_DEG); // Kanan + Vr_max = Vmax; + } + else if ((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ + a = (180/RAD_TO_DEG); // Kiri + Vr_max = Vmax; + } + else { + Vr_max = 0; + } + + +} +void case_pneumatic(){ + // Control Pneumatic + + if((ps2.read(PS_PAD::PAD_X)==0) && (ps2.read(PS_PAD::PAD_SQUARE)==0) && (ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)) { + countX=0; + countKotak=0; + countSegitiga=0; + countCircle=0; + } else if ((ps2.read(PS_PAD::PAD_X)==1)) { + //SILANG : sekuensial tiap tangan gripper + countX++; + } + + if (countX>5 && countX<=300) { + pn2=0; + countX=325; + if (pn<5 && 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(70); + pneumatik[2] = 1; + } else if (pn==3){ + pneumatik[0]=1; + pneumatik[2]=1; + } else if(pn==4){ + pneumatik[3]=0; + } else if (pn==5){ + pneumatik[1] = 0; + wait_ms(20); + pneumatik[3] = 1; + } + + } + + if (ps2.read(PS_PAD::PAD_SQUARE)==1){ + //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran + countKotak++; + } + + if (countKotak>5 && countKotak<300){ + pn=0; + countKotak=325; + if (pn2<5 && 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[0]=0; + pneumatik[1]=0; + pneumatik[2]=1; + pneumatik[3]=1; + } else if (pn2==3){ + pneumatik[0]=0; + pneumatik[1]=0; + } + + } + + if (ps2.read(PS_PAD::PAD_TRIANGLE)==1 && countSegitiga<400) + countSegitiga++; + if (countSegitiga>=7 && countSegitiga<300){ + //SEGITIGA : buka tutup kedua gripper + countSegitiga=500; + pneumatik[0]=!pneumatik[0]; + pneumatik[1]=!pneumatik[1]; + pneumatik[2]=1; + pneumatik[3]=1; + pn=0; + pn2=0; + } + + + if (ps2.read(PS_PAD::PAD_CIRCLE)==1 && countCircle<400) + countCircle++; + if (countCircle>=5 && countCircle<200){ + //LINGKARAN : glider AB mundur, gripper AB buka, (kombinasikan dg kotak utk passing golde sc) + countCircle=500; + pneumatik[0]=0; //gripA + pneumatik[1]=0; //gripB + pneumatik[2]=1; //sliderA + pneumatik[3]=1; //sliderB + pn=0; + pn2=0; + } + +}