Revision 212
update lemmings branch
updated smart_run_around code to latest version
for lemmings_dsschult:
leader state works
follower state has sensor matrix and/or bom issues
suggest test when no one else is using wireless
| smart_run_around_fsm.c | ||
|---|---|---|
| 68 | 68 |
} |
| 69 | 69 |
|
| 70 | 70 |
//Checks the forward distance to see if it should back up, if so...state backwards. |
| 71 |
if((d2!=-1)&&(d2 >150)){
|
|
| 71 |
if((d2!=-1)&&(d2 < 150)){
|
|
| 72 | 72 |
backup_count=BACKUP_MAX; |
| 73 | 73 |
avoid_state=BACKWARDS; |
| 74 | 74 |
evaluate_state(); |
| 75 | 75 |
return; |
| 76 | 76 |
} |
| 77 |
|
|
| 77 |
/* |
|
| 78 |
if(d1 < 120 || d3 < 120) {
|
|
| 79 |
avoid_state = BACKWARDS; |
|
| 80 |
backup_count = BACKUP_MAX; |
|
| 81 |
evaluate_state(); |
|
| 82 |
return; |
|
| 83 |
} |
|
| 84 |
*/ |
|
| 78 | 85 |
if(backup_count<BACKUP_MAX){
|
| 79 | 86 |
avoid_state=BACKWARDS; |
| 80 | 87 |
if(backup_count<0) |
| ... | ... | |
| 90 | 97 |
/*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/ |
| 91 | 98 |
|
| 92 | 99 |
/*Debug stuff:*/ |
| 93 |
usb_puts("pControl evaluating: ");
|
|
| 100 |
/*usb_puts("pControl evaluating: ");
|
|
| 94 | 101 |
usb_puti(pControl); |
| 95 | 102 |
usb_puts("\n\r");
|
| 96 | 103 |
usb_puts("IR1: ");
|
| ... | ... | |
| 103 | 110 |
usb_puti(d4); |
| 104 | 111 |
usb_puts(" IR5: ");
|
| 105 | 112 |
usb_puti(d5); |
| 106 |
usb_puts("\n\r");
|
|
| 113 |
usb_puts("\n\r");*/
|
|
| 107 | 114 |
|
| 108 | 115 |
evaluate_state(); |
| 109 | 116 |
} |
| ... | ... | |
| 113 | 120 |
void evaluate_state(){
|
| 114 | 121 |
switch(avoid_state){
|
| 115 | 122 |
case(MOVING): orb_set_color(GREEN); |
| 116 |
move(STRAIT_SPEED,pControl); |
|
| 123 |
move(STRAIT_SPEED,-pControl);
|
|
| 117 | 124 |
break; |
| 118 | 125 |
|
| 119 | 126 |
case(BACKWARDS): orb_set_color(ORANGE); |
| ... | ... | |
| 122 | 129 |
|
| 123 | 130 |
case(CRAZY): orb_set_color(RED); |
| 124 | 131 |
/*TODO: Implement a crazy state.*/ |
| 125 |
move(STRAIT_SPEED,pControl); |
|
| 132 |
move(STRAIT_SPEED,-pControl);
|
|
| 126 | 133 |
break; |
| 127 | 134 |
|
| 128 | 135 |
default: |
Also available in: Unified diff