Robot code for searching an object and charging at it.
Dependencies: HCSR04 Motor mbed
Diff: functions.cpp
- Revision:
- 26:5ee2a32949e6
- Parent:
- 25:7662e93a77eb
- Child:
- 27:96a0ff2f474c
diff -r 7662e93a77eb -r 5ee2a32949e6 functions.cpp --- a/functions.cpp Sun Jun 07 15:17:02 2015 +0000 +++ b/functions.cpp Sun Jun 07 16:36:03 2015 +0000 @@ -40,6 +40,22 @@ myled4 = 0; } +void turn_led_left() { + myled1 = 1; + myled2 = 1; + wait(0.1); + myled1 = 0; + myled2 = 0; +} + +void turn_led_right() { + myled3 = 1; + myled4 = 1; + wait(0.1); + myled3 = 0; + myled4 = 0; +} + void flash_leds() { Timer t; t.start(); @@ -110,37 +126,39 @@ int line1val = read_line1(); int line2val = read_line2(); - if ((line1val-line2val) > 50) { - printf("Line detected from front"); + printf("LineSensor readings: %d \n", line1val-line2val); + + if ((line1val-line2val) > 70) { + //printf("Line detected from front"); return 1; - } else if ((line1val-line2val) < -50) { - printf("Line detected from back"); + } else if ((line1val-line2val) < -70) { + //printf("Line detected from back"); return -1; } else { - printf("Line not detected"); + //printf("Line not detected"); return 0; } } -void reverse(float speed = moveforwardspeed) { - printf("Reverse\n"); +void reverse(float speed) { + //printf("Reverse\n"); MotorLeft.speed(-(speed)); MotorRight.speed(-(speed)); } -void turn(float speed = moveforwardspeed) { - printf("Turning\n"); +void turn(float speed) { + //printf("Turning\n"); MotorLeft.speed(speed); MotorRight.speed(-(speed)); } -void reverseandturn(float speed = moveforwardspeed) { - printf("Reverse and turn\n"); +void reverseandturn(float speed) { + //printf("Reverse and turn\n"); MotorLeft.speed((speed-0.3)); MotorRight.speed(-(speed-0.1)); } -void move_forward(float speed = moveforwardspeed) { +void move_forward(float speed) { MotorLeft.speed(speed); MotorRight.speed(speed); } @@ -152,7 +170,7 @@ int fwd; float random_time; - printf("Moving randomly\n"); + //printf("Moving randomly\n"); counter = rand() % 5; for (int i = 0; i < counter; i++) @@ -166,16 +184,16 @@ fwd = rand()%2; // If fwd == 1 move forward if (fwd == 1) { - printf("Moving forward\n"); + //printf("Moving forward\n"); move_detect(speed, fwd_bck, random_time); // If fwd == 0 move bacward } else { - printf("Moving bacwards\n"); + //printf("Moving bacwards\n"); move_detect(speed, fwd_bck, random_time); } // Turn } else { - printf("Turning\n"); + //printf("Turning\n"); move_detect(speed, fwd_bck, random_time); } } @@ -186,7 +204,7 @@ MotorRight.speed(0.0); } -int detect_object(int range_t = range, float speed = moveforwardspeed) { +int detect_object(int range_t, float speed) { // Start a timer - finds an object for 5 seconds // if it doesn't find anything returns 0 Timer usensor_t, inner_t; @@ -197,13 +215,15 @@ sense = 0; dist = 0; reverse = 0; - - while (usensor_t.read_ms() < 5000) { + + while (usensor_t.read_ms() < 15000) { // Start the ultrasonic sensor usensor.start(); inner_t.start(); dist = usensor.get_dist_cm(); + printf("UltraSonic readings: %d \n", dist); + // If an object is detected based on out set range return 1 if (dist <= range_t && dist >= 1) { sense = 1;