Robot code for searching an object and charging at it.
Dependencies: HCSR04 Motor mbed
Diff: functions.cpp
- Revision:
- 4:0507835a3dce
- Parent:
- 1:bd88d4062c97
- Child:
- 6:af897173cb75
diff -r 7f7a82bf59b9 -r 0507835a3dce functions.cpp --- a/functions.cpp Sat May 23 14:33:29 2015 +0000 +++ b/functions.cpp Sat May 23 15:05:18 2015 +0000 @@ -5,6 +5,23 @@ #include "mbed.h" #include "Motor.h" #include "hcsr04.h" +#include "functions.h" + +// 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. +// Setting pins for line sensor +DigitalInOut line1(p20); +DigitalInOut line2(p19); + +// Setting pins for motor, as follows: +// Example: Motor____(PWM, Forward, Reverse) +Motor MotorLeft(p23, p28, p27); +Motor MotorRight(p22, p29, p30); + +// Setting pins for ultrasonic sensor, as follows: +// Example: usensor(Trigger, Echo) +HCSR04 usensor(p25,p26); // Returns value from the QRE1113 // lower numbers mean more refleacive @@ -41,12 +58,12 @@ line2.input(); // Start timer - t.start(); + temp_t.start(); // Time how long the input is HIGH, but quit // after 1ms as nothing happens after that while (line2 == 1 && temp_t.read_us() < 1000); - return t.read_us(); + return temp_t.read_us(); } int detect_line() @@ -66,32 +83,39 @@ } } -void reverse() +void reverse(float speed) { printf("Reverse\n"); - MotorLeft.speed((chargespeed)); - MotorRight.speed(-(chargespeed)); + MotorLeft.speed((speed)); + MotorRight.speed(-(speed)); wait_ms(1000); return; } -void reverseandturn() +void reverseandturn(float speed) { printf("Reverse and turn\n"); - MotorLeft.speed((chargespeed-0.3)); - MotorRight.speed(-(chargespeed-0.1)); + MotorLeft.speed((speed-0.3)); + MotorRight.speed(-(speed-0.1)); wait_ms(2000); return; } -void charge() +void charge(float speed) { - MotorLeft.speed(chargespeed); - MotorRight.speed(chargespeed); + MotorLeft.speed(speed); + MotorRight.speed(speed); return; } -int detect_object(int smooth) +void stop() +{ + MotorLeft.speed(0.0); + MotorRight.speed(0.0); + return; +} + +int detect_object(int range, float speed) { // Start a timer - finds an object for 10 seconds // if it doesn't find anything returns 0 @@ -99,9 +123,11 @@ usensor_t.start(); // Variable to store sensed value - int sense; + int sense, dist; + sense = 0; + dist = 0; - while (usensor_t.readms < 10000) + while (usensor_t.read_ms() < 10000) { // Start the ultrasonic sensor usensor.start(); @@ -113,8 +139,8 @@ break; } else { sense = 0; - MotorLeft.speed(searchspeed); - MotorRight.speed(-(searchspeed)); + MotorLeft.speed(speed); + MotorRight.speed(-(speed)); } } return sense;