root / branches / autonomous_recharging / code / projects / autonomous_recharging / dragonfly / departing.c @ 100
History | View | Annotate | Download (1.02 KB)
1 |
#include "departing.h" |
---|---|
2 |
|
3 |
#include <rangefinder.h> |
4 |
#include <move.h> |
5 |
|
6 |
/** We stop backing up when this is our distance from the wall
|
7 |
* in our front rangefinder.
|
8 |
**/
|
9 |
#define LEAVE_HIT_WALL_DIST_THRESH 10 |
10 |
|
11 |
/**
|
12 |
* The velocity we travel at when leaving the station.
|
13 |
**/
|
14 |
#define LEAVE_VELOCITY 200 |
15 |
|
16 |
/**
|
17 |
* The counter for when we are done leaving the station.
|
18 |
**/
|
19 |
int leave_station_count = 0; |
20 |
|
21 |
/**
|
22 |
* Leaves the charging station. The robot will
|
23 |
* continue to back up until it collides with
|
24 |
* a wall. range_init must be called before this
|
25 |
* function may be used.
|
26 |
*
|
27 |
* @return nonzero if the departure is complete,
|
28 |
* zero otherwise
|
29 |
**/
|
30 |
int depart_station(void) |
31 |
{ |
32 |
int front_distance = range_read_distance(IR2);
|
33 |
|
34 |
if(front_distance > LEAVE_HIT_WALL_DIST_THRESH)
|
35 |
leave_station_count++; |
36 |
else
|
37 |
leave_station_count = 0;
|
38 |
|
39 |
if(leave_station_count > 5) |
40 |
{ |
41 |
//stop
|
42 |
move(0,0); |
43 |
leave_station_count = 0;
|
44 |
return 1; |
45 |
} |
46 |
|
47 |
// continue moving backwards
|
48 |
move(LEAVE_VELOCITY, 0);
|
49 |
return 0; |
50 |
} |
51 |
|