root / branches / autonomous_recharging / code / projects / autonomous_recharging / dragonfly / departing.c @ 743
History | View | Annotate | Download (1.07 KB)
1 | 93 | jykong | #include "departing.h" |
---|---|---|---|
2 | |||
3 | 100 | bcoltin | #include <rangefinder.h> |
4 | 93 | jykong | #include <move.h> |
5 | |||
6 | 137 | bcoltin | #include "recharge_defs.h" |
7 | |||
8 | 100 | bcoltin | /** We stop backing up when this is our distance from the wall
|
9 | * in our front rangefinder.
|
||
10 | **/
|
||
11 | 137 | bcoltin | #define LEAVE_HIT_WALL_DIST_THRESH 70 |
12 | 100 | bcoltin | |
13 | 93 | jykong | /**
|
14 | 100 | bcoltin | * 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 | 198 | bcoltin | int old_distance = -1; |
23 | 100 | bcoltin | /**
|
24 | 93 | jykong | * 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 | 195 | cmar | int front_distance = range_read_distance(IR2);
|
35 | 93 | jykong | |
36 | 195 | cmar | if(front_distance != -1 && front_distance < 160) |
37 | 100 | bcoltin | leave_station_count++; |
38 | 195 | cmar | else
|
39 | leave_station_count = 0;
|
||
40 | 93 | jykong | |
41 | 198 | bcoltin | if(leave_station_count > 10) |
42 | 93 | jykong | { |
43 | //stop
|
||
44 | move(0,0); |
||
45 | 100 | bcoltin | leave_station_count = 0;
|
46 | 93 | jykong | return 1; |
47 | } |
||
48 | |||
49 | // continue moving backwards
|
||
50 | 100 | bcoltin | move(LEAVE_VELOCITY, 0);
|
51 | 93 | jykong | return 0; |
52 | } |