Revision 148
cleaned up seeking, moved homing sensor code to homing.c/h (tested successfully)
| 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 |
|
Also available in: Unified diff