Project

General

Profile

Revision ae21730e

IDae21730ed52fe1d5e5638d166cca7dcef668ed08
Parent 26562d0c
Child f8655f01

Added by Priya over 5 years ago

Sim line can make turns now! yay

View differences:

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