Project

General

Profile

Revision 740

Added by David Schultz almost 16 years ago

fixed speeds, stop distances for bfs

View differences:

trunk/code/behaviors/bfs_fsm/test_decoy/decoy.c
15 15 
  orb_init();
16 16 
  orb_set_color(PURPLE);
17 17 
  wl_init();
18 
  wl_set_channel(0xF);
18 19 
  wl_token_ring_register();
19 20 
  wl_token_ring_join(); // join token ring
20 21 
  usb_init();
trunk/code/behaviors/bfs_fsm/test_decoy/Makefile
14 14 
USE_WIRELESS = 1
15 15 

  		




  16
  16
  
    
# com1 = serial port. Use lpt1 to connect to parallel port.


  		




  17
  
  
    
AVRDUDE_PORT = com3


  		




  
  17
  
    
AVRDUDE_PORT = com7


  		




  18
  18
  
    
#/dev/tty.usbserial*


  		




  19
  19
  
    
#


  		




  20
  20
  
    
#


  		












  

    
      trunk/code/behaviors/bfs_fsm/bfs_fsm.h
    
  





  11
  11
  
    
#define BFS_STOP      16           //Stop.  The default and ending state


  		




  12
  12
  
    

  		




  13
  13
  
    

  		




  14
  
  
    
#define BFS_STRAIGHT_SPEED 160


  		




  
  14
  
    
#define BFS_STRAIGHT_SPEED 170


  		




  15
  15
  
    

  		




  16
  
  
    
#define BFS_SLOW_DISTANCE 350


  		




  17
  
  
    
#define BFS_SLOW_SPEED    150


  		




  
  16
  
    
#define BFS_SLOW_DISTANCE 250


  		




  
  17
  
    
#define BFS_SLOW_SPEED    160


  		




  18
  18
  
    

  		




  19
  
  
    
#define BFS_ORBIT_DISTANCE 250 /* distance to start orbit around robot */


  		




  
  19
  
    
#define BFS_ORBIT_DISTANCE 175 /* distance to start orbit around robot */


  		




  20
  20
  
    

  		




  21
  21
  
    
#define BFS_STOP_DISTANCE 120 /* distance to stop for object */


  		




  22
  22
  
    

  		












  

    
      trunk/code/behaviors/bfs_fsm/orbit_fsm.c
    
  





  106
  106
  
    
  if (orbit_state == ORBIT_INSERTION &&


  		




  107
  107
  
    
    (orbit_bom == ORBIT_DIRECTION || (orbit_bom + 1) == ORBIT_DIRECTION || (orbit_bom - 1) == ORBIT_DIRECTION))


  		




  108
  108
  
    
    orbit_state = ORBIT_ORBITING; // orbit achieved


  		




  109
  
  
    
  if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_DISTANCE) 


  		




  
  109
  
    
  if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_STOP_DISTANCE) 


  		




  110
  110
  
    
    || (orbit_theta >= orbit_theta_stop)))


  		




  111
  111
  
    
    orbit_state = ORBIT_STOP; // orbit obstructed


  		




  112
  112
  
    
  


  		




  ......




  127
  127
  
    
    case(ORBIT_INSERTION): orb_set_color(GREEN);


  		




  128
  128
  
    
      // rotate into orbit, perpendicular to other robot


  		




  129
  129
  
    
      orbit_pControl = -ORBIT_DIRECTION*10;


  		




  130
  
  
    
      move(ORBIT_STRAIGHT_SPEED/3,orbit_pControl);


  		




  
  130
  
    
      move(ORBIT_STRAIGHT_SPEED/2,orbit_pControl);


  		




  131
  131
  
    
      break;


  		




  132
  132
  
    
      


  		




  133
  133
  
    
    case(ORBIT_ORBITING): orb_set_color(BLUE);


  		












  

    
      trunk/code/behaviors/bfs_fsm/orbit_fsm.h
    
  





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


  		




  12
  12
  
    

  		




  13
  13
  
    

  		




  14
  
  
    
#define ORBIT_STRAIGHT_SPEED 150


  		




  
  14
  
    
#define ORBIT_STRAIGHT_SPEED 160


  		




  15
  15
  
    
#define ORBIT_CORRECTION 9


  		




  16
  16
  
    

  		




  17
  17
  
    

  		




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


  		




  19
  19
  
    

  		




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


  		




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


  		




  
  21
  
    
#define ORBIT_STOP_DISTANCE 120


  		




  21
  22
  
    

  		




  22
  23
  
    

  		




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


  		












  

    
      trunk/code/behaviors/bfs_fsm/driver.c
    
  





  1
  
  
    
/** driver for orbit code


  		




  2
  
  
    
	execute orbit behavior


  		




  
  1
  
    
/** driver for bfs swarming


  		




  
  2
  
    
	execute bfs behavior


  		




  3
  3
  
    
*/


  		




  4
  4
  
    

  		




  5
  5
  
    
#include <dragonfly_lib.h>


  		




  ......




  15
  15
  
    
  orb_init();


  		




  16
  16
  
    
  orb_set_color(PURPLE);


  		




  17
  17
  
    
  wl_init();


  		




  
  18
  
    
  wl_set_channel(0xF);


  		




  18
  19
  
    
  wl_token_ring_register();


  		




  19
  20
  
    
  wl_token_ring_join(); // join token ring


  		




  20
  21
  
    
  usb_init();


  		




  21
  22
  
    
  


  		




  22
  23
  
    

  		




  23
  
  
    
  bfs_init(4); // set robot_id to find


  		




  
  24
  
    
  bfs_init(6); // set robot_id to find


  		




  24
  25
  
    

  		




  25
  26
  
    
  usb_puts("start 4\n\r");


  		




  26
  27
  
    

  		












  

    
      trunk/code/behaviors/bfs_fsm/Makefile
    
  





  14
  14
  
    
USE_WIRELESS = 1


  		




  15
  15
  
    

  		




  16
  16
  
    
# com1 = serial port. Use lpt1 to connect to parallel port.


  		




  17
  
  
    
AVRDUDE_PORT = com3


  		




  
  17
  
    
AVRDUDE_PORT = com9


  		




  18
  18
  
    
#/dev/tty.usbserial*


  		




  19
  19
  
    
#


  		




  20
  20
  
    
#


  		












Also available in:  Unified diff