Robot code for searching an object and charging at it.
Dependencies: HCSR04 Motor mbed
Diff: functions.cpp
- Revision:
- 19:67ea4e8be9e1
- Parent:
- 17:47aa9ef2bec6
- Child:
- 20:37a89edd1cde
--- a/functions.cpp Sat May 30 16:18:31 2015 +0000 +++ b/functions.cpp Sat May 30 19:08:26 2015 +0000 @@ -7,6 +7,11 @@ #include "hcsr04.h" #include "functions.h" +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); + // Two sensors are used to detect a line, line_sens1 and line_sens2 // if there is a difference between the readings of these sensors // the robot has detected a line. @@ -23,6 +28,43 @@ // Example: usensor(Trigger, Echo) HCSR04 usensor(p25,p26); +void turn_leds_on() { + myled1 = 1; + myled2 = 1; + myled3 = 1; + myled4 = 1; + wait(0.1); + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 0; +} + +void flash_leds() { + Timer t; + t.start(); + while(t.read_ms() < 1000) { + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 0; + wait(0.15); + myled1 = 1; + wait(0.15); + myled2 = 1; + wait(0.15); + myled3 = 1; + wait(0.15); + myled4 = 1; + wait(0.15); + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 0; + } + t.stop(); +} + // Returns value from the QRE1113 // lower numbers mean more refleacive // more than 3000 means nothing was reflected. @@ -98,7 +140,7 @@ MotorRight.speed(-(speed-0.1)); } -void charge(float speed) { +void move_forward(float speed) { MotorLeft.speed(speed); MotorRight.speed(speed); } @@ -112,20 +154,23 @@ float random_speed; printf("Moving randomly\n"); - counter = rand() % 10; + counter = rand() % 5; for (int i = 0; i < counter; i++) { random_time = rand() % 1000 + 1000; fwd_bck = rand()%2; - random_speed = (float)rand()/(float)(RAND_MAX); + //random_speed = (float)rand()/(float)(RAND_MAX); - if (random_speed < 0.6) { - random_speed = 0.5; - } else if (random_speed >= 0.9) { - random_speed = 0.9; - } + //if (random_speed < 0.6) { + //random_speed = 0.5; + //} else if (random_speed >= 0.8) { + //random_speed = 0.8; + //} + + random_speed = 0.5; + // If fwd_back == 1 move forward or backwards if (fwd_bck == 1) { fwd = rand()%2; @@ -172,6 +217,8 @@ // If an object is detected based on out set range return 1 if (dist <= range && dist >= 1) { sense = 1; + stop(); + flash_leds(); break; } else { sense = 0; @@ -200,24 +247,31 @@ t.start(); int detect = 0; + if(fwd_bck == 1) { + move_forward(speed); + wait_ms(500); + } else { + turn(speed + 0.2); + } + while (t.read_ms() < time) { - if(fwd_bck == 1) { - charge(speed); - } else { - turn(speed); - } detect = detect_line(); // If line is detected from front then reverse if(detect == 1) { - move_detect(-speed,1,1500); + stop(); + turn_leds_on(); + move_detect(-0.5,1,1000); stop(); break; // If line is detected from back just keep on moving forward } else if (detect == -1) { - move_detect(speed,1,1500); + stop(); + turn_leds_on(); + move_detect(0.5,1,1000); stop(); break; } } + stop(); t.stop(); } \ No newline at end of file