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