base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
Diff: main.cpp
- Revision:
- 3:43d4cb3ece1b
- Parent:
- 2:df6c49846367
- Child:
- 4:7a7a8aa33fd5
diff -r df6c49846367 -r 43d4cb3ece1b main.cpp --- a/main.cpp Fri Apr 22 08:39:32 2016 +0000 +++ b/main.cpp Tue Apr 26 14:57:35 2016 +0000 @@ -4,7 +4,6 @@ #include "mbed.h" #include "Motor.h" #include "PS_PAD.h" -#include "PID.h" #include "esc.h" #include "Servo.h" @@ -21,24 +20,12 @@ //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 motor4(PA_10, PB_5, PB_4); +Motor motorS(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); -*/ +//Motor motor4(PB_6, PA_7, PB_12); //pnuematik DigitalOut pnuematik1(PC_11); @@ -52,8 +39,10 @@ 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); +DigitalIn field(PB_10 ,PullUp); + +//laser pointer +DigitalOut laser(PB_3); //servo(PWM) Servo servoEDF(PC_8); @@ -61,30 +50,30 @@ //EDF(PWM, timer) ESC edf(PC_6,20); -//Timer Pnuematik -//Timer timer; +//Timer +Ticker 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 gMax_speed = 0.85; +//const float gMin_speed = 0.1; +const float v0 = 0.4; +const float ax = 0.0007; +float vcurr = v0; const float tuning1 = 0.0; -const float tuning2 = 0.16; -const float tuning3 = 0.03; -const float tuning4 = 0.22; +const float tuning2 = 0.04; +const float tuning3 = 0.0; +const float tuning4 = 0.04; +const float driver0 = 1; +const float driver1 = 0.85; +const float driver2 = 0.65; +const float driver3 = 0.2; -float vcurr = v0; // inisialisasi pwm awal servo double pwm = 0.00; @@ -102,12 +91,14 @@ //data sensor garis dan line following int datasensor[6]; int over; +int g_flag; /////// -void aktuator(); +void aktuator(int f); void edf_servo(); void init_sensor(); -void line(float speed); +void linetracer(float speed); +void flag_sensor(); /*********************************************************************************************/ /*********************************************************************************************/ @@ -119,6 +110,9 @@ pc.baud(115200); com.baud(115200); + if(field == 1) sudut = -85; + else sudut = 85; + //inisiasi joystick ps2.init(); @@ -129,67 +123,59 @@ edf.setThrottle(0); edf.pulse(); + //inisisasi laser + laser = 1; + //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(); + pnuematik3 = 0; + pnuematik4 = 0; //inisisasi TIMER - //timer.start(); + timer.attach(&flag_sensor,0.0005); + timer.detach(); //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"); - - }*/ - + bool pool= false; + while(manual){ ps2.poll(); - aktuator(); + aktuator(field); edf_servo(); - if(limit3==0) manual = false; + if(ps2.read(PS_PAD::ANALOG_LEFT)==1) manual = false; } + timer.attach(&flag_sensor,0.0005); + + while(!pool){ + init_sensor(); + + if(vcurr > 0.3) vcurr = (float) 0.3; + + linetracer(vcurr); + //laser = 1; + + vcurr+=ax; + + if(limit3==0) pool=true; + + } motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); + timer.detach(); + pnuematik1=0; wait_ms(1500); - + while(limit4!=0){ motorC1.speed(1); motorC2.speed(-1); @@ -198,11 +184,20 @@ motorC1.brake(1); motorC2.brake(1); - pnuematik3 = 0; - wait_ms(1500); - pnuematik2 = 1; - wait_ms(500); - pnuematik3 = 1; + if(field==1){ + pnuematik3 = 0; + wait_ms(1500); + pnuematik2 = 1; + wait_ms(500); + pnuematik3 = 1; + } + else{ + pnuematik4 = 0; + wait_ms(1500); + pnuematik2 = 1; + wait_ms(500); + pnuematik4 = 1; + } return 0; } @@ -210,27 +205,28 @@ /*********************************************************************************************/ /** ALGORITMA PROSEDUR DAN FUNGSI **/ /*********************************************************************************************/ -void aktuator(){ +void aktuator(int f){ float speed = vcurr; if(vcurr >= gMax_speed) vcurr = gMax_speed; + if(f == 1){ 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)); + motor2.speed((float)0.5*(speed-tuning2)); + motor4.speed((float)-0.5*(speed-tuning4)); + motor1.speed((float)0.5*(speed-tuning1)); + motor3.speed((float)-0.5*(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)); + motor2.speed((float)-0.5*(speed-tuning2)); + motor4.speed((float)0.5*(speed-tuning4)); + motor1.speed((float)-0.5*(speed-tuning1)); + motor3.speed((float)0.5*(speed-tuning3)); pc.printf("pivot kanan \n"); vcurr += ax; @@ -324,56 +320,169 @@ vcurr = v0; } - - if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){ - //POWER WINDOW ATAS - motorS.speed(1); + } + else{ + if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ + //pivot kiri + motor2.speed((float)0.5*(speed-tuning2)); + motor4.speed((float)-0.5*(speed-tuning4)); + motor1.speed((float)0.5*(speed-tuning1)); + motor3.speed((float)-0.5*(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.5*(speed-tuning2)); + motor4.speed((float)0.5*(speed-tuning4)); + motor1.speed((float)-0.5*(speed-tuning1)); + motor3.speed((float)0.5*(speed-tuning3)); + pc.printf("pivot kanan \n"); - if (limit1 == 0){ - motorS.brake(1); - } + 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"); - pc.printf("up \n"); - c++; + vcurr += ax; } - else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){ - //POWER WINDOW BAWAH - motorS.speed(-0.5); + 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"); - if (limit2 ==0){ - motorS.brake(1); - } + 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"); - pc.printf("down \n"); - c--; + 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{ - motorS.brake(1); - if ((c <= batas_delay) && (c>=-batas_delay)){ - c=0; - } + motor1.brake(1); + motor3.brake(1); + motor2.brake(1); + motor4.brake(1); + pc.printf("diam \n"); - pc.printf("diam \n"); - } - if((c > batas_delay) && (limit1 == 0)){ - c = 0; + 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); } - else if((c < -batas_delay) && (limit2 == 0)){ - c = 0; - motorS.brake(1); + + pc.printf("down \n"); + c--; + } + else{ + motorS.brake(1); + if ((c <= batas_delay) && (c>=-batas_delay)){ + c=0; } - else if( (c > batas_delay) && (limit1 != 0)){ - motorS.speed(1); - } - else if ((c<-batas_delay) && (limit2 != 0)){ - motorS.speed(-0.7); - } + + 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)) - { + if ((ps2.read(PS_PAD::PAD_SELECT)==1)) + { //mekanisme ambil gripper pc.printf("mekanisme gripper"); if (g==1){ @@ -388,7 +497,7 @@ wait_ms(400); g=1; } - } + } } void edf_servo(){ @@ -424,14 +533,20 @@ if(ps2.read(PS_PAD::PAD_START)==1){ sudut = 0; - pwm = 0.25; + pwm = 0.22; + + pnuematik3 = 1; + pnuematik4 = 1; } - + + servoEDF.position((float)sudut); edf.setThrottle((float)pwm); edf.pulse(); } + +/////////////////LINE FOLLOWER///////////////////////// void init_sensor(){ char data; if(com.readable()){ @@ -443,108 +558,112 @@ } } -void line(float speed){ - int pv; +void linetracer(float speed){ float speed1,speed2,speed3,speed4; - + //////////////////logic dari PV (present Value)///////////////////////// - if(datasensor[0]){ - pv = -5; - over=1; + if(datasensor[2] && datasensor[3]){ + speed1 = speed*driver0; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver0; } - else if(datasensor[5]){ - pv = 5; - over=2; + else if(datasensor[2]){ + speed1 = speed*driver1; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver1; } - else if(datasensor[0] && datasensor[1]){ - pv = -4; - } - else if(datasensor[4] && datasensor[5]){ - pv = 4; + else if(datasensor[3]){ + speed1 = speed*driver0; + speed2 = speed*driver1; + speed3 = speed*driver1; + speed4 = speed*driver0; } else if(datasensor[1]){ - pv = -3; + speed1 = speed*driver2; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver2; } else if(datasensor[4]){ - pv = 3; - } - else if(datasensor[1] && datasensor[2]){ - pv = -2; - } - else if(datasensor[3] && datasensor[4]){ - pv = 2; + speed1 = speed*driver0; + speed2 = speed*driver2; + speed3 = speed*driver2; + speed4 = speed*driver0; } - else if(datasensor[2]){ - pv = -1; + else if(datasensor[0]){ + speed1 = speed*driver3; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver3; } - else if(datasensor[3]){ - pv = 1; + else if(datasensor[5]){ + speed1 = speed*driver0; + speed2 = speed*driver3; + speed3 = speed*driver3; + speed4 = speed*driver0; } - 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); - + if(g_flag == 0){ + speed1 = speed*driver0; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver0; + } + else if(g_flag == 3){ + speed1 = speed*driver1; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver1; + } + else if(g_flag == 4){ + speed1 = speed*driver0; + speed2 = speed*driver1; + speed3 = speed*driver1; + speed4 = speed*driver0; } - 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); + else if(g_flag == 2){ + speed1 = speed*driver2; + speed2 = speed*driver0; + speed3 = speed*driver0; + speed4 = speed*driver2; + } + else if(g_flag == 5){ + speed1 = speed*driver0; + speed2 = speed*driver2; + speed3 = speed*driver2; + speed4 = speed*driver0; + } + else if(g_flag == 1){ + speed1 = -speed*driver2; + speed2 = speed*driver2; + speed3 = speed*driver2; + speed4 = -speed*driver2; + } + else if(g_flag == 6){ + speed1 = speed*driver2; + speed2 = -speed*driver2; + speed3 = -speed*driver2; + speed4 = speed*driver2; } } - 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)); + motor1.speed((float)-(speed1-0.0)); + motor2.speed((float)-(speed2-0.04)); + motor3.speed((float)-(speed3-0.0)); + motor4.speed((float)-(speed4-0.0)); + +} + +void flag_sensor(){ + if((datasensor[2] == 1) && (datasensor[3] == 1)) g_flag = 0; + else if(datasensor[2] == 1) g_flag = 3; + else if(datasensor[3] == 1) g_flag = 4; + else if(datasensor[1] == 1) g_flag = 2; + else if(datasensor[4] == 1) g_flag = 5; + else if(datasensor[0] == 1) g_flag = 1; + else if(datasensor[5] == 1) g_flag = 6; } \ No newline at end of file