Kode Taker 2018
Dependencies: mbed PS_PAD encoderKRAI Motor_1 millis
main.cpp
- Committer:
- gatulz
- Date:
- 2018-07-03
- Revision:
- 1:1dc7c9cb4f8c
- Parent:
- 0:8c6f29487664
File content as of revision 1:1dc7c9cb4f8c:
//////////////////////////////////////////////////////////////////////////////// // 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; pn3=0; countX=375; 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; pn3=0; countKotak=575; 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>=5 && countSegitiga<300){ //SEGITIGA : buka tutup kedua gripper countSegitiga=575; pneumatik[0]=!pneumatik[0]; pneumatik[1]=!pneumatik[1]; pneumatik[2]=1; pneumatik[3]=1; pn=0; pn2=0; pn3=0; } if (ps2.read(PS_PAD::PAD_CIRCLE)==1 && countCircle<400) countCircle++; if (countCircle>=5 && countCircle<300){ //LINGKARAN : glider AB mundur, gripper AB buka, (kombinasikan dg kotak utk passing golde sc) countCircle=575; if (pn3<2 && pn3>=0) pn3++; else pn3=0; if(pn3==0){ pneumatik[0]=1; pneumatik[1]=1; pneumatik[2]=1; pneumatik[3]=1; } else if(pn3==1){ pneumatik[3]=0; } else if(pn3==2){ pneumatik[1]=0; wait_ms(50); pneumatik[1]=0; pneumatik[3]=1; } pn=0; pn2=0; } }