Revision 1201
Changed smart run-around from old version to new version, which seems kind
of silly, since the rangefinder functions didn't actually change.
| smart_run_around_fsm.h | ||
|---|---|---|
| 1 |
//Obstacle Avoid Numbers |
|
| 1 |
// smart_run_around_fsm.h |
|
| 2 |
// required to run Smart Run Around functions |
|
| 3 |
// declare functions and global variables |
|
| 2 | 4 |
|
| 3 |
|
|
| 4 | 5 |
#ifndef _RUN_AROUND_FSM_H_ |
| 5 | 6 |
#define _RUN_AROUND_FSM_H_ |
| 6 | 7 |
|
| 7 |
//The States: |
|
| 8 |
#define MOVING 12 //Move strait. |
|
| 9 |
#define BACKWARDS 15 //Move backwards. (Front close to wall.) |
|
| 10 |
#define STOP 16 //Stop. The default state, (Something broke). |
|
| 11 |
#define CRAZY 40 //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.) |
|
| 8 |
// states (robot shall never move in reverse) |
|
| 9 |
// these constants may be changed (but can't overlap) without effect |
|
| 10 |
#define FORWARD 0 // drive straight forward |
|
| 11 |
#define LEFT 1 // drive forward and to the left |
|
| 12 |
#define RIGHT 2 // drive forward and to the right |
|
| 13 |
#define SPIN 3 // turn without traveling (last resort if otherwise stuck) |
|
| 12 | 14 |
|
| 13 |
#define BACKUP_MAX 60 |
|
| 14 |
#define CRAZY_MAX 200 //The number of counts between "crazy moments" |
|
| 15 |
#define STRAIT_SPEED 185 //The speed when going strait or backing up. |
|
| 16 |
#define TURN_CONSTANT 2 |
|
| 17 |
#define PCONTROL_CRAZY_LIMIT 80 |
|
| 15 |
/* conditions on rangefinders (in mm) informing state changes |
|
| 16 |
* distances are 50 greater than in real life, cannot be smaller than 100 |
|
| 17 |
* naming format: SENSOR_URGENCY |
|
| 18 |
* changing these constants affects robot operation |
|
| 19 |
*/ |
|
| 20 |
#define IR2_DANGER 150 // 10 cm: dangerously close to front |
|
| 21 |
#define IR13_DANGER 200 // 15 cm: dangerously close to sides (looking ahead) |
|
| 22 |
#define IR2_INTEREST 300 // 25 cm: notably close to front |
|
| 23 |
#define IR45_DANGER 150 // dangerously close to side (looking sideways) |
|
| 18 | 24 |
|
| 19 |
int avoid_state; |
|
| 20 |
int prev_state; /*State machine variable.*/ |
|
| 21 |
int crazy_count; /*Counter for a 'get unstuck' behavior.*/ |
|
| 25 |
uint8_t state; // current state of FSM: FORWARD, LEFT, RIGHT, SPIN |
|
| 26 |
int16_t d1, d2, d3, d4, d5; // values returned by rangefinders |
|
| 22 | 27 |
|
| 23 |
int backup_count; /*Counter for backup duration.*/ |
|
| 24 |
int pControl; /*Proportional control variable, determines turn direction.*/ |
|
| 25 |
int d1,d2,d3,d4,d5; /*The five distances taken in by IR.*/ |
|
| 28 |
// initialize rangefinders and global variables |
|
| 29 |
void run_around_init(void); |
|
| 26 | 30 |
|
| 27 |
void run_around_init(void); |
|
| 31 |
/* evaluate rangefinder input and update state |
|
| 32 |
* call this function as often as possible to avoid collisions |
|
| 33 |
*/ |
|
| 28 | 34 |
void run_around_FSM(void); |
| 35 |
|
|
| 36 |
// behave according to current state |
|
| 29 | 37 |
void evaluate_state(void); |
| 30 | 38 |
|
| 31 | 39 |
#endif |
| 40 |
|
|
Also available in: Unified diff