Revision fcd68ec1 scout/motors/src/motors.cpp

View differences:

scout/motors/src/motors.cpp
56 56
 */
57 57
Motor::Motor(int pwm_gpt, int in1_gpio, int in2_gpio)
58 58
{
59
  char buf[60];
60

  
61
  // open device files
62
  sprintf(buf, "/dev/pwm%d", pwm_gpt);
63
  fpwm.open(buf, ios::out);
64
  sprintf(buf, "/sys/class/gpio/gpio%d/value", in1_gpio);
65
  fin1.open(buf, ios::out);
66
  sprintf(buf, "/sys/class/gpio/gpio%d/value", in2_gpio);
67
  fin2.open(buf, ios::out);
68

  
69
  /* I set speed to 0 here rather than reading in what the current speed is
70
   * because the pwm driver does not support seeking in the device files. Doing
71
   * both reading and writing causes fpwm to attempt seeking, which fails,
72
   * causing fpwm to refuse to do any more io. So, we can only write.
73
   */
74
  speed = 0;
75

  
76
  // ensure we are in a consistent state by writing to hardware
77
  set_speed(speed);
59
    char buf[60];
60

  
61
    // open device files
62
    sprintf(buf, "/dev/pwm%d", pwm_gpt);
63
    fpwm.open(buf, ios::out);
64
    sprintf(buf, "/sys/class/gpio/gpio%d/value", in1_gpio);
65
    fin1.open(buf, ios::out);
66
    sprintf(buf, "/sys/class/gpio/gpio%d/value", in2_gpio);
67
    fin2.open(buf, ios::out);
68

  
69
    /* I set speed to 0 here rather than reading in what the current speed is
70
     * because the pwm driver does not support seeking in the device files.
71
     * Doing both reading and writing causes fpwm to attempt seeking, which
72
     * fails, causing fpwm to refuse to do any more io. So, we can only write.
73
     */
74
    speed = 0;
75

  
76
    // ensure we are in a consistent state by writing to hardware
77
    set_speed(speed);
78 78
}
79 79

  
80 80
/**
......
94 94
 */
95 95
int Motor::get_speed()
96 96
{
97
  return speed;
97
    return speed;
98 98
}
99 99

  
100 100
/**
......
106 106
 */
107 107
void Motor::set_speed(int new_speed)
108 108
{
109
  int pwm, in1, in2;
110

  
111
  speed = new_speed;
112

  
113
  // convert to hardware units
114
  if (speed == 0)
115
  {
116
    /// @todo should this be off (00) or short brake (11)?
117
    pwm = 0;
118
    in1 = 0;
119
    in2 = 0;
120
  }
121
  else if (speed > 0)
122
  {
123
    // CW
124
    pwm = speed;
125
    in1 = 1;
126
    in2 = 0;
127
  }
128
  else
129
  {
130
    // CCW
131
    pwm = -speed;
132
    in1 = 0;
133
    in2 = 1;
134
  }
135

  
136
  // write to hardware
137
  fpwm << pwm << flush;
138
  fin1 << in1 << flush;
139
  fin2 << in2 << flush;
109
    int pwm, in1, in2;
110

  
111
    speed = new_speed;
112

  
113
    // convert to hardware units
114
    if (speed == 0)
115
    {
116
        /// @todo should this be off (00) or short brake (11)?
117
        pwm = 0;
118
        in1 = 0;
119
        in2 = 0;
120
    }
121
    else if (speed > 0)
122
    {
123
        // CW
124
        pwm = speed;
125
        in1 = 1;
126
        in2 = 0;
127
    }
128
    else
129
    {
130
        // CCW
131
        pwm = -speed;
132
        in1 = 0;
133
        in2 = 1;
134
    }
135

  
136
    // write to hardware
137
    fpwm << pwm << flush;
138
    fin1 << in1 << flush;
139
    fin2 << in2 << flush;
140 140
}
141 141

  
142 142
/// @todo change these to the correct motor locations / directions
......
158 158
{
159 159
    if(msg->fl_set)
160 160
    {
161
      motor_fl.set_speed(msg->fl_speed);
161
        motor_fl.set_speed(msg->fl_speed);
162 162
    }
163 163
    if(msg->fr_set)
164 164
    {
165
      motor_fr.set_speed(msg->fr_speed);
165
        motor_fr.set_speed(msg->fr_speed);
166 166
    }
167 167
    if(msg->bl_set)
168 168
    {
169
      motor_bl.set_speed(msg->bl_speed);
169
        motor_bl.set_speed(msg->bl_speed);
170 170
    }
171 171
    if(msg->br_set)
172 172
    {
173
      motor_br.set_speed(msg->br_speed);
173
        motor_br.set_speed(msg->br_speed);
174 174
    }
175 175

  
176 176
    ROS_DEBUG("Motor speeds set");

Also available in: Unified diff