base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
Diff: main.cpp
- Revision:
- 4:7a7a8aa33fd5
- Parent:
- 3:43d4cb3ece1b
- Child:
- 5:256f6eac0358
diff -r 43d4cb3ece1b -r 7a7a8aa33fd5 main.cpp --- a/main.cpp Tue Apr 26 14:57:35 2016 +0000 +++ b/main.cpp Wed Apr 27 14:28:05 2016 +0000 @@ -6,6 +6,7 @@ #include "PS_PAD.h" #include "esc.h" #include "Servo.h" +#include "Ping.h" /*********************************************************************************************/ /** DEKLARASI INPUT OUTPUT **/ @@ -22,9 +23,9 @@ Motor motor2(PA_11, PA_6, PA_5); Motor motor3(PA_9, PC_2, PC_3); Motor motor4(PA_10, PB_5, PB_4); -Motor motorS(PB_7, PA_14, PA_15); +Motor motorC2(PB_7, PA_14, PA_15); Motor motorC1(PB_9, PA_12, PC_5); -Motor motorC2(PB_8, PB_1, PA_13); +Motor motorS(PB_8, PB_1, PA_13); //Motor motor4(PB_6, PA_7, PB_12); //pnuematik @@ -53,7 +54,8 @@ //Timer Ticker timer; - +//Sensor Ping +Ping ping(PB_2); /*********************************************************************************************/ /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/ @@ -70,9 +72,9 @@ const float tuning4 = 0.04; const float driver0 = 1; -const float driver1 = 0.85; -const float driver2 = 0.65; -const float driver3 = 0.2; +const float driver1 = 0.82; +const float driver2 = 0.6; +const float driver3 = 0.15; // inisialisasi pwm awal servo @@ -93,13 +95,16 @@ int over; int g_flag; -/////// + void aktuator(int f); void edf_servo(); + void init_sensor(); void linetracer(float speed); void flag_sensor(); +float read_jarak(); + /*********************************************************************************************/ /*********************************************************************************************/ /** PROGRAM UTAMA **/ @@ -129,8 +134,8 @@ //inisiasi pnuematik pnuematik1 = 1; pnuematik2 = 1; - pnuematik3 = 0; - pnuematik4 = 0; + pnuematik3 = 1; + pnuematik4 = 1; //inisisasi TIMER timer.attach(&flag_sensor,0.0005); @@ -139,7 +144,8 @@ //kondisi robot bool manual=true; bool pool= false; - + + //running manual while(manual){ ps2.poll(); @@ -147,24 +153,28 @@ aktuator(field); edf_servo(); - if(ps2.read(PS_PAD::ANALOG_LEFT)==1) manual = false; + if(limit4==0) manual = false; } + //running otomatis timer.attach(&flag_sensor,0.0005); while(!pool){ init_sensor(); - if(vcurr > 0.3) vcurr = (float) 0.3; + if(vcurr > 0.4) vcurr = (float) 0.4; + //else if (vcurr < 0.2) vcurr = (float) 0.2; linetracer(vcurr); //laser = 1; - vcurr+=ax; + if((limit3==0) || (read_jarak() <= 9.0)) pool=true; - if(limit3==0) pool=true; - + //if(read_jarak() <= 45.0) vcurr -= ax; + //else vcurr += ax; + vcurr += ax; + } } motor1.brake(1); motor2.brake(1); @@ -175,9 +185,9 @@ pnuematik1=0; wait_ms(1500); - + while(limit4!=0){ - motorC1.speed(1); + motorC1.speed(0.95); motorC2.speed(-1); } @@ -504,7 +514,7 @@ if(ps2.read(PS_PAD::PAD_X)==1){ //PWM ++ pwm += 0.0007; - if( pwm > 0.7) pwm = 0.8; + if( pwm > 0.8) pwm = 0.8; pc.printf("gaspol \n"); } else if(ps2.read(PS_PAD::PAD_SQUARE)==1){ @@ -534,9 +544,6 @@ sudut = 0; pwm = 0.22; - - pnuematik3 = 1; - pnuematik4 = 1; } @@ -545,8 +552,8 @@ edf.pulse(); } +/////////////////////////////////////////LINE FOLLOWER///////////////////////// -/////////////////LINE FOLLOWER///////////////////////// void init_sensor(){ char data; if(com.readable()){ @@ -651,10 +658,10 @@ } } - motor1.speed((float)-(speed1-0.0)); - motor2.speed((float)-(speed2-0.04)); - motor3.speed((float)-(speed3-0.0)); - motor4.speed((float)-(speed4-0.0)); + motor1.speed(-(float)(speed1-0.04)); + motor2.speed(-(float)(speed2-0.04)); + motor3.speed(-(float)(speed3-0.0)); + motor4.speed(-(float)(speed4-0.0)); } @@ -666,4 +673,15 @@ else if(datasensor[4] == 1) g_flag = 5; else if(datasensor[0] == 1) g_flag = 1; else if(datasensor[5] == 1) g_flag = 6; +} + + +////////////////////////SENSOR PING/////////////////////////////////////// +float read_jarak() { + float jarak; + + ping.Send(); + wait_ms(45); + jarak = ping.Read_cm()/2; + return jarak; } \ No newline at end of file