Revision ae21730e
ID | ae21730ed52fe1d5e5638d166cca7dcef668ed08 |
Sim line can make turns now! yay
scout/libscout/src/behaviors/sim_line.cpp | ||
---|---|---|
29 | 29 |
|
30 | 30 |
static int motor_l; |
31 | 31 |
static int motor_r; |
32 |
static double last_ret; |
|
33 | 32 |
|
34 |
/** TODO: This should be part of the line sensor library */ |
|
35 |
double sim_line::get_line_pos(vector<uint32_t> readings) |
|
33 |
void sim_line::follow_line() |
|
36 | 34 |
{ |
37 |
unsigned int total_read = 0; |
|
38 |
unsigned int weighted_total = 0; |
|
39 |
|
|
40 |
for (int i = 0; i < 8; i++) |
|
35 |
while(!linesensor->fullline()) |
|
41 | 36 |
{ |
42 |
total_read += readings[i]; |
|
43 |
weighted_total += i * readings[i]; |
|
44 |
} |
|
37 |
double line_loc = linesensor->readline(); |
|
45 | 38 |
|
46 |
if (total_read == 0) |
|
47 |
{ |
|
48 |
return last_ret; |
|
39 |
cout << "line_loc: " << line_loc << endl; |
|
40 |
|
|
41 |
motor_l = -MOTOR_BASE + SCALE * line_loc; |
|
42 |
motor_r = -MOTOR_BASE - SCALE * line_loc; |
|
43 |
|
|
44 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
49 | 45 |
} |
50 |
|
|
51 |
double ret_val = last_ret = ((double) weighted_total / total_read) - 4; |
|
52 |
return ret_val; |
|
46 |
halt(); |
|
53 | 47 |
} |
54 | 48 |
|
55 |
void sim_line::run()
|
|
49 |
void sim_line::turn_left()
|
|
56 | 50 |
{ |
51 |
bool first = true; |
|
52 |
do |
|
53 |
{ |
|
57 | 54 |
motor_l = -MOTOR_BASE; |
58 |
motor_r = -MOTOR_BASE;
|
|
55 |
motor_r = MOTOR_BASE/8;
|
|
59 | 56 |
|
60 |
while(ok()) |
|
57 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
58 |
|
|
59 |
double line_loc = linesensor->readline(); |
|
60 |
cout << "line_loc: " << line_loc << endl; |
|
61 |
if(first) |
|
61 | 62 |
{ |
62 |
double line_loc = linesensor->readline(); |
|
63 |
loop_rate->sleep(); |
|
64 |
loop_rate->sleep(); |
|
65 |
loop_rate->sleep(); |
|
66 |
loop_rate->sleep(); |
|
67 |
first = false; |
|
68 |
} |
|
69 |
} |
|
70 |
while(linesensor->readline()); |
|
71 |
} |
|
63 | 72 |
|
64 |
cout << "line_loc: " << line_loc << endl; |
|
73 |
void sim_line::halt() |
|
74 |
{ |
|
75 |
motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
|
76 |
} |
|
65 | 77 |
|
66 |
motor_l = -MOTOR_BASE + SCALE * line_loc; |
|
67 |
motor_r = -MOTOR_BASE - SCALE * line_loc; |
|
78 |
void sim_line::turn_right() |
|
79 |
{ |
|
80 |
bool first = true; |
|
81 |
do |
|
82 |
{ |
|
83 |
motor_l = MOTOR_BASE/8; |
|
84 |
motor_r = -MOTOR_BASE; |
|
68 | 85 |
|
69 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
|
|
86 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
70 | 87 |
|
71 |
spinOnce(); |
|
88 |
double line_loc = linesensor->readline(); |
|
89 |
cout << "line_loc: " << line_loc << endl; |
|
90 |
if(first) |
|
91 |
{ |
|
92 |
loop_rate->sleep(); |
|
93 |
loop_rate->sleep(); |
|
94 |
loop_rate->sleep(); |
|
72 | 95 |
loop_rate->sleep(); |
96 |
first = false; |
|
73 | 97 |
} |
98 |
} |
|
99 |
while(linesensor->readline()); |
|
100 |
} |
|
101 |
|
|
102 |
void sim_line::run() |
|
103 |
{ |
|
104 |
follow_line(); |
|
105 |
turn_right(); |
|
106 |
follow_line(); |
|
107 |
turn_left(); |
|
108 |
follow_line(); |
|
109 |
turn_right(); |
|
110 |
follow_line(); |
|
74 | 111 |
} |
Also available in: Unified diff