base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
main.cpp
- Committer:
- rizqicahyo
- Date:
- 2016-04-22
- Revision:
- 2:df6c49846367
- Parent:
- 1:fc1535231c0d
- Child:
- 3:43d4cb3ece1b
File content as of revision 2:df6c49846367:
/*********************************************************************************************/ /** FILE HEADER **/ /*********************************************************************************************/ #include "mbed.h" #include "Motor.h" #include "PS_PAD.h" #include "PID.h" #include "esc.h" #include "Servo.h" /*********************************************************************************************/ /** DEKLARASI INPUT OUTPUT **/ /*********************************************************************************************/ //serial Serial pc(USBTX,USBRX); //debug Serial com(PA_0,PA_1); //sensor //joystick PS2 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) //motor (PWM, forward, reverse) Motor motor1(PA_8, PB_0, PC_15); Motor motor2(PA_11, PA_6, PA_5); //Motor gripper(PB_6, PA_7, PB_12); Motor motor3(PA_9, PC_2, PC_3); Motor motor4(PB_7, PA_14, PA_15); Motor motorC1(PB_9, PA_12, PC_5); Motor motorC2(PB_8, PB_1, PA_13); Motor motorS(PA_10, PB_5, PB_4); /* coba coba //motor (PWM, forward, reverse) Motor motor1(PA_8, PB_0, PC_15); Motor motorC1(PA_11, PA_6, PA_5); //Motor motor3(PB_6, PA_7, PB_12); Motor motorC2(PA_9, PC_2, PC_3); Motor motor4(PB_7, PA_14, PA_15); Motor motor2(PB_9, PA_12, PC_5); Motor motor3(PB_8, PB_1, PA_13); Motor motorS(PA_10, PB_5, PB_4); */ //pnuematik DigitalOut pnuematik1(PC_11); DigitalOut pnuematik2(PC_10); DigitalOut pnuematik3(PD_2); DigitalOut pnuematik4(PC_12); //Limit Switch DigitalIn limit1(PC_13 ,PullUp); DigitalIn limit2(PC_14 ,PullUp); DigitalIn limit3(PC_1 ,PullUp); DigitalIn limit4(PC_0 ,PullUp); //PID line follower(P, I, D, Time Sampling) PID PID(0.92,0.000,0.61,0.001); //servo(PWM) Servo servoEDF(PC_8); //EDF(PWM, timer) ESC edf(PC_6,20); //Timer Pnuematik //Timer timer; /*********************************************************************************************/ /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/ /*********************************************************************************************/ const float gMax_speed = 0.8; const float gMin_speed = 0.1; /*const float tuning1 = 0.0; const float tuning2 = 0.2; const float tuning3 = 0.06; const float tuning4 = 0.24;*/ const float v0 = 0.35; const float ax = 0.0006; const float tuning1 = 0.0; const float tuning2 = 0.16; const float tuning3 = 0.03; const float tuning4 = 0.22; float vcurr = v0; // inisialisasi pwm awal servo double pwm = 0.00; // inisialisasi sudut awal double sudut = -85; // variabel kondisi pnuematik int g = 1; //slider auto int c =0; int batas_delay = 270; //data sensor garis dan line following int datasensor[6]; int over; /////// void aktuator(); void edf_servo(); void init_sensor(); void line(float speed); /*********************************************************************************************/ /*********************************************************************************************/ /** PROGRAM UTAMA **/ /*********************************************************************************************/ /*********************************************************************************************/ int main() { //inisiasi serial pc.baud(115200); com.baud(115200); //inisiasi joystick ps2.init(); //set inisiasi servo pada posisi 0 servoEDF.position(sudut); //set edf pada posisi bukan kalibrasi, yaitu set edf 0 edf.setThrottle(0); edf.pulse(); //inisiasi pnuematik pnuematik1 = 1; pnuematik2 = 1; pnuematik3 = 1; pnuematik4 = 1; //inisiasi PID sensor PID.setInputLimits(-15,15); PID.setOutputLimits(-1.0,1.0); PID.setMode(1.0); PID.setBias(0.0); PID.reset(); //inisisasi TIMER //timer.start(); //kondisi robot bool manual=true; /* while(1) { ps2.poll(); init_sensor(); if(ps2.read(PS_PAD::PAD_X)==1){ line(0.3); } else{ motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); //v =0.1; } for(int i=0; i<6; i++){ pc.printf("%d ",datasensor[i]); } pc.printf("\n"); }*/ while(manual){ ps2.poll(); aktuator(); edf_servo(); if(limit3==0) manual = false; } motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); pnuematik1=0; wait_ms(1500); while(limit4!=0){ motorC1.speed(1); motorC2.speed(-1); } motorC1.brake(1); motorC2.brake(1); pnuematik3 = 0; wait_ms(1500); pnuematik2 = 1; wait_ms(500); pnuematik3 = 1; return 0; } /*********************************************************************************************/ /** ALGORITMA PROSEDUR DAN FUNGSI **/ /*********************************************************************************************/ void aktuator(){ float speed = vcurr; if(vcurr >= gMax_speed) vcurr = gMax_speed; if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ //pivot kiri motor2.speed((float)0.2*(speed-tuning2)); motor4.speed((float)-0.2*(speed-tuning4)); motor1.speed((float)0.2*(speed-tuning1)); motor3.speed((float)-0.2*(speed-tuning3)); pc.printf("pivot kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ //pivot kanan motor2.speed((float)-0.2*(speed-tuning2)); motor4.speed((float)0.2*(speed-tuning4)); motor1.speed((float)-0.2*(speed-tuning1)); motor3.speed((float)0.2*(speed-tuning3)); pc.printf("pivot kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){ //serong atas kanan motor2.speed(speed-tuning2); motor4.brake(1); motor1.brake(1); motor3.speed(speed-tuning3); pc.printf("serong atas kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //serong atas kiri motor2.brake(1); motor4.speed(-(speed-tuning4)); motor1.speed(-(speed-tuning1)); motor3.brake(1); pc.printf("serong atas kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){ //serong bawah kiri motor2.speed(-(speed-tuning2)); motor4.brake(1); motor1.brake(1); motor3.speed(-(speed-tuning3)); pc.printf("serong bawah kiri \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){ //serong bawah kanan motor2.brake(1); motor4.speed(speed-tuning4); motor1.speed(speed-tuning1); motor3.brake(1); pc.printf("serong bawah kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){ //maju motor1.speed(-(speed-tuning1)); motor3.speed(speed-tuning3); motor2.speed(speed-tuning2); motor4.speed(-(speed-tuning4)); pc.printf("maju \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){ //mundur motor1.speed(speed-tuning1); motor3.speed(-(speed-tuning3)); motor2.speed(-(speed-tuning2)); motor4.speed(speed-tuning4); pc.printf("mundur \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){ //kanan motor2.speed(speed-tuning2); motor4.speed(speed-tuning4); motor1.speed(speed-tuning1); motor3.speed(speed-tuning3); pc.printf("kanan \n"); vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //kiri motor2.speed(-(speed-tuning2)); motor4.speed(-(speed-tuning4)); motor1.speed(-(speed-tuning1)); motor3.speed(-(speed-tuning3)); pc.printf("kiri \n"); vcurr += ax; } else{ motor1.brake(1); motor3.brake(1); motor2.brake(1); motor4.brake(1); pc.printf("diam \n"); vcurr = v0; } if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){ //POWER WINDOW ATAS motorS.speed(1); if (limit1 == 0){ motorS.brake(1); } pc.printf("up \n"); c++; } else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){ //POWER WINDOW BAWAH motorS.speed(-0.5); if (limit2 ==0){ motorS.brake(1); } pc.printf("down \n"); c--; } else{ motorS.brake(1); if ((c <= batas_delay) && (c>=-batas_delay)){ c=0; } pc.printf("diam \n"); } if((c > batas_delay) && (limit1 == 0)){ c = 0; motorS.brake(1); } else if((c < -batas_delay) && (limit2 == 0)){ c = 0; motorS.brake(1); } else if( (c > batas_delay) && (limit1 != 0)){ motorS.speed(1); } else if ((c<-batas_delay) && (limit2 != 0)){ motorS.speed(-0.7); } if ((ps2.read(PS_PAD::PAD_SELECT)==1)) { //mekanisme ambil gripper pc.printf("mekanisme gripper"); if (g==1){ pc.printf("ambil 1"); pnuematik2 = 0; g=2; wait_ms(400); } else { pnuematik2 = 1; wait_ms(400); g=1; } } } void edf_servo(){ if(ps2.read(PS_PAD::PAD_X)==1){ //PWM ++ pwm += 0.0007; if( pwm > 0.7) pwm = 0.8; pc.printf("gaspol \n"); } else if(ps2.read(PS_PAD::PAD_SQUARE)==1){ //PWM-- pwm -= 0.0007; if(pwm < 0) pwm = 0.0; pc.printf("rem ndeng \n"); } if(ps2.read(PS_PAD::PAD_R2)==1){ //SERVO -- sudut += 0.5; if(sudut > 90) sudut = 90; pc.printf("servo max \n"); } else if(ps2.read(PS_PAD::PAD_L2)==1){ //SERVO ++ sudut -= 0.5; if(sudut < -90) sudut = -90; pc.printf("servo min \n"); } if(ps2.read(PS_PAD::PAD_START)==1){ sudut = 0; pwm = 0.25; } servoEDF.position((float)sudut); edf.setThrottle((float)pwm); edf.pulse(); } void init_sensor(){ char data; if(com.readable()){ data = com.getc(); for(int i=0; i<6; i++){ datasensor[i] = (data >> i) & 1; } } } void line(float speed){ int pv; float speed1,speed2,speed3,speed4; //////////////////logic dari PV (present Value)///////////////////////// if(datasensor[0]){ pv = -5; over=1; } else if(datasensor[5]){ pv = 5; over=2; } else if(datasensor[0] && datasensor[1]){ pv = -4; } else if(datasensor[4] && datasensor[5]){ pv = 4; } else if(datasensor[1]){ pv = -3; } else if(datasensor[4]){ pv = 3; } else if(datasensor[1] && datasensor[2]){ pv = -2; } else if(datasensor[3] && datasensor[4]){ pv = 2; } else if(datasensor[2]){ pv = -1; } else if(datasensor[3]){ pv = 1; } else if(datasensor[2] && datasensor[3]){ pv = 0; } else { ///////////////// robot bergerak keluar dari sensor///////////////////// if(over==1){ /*if(speed_ka > 0){ wait_ms(10); motor2.speed(-speed_ka); wait_ms(10); } else{ motor2.speed(speed_ka); } wait_ms(10);*/ motor1.brake(1); motor4.brake(1); //wait_ms(100); } else if(over==2){ /*if(speed_ki > 0){ wait_ms(10); motor1.speed(-speed_ki); wait_ms(10); } else{ wait_ms(10); motor1.speed(speed_ki); wait_ms(10); } wait_ms(10);*/ motor3.brake(1); motor2.brake(1); //wait_ms(100); } } PID.setProcessValue(pv); PID.setSetPoint(0); // memulai perhitungan PID speed2 = speed - 0.0 + PID.compute(); speed1 = speed - 0.0 + PID.compute(); speed3 = speed - 0.2 - PID.compute(); speed4 = speed - 0.3 - PID.compute(); if(speed1 > speed) speed1 = speed; else if(speed1 < gMin_speed) speed1 = gMin_speed; if(speed2 > speed) speed2 = speed; else if(speed2 < gMin_speed) speed2 = gMin_speed; if(speed3 > speed) speed3 = speed; else if(speed3 < gMin_speed) speed3 = gMin_speed; if(speed4 > speed) speed4 = speed; else if(speed4 < gMin_speed) speed4 = gMin_speed; motor3.speed(-(speed3)); motor2.speed(-(speed2)); motor1.speed(-(speed1)); motor4.speed(-(speed4)); }