Project

General

Profile

Revision 734

Added by Chris Mar almost 16 years ago

fiddling w/ run_around. i wrote a function that does move(0,0) and delays whenever transitioning between moving forwards and backwards. it calls wl_do() simultaneously, and wireless still appears to work.

View differences:

branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/recharge_i2c.c
63 63
 **/
64 64
void recharge_i2c_receive(char i2c_byte)
65 65
{
66
    //usb_putc(i2c_byte);
66
    usb_putc(i2c_byte);
67 67
    if(recharge_i2c_prev_byte == I2C_MSG_HOMING_DATA)
68 68
	{
69 69
		recharge_i2c_homing_sensor_data = i2c_byte;
branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/main.c
12 12
int main(void)
13 13
{
14 14
	dragonfly_init(ALL_ON);
15
	range_init();
16
	orb_enable();
15
	//range_init();
16
	//orb_enable();
17 17
	usb_puts("Turned on!\n");
18 18
	wl_init();
19 19
	usb_puts("Wireless initialized!\n");
branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/smart_run_around_fsm.c
1 1
#include "dragonfly_lib.h"
2 2
#include "smart_run_around_fsm.h"
3
#include <wireless.h>
3 4

  
4 5
/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.
5 6
Could be better at not getting stuck.
......
14 15
 
15 16
  /*Start in the default state, MOVING*/ 
16 17
  avoid_state=MOVING;
18
  prev_state = avoid_state;
17 19
  /*Set timers to their maximum values.*/
18 20
  crazy_count=CRAZY_MAX;
19 21
  backup_count=BACKUP_MAX; 
......
22 24
  /*Initialize distances to zero.*/ 
23 25
  d1=0; d2=0; d3=0; d4=0; d5=0;
24 26
  
25
  orb_set_color(GREEN);
27
  orb_set_color(CYAN);
26 28

  
27 29
}
28 30

  
29 31
/*The main function, call this to update states as frequently as possible.*/
30 32
void run_around_FSM(void) {
33
  prev_state = avoid_state;
31 34
  /*Default to moving.*/ 
32
  avoid_state=MOVING;
35
  //avoid_state=MOVING;
33 36
  
34 37
  /*The following lines ensure that undefined (-1) values
35 38
  will not update the distances.*/ 
......
64 67
  
65 68
  //Checks the forward distance to see if it should back up, if so...state backwards.
66 69
  if((d2!=-1)&&(d2 < 150)){
70
      if(prev_state == MOVING) {
71
        delay_motors();
72
      }
67 73
      backup_count=BACKUP_MAX;
68 74
      avoid_state=BACKWARDS;
69 75
      evaluate_state();
70 76
      return;
71 77
  }
72
  /*
73
  if(d1 < 120 || d3 < 120) {
74
		avoid_state = BACKWARDS;
75
		backup_count = BACKUP_MAX;
76
		evaluate_state();
77
		return;
78
  }
79
  */
78
  
80 79
  if(backup_count<BACKUP_MAX){
80
    usb_puts("DO I EVER GET HERE?\n\r");
81 81
    avoid_state=BACKWARDS; 
82 82
    if(backup_count<0)
83 83
      backup_count=BACKUP_MAX;
......
85 85
    return;
86 86
  }
87 87
  
88
  if(prev_state == BACKWARDS) {
89
        delay_motors();
90
  }
91
  //usb_puts("avoid_state set to MOVING\n\r");
92
  avoid_state = MOVING;
93
  
88 94
  /*Should evaluate an expression from -255 to 255 to pass to move.*/
89 95
  pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;
90 96
  
......
114 120
//Acts on state change.
115 121
void evaluate_state(){
116 122
    switch(avoid_state){
117
    case(MOVING): orb_set_color(GREEN);
123
    case(MOVING): //orb_set_color(GREEN);
118 124
      move(STRAIT_SPEED,-pControl);
125
      //motor1_set(FORWARD,200);
126
      //motor2_set(FORWARD,200);
119 127
      break;
120 128
    
121
    case(BACKWARDS): orb_set_color(MAGENTA);
129
    case(BACKWARDS): //orb_set_color(RED);
122 130
      move(-STRAIT_SPEED-50,0);
131
      //motor1_set(BACKWARD,200);
132
      //motor2_set(BACKWARD,200);
123 133
      break;
124 134
      
125
    case(CRAZY): orb_set_color(RED);
135
    case(CRAZY): //orb_set_color(RED);
126 136
      /*TODO: Implement a crazy state.*/
127 137
      move(STRAIT_SPEED,-pControl);
128 138
      break;
129 139
      
130 140
    default:
131 141
      /*Should never get here, go strait.*/
132
      move(100,0); orb_set_color(BLUE);
142
      move(100,0); //orb_set_color(BLUE);
133 143
      break;
134 144
  }
135 145
}
136 146

  
137

  
147
void delay_motors() {
148
  int count = 0;
149
  move(0,0);
150
  //usb_puts("called delay_motors()\n\r");
151
  while(count < 300) {
152
    delay_ms(1);
153
    wl_do();
154
    count++;
155
  }
156
}
branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/smart_run_around_fsm.h
15 15

  
16 16
#define BACKUP_MAX 60
17 17
#define CRAZY_MAX 200       //The number of counts between "crazy moments"
18
#define STRAIT_SPEED 155    //The speed when going strait or backing up.
18
#define STRAIT_SPEED 150    //The speed when going strait or backing up.
19 19
#define TURN_CONSTANT 5
20 20
#define PCONTROL_CRAZY_LIMIT 80
21 21

  
......
31 31
void run_around_FSM(void);
32 32
void evaluate_state(void);
33 33

  
34
void delay_motors(void);
35

  
34 36
#endif
branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/Makefile
15 15
 USE_WIRELESS = 1
16 16

  
17 17
# com1 = serial port. Use lpt1 to connect to parallel port.
18
AVRDUDE_PORT = /dev/ttyUSB0
18
AVRDUDE_PORT = com4
19 19
#
20 20
#
21 21
###################################

Also available in: Unified diff