Revision 148
cleaned up seeking, moved homing sensor code to homing.c/h (tested successfully)
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/homing.c | ||
|---|---|---|
| 1 |
#include "homing.h" |
|
| 2 |
#include "recharge_defs.h" |
|
| 3 |
|
|
| 4 |
// values for homing sensor readings |
|
| 5 |
#define LEFT_LOW 6 |
|
| 6 |
#define LEFT_HIGH 7 |
|
| 7 |
#define CENTER_LOW 18 |
|
| 8 |
#define CENTER_HIGH 19 |
|
| 9 |
#define RIGHT_LOW 12 |
|
| 10 |
#define RIGHT_HIGH 13 |
|
| 11 |
|
|
| 12 |
/** |
|
| 13 |
* translates homing sensor data into the |
|
| 14 |
* direction the robot needs to turn |
|
| 15 |
* (or if it needs to use the BOM). |
|
| 16 |
**/ |
|
| 17 |
int homing_direction(int beaconcount) |
|
| 18 |
{
|
|
| 19 |
if (beaconcount < LEFT_LOW) //can't use beacon, switch to BOM |
|
| 20 |
return HOMING_NONE; |
|
| 21 |
else if(beaconcount >= CENTER_LOW) |
|
| 22 |
return HOMING_FORWARD; |
|
| 23 |
else |
|
| 24 |
{
|
|
| 25 |
if(beaconcount >= LEFT_LOW && beaconcount <= LEFT_HIGH) |
|
| 26 |
return HOMING_LEFT; |
|
| 27 |
else if (beaconcount >= RIGHT_LOW && beaconcount <= RIGHT_HIGH) |
|
| 28 |
return HOMING_RIGHT; |
|
| 29 |
else |
|
| 30 |
{
|
|
| 31 |
RECHARGE_DEBUG_PRINT("Unexpected homing sensor value of ");
|
|
| 32 |
RECHARGE_DEBUG_PUTI(beaconcount); |
|
| 33 |
RECHARGE_DEBUG_PRINT(".\n");
|
|
| 34 |
} |
|
| 35 |
} |
|
| 36 |
return HOMING_NONE; |
|
| 37 |
} |
|
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/seeking.c | ||
|---|---|---|
| 5 | 5 |
|
| 6 | 6 |
#include "recharge_i2c.h" |
| 7 | 7 |
#include "recharge_defs.h" |
| 8 |
#include "homing.h" |
|
| 8 | 9 |
|
| 9 | 10 |
#include <wl_token_ring.h> |
| 10 | 11 |
|
| ... | ... | |
| 14 | 15 |
#define SEEKING_BOM_VELOCITY -170 |
| 15 | 16 |
#define SEEKING_HOMING_VELOCITY -160 |
| 16 | 17 |
|
| 17 |
// values for homing sensor readings |
|
| 18 |
#define LEFT_LOW 6 |
|
| 19 |
#define LEFT_HIGH 7 |
|
| 20 |
#define CENTER_LOW 18 |
|
| 21 |
#define CENTER_HIGH 19 |
|
| 22 |
#define RIGHT_LOW 12 |
|
| 23 |
#define RIGHT_HIGH 13 |
|
| 24 |
|
|
| 25 | 18 |
//function prototypes |
| 26 | 19 |
int seek_station_with_bom(int station); |
| 27 | 20 |
int seek_station_with_homing_sensor(void); |
| ... | ... | |
| 75 | 68 |
|
| 76 | 69 |
int seek_station_with_homing_sensor(void) |
| 77 | 70 |
{
|
| 78 |
//TODO: create function in homing.h to return LEFT, FORWARD, RIGHT, or NONE |
|
| 79 | 71 |
int widthcount = recharge_i2c_get_homing_reading(); |
| 80 |
|
|
| 81 |
if (widthcount < LEFT_LOW) //can't use beacon, switch to BOM |
|
| 82 |
return SEEK_WITH_BOM; |
|
| 83 |
else if(widthcount >= CENTER_LOW) |
|
| 84 |
move(SEEKING_HOMING_VELOCITY, 0); |
|
| 85 |
else |
|
| 86 |
{
|
|
| 87 |
if(widthcount >= LEFT_LOW && widthcount <= LEFT_HIGH) |
|
| 88 |
move(SEEKING_HOMING_VELOCITY, (widthcount) / 3); |
|
| 89 |
else if (widthcount >= RIGHT_LOW && widthcount <= RIGHT_HIGH) |
|
| 90 |
move(SEEKING_HOMING_VELOCITY, -(widthcount) / 3); |
|
| 91 |
else |
|
| 92 |
{
|
|
| 93 |
RECHARGE_DEBUG_PRINT("Unexpected homing sensor value of ");
|
|
| 94 |
RECHARGE_DEBUG_PUTI(widthcount); |
|
| 95 |
RECHARGE_DEBUG_PRINT(".\n");
|
|
| 96 |
} |
|
| 97 |
} |
|
| 72 |
int direction = homing_direction(widthcount); |
|
| 98 | 73 |
|
| 74 |
switch(direction) |
|
| 75 |
{
|
|
| 76 |
case HOMING_NONE: |
|
| 77 |
return SEEK_WITH_BOM; |
|
| 78 |
break; |
|
| 79 |
case HOMING_FORWARD: |
|
| 80 |
move(SEEKING_HOMING_VELOCITY, 0); |
|
| 81 |
break; |
|
| 82 |
case HOMING_LEFT: |
|
| 83 |
move(SEEKING_HOMING_VELOCITY, 3); |
|
| 84 |
break; |
|
| 85 |
case HOMING_RIGHT: |
|
| 86 |
move(SEEKING_HOMING_VELOCITY, -3); |
|
| 87 |
break; |
|
| 88 |
} |
|
| 89 |
|
|
| 99 | 90 |
return SEEK_WITH_HOMING; |
| 100 | 91 |
} |
| 101 | 92 |
|
| branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/homing.h | ||
|---|---|---|
| 1 |
/** |
|
| 2 |
* @file homing.h Homing |
|
| 3 |
* |
|
| 4 |
* Function(s) related to the homing sensor and robot movement. |
|
| 5 |
**/ |
|
| 6 |
|
|
| 7 |
/** |
|
| 8 |
* @defgroup homing Homing |
|
| 9 |
* |
|
| 10 |
* @brief interpreting the homing sensor data |
|
| 11 |
* |
|
| 12 |
* Function(s) related to the homing sensor and robot movement. |
|
| 13 |
* |
|
| 14 |
* @{
|
|
| 15 |
**/ |
|
| 16 |
|
|
| 17 |
// flags for driving straight, turning, etc. |
|
| 18 |
#define HOMING_NONE 0 |
|
| 19 |
#define HOMING_FORWARD 1 |
|
| 20 |
#define HOMING_LEFT 2 |
|
| 21 |
#define HOMING_RIGHT 3 |
|
| 22 |
|
|
| 23 |
/** @brief Seek the charging station **/ |
|
| 24 |
int homing_direction(int beacon_count); |
|
| 25 |
|
|
| 26 |
/** @} **/ |
|
Also available in: Unified diff