Project

General

Profile

Revision 8913c26d

ID8913c26d9ebc512b1533bd368257904666391c1f

Added by Thomas Mullins almost 4 years ago

Fixed motors node to work around pwm driver bug

The node can now only do writes to /dev/pwm*, because doing both reads
and writes causes the fstream to try seeking, which fails because it's
not supported by the driver. Then the fstream refuses to do any more io.

View differences:

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

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

  
70
  // read current values from device files
71
  fpwm >> pwm;
72
  fin1 >> in1;
73
  fin2 >> in2;
67
  fin2.open(buf, ios::out);
74 68

  
75
  // convert to our speed units
76
  if (in1)
77
  {
78
    if (in2)
79
    {
80
      // short brake
81
      speed = 0;
82
    }
83
    else
84
    {
85
      // CW
86
      speed = pwm;
87
    }
88
  }
89
  else
90
  {
91
    if (in2)
92
    {
93
      // CCW
94
      speed = -pwm;
95
    }
96
    else
97
    {
98
      // off
99
      speed = 0;
100
    }
101
  }
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;
102 75

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

  
107 80
/**
81
 * @brief Motor destructor
82
 *
83
 * Sets the motor's speed to zero in preparation for the node exiting
84
 */
85
Motor::~Motor()
86
{
87
    set_speed(0);
88
}
89

  
90
/**
108 91
 * @brief Returns current motor speed
109 92
 *
110 93
 * Note that this doesn't read from the hardware
......
156 139
  fin2 << in2 << flush;
157 140
}
158 141

  
159
/// @todo change these to the correct motor locations
142
/// @todo change these to the correct motor locations / directions
160 143
// Motor state variables
161 144
static Motor motor_fl( 8, 70, 71);
162 145
static Motor motor_fr( 9, 72, 73);
......
241 224
    ROS_INFO("Motors node ready.");
242 225
    ros::spin();
243 226

  
227
    motor_fl.set_speed(0);
228
    motor_fr.set_speed(0);
229
    motor_bl.set_speed(0);
230
    motor_br.set_speed(0);
231

  
244 232
    return 0;
245 233
}
246 234

  

Also available in: Unified diff