Revision 100
Did more work on autonomous recharging for the robot.
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/wl_recharge_group.c | ||
|---|---|---|
| 146 | 146 |
} |
| 147 | 147 |
|
| 148 | 148 |
/** |
| 149 |
* Returns the station we are currently homing to, or -1 |
|
| 150 |
* if no such station exists. |
|
| 151 |
* |
|
| 152 |
* @return the 16-bit XBee address of the charging station |
|
| 153 |
**/ |
|
| 154 |
int wl_recharge_get_station(void) |
|
| 155 |
{
|
|
| 156 |
return station; |
|
| 157 |
} |
|
| 158 |
|
|
| 159 |
|
|
| 160 |
/** |
|
| 149 | 161 |
* Sends a request polling for charging stations. |
| 150 | 162 |
**/ |
| 151 | 163 |
void wl_recharge_send_poll(void) |
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/recharge.c | ||
|---|---|---|
| 1 |
#include <move.h> |
|
| 2 |
#include <battery.h> |
|
| 3 |
#include <dio.h> |
|
| 4 |
|
|
| 1 | 5 |
#include <wl_defs.h> |
| 2 | 6 |
#include "wl_recharge_group.h" |
| 7 |
#include "seeking.h" |
|
| 8 |
#include "departing.h" |
|
| 3 | 9 |
|
| 4 |
#include <move.h> |
|
| 10 |
// Function prototypes |
|
| 11 |
void recharge_check_low_battery(void); |
|
| 12 |
void recharge_check_charging_complete(void); |
|
| 13 |
void recharge_seek(void); |
|
| 14 |
void recharge_depart(void); |
|
| 5 | 15 |
|
| 6 | 16 |
/** |
| 7 | 17 |
* Initializes recharging. Before this function may be called, |
| ... | ... | |
| 61 | 71 |
WL_DEBUG_PRINT("Unexpected state.\n");
|
| 62 | 72 |
break; |
| 63 | 73 |
} |
| 74 |
|
|
| 75 |
// show colors if we are debugging |
|
| 76 |
#ifdef RECHARGE_DEBUG |
|
| 77 |
switch (wl_recharge_get_state()) |
|
| 78 |
{
|
|
| 79 |
|
|
| 80 |
} |
|
| 81 |
#endif |
|
| 64 | 82 |
|
| 65 | 83 |
return wl_recharge_get_state() != NOT_RECHARGING; |
| 66 | 84 |
} |
| ... | ... | |
| 72 | 90 |
**/ |
| 73 | 91 |
void recharge_check_low_battery() |
| 74 | 92 |
{
|
| 75 |
//TODO: implement |
|
| 76 |
if (battery is low) |
|
| 93 |
//TODO: use battery_low instead of button press |
|
| 94 |
//if (battery_low()) |
|
| 95 |
if (button2_click()) |
|
| 77 | 96 |
{
|
| 78 | 97 |
wl_recharge_begin(); |
| 79 | 98 |
move(0, 0); |
| ... | ... | |
| 89 | 108 |
**/ |
| 90 | 109 |
void recharge_seek() |
| 91 | 110 |
{
|
| 92 |
seek_station(); |
|
| 111 |
seek_station(wl_recharge_get_station());
|
|
| 93 | 112 |
|
| 94 |
if (battery is charging) |
|
| 113 |
//TODO: check if battery is charging |
|
| 114 |
//if (battery is charging) |
|
| 115 |
if (button2_click()) |
|
| 95 | 116 |
wl_recharge_dock(); |
| 96 | 117 |
} |
| 97 | 118 |
|
| ... | ... | |
| 102 | 123 |
**/ |
| 103 | 124 |
void recharge_check_charging_complete() |
| 104 | 125 |
{
|
| 105 |
if (battery is charged) |
|
| 126 |
//TODO: implement |
|
| 127 |
//if (battery is charged) |
|
| 128 |
if (button2_click()) |
|
| 106 | 129 |
wl_recharge_depart(); |
| 107 | 130 |
} |
| 108 | 131 |
|
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/wl_recharge_group.h | ||
|---|---|---|
| 45 | 45 |
void wl_recharge_depart(void); |
| 46 | 46 |
/**@brief Get the current robot state **/ |
| 47 | 47 |
int wl_recharge_get_state(void); |
| 48 |
/**@brief Get the station we are homing to **/ |
|
| 49 |
int wl_recharge_get_station(void); |
|
| 48 | 50 |
|
| 49 | 51 |
/** @} **/ // end defgroup |
| 50 | 52 |
|
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/seeking.c | ||
|---|---|---|
| 1 | 1 |
#include "seeking.h" |
| 2 | 2 |
|
| 3 |
#include <homing.h> |
|
| 4 | 3 |
#include <move.h> |
| 5 | 4 |
|
| 6 | 5 |
#include <wl_token_ring.h> |
| ... | ... | |
| 8 | 7 |
#define SEEK_WITH_BOM 1 |
| 9 | 8 |
#define SEEK_WITH_HOMING 2 |
| 10 | 9 |
|
| 10 |
#define SEEKING_BOM_VELOCITY -160 |
|
| 11 |
#define SEEKING_HOMING_VELOCITY -160 |
|
| 12 |
|
|
| 13 |
// values for homing sensor readings |
|
| 14 |
#define LEFT_LOW 6 |
|
| 15 |
#define LEFT_HIGH 7 |
|
| 16 |
#define CENTER_LOW 18 |
|
| 17 |
#define CENTER_HIGH 19 |
|
| 18 |
#define RIGHT_LOW 12 |
|
| 19 |
#define RIGHT_HIGH 13 |
|
| 20 |
|
|
| 21 |
//function prototypes |
|
| 22 |
int seek_station_with_bom(int station); |
|
| 23 |
int seek_station_with_homing_sensor(void); |
|
| 24 |
|
|
| 11 | 25 |
int state = SEEK_WITH_BOM; |
| 12 | 26 |
|
| 13 | 27 |
void seek_station(int station) |
| ... | ... | |
| 76 | 90 |
move(SEEKING_HOMING_VELOCITY, -(widthcount) / 3); |
| 77 | 91 |
else |
| 78 | 92 |
{
|
| 79 |
WL_DEBUG_PRINT("Unexpected homing sensor value of ");
|
|
| 80 |
WL_DEBUG_PUTI(widthcount);
|
|
| 81 |
WL_DEBUG_PRINT(".\n");
|
|
| 93 |
RECHARGE_DEBUG_PRINT("Unexpected homing sensor value of ");
|
|
| 94 |
RECHARGE_DEBUG_PUTI(widthcount);
|
|
| 95 |
RECHARGE_DEBUG_PRINT(".\n");
|
|
| 82 | 96 |
} |
| 83 | 97 |
} |
| 84 | 98 |
|
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/seeking.h | ||
|---|---|---|
| 17 | 17 |
**/ |
| 18 | 18 |
|
| 19 | 19 |
/** @brief Seek the charging station **/ |
| 20 |
void seek_station(void);
|
|
| 20 |
void seek_station(int station);
|
|
| 21 | 21 |
|
| 22 | 22 |
/** @} **/ |
| 23 | 23 |
|
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/departing.c | ||
|---|---|---|
| 1 | 1 |
#include "departing.h" |
| 2 | 2 |
|
| 3 |
#include <range.h> |
|
| 3 |
#include <rangefinder.h>
|
|
| 4 | 4 |
#include <move.h> |
| 5 | 5 |
|
| 6 |
/** We stop backing up when this is our distance from the wall |
|
| 7 |
* in our front rangefinder. |
|
| 8 |
**/ |
|
| 9 |
#define LEAVE_HIT_WALL_DIST_THRESH 10 |
|
| 10 |
|
|
| 6 | 11 |
/** |
| 12 |
* The velocity we travel at when leaving the station. |
|
| 13 |
**/ |
|
| 14 |
#define LEAVE_VELOCITY 200 |
|
| 15 |
|
|
| 16 |
/** |
|
| 17 |
* The counter for when we are done leaving the station. |
|
| 18 |
**/ |
|
| 19 |
int leave_station_count = 0; |
|
| 20 |
|
|
| 21 |
/** |
|
| 7 | 22 |
* Leaves the charging station. The robot will |
| 8 | 23 |
* continue to back up until it collides with |
| 9 | 24 |
* a wall. range_init must be called before this |
| ... | ... | |
| 14 | 29 |
**/ |
| 15 | 30 |
int depart_station(void) |
| 16 | 31 |
{
|
| 17 |
int front_distance = read_distance(IR2); |
|
| 32 |
int front_distance = range_read_distance(IR2);
|
|
| 18 | 33 |
|
| 19 | 34 |
if(front_distance > LEAVE_HIT_WALL_DIST_THRESH) |
| 20 |
seek_leave_dist_count++;
|
|
| 35 |
leave_station_count++;
|
|
| 21 | 36 |
else |
| 22 |
seek_leave_dist_count = 0;
|
|
| 37 |
leave_station_count = 0;
|
|
| 23 | 38 |
|
| 24 |
if(seek_leave_dist_count > 5)
|
|
| 39 |
if(leave_station_count > 5)
|
|
| 25 | 40 |
{
|
| 26 | 41 |
//stop |
| 27 | 42 |
move(0,0); |
| 43 |
leave_station_count = 0; |
|
| 28 | 44 |
return 1; |
| 29 | 45 |
} |
| 30 | 46 |
|
| 31 | 47 |
// continue moving backwards |
| 32 |
move(SEEK_LEAVE_STATION_VELOCITY_HIT_WALL, 0);
|
|
| 48 |
move(LEAVE_VELOCITY, 0);
|
|
| 33 | 49 |
return 0; |
| 34 | 50 |
} |
| 35 | 51 |
|
Also available in: Unified diff