Revision 1094
simulating rangefinder and smart run around
changed the position of IR sensors defined in libsim
libsim and libdragonfly use different numbering for IRs (#418, #419)
added smart_run_around to template, right now it just hits wall, backs
up then segfaults
| rangefinder.c | ||
|---|---|---|
| 56 | 56 |
#define MIN_IR_ADC8 20 |
| 57 | 57 |
#define MAX_IR_ADC8 133 |
| 58 | 58 |
|
| 59 |
// hacky conversion |
|
| 60 |
#define IR_MAX 800 |
|
| 61 |
#define IR_MIN 100 |
|
| 62 |
#define IR_CONVERSION_FACTOR 2.0 |
|
| 63 |
|
|
| 59 | 64 |
void range_init(void) |
| 60 | 65 |
{
|
| 61 | 66 |
/* simulator does not need this */ |
| 62 | 67 |
} |
| 63 | 68 |
|
| 69 |
// should return value between 101 and 800 |
|
| 64 | 70 |
int range_read_distance (int range_id) |
| 65 | 71 |
{
|
| 66 |
int temp=0; |
|
| 72 |
short temp; |
|
| 73 |
short ir_value; // simulate what libdragonfly rangefiner returns |
|
| 67 | 74 |
/* read from array in shared memory, if valid */ |
| 68 |
if (range_id > 0 && range_id < 6) {
|
|
| 69 |
temp = (shared_state->ranges).d[range_id]; |
|
| 70 |
if (temp < 0 || temp > 1000) |
|
| 71 |
return -1; |
|
| 72 |
else |
|
| 73 |
return temp; |
|
| 75 |
/* TODO: fix numbering in libdragonfly then fix it here */ |
|
| 76 |
switch (range_id) {
|
|
| 77 |
case IR1: |
|
| 78 |
temp = (shared_state->ranges).d[0]; |
|
| 79 |
break; |
|
| 80 |
case IR2: |
|
| 81 |
temp = (shared_state->ranges).d[1]; |
|
| 82 |
break; |
|
| 83 |
case IR3: |
|
| 84 |
temp = (shared_state->ranges).d[2]; |
|
| 85 |
break; |
|
| 86 |
case IR4: |
|
| 87 |
temp = (shared_state->ranges).d[3]; |
|
| 88 |
break; |
|
| 89 |
case IR5: |
|
| 90 |
temp = (shared_state->ranges).d[4]; |
|
| 91 |
break; |
|
| 74 | 92 |
} |
| 93 |
ir_value = ((temp / IR_CONVERSION_FACTOR) + IR_MIN); |
|
| 94 |
ir_value = ir_value > IR_MAX ? IR_MAX : ir_value; |
|
| 95 |
printf("[%d]: %d\n", range_id, ir_value);
|
|
| 96 |
if (temp >= 0 && temp <= 1000) return ir_value; |
|
| 75 | 97 |
return -1; |
| 76 | 98 |
} |
| 77 | 99 |
|
Also available in: Unified diff