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