base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
Diff: main.cpp
- Revision:
- 2:df6c49846367
- Parent:
- 1:fc1535231c0d
- Child:
- 3:43d4cb3ece1b
--- a/main.cpp Tue Apr 19 13:32:26 2016 +0000 +++ b/main.cpp Fri Apr 22 08:39:32 2016 +0000 @@ -21,7 +21,7 @@ //motor (PWM, forward, reverse) Motor motor1(PA_8, PB_0, PC_15); Motor motor2(PA_11, PA_6, PA_5); -//Motor motor3(PB_6, PA_7, PB_12); +//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); @@ -53,7 +53,7 @@ DigitalIn limit4(PC_0 ,PullUp); //PID line follower(P, I, D, Time Sampling) -PID PID(0.992,0.000,0.81,0.001); +PID PID(0.92,0.000,0.61,0.001); //servo(PWM) Servo servoEDF(PC_8); @@ -62,14 +62,29 @@ ESC edf(PC_6,20); //Timer Pnuematik -Timer timer; +//Timer timer; + + /*********************************************************************************************/ /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/ /*********************************************************************************************/ -float gMax_speed = 1; -float v0 = 0.6; -float ax = 0.0005; +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; @@ -80,9 +95,20 @@ // 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 **/ @@ -117,11 +143,33 @@ PID.reset(); //inisisasi TIMER - timer.start(); + //timer.start(); //kondisi robot - bool manual=true; - + 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){ @@ -156,8 +204,6 @@ wait_ms(500); pnuematik3 = 1; - - return 0; } @@ -165,33 +211,29 @@ /** ALGORITMA PROSEDUR DAN FUNGSI **/ /*********************************************************************************************/ void aktuator(){ - float speed = v0; - float tuning1 = 0.0; - float tuning2 = 0.23; - float tuning3 = 0.1; - float tuning4 = 0.26; - - if(v0 >= gMax_speed) v0 = gMax_speed; + 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.6*(speed-tuning2)); - motor4.speed((float)-0.6*(speed-tuning4)); - motor1.speed((float)0.6*(speed-tuning1)); - motor3.speed((float)-0.6*(speed-tuning3)); + 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"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ //pivot kanan - motor2.speed((float)-0.6*(speed-tuning2)); - motor4.speed((float)0.6*(speed-tuning4)); - motor1.speed((float)-0.6*(speed-tuning1)); - motor3.speed((float)0.6*(speed-tuning3)); + 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"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){ //serong atas kanan @@ -201,7 +243,7 @@ motor3.speed(speed-tuning3); pc.printf("serong atas kanan \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //serong atas kiri @@ -211,7 +253,7 @@ motor3.brake(1); pc.printf("serong atas kiri \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){ //serong bawah kiri @@ -221,7 +263,7 @@ motor3.speed(-(speed-tuning3)); pc.printf("serong bawah kiri \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){ //serong bawah kanan @@ -231,7 +273,7 @@ motor3.brake(1); pc.printf("serong bawah kanan \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){ //maju @@ -241,7 +283,7 @@ motor4.speed(-(speed-tuning4)); pc.printf("maju \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){ //mundur @@ -251,7 +293,7 @@ motor4.speed(speed-tuning4); pc.printf("mundur \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){ //kanan @@ -261,7 +303,7 @@ motor3.speed(speed-tuning3); pc.printf("kanan \n"); - v0 += ax; + vcurr += ax; } else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){ //kiri @@ -271,7 +313,7 @@ motor3.speed(-(speed-tuning3)); pc.printf("kiri \n"); - v0 += ax; + vcurr += ax; } else{ motor1.brake(1); @@ -280,24 +322,55 @@ motor4.brake(1); pc.printf("diam \n"); - v0 = 0.6; + 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(-1); + motorS.speed(-0.5); + + if (limit2 ==0){ + motorS.brake(1); + } + pc.printf("down \n"); + c--; } else{ motorS.brake(1); - pc.printf("diam \n"); + 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)) { @@ -321,13 +394,13 @@ void edf_servo(){ if(ps2.read(PS_PAD::PAD_X)==1){ //PWM ++ - pwm += 0.002; - if( pwm > 0.7) pwm = 0.7; + 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.002; + pwm -= 0.0007; if(pwm < 0) pwm = 0.0; pc.printf("rem ndeng \n"); @@ -357,4 +430,121 @@ 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)); } \ No newline at end of file