/ - Diff - Colony - Robotics Club

Revision 536

Added by David Schultz over 16 years ago

removed any conflict with other fsm variables.
added tentative orbit stop, which needs encoders to be accurate.

       Must be within BOM range of robot before activating
       orbit_theta_stop requires encoders, and is incomplete
       Assumes:
         both robots are already in a token ring
     */
     /* private function prototype */
     void evaluate_state(void);
     /* primary init */
     void orbit_init(int robot) {
       range_init();
       analog_init();
       analog_init(1);
       motors_init();
       orb_init();
       orb_enable();
       //usb_init();
       /*Start in the start state, SEEK */
       state = SEEK;
       /*Start in the start state, ORBIT_SEEK */
       orbit_state = ORBIT_SEEK;
       otherRobot = robot; // set which robot to seek and orbit
       orbit_otherRobot = robot; // set which robot to seek and orbit
       pControl=0;
       bom = 0;
       orbit_pControl=0;
       orbit_bom = 0;
       orbit_theta = 0;
       orbit_theta_stop = -1;
       /*Initialize distances to zero.*/
       d1=1000; d2=1000; d3=1000; d4=1000; d5=1000;
       orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;
+    }
     /* secondary init */
     void orbit_init_theta(int robot,int theta) {
       range_init();
       analog_init(1);
       motors_init();
       orb_init();
       orb_enable();
       //usb_init();
       /*Start in the start state, ORBIT_SEEK */
       orbit_state = ORBIT_SEEK;
       orbit_otherRobot = robot; // set which robot to seek and orbit
       orbit_pControl=0;
       orbit_bom = 0;
       orbit_theta = 0;
       orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop
       /*Initialize distances to zero.*/
       orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;
+    }
     /*The main function, call this to update states as frequently as possible.*/
     void orbit_fsm(void) {
-...
       /*The following lines ensure that undefined (-1) values
       will not update the distances.*/
       int temp;
       wl_do(); // update wireless
       temp=range_read_distance(IR1);
       d1=(temp == -1) ? d1 : temp;
       orbit_d1=(temp == -1) ? orbit_d1 : temp;
       temp=range_read_distance(IR2);
       d2=(temp == -1) ? d2 : temp;
       orbit_d2=(temp == -1) ? orbit_d2 : temp;
       temp=range_read_distance(IR3);
       d3=(temp == -1) ? d3 : temp;
       orbit_d3=(temp == -1) ? orbit_d3 : temp;
       temp=range_read_distance(IR4);
       d4=(temp == -1) ? d4 : temp;
       orbit_d4=(temp == -1) ? orbit_d4 : temp;
       temp=range_read_distance(IR5);
       d5=(temp == -1) ? d5 : temp;
       orbit_d5=(temp == -1) ? orbit_d5 : temp;
       wl_do(); // update wireless
       // get bom reading
       temp = wl_token_get_my_sensor_reading(otherRobot);
       bom = (temp == -1) ? bom : temp;
       temp = wl_token_get_my_sensor_reading(orbit_otherRobot);
       orbit_bom = (temp == -1) ? orbit_bom : temp;
       // modify bom reading so right is negative, left is positive
       if (bom <= 12)
         bom -= 4;
       if (orbit_bom <= 12)
         orbit_bom -= 4;
       else
         bom -= 20;
         orbit_bom -= 20;
       if (state == SEEK && d2 < ORBIT_DISTANCE)
         state = TO_ORBIT; // begin to orbit
       if (state == TO_ORBIT && bom == ORBIT_DIRECTION)
         state = ORBIT; // orbit achieved
       if (state == ORBIT && d2 < ORBIT_DISTANCE)
         state = STOP; // orbit obstructed
       if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE)
         orbit_state = ORBIT_INSERTION; // begin to orbit
       if (orbit_state == ORBIT_INSERTION && orbit_bom == ORBIT_DIRECTION)
         orbit_state = ORBIT_ORBITING; // orbit achieved
       if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_DISTANCE)
         || (orbit_theta >= orbit_theta_stop)))
         orbit_state = ORBIT_STOP; // orbit obstructed
       // evaluate state
       evaluate_state();
-...
     //Acts on state change.
     void evaluate_state(){
         switch(state){
         case(SEEK): orb_set_color(RED);
         switch(orbit_state){
         case(ORBIT_SEEK): orb_set_color(RED);
           // move towards robot
           pControl = bom*10;
           move(STRAIGHT_SPEED,pControl);
           orbit_pControl = orbit_bom*10;
           move(ORBIT_STRAIGHT_SPEED,orbit_pControl);
           break;
         case(TO_ORBIT): orb_set_color(GREEN);
         case(ORBIT_INSERTION): orb_set_color(GREEN);
           // rotate into orbit, perpendicular to other robot
           pControl = -ORBIT_DIRECTION*10;
           move(STRAIGHT_SPEED/3,pControl);
           orbit_pControl = -ORBIT_DIRECTION*10;
           move(ORBIT_STRAIGHT_SPEED/3,orbit_pControl);
           break;
         case(ORBIT): orb_set_color(BLUE);
         case(ORBIT_ORBITING): orb_set_color(BLUE);
           // go straight with slight rotation
           if (bom == ORBIT_DIRECTION)
             pControl = ORBIT_DIRECTION*2;
           else if (bom < ORBIT_DIRECTION)
             pControl = ORBIT_DIRECTION-10;
           if (orbit_bom == ORBIT_DIRECTION)
             orbit_pControl = ORBIT_DIRECTION;
           else if (orbit_bom < ORBIT_DIRECTION)
             orbit_pControl = ORBIT_DIRECTION-5;
           else
             pControl = ORBIT_DIRECTION+10;
             orbit_pControl = ORBIT_DIRECTION+5;
           move(STRAIGHT_SPEED,pControl);
           move(ORBIT_STRAIGHT_SPEED,orbit_pControl);
           break;
         case(STOP): orb_set_color(YELLOW);
         case(ORBIT_STOP): orb_set_color(YELLOW);
           move(0,0);
           break;

     #define _ORBIT_FSM_H_
     //The States:
     #define SEEK 12           //Move straight towards robot
     #define TO_ORBIT 13       //Rotate into orbit
     #define ORBIT 15          //Orbit robot
     #define STOP 16           //Stop.  The default state, (Something broke).
     #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 STRAIGHT_SPEED 160
     #define ORBIT_STRAIGHT_SPEED 160
     #define ORBIT_DIRECTION 4     // if 4, then orbit to the right; if -4, then orbit to the left
-...
     #define ORBIT_DISTANCE 250 // orbit distance as measured by rangefinders
     int state;    /*State machine variable.*/
     int orbit_state;    /*State machine variable.*/
     int otherRobot; /* must be set prior to running FSM */
     int orbit_otherRobot; /* must be set prior to running FSM */
     int pControl;		/*Proportional control variable, determines turn direction.*/
     int d1,d2,d3,d4,d5;	/*The five distances taken in by IR.*/
     int bom; /* bom data */
     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);
     void evaluate_state(void);
     #endif

Also available in: Unified diff

Project

General

Profile

Colony

Revision 536