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