Revision 83db6c2c
Line following code made bettergit status
scout/libscout/src/behaviors/sim_line.cpp | ||
---|---|---|
27 | 27 |
|
28 | 28 |
using namespace std; |
29 | 29 |
|
30 |
int motor_l; |
|
31 |
int motor_r; |
|
30 |
static int motor_l; |
|
31 |
static int motor_r; |
|
32 |
static double last_ret; |
|
32 | 33 |
|
34 |
|
|
35 |
/** TODO: This should be part of the line sensor library */ |
|
33 | 36 |
double sim_line::get_line_pos(vector<uint32_t> readings) |
34 | 37 |
{ |
35 | 38 |
unsigned int total_read = 0; |
... | ... | |
43 | 46 |
|
44 | 47 |
if (total_read == 0) |
45 | 48 |
{ |
46 |
return 0;
|
|
49 |
return last_ret;
|
|
47 | 50 |
} |
48 |
|
|
49 |
return ((double) weighted_total / total_read) - 4; |
|
51 |
|
|
52 |
double ret_val = last_ret = ((double) weighted_total / total_read) - 4; |
|
53 |
return ret_val; |
|
50 | 54 |
} |
51 | 55 |
|
52 | 56 |
void sim_line::run() |
53 | 57 |
{ |
54 |
motor_l = -MOTOR_MAX;
|
|
55 |
motor_r = -MOTOR_MAX;
|
|
58 |
motor_l = -MOTOR_BASE;
|
|
59 |
motor_r = -MOTOR_BASE;
|
|
56 | 60 |
|
57 | 61 |
while(ok()) |
58 | 62 |
{ |
... | ... | |
60 | 64 |
|
61 | 65 |
cout << "line_loc: " << line_loc << endl; |
62 | 66 |
|
63 |
if (line_loc < 0) |
|
64 |
{ |
|
65 |
line_loc = abs(line_loc); |
|
66 |
motor_l = -MOTOR_MAX; |
|
67 |
motor_r = -MOTOR_MAX + (MOTOR_MAX / 2) * line_loc; |
|
68 |
} |
|
69 |
else |
|
70 |
{ |
|
71 |
motor_l = -MOTOR_MAX + (MOTOR_MAX / 2) * line_loc; |
|
72 |
motor_r = -MOTOR_MAX; |
|
73 |
} |
|
67 |
motor_l = -MOTOR_BASE + SCALE * line_loc; |
|
68 |
motor_r = -MOTOR_BASE - SCALE * line_loc; |
|
74 | 69 |
|
75 | 70 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
76 | 71 |
|
Also available in: Unified diff