root / trunk / code / projects / colonet / testing / dongle / robot_receiver / follow.c @ 13
History | View | Annotate | Download (795 Bytes)
1 |
|
---|---|
2 |
#include "firefly+_lib.h" |
3 |
#include "follow.h" |
4 |
|
5 |
#define FWD 0 |
6 |
#define BCK 1 |
7 |
|
8 |
#define TOTAL 16 |
9 |
|
10 |
|
11 |
/*
|
12 |
int find_max
|
13 |
returns the index of the BOM with the highest reading
|
14 |
* note: index, not value
|
15 |
*/
|
16 |
|
17 |
void seek(int relative_position) |
18 |
{ |
19 |
int speed = 255; |
20 |
|
21 |
//MOTOR1 - left
|
22 |
//MOTOR2 - right
|
23 |
|
24 |
if(relative_position < 5 || relative_position > 13) |
25 |
{ //turn right
|
26 |
motor2_set(BCK, 0);
|
27 |
motor1_set(FWD, speed); |
28 |
} |
29 |
else if(relative_position > 5 && relative_position <= 13) |
30 |
{ //turn left
|
31 |
motor2_set(FWD, speed); |
32 |
motor1_set(BCK, 0);
|
33 |
} |
34 |
else
|
35 |
{ //go forward
|
36 |
//turn_off_motors();
|
37 |
motor1_set(FWD, speed); |
38 |
motor2_set(FWD, speed); |
39 |
} |
40 |
} |
41 |
|
42 |
void drive_forward (void) |
43 |
{ |
44 |
int speed = 255; |
45 |
motor1_set(FWD, speed); |
46 |
motor2_set(FWD, speed); |
47 |
} |