Revision 8913c26d
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.
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 |
|
scout/motors/src/motors.h | ||
---|---|---|
59 | 59 |
{ |
60 | 60 |
public: |
61 | 61 |
Motor(int pwm_gpt, int in1_gpio, int in2_gpio); |
62 |
~Motor(); |
|
62 | 63 |
int get_speed(); |
63 | 64 |
void set_speed(int speed); |
64 | 65 |
private: |
Also available in: Unified diff