root / branches / autonomous_recharging / code / projects / autonomous_recharging / dragonfly / departing.c @ 820
History | View | Annotate | Download (1.29 KB)
1 |
/**
|
---|---|
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 |
#include "departing.h" |
12 |
|
13 |
#include <rangefinder.h> |
14 |
#include <move.h> |
15 |
|
16 |
#include "recharge_defs.h" |
17 |
|
18 |
/** We stop backing up when this is our distance from the wall
|
19 |
* in our front rangefinder.
|
20 |
**/
|
21 |
#define LEAVE_HIT_WALL_DIST_THRESH 70 |
22 |
|
23 |
/**
|
24 |
* 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 |
int old_distance = -1; |
33 |
/**
|
34 |
* 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 |
int front_distance = range_read_distance(IR2);
|
45 |
|
46 |
if(front_distance != -1 && front_distance < 200) |
47 |
leave_station_count++; |
48 |
else
|
49 |
leave_station_count = 0;
|
50 |
|
51 |
if(leave_station_count > 10) |
52 |
{ |
53 |
//stop
|
54 |
move(0,0); |
55 |
leave_station_count = 0;
|
56 |
return 1; |
57 |
} |
58 |
|
59 |
// continue moving backwards
|
60 |
move(LEAVE_VELOCITY, 0);
|
61 |
return 0; |
62 |
} |
63 |
|