Project

General

Profile

Revision 615

It worked in initial trials without the orbit code, and worked a couple of times after the addition of the orbit code. Still not stable though.

View differences:

branches/orbit/code/behaviors/bfs_fsm/bfs_fsm.c
5 5
#include <string.h>
6 6
#include "bfs_fsm.h"
7 7
#include "smart_run_around_fsm.h"
8
//#include "orbit_fsm.h"
8
#include "orbit_fsm.h"
9 9

  
10 10
/* Used to find a robot (or other object)
11 11

  
......
28 28
  motors_init();
29 29
  orb_init();
30 30
  orb_enable();
31
  //usb_init();
31
  usb_init();
32 32
  run_around_init();
33 33
 
34 34
  /*Start in the start state, BFS_SEEK */ 
......
56 56
  int temp;  
57 57
    
58 58
  wl_do(); // update wireless
59
  usb_puts("\n\rwl_do");
60
  usb_puts("|state=");
61
  usb_puti(bfs_state);
62
  usb_puts("|");
59 63
  
60 64
    
61 65
  if (bfs_state == BFS_SEEK) {
62 66
    bfs_follow_id = bfs_follow();
63
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS)
67
    usb_puti(bfs_follow_id);
68
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {
64 69
      bfs_state = BFS_FOLLOW; // move to follow state
70
      bfs_d2=1000;
71
    }
65 72
  }
66 73
  if (bfs_state == BFS_FOLLOW) {
67 74
    // get bom reading
......
76 83
    }
77 84
    else {
78 85
      bfs_bom = temp;
79
    
86
          
80 87
      // modify bom reading so right is negative, left is positive
81 88
      if (bfs_bom <= 12)
82 89
        bfs_bom -= 4;
......
84 91
        bfs_bom -= 20;
85 92
      
86 93
      bfs_pControl = bfs_bom*4;
94
      usb_puti(bfs_bom);
87 95
      
88 96
      
89 97
      // get range reading for front
90 98
      temp=range_read_distance(IR2);
91 99
      bfs_d2=(temp == -1) ? bfs_d2 : temp;
100
      usb_puti(bfs_d2);
92 101
      
93 102
      if (bfs_d2 < BFS_ORBIT_DISTANCE) {
94 103
        bfs_state = BFS_ORBIT; // move to orbit state
95
        //orbit_init(bfs_follow_id);
104
        orbit_init(bfs_follow_id);
105
        bfs_d2=1000;
96 106
      }
97 107
    }  
98 108
  }
99
  else if (bfs_state == BFS_ORBIT) {
100
    
101
      // get range reading for front
102
      temp=range_read_distance(IR2);
103
      bfs_d2=(temp == -1) ? bfs_d2 : temp;
104
      
105
      if (bfs_d2 < BFS_ORBIT_DISTANCE)
106
        bfs_state = BFS_STOP; // move to stop state  
109
  if (bfs_state == BFS_ORBIT) {   
110
    while(orbit_state != ORBIT_STOP)
111
    { orbit_fsm(); }
112
    bfs_state = BFS_STOP; // move to stop state  
107 113
  }
108 114
  
109 115
  // evaluate state
......
124 130
      move(BFS_STRAIGHT_SPEED,bfs_pControl);
125 131
      break;
126 132
      
127
    case(BFS_ORBIT): orb_set_color(BLUE);
133
    case(BFS_ORBIT): //orb_set_color(BLUE);
128 134
      // orbit a robot
129 135
      //orbit_fsm();
130
      move(0,0);
136
      //move(0,0);
131 137
      break;
132 138
      
133 139
    case(BFS_STOP): orb_set_color(YELLOW);
......
135 141
      move(0,0);
136 142
      break;
137 143
      
138
    default: orb_set_color(YELLOW);
144
    default: orb_set_color(MAGENTA);
139 145
      /*Should never get here, so stop.*/
140 146
      move(0,0);
141 147
      break;
branches/orbit/code/behaviors/bfs_fsm/bfs_fsm.h
15 15

  
16 16
#define BFS_ORBIT_DISTANCE 120 /* distance to start orbit around robot */
17 17

  
18
#define BFS_STOP_DISTANCE 100 /* distance to stop for object */
19

  
18 20
#define BFS_MAX_ROBOTS 20 /* max id of robot in project */
19 21

  
20 22
#define BFS_NO_VAL 255
branches/orbit/code/behaviors/bfs_fsm/orbit_fsm.c
1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <wl_token_ring.h>
4
#include "orbit_fsm.h"
5
#include "bfs_fsm.h"
6

  
7
/* Used to orbit a robot
8

  
9
  Must be within BOM range of robot before activating
10
  
11
  orbit_theta_stop requires encoders, and is incomplete
12
  
13
  Assumes:
14
    both robots are already in a token ring
15
*/
16

  
17

  
18
/* private function prototype */
19
void orbit_evaluate_state(void);
20

  
21

  
22
/* primary init */
23
void orbit_init(int robot) {
24
  //range_init();
25
  //analog_init(1);
26
  //motors_init();
27
  //orb_init();
28
  //orb_enable();
29
  //usb_init();
30
 
31
  /*Start in the start state, ORBIT_SEEK */ 
32
  orbit_state = ORBIT_INSERTION;
33
  
34
  orbit_otherRobot = robot; // set which robot to seek and orbit
35
  
36
  orbit_pControl=0;
37
  orbit_bom = bfs_bom;
38
  orbit_theta = 0;
39
  orbit_theta_stop = 1000;
40
  
41
  /*Initialize distances to zero.*/ 
42
  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;
43
  
44
}
45
/* secondary init */
46
void orbit_init_theta(int robot,int theta) {
47
  //range_init();
48
  //analog_init(1);
49
  //motors_init();
50
  //orb_init();
51
  //orb_enable();
52
  //usb_init();
53
 
54
  /*Start in the start state, ORBIT_INSERTION */ 
55
  orbit_state = ORBIT_INSERTION;
56
  
57
  orbit_otherRobot = robot; // set which robot to seek and orbit
58
  
59
  orbit_pControl=0;
60
  orbit_bom = bfs_bom;
61
  orbit_theta = 0;
62
  orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop
63
  
64
  /*Initialize distances to zero.*/ 
65
  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;
66
  
67
}
68

  
69
/*The main function, call this to update states as frequently as possible.*/
70
void orbit_fsm(void) {
71
  
72
  /*The following lines ensure that undefined (-1) values
73
  will not update the distances.*/ 
74
  int temp;  
75
    
76
  //temp=range_read_distance(IR1);
77
  //orbit_d1=(temp == -1) ? orbit_d1 : temp;
78
  
79
  temp=range_read_distance(IR2);
80
  orbit_d2=(temp == -1) ? orbit_d2 : temp;
81
  
82
  //temp=range_read_distance(IR3);
83
  //orbit_d3=(temp == -1) ? orbit_d3 : temp;
84
  
85
  //temp=range_read_distance(IR4);
86
  //orbit_d4=(temp == -1) ? orbit_d4 : temp;
87
  
88
  //temp=range_read_distance(IR5);
89
  //orbit_d5=(temp == -1) ? orbit_d5 : temp;
90
  
91
  wl_do(); // update wireless
92
  
93
  // get bom reading
94
  temp = wl_token_get_my_sensor_reading(orbit_otherRobot);
95
  orbit_bom = (temp == -1) ? orbit_bom : temp;
96
  
97
  // modify bom reading so right is negative, left is positive
98
  if (orbit_bom <= 12)
99
    orbit_bom -= 4;
100
  else
101
    orbit_bom -= 20;
102
  
103
    
104
  if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE)
105
    orbit_state = ORBIT_INSERTION; // begin to orbit
106
  if (orbit_state == ORBIT_INSERTION &&
107
    (orbit_bom == ORBIT_DIRECTION || (orbit_bom + 1) == ORBIT_DIRECTION || (orbit_bom - 1) == ORBIT_DIRECTION))
108
    orbit_state = ORBIT_ORBITING; // orbit achieved
109
  if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_DISTANCE) 
110
    || (orbit_theta >= orbit_theta_stop)))
111
    orbit_state = ORBIT_STOP; // orbit obstructed
112
  
113
  // evaluate state
114
  orbit_evaluate_state();
115
}
116

  
117

  
118
//Acts on state change.
119
void orbit_evaluate_state(){
120
    switch(orbit_state){
121
    case(ORBIT_SEEK): orb_set_color(RED);
122
      // move towards robot
123
      orbit_pControl = orbit_bom*10;      
124
      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);
125
      break;
126
    
127
    case(ORBIT_INSERTION): orb_set_color(GREEN);
128
      // rotate into orbit, perpendicular to other robot
129
      orbit_pControl = -ORBIT_DIRECTION*10;
130
      move(ORBIT_STRAIGHT_SPEED/3,orbit_pControl);
131
      break;
132
      
133
    case(ORBIT_ORBITING): orb_set_color(BLUE);
134
      // go straight with slight rotation
135
      if (orbit_bom == ORBIT_DIRECTION)
136
        orbit_pControl = ORBIT_DIRECTION;
137
      else if (orbit_bom < ORBIT_DIRECTION)
138
        orbit_pControl = ORBIT_DIRECTION-ORBIT_CORRECTION;
139
      else
140
        orbit_pControl = ORBIT_DIRECTION+ORBIT_CORRECTION;
141
      
142
      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);
143
      break;
144
      
145
    case(ORBIT_STOP): orb_set_color(YELLOW);
146
      move(0,0);
147
      break;
148
      
149
    default: orb_set_color(YELLOW);
150
      /*Should never get here, so stop.*/
151
      move(0,0);
152
      break;
153
  }
154
}
155

  
156

  
branches/orbit/code/behaviors/bfs_fsm/smart_run_around_fsm.c
1
#include "dragonfly_lib.h"
1
#include <dragonfly_lib.h>
2 2
#include "smart_run_around_fsm.h"
3 3

  
4 4
/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.
branches/orbit/code/behaviors/bfs_fsm/orbit_fsm.h
1
//Obstacle Avoid Numbers
2

  
3

  
4
#ifndef _ORBIT_FSM_H_
5
#define _ORBIT_FSM_H_
6

  
7
//The States: 
8
#define ORBIT_SEEK      12           //Move straight towards robot
9
#define ORBIT_INSERTION 13           //Rotate into orbit
10
#define ORBIT_ORBITING  15           //Orbit robot
11
#define ORBIT_STOP      16           //Stop.  The default and ending state
12

  
13

  
14
#define ORBIT_STRAIGHT_SPEED 160
15
#define ORBIT_CORRECTION 9
16

  
17

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

  
20
#define ORBIT_DISTANCE 250 // orbit distance as measured by rangefinders
21

  
22

  
23
int orbit_state;    /*State machine variable.*/
24

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

  
27

  
28
int orbit_pControl;		/*Proportional control variable, determines turn direction.*/
29
int orbit_d1,orbit_d2,orbit_d3,orbit_d4,orbit_d5;	/*The five distances taken in by IR.*/
30
int orbit_bom; /* bom data */
31
int orbit_theta; /* number of degrees traveled in orbit */
32
int orbit_theta_stop; /* stop condition in number of degrees traveled */
33

  
34
/* orbit_init
35
   argument: robot_id that you want to orbit
36
   notes: must call before orbit_fsm
37
*/
38
void orbit_init(int robot);
39

  
40
/* orbit_init_theta
41
   argument: robot_id that you want to orbit
42
             theta that you want to stop orbiting at
43
   notes: must call before orbit_fsm
44
*/
45
void orbit_init_theta(int robot,int theta);
46

  
47
/* orbit_fsm
48
   argument: none
49
   notes: call in a while loop to perform FSM action
50
*/
51
void orbit_fsm(void);
52

  
53
#endif

Also available in: Unified diff