31 |
31 |
*
|
32 |
32 |
* @author Colony Project, CMU Robotics Club
|
33 |
33 |
* @author Ben Wasserman
|
|
34 |
* @author Tom Mullins
|
34 |
35 |
*/
|
35 |
36 |
|
36 |
37 |
#include <ros/ros.h>
|
37 |
38 |
#include <cstdlib>
|
38 |
|
#include <fstream>
|
39 |
39 |
#include "motors.h"
|
40 |
40 |
|
41 |
41 |
using namespace std;
|
... | ... | |
47 |
47 |
* @{
|
48 |
48 |
*/
|
49 |
49 |
|
50 |
|
/* Motor state variables
|
51 |
|
* Speeds expressed as absolute speed out of max speed (0 - +-MAXSPEED)
|
52 |
|
* Absolute speed is the speed written to the hardware to move the motors
|
|
50 |
/**
|
|
51 |
* @brief Motor constructor
|
|
52 |
*
|
|
53 |
* Opens device file and reads initial PWM value
|
|
54 |
*
|
|
55 |
* @param filename The name of the PWM device file
|
53 |
56 |
*/
|
54 |
|
static int motor_fl_speed; /**< The current speed of the front left motor. */
|
55 |
|
static int motor_fr_speed; /**< The current speed of the front right motor. */
|
56 |
|
static int motor_bl_speed; /**< The current speed of the back left motor. */
|
57 |
|
static int motor_br_speed; /**< The current speed of the back right motor. */
|
|
57 |
Motor::Motor(const char* pwmfile, const char* in1file, const char* in2file)
|
|
58 |
: fpwm(pwmfile), fin1(in1file), fin2(in2file)
|
|
59 |
{
|
|
60 |
int pwm, in1, in2;
|
|
61 |
|
|
62 |
// read current values from device files
|
|
63 |
fpwm >> pwm;
|
|
64 |
fin1 >> in1;
|
|
65 |
fin2 >> in2;
|
|
66 |
|
|
67 |
// convert to our speed units
|
|
68 |
if (in1)
|
|
69 |
{
|
|
70 |
if (in2)
|
|
71 |
{
|
|
72 |
// short brake
|
|
73 |
speed = 0;
|
|
74 |
}
|
|
75 |
else
|
|
76 |
{
|
|
77 |
// CW
|
|
78 |
speed = pwm;
|
|
79 |
}
|
|
80 |
}
|
|
81 |
else
|
|
82 |
{
|
|
83 |
if (in2)
|
|
84 |
{
|
|
85 |
// CCW
|
|
86 |
speed = -pwm;
|
|
87 |
}
|
|
88 |
else
|
|
89 |
{
|
|
90 |
// off
|
|
91 |
speed = 0;
|
|
92 |
}
|
|
93 |
}
|
58 |
94 |
|
59 |
|
static fstream motor_fl_file; /**< The device file for the front left motor. */
|
60 |
|
static fstream motor_fr_file; /**< The device file for the front right motor. */
|
61 |
|
static fstream motor_bl_file; /**< The device file for the back left motor. */
|
62 |
|
static fstream motor_br_file; /**< The device file for the back right motor. */
|
|
95 |
// ensure we are in a consistent state by writing back to hardware
|
|
96 |
set_speed(speed);
|
|
97 |
}
|
|
98 |
|
|
99 |
/**
|
|
100 |
* @brief Returns current motor speed
|
|
101 |
*
|
|
102 |
* Note that this doesn't read from the hardware
|
|
103 |
*/
|
|
104 |
int Motor::get_speed()
|
|
105 |
{
|
|
106 |
return speed;
|
|
107 |
}
|
|
108 |
|
|
109 |
/**
|
|
110 |
* @brief Sets motor speed
|
|
111 |
*
|
|
112 |
* This will set the member this->speed and write the new speed to the hardware
|
|
113 |
*
|
|
114 |
* @param new_speed The speed to set, between +/- MAXSPEED inclusive
|
|
115 |
*/
|
|
116 |
void Motor::set_speed(int new_speed)
|
|
117 |
{
|
|
118 |
int pwm, in1, in2;
|
|
119 |
|
|
120 |
speed = new_speed;
|
|
121 |
|
|
122 |
// convert to hardware units
|
|
123 |
if (speed == 0)
|
|
124 |
{
|
|
125 |
/// @todo should this be off (00) or short brake (11)?
|
|
126 |
pwm = 0;
|
|
127 |
in1 = 0;
|
|
128 |
in2 = 0;
|
|
129 |
}
|
|
130 |
else if (speed > 0)
|
|
131 |
{
|
|
132 |
// CW
|
|
133 |
pwm = speed;
|
|
134 |
in1 = 1;
|
|
135 |
in2 = 0;
|
|
136 |
}
|
|
137 |
else
|
|
138 |
{
|
|
139 |
// CCW
|
|
140 |
pwm = -speed;
|
|
141 |
in1 = 0;
|
|
142 |
in2 = 1;
|
|
143 |
}
|
|
144 |
|
|
145 |
// write to hardware
|
|
146 |
fpwm << pwm;
|
|
147 |
fin1 << in1;
|
|
148 |
fin2 << in2;
|
|
149 |
}
|
|
150 |
|
|
151 |
/// @todo change these to the correct files
|
|
152 |
/// @todo also, a problem: gpio can only be done as root!
|
|
153 |
// Motor state variables
|
|
154 |
static Motor motor_fl("/dev/pwm8", "/dev/null", "/dev/null");
|
|
155 |
static Motor motor_fr("/dev/pwm9", "/dev/null", "/dev/null");
|
|
156 |
static Motor motor_bl("/dev/pwm10", "/dev/null", "/dev/null");
|
|
157 |
static Motor motor_br("/dev/pwm11", "/dev/null", "/dev/null");
|
63 |
158 |
|
64 |
159 |
/**
|
65 |
160 |
* @brief Sets motor speed
|
... | ... | |
73 |
168 |
{
|
74 |
169 |
if(msg->fl_set)
|
75 |
170 |
{
|
76 |
|
motor_fl_speed = msg->fl_speed;
|
77 |
|
motor_fl_file << motor_fl_speed;
|
|
171 |
motor_fl.set_speed(msg->fl_speed);
|
78 |
172 |
}
|
79 |
173 |
if(msg->fr_set)
|
80 |
174 |
{
|
81 |
|
motor_fr_speed = msg->fr_speed;
|
82 |
|
motor_fr_file << motor_fr_speed;
|
|
175 |
motor_fr.set_speed(msg->fr_speed);
|
83 |
176 |
}
|
84 |
177 |
if(msg->bl_set)
|
85 |
178 |
{
|
86 |
|
motor_bl_speed = msg->bl_speed;
|
87 |
|
motor_bl_file << motor_bl_speed;
|
|
179 |
motor_bl.set_speed(msg->bl_speed);
|
88 |
180 |
}
|
89 |
181 |
if(msg->br_set)
|
90 |
182 |
{
|
91 |
|
motor_br_speed = msg->br_speed;
|
92 |
|
motor_br_file << motor_br_speed;
|
|
183 |
motor_br.set_speed(msg->br_speed);
|
93 |
184 |
}
|
94 |
185 |
|
95 |
186 |
ROS_DEBUG("Motor speeds set");
|
... | ... | |
106 |
197 |
bool motors_query(motors::query_motors::Request &req,
|
107 |
198 |
motors::query_motors::Response &res)
|
108 |
199 |
{
|
109 |
|
/** @todo Are we checking hardware or just using the saved values?
|
110 |
|
* Saved values sound fine to me (Alex). Pleas confirm.
|
111 |
|
* I think saved values are ok; they're read in from hardware at the
|
112 |
|
* beginning (Tom). */
|
113 |
|
res.fl_speed = motor_fl_speed;
|
114 |
|
res.fr_speed = motor_fr_speed;
|
115 |
|
res.bl_speed = motor_bl_speed;
|
116 |
|
res.br_speed = motor_br_speed;
|
|
200 |
res.fl_speed = motor_fl.get_speed();
|
|
201 |
res.fr_speed = motor_fr.get_speed();
|
|
202 |
res.bl_speed = motor_bl.get_speed();
|
|
203 |
res.br_speed = motor_br.get_speed();
|
117 |
204 |
|
118 |
205 |
ROS_DEBUG("Motor speeds queried");
|
119 |
206 |
|
... | ... | |
144 |
231 |
/* Subscribe to the set_motors topic */
|
145 |
232 |
ros::Subscriber sub0 = node.subscribe("set_motors", QUEUE_SIZE, motors_set);
|
146 |
233 |
|
147 |
|
/* Open device files for PWM values */
|
148 |
|
/** @todo These are probably not the correct order */
|
149 |
|
motor_fl_file.open("/dev/pwm8");
|
150 |
|
motor_fr_file.open("/dev/pwm9");
|
151 |
|
motor_bl_file.open("/dev/pwm10");
|
152 |
|
motor_br_file.open("/dev/pwm11");
|
153 |
|
|
154 |
|
/* Get initial values, for motor queries */
|
155 |
|
motor_fl_file >> motor_fl_speed;
|
156 |
|
motor_fr_file >> motor_fr_speed;
|
157 |
|
motor_bl_file >> motor_bl_speed;
|
158 |
|
motor_br_file >> motor_br_speed;
|
159 |
|
|
160 |
234 |
ROS_INFO("Motors node ready.");
|
161 |
235 |
ros::spin();
|
162 |
236 |
|