Project

General

Profile

Revision ae21730e

IDae21730ed52fe1d5e5638d166cca7dcef668ed08

Added by Priya over 6 years ago

Sim line can make turns now! yay

View differences:

scout/libscout/src/LinesensorControl.cpp
91 91

  
92 92
bool LinesensorControl::fullline()
93 93
{
94
    return false; 
95
}
96

  
97
void LinesensorControl::turnLeft()
98
{
99 94
    vector<uint32_t> readings = query();
95
    for(int i=0; i<8; i++)
96
    {
97
        if(readings[i] < 200)
98
          return false;
99
    }
100
    return true; 
100 101
}
101 102

  
102
void LinesensorControl::turnRight()
103
{
104
    vector<uint32_t> readings = query();
105

  
106
}
scout/libscout/src/LinesensorControl.h
52 52
        /** Get line position */
53 53
        double readline();
54 54

  
55
        /** Implement turn functions */
56
        void turnLeft();
57
        void turnRight();
55
        /** Check if we are at an intersecetion */
56
        bool fullline();
58 57

  
59 58
    private:
60 59
        /* ROS publisher and client declaration */
61 60
        ros::ServiceClient query_client;
62 61
        ros::NodeHandle node;
63

  
64
        /** Check if we are at an intersecetion */
65
        bool fullline();
66 62
};
67 63

  
68 64
#endif /* _LINESENSOR_CONTORL_H_ */
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
}
scout/libscout/src/behaviors/sim_line.h
40 40
        void run();
41 41

  
42 42
    private:
43
        double get_line_pos(std::vector<uint32_t> readings);
43
        void turn_left();
44
        void turn_right();
45
        void follow_line();
46
        void halt();
44 47
};
45 48

  
46 49
#endif

Also available in: Unified diff