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