Revision 734
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.
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