Skip to content

Commit f067a9f

Browse files
epw1624epw1624
committed
Merge branch 'hotfix/emily/motor-control-position-rescale' into feat/emily/more_sophisticated_drive_control
2 parents 14b1200 + a9807ff commit f067a9f

File tree

3 files changed

+78
-53
lines changed

3 files changed

+78
-53
lines changed

‎src/drive_control/include/motor_control.h‎

Lines changed: 19 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -10,24 +10,27 @@
1010
#include <algorithm> // For std::clamp
1111

1212
// Motor and wheel specs for calulcating rescale factors
13-
#define MOTOR_RPM 41
1413
#define MOTOR_GEAR_RATIO 23
1514
#define MOTOR_NUM_POLES 4
1615
#define MOTOR_NUM_PHASES 3
17-
#define WHEEL_RADIUS_METERS 0.1
1816

1917
/**
2018
* Converts commutations into radians
21-
* Used for velocity and target velocity in the velocity controller and position in the motor controller
19+
* Allows communication with the position controller in radians rather than commutations
2220
*/
2321
#define MOTOR_RESCALE_FACTOR (2.0 * M_PI) / (MOTOR_GEAR_RATIO * MOTOR_NUM_POLES * MOTOR_NUM_PHASES)
2422

23+
#define MAX_VELOCITY_MS 5
24+
#define MIN_VELOCITY_MS 0.05
25+
#define WHEEL_RADIUS_METERS 0.1
26+
2527
/**
26-
* Theoretical max motor velocity in radians/s
28+
* Max velocity of the motors in radians per second
2729
*/
28-
#define MOTOR_MAX_VELOCITY_RADIANS (MOTOR_RPM / 60.0) * (2.0 * M_PI)
30+
#define MAX_VELOCITY_RADS MAX_VELOCITY_MS / WHEEL_RADIUS_METERS
2931

3032
#define NUM_MOTORS 6
33+
#define MOTOR_CONTROL_LOOP_FREQUENCY_MS 50 // Frequency for the motor control loop
3134
#define DRIVE_FEEDBACK_PUBLISH_FREQUENCY_MS 100 // Publish frequency for drive_feedback_pub_
3235
#define MOTOR_FAILSAFE_INTERVAL_MS 500 // Interval for the Phidget failsafe to shut down the motors (in ms)
3336

@@ -47,8 +50,9 @@ class MotorControlNode : public rclcpp::Node {
4750
// Phidget error handling function
4851
void handlePhidgetError(PhidgetReturnCode ret, const std::string& action, int motor_id);
4952

50-
// Method to run motors at a specified velocity
51-
void runMotors(const std::vector<int>& selected_motors, float velocity);
53+
// Loop to interact with motors
54+
void motorControlLoop();
55+
void setVelocity(const std::vector<int>& selected_motors, float velocity);
5256

5357
// Callback functions for left and right wheel velocity subscriptions
5458
void leftWheelCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
@@ -64,9 +68,14 @@ class MotorControlNode : public rclcpp::Node {
6468
// Timer to reset the motor failsafe
6569
rclcpp::TimerBase::SharedPtr failsafe_timer_;
6670

67-
// Constants and motor-related variables
68-
PhidgetBLDCMotorHandle motors[NUM_MOTORS]; // Array of motors
69-
PhidgetMotorVelocityControllerHandle motor_velocity_controllers[NUM_MOTORS]; // Array of motor velocity controllers
71+
// Timer for the main motor control loop
72+
rclcpp::TimerBase::SharedPtr motor_control_timer_;
73+
74+
// Motor-related variables
75+
PhidgetMotorPositionControllerHandle motors[NUM_MOTORS]; // Array of motors
76+
double target_positions[NUM_MOTORS] = {0.0}; // Array of target motor position, used as input to the Position Controllers
77+
double velocities[NUM_MOTORS] = {0.0}; // Array of the current motor velocities, tracked for drive/feedback odometry
78+
7079
const char* errorString; // Error string for logging
7180
char errorDetail[100]; // Detailed error message
7281
size_t errorDetailLen = 100; // Length of the error detail buffer

‎src/drive_control/src/motor_control.cpp‎

Lines changed: 59 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -12,27 +12,36 @@ MotorControlNode::MotorControlNode() : Node("motor_control_node") {
1212
for (int i = 0; i < NUM_MOTORS; i++) {
1313
PhidgetReturnCode ret;
1414

15-
ret = PhidgetBLDCMotor_create(&motors[i]);
15+
ret = PhidgetMotorPositionController_create(&motors[i]);
1616
handlePhidgetError(ret, "creating motor", i);
17-
ret = PhidgetMotorVelocityController_create(&motor_velocity_controllers[i]);
18-
handlePhidgetError(ret, "creating velocity controller", i);
1917

2018
ret = Phidget_setHubPort((PhidgetHandle)motors[i], i);
2119
handlePhidgetError(ret, "setting motor hub port", i);
22-
ret = Phidget_setHubPort((PhidgetHandle)motor_velocity_controllers[i], i);
23-
handlePhidgetError(ret, "setting velocity controller hub port", i);
2420

2521
ret = Phidget_openWaitForAttachment((PhidgetHandle)motors[i], 5000);
26-
handlePhidgetError(ret, "opening motor", i);
27-
ret = Phidget_openWaitForAttachment((PhidgetHandle)motor_velocity_controllers[i], 5000);
28-
handlePhidgetError(ret, "opening velocity controller", i);
22+
handlePhidgetError(ret, "opening motor connection", i);
2923

30-
ret = PhidgetBLDCMotor_setRescaleFactor(motors[i], MOTOR_RESCALE_FACTOR);
31-
handlePhidgetError(ret, "set motor rescale factor", i);
32-
ret = PhidgetMotorVelocityController_setRescaleFactor(motor_velocity_controllers[i], MOTOR_RESCALE_FACTOR);
33-
handlePhidgetError(ret, "set rescale factor", i);
24+
ret = PhidgetMotorPositionController_setNormalizePID(motors[i], 1);
25+
handlePhidgetError(ret, "normalizing PID controller", i);
3426

35-
PhidgetMotorVelocityController_setEngaged(motor_velocity_controllers[i], 1);
27+
// Setup motor position settings so wheels are stopped by default
28+
double position;
29+
ret = PhidgetMotorPositionController_getPosition(motors[i], &position);
30+
handlePhidgetError(ret, "getting initial motor position", i);
31+
32+
target_positions[i] = position;
33+
34+
ret = PhidgetMotorPositionController_setTargetPosition(motors[i], position);
35+
handlePhidgetError(ret, "setting initial motor target position", i);
36+
37+
PhidgetMotorPositionController_setEngaged(motors[i], 1);
38+
handlePhidgetError(ret, "engaging motor", i);
39+
40+
ret = PhidgetMotorPositionController_setRescaleFactor(motors[i], MOTOR_RESCALE_FACTOR);
41+
handlePhidgetError(ret, "setting motor rescale factor", i);
42+
43+
ret = PhidgetMotorPositionController_setVelocityLimit(motors[i], MAX_VELOCITY_RADS);
44+
handlePhidgetError(ret, "setting motor max velocity", i);
3645
}
3746

3847
auto qos = rclcpp::QoS(rclcpp::KeepLast(64));
@@ -49,7 +58,7 @@ MotorControlNode::MotorControlNode() : Node("motor_control_node") {
4958

5059
// Enable failsafe for all motors
5160
for (int i = 0; i < NUM_MOTORS; i++) {
52-
PhidgetReturnCode ret = PhidgetBLDCMotor_enableFailsafe(motors[i], MOTOR_FAILSAFE_INTERVAL_MS);
61+
PhidgetReturnCode ret = PhidgetMotorPositionController_enableFailsafe(motors[i], MOTOR_FAILSAFE_INTERVAL_MS);
5362
handlePhidgetError(ret, "enable failsafe", i);
5463
}
5564

@@ -59,6 +68,11 @@ MotorControlNode::MotorControlNode() : Node("motor_control_node") {
5968
std::bind(&MotorControlNode::resetFailsafe, this)
6069
);
6170

71+
motor_control_timer_ = this->create_wall_timer(
72+
std::chrono::milliseconds(MOTOR_CONTROL_LOOP_FREQUENCY_MS),
73+
std::bind(&MotorControlNode::motorControlLoop, this)
74+
);
75+
6276
// Create subscribers for left and right wheel velocity commands
6377
left_wheel_sub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
6478
"left_wheel_speeds", rclcpp::QoS(10), std::bind(&MotorControlNode::leftWheelCallback, this, std::placeholders::_1));
@@ -71,19 +85,21 @@ MotorControlNode::~MotorControlNode() {
7185
RCLCPP_WARN(this->get_logger(), "Motor Control Node shutting down.");
7286
for (int i = 0; i < NUM_MOTORS; i++) {
7387
if (motors[i] != nullptr) {
74-
PhidgetBLDCMotor_delete(&motors[i]);
88+
PhidgetMotorPositionController_delete(&motors[i]);
7589
}
7690
}
7791
}
7892

7993
void MotorControlNode::leftWheelCallback(const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) {
8094
float left_velocity = msg->data.empty() ? 0.0f : msg->data[0]; // Extract velocity safely
81-
runMotors({3, 4, 5}, left_velocity); // Corrected function name
95+
setVelocity({3, 4, 5}, left_velocity);
96+
// runMotors({3, 4, 5}, left_velocity); // Corrected function name
8297
}
8398

8499
void MotorControlNode::rightWheelCallback(const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) {
85100
float right_velocity = msg->data.empty() ? 0.0f : msg->data[0];
86-
runMotors({0, 1, 2}, right_velocity); // Corrected function name
101+
setVelocity({0, 1, 2}, right_velocity);
102+
// runMotors({0, 1, 2}, right_velocity); // Corrected function name
87103
}
88104

89105
void MotorControlNode::handlePhidgetError(PhidgetReturnCode ret, const std::string& action, int i) {
@@ -99,15 +115,29 @@ void MotorControlNode::handlePhidgetError(PhidgetReturnCode ret, const std::stri
99115
}
100116
}
101117

102-
void MotorControlNode::runMotors(const std::vector<int>& selected_motors, float velocity) {
103-
PhidgetLog_enable(PHIDGET_LOG_INFO, "phidgetlog.log");
118+
void MotorControlNode::motorControlLoop() {
119+
PhidgetReturnCode ret;
120+
for (int i = 0; i < NUM_MOTORS; i++) {
121+
double current_position;
122+
ret = PhidgetMotorPositionController_getPosition(motors[i], &current_position);
104123

105-
double velocity_rad_s = velocity / WHEEL_RADIUS_METERS;
106-
velocity_rad_s = std::clamp(velocity_rad_s, -MOTOR_MAX_VELOCITY_RADIANS, MOTOR_MAX_VELOCITY_RADIANS);
107-
108-
for (int motor_index : selected_motors) {
109-
PhidgetReturnCode ret = PhidgetMotorVelocityController_setTargetVelocity(motor_velocity_controllers[motor_index], (double)velocity_rad_s);
110-
handlePhidgetError(ret, "set target velocity", motor_index);
124+
target_positions[i] = current_position + (velocities[i] * MOTOR_CONTROL_LOOP_FREQUENCY_MS);
125+
ret = PhidgetMotorPositionController_setTargetPosition(motors[i], target_positions[i]);
126+
handlePhidgetError(ret, "setting motor position", i);
127+
}
128+
}
129+
130+
void MotorControlNode::setVelocity(const std::vector<int>& selected_motors, float velocity) {
131+
double velocity_rads;
132+
if (std::abs(velocity) < MIN_VELOCITY_MS) {
133+
velocity_rads = 0.0;
134+
}
135+
else {
136+
velocity_rads = std::clamp(velocity / WHEEL_RADIUS_METERS, -MAX_VELOCITY_RADS, MAX_VELOCITY_RADS);
137+
}
138+
139+
for (int i : selected_motors) {
140+
velocities[i] = velocity_rads;
111141
}
112142
}
113143

@@ -116,37 +146,24 @@ void MotorControlNode::publishDriveFeedback() {
116146

117147
message.valid_data.resize(NUM_MOTORS, true);
118148
message.velocities.resize(NUM_MOTORS);
119-
message.target_velocities.resize(NUM_MOTORS);
120149
message.positions.resize(NUM_MOTORS);
121150

122151
for (int i = 0; i < NUM_MOTORS; i++) {
123152
PhidgetReturnCode ret;
124153

125-
ret = PhidgetMotorVelocityController_getVelocity(motor_velocity_controllers[i], &message.velocities[i]);
126-
if (ret != EPHIDGET_OK) {
127-
handlePhidgetError(ret, "get velocity", i);
128-
message.valid_data[i] = false;
129-
}
130-
131-
ret = PhidgetMotorVelocityController_getTargetVelocity(motor_velocity_controllers[i], &message.target_velocities[i]);
154+
ret = PhidgetMotorPositionController_getPosition(motors[i], &message.positions[i]);
132155
if (ret != EPHIDGET_OK) {
133-
handlePhidgetError(ret, "get target velocity", i);
134-
message.valid_data[i] = false;
135-
}
136-
137-
ret = PhidgetBLDCMotor_getPosition(motors[i], &message.positions[i]);
138-
if (ret != EPHIDGET_OK) {
139-
handlePhidgetError(ret, "get position", i);
156+
handlePhidgetError(ret, "getting motor position", i);
140157
message.valid_data[i] = false;
141158
}
159+
message.velocities[i] = velocities[i];
142160
}
143-
144161
drive_feedback_pub_->publish(message);
145162
}
146163

147164
void MotorControlNode::resetFailsafe() {
148165
for (int i = 0; i < NUM_MOTORS; i++) {
149-
PhidgetReturnCode ret = PhidgetBLDCMotor_resetFailsafe(motors[i]);
166+
PhidgetReturnCode ret = PhidgetMotorPositionController_resetFailsafe(motors[i]);
150167
handlePhidgetError(ret, "failsafe", i);
151168
}
152169
}

‎src/rover_msgs/msg/DriveFeedback.msg‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,3 @@
11
bool[] valid_data
22
float64[] velocities
3-
float64[] target_velocities
43
float64[] positions

0 commit comments

Comments
 (0)