root / branches / autonomous_recharging / code / projects / autonomous_recharging / dragonfly / recharge.c @ 820
History | View | Annotate | Download (3.99 KB)
1 |
/**
|
---|---|
2 |
* @file recharge.c
|
3 |
* @brief Robot recharging code
|
4 |
*
|
5 |
* Contains functions for autonomous recharging.
|
6 |
*
|
7 |
* @author Colony Project, CMU Robotics Club
|
8 |
**/
|
9 |
|
10 |
#include <move.h> |
11 |
#include <battery.h> |
12 |
#include <dio.h> |
13 |
#include <lights.h> |
14 |
#include <motor.h> |
15 |
#include <serial.h> |
16 |
|
17 |
#include <wl_defs.h> |
18 |
#include "wl_recharge_group.h" |
19 |
#include "seeking.h" |
20 |
#include "departing.h" |
21 |
#include "recharge_i2c.h" |
22 |
#include "recharge_defs.h" |
23 |
|
24 |
// Function prototypes
|
25 |
void recharge_check_low_battery(void); |
26 |
void recharge_check_charging_complete(void); |
27 |
void recharge_seek(void); |
28 |
void recharge_depart(void); |
29 |
|
30 |
//Global variables
|
31 |
#ifdef RECHARGE_DEBUG
|
32 |
int old_state = -1; |
33 |
#endif
|
34 |
|
35 |
/**
|
36 |
* Initializes recharging. Before this function may be called,
|
37 |
* wl_init must be called.
|
38 |
**/
|
39 |
void recharge_init(void) |
40 |
{ |
41 |
wl_recharge_register(); |
42 |
recharge_i2c_init(); |
43 |
} |
44 |
|
45 |
/**
|
46 |
* Uninitialize recharging. Recharging and wireless must be
|
47 |
* initialized before this function may be used.
|
48 |
**/
|
49 |
void recharge_uninit(void) |
50 |
{ |
51 |
wl_recharge_unregister(); |
52 |
} |
53 |
|
54 |
/**
|
55 |
* This function must be called frequently to perform recharging.
|
56 |
*
|
57 |
* wl_do must be called often in conjunction with this function.
|
58 |
*
|
59 |
* If recharge_do returns zero, then the robot is not charging,
|
60 |
* and the user has control of the robot. If this function returns
|
61 |
* nonzero, then the robot is attempting to charge, and the user
|
62 |
* should do nothing other than continously call this function
|
63 |
* and wl_do.
|
64 |
*
|
65 |
* recharge_init must be called before this function
|
66 |
* may be used.
|
67 |
*
|
68 |
* @return nonzero if the robot is recharging, zero otherwise
|
69 |
**/
|
70 |
int recharge_do(void) |
71 |
{ |
72 |
switch (wl_recharge_get_state())
|
73 |
{ |
74 |
case NOT_RECHARGING:
|
75 |
recharge_check_low_battery(); |
76 |
break;
|
77 |
case POLLING:
|
78 |
case REQUESTING:
|
79 |
//do nothing, wait for succesful request
|
80 |
break;
|
81 |
case SEEKING:
|
82 |
recharge_seek(); |
83 |
break;
|
84 |
case DOCKED:
|
85 |
recharge_check_charging_complete(); |
86 |
break;
|
87 |
case DEPARTING:
|
88 |
recharge_depart(); |
89 |
break;
|
90 |
default:
|
91 |
WL_DEBUG_PRINT("Unexpected state.\n");
|
92 |
break;
|
93 |
} |
94 |
|
95 |
// show colors if we are debugging
|
96 |
#ifdef RECHARGE_DEBUG
|
97 |
if (old_state != wl_recharge_get_state())
|
98 |
{ |
99 |
old_state = wl_recharge_get_state(); |
100 |
switch (old_state)
|
101 |
{ |
102 |
case NOT_RECHARGING:
|
103 |
RECHARGE_DEBUG_PRINT("Not recharging.\n");
|
104 |
//orb_set_color(GREEN);
|
105 |
break;
|
106 |
case POLLING:
|
107 |
RECHARGE_DEBUG_PRINT("Polling available stations.\n");
|
108 |
//orb_set_color(CYAN);
|
109 |
break;
|
110 |
case REQUESTING:
|
111 |
RECHARGE_DEBUG_PRINT("Requesting a station.\n");
|
112 |
//orb_set_color(RED);
|
113 |
break;
|
114 |
case SEEKING:
|
115 |
RECHARGE_DEBUG_PRINT("Seeking.\n");
|
116 |
//orb_set_color(YELLOW);
|
117 |
break;
|
118 |
case DOCKED:
|
119 |
RECHARGE_DEBUG_PRINT("Docked.\n");
|
120 |
//orb_set_color(BLUE);
|
121 |
break;
|
122 |
case DEPARTING:
|
123 |
RECHARGE_DEBUG_PRINT("Departing.\n");
|
124 |
//orb_set_color(PURPLE);
|
125 |
break;
|
126 |
default:
|
127 |
WL_DEBUG_PRINT("Unexpected state.\n");
|
128 |
break;
|
129 |
} |
130 |
} |
131 |
#endif
|
132 |
|
133 |
return wl_recharge_get_state() != NOT_RECHARGING;
|
134 |
} |
135 |
|
136 |
/**
|
137 |
* Called to check if the battery is low. If the battery
|
138 |
* is low and the robot needs to charge, the state
|
139 |
* is changed to POLLING.
|
140 |
**/
|
141 |
void recharge_check_low_battery()
|
142 |
{ |
143 |
//if (battery_low() || button2_click())
|
144 |
if (button2_click())
|
145 |
{ |
146 |
recharge_i2c_set_battery_full(0);
|
147 |
wl_recharge_begin(); |
148 |
move(0, 0); |
149 |
} |
150 |
} |
151 |
|
152 |
/**
|
153 |
* Called when the robot is seeking the charging station.
|
154 |
* Controls the robot's movement, and use of the BOM
|
155 |
* and the homing sensor. Also checks if the robot
|
156 |
* is currently charging, and changes its state
|
157 |
* accordingly.
|
158 |
**/
|
159 |
void recharge_seek()
|
160 |
{ |
161 |
seek_station(wl_recharge_get_station()); |
162 |
|
163 |
if (recharge_i2c_is_battery_charging() || button2_click())
|
164 |
{ |
165 |
wl_recharge_dock(); |
166 |
move(0, 0); |
167 |
} |
168 |
} |
169 |
|
170 |
/**
|
171 |
* Called to check if charging is complete.
|
172 |
* If charging is complete, the state is
|
173 |
* changed to DEPARTING.
|
174 |
**/
|
175 |
void recharge_check_charging_complete()
|
176 |
{ |
177 |
if (recharge_i2c_is_battery_full() || button2_click())
|
178 |
{ |
179 |
wl_recharge_depart(); |
180 |
} |
181 |
} |
182 |
|
183 |
/**
|
184 |
* Called when the robot is departing from the
|
185 |
* charging station.
|
186 |
**/
|
187 |
void recharge_depart()
|
188 |
{ |
189 |
if (depart_station())
|
190 |
wl_recharge_stop(); |
191 |
} |
192 |
|