Colony

//Obstacle Avoid Numbers

#ifndef _ORBIT_FSM_H_

#define _ORBIT_FSM_H_

//The States: 

#define ORBIT_SEEK      12           //Move straight towards robot

#define ORBIT_INSERTION 13           //Rotate into orbit

#define ORBIT_ORBITING  15           //Orbit robot

#define ORBIT_STOP      16           //Stop.  The default and ending state

#define ORBIT_STRAIGHT_SPEED 160

#define ORBIT_CORRECTION 9

#define ORBIT_DIRECTION 4     // if 4, then orbit to the right; if -4, then orbit to the left

#define ORBIT_DISTANCE 175 // orbit distance as measured by rangefinders

#define ORBIT_STOP_DISTANCE 120

int orbit_state;    /*State machine variable.*/

int orbit_otherRobot; /* must be set prior to running FSM */

int orbit_pControl;                /*Proportional control variable, determines turn direction.*/

int orbit_d1,orbit_d2,orbit_d3,orbit_d4,orbit_d5;        /*The five distances taken in by IR.*/

int orbit_bom; /* bom data */

int orbit_theta; /* number of degrees traveled in orbit */

int orbit_theta_stop; /* stop condition in number of degrees traveled */

/* orbit_init

   argument: robot_id that you want to orbit

   notes: must call before orbit_fsm

*/

void orbit_init(int robot);

/* orbit_init_theta

   argument: robot_id that you want to orbit

             theta that you want to stop orbiting at

   notes: must call before orbit_fsm

*/

void orbit_init_theta(int robot,int theta);

/* orbit_fsm

   argument: none

   notes: call in a while loop to perform FSM action

*/

void orbit_fsm(void);

#endif