diff --git a/include/chip_bldc_driver/bldc_controller.h b/include/chip_bldc_driver/bldc_controller.h index 0e42e21..26c7443 100644 --- a/include/chip_bldc_driver/bldc_controller.h +++ b/include/chip_bldc_driver/bldc_controller.h @@ -5,6 +5,9 @@ #include "chip_bldc_driver/bldc_serial.h" #include "chip_bldc_driver/Command.h" +#include "std_msgs/String.h" +#include "std_msgs/UInt16.h" + namespace bldc_controller { @@ -18,9 +21,37 @@ class BldcController ~BldcController(); private: + uint16_t motor_kp; + uint16_t motor_ki; + uint16_t motor_kd; + + bldc_serial::BldcSerial *serial; + ros::Timer timeout_timer_; + ros::Subscriber sub_cmd_ , sub_KP , sub_KI , sub_KD; + ros::NodeHandle nh_; + void motorCommand(const chip_bldc_driver::Command& command); + void timeoutCallback(const ros::TimerEvent&); + + void KP_Callback(const std_msgs::UInt16::ConstPtr& msg); // callback for motor KP + void KI_Callback(const std_msgs::UInt16::ConstPtr& msg); // callback for motor KI + void KD_Callback(const std_msgs::UInt16::ConstPtr& msg); // callback for motor KD + +}; + +class BldcController_right +{ +public: + BldcController_right(bldc_serial::BldcSerial *s); + ~BldcController_right(); + +private: + uint16_t motor_kp; + uint16_t motor_ki; + uint16_t motor_kd; + bldc_serial::BldcSerial *serial; ros::Timer timeout_timer_; - ros::Subscriber sub_cmd_; + ros::Subscriber sub_cmd_ , sub_KP , sub_KI , sub_KD ; ros::NodeHandle nh_; void motorCommand(const chip_bldc_driver::Command& command); void timeoutCallback(const ros::TimerEvent&); diff --git a/include/chip_bldc_driver/bldc_serial.h b/include/chip_bldc_driver/bldc_serial.h index 2b177d0..1436784 100644 --- a/include/chip_bldc_driver/bldc_serial.h +++ b/include/chip_bldc_driver/bldc_serial.h @@ -7,6 +7,9 @@ #include #include "chip_bldc_driver/Status.h" #include "chip_bldc_driver/Feedback.h" +#include "std_msgs/String.h" +#include "std_msgs/UInt16.h" + namespace bldc_serial { @@ -17,14 +20,23 @@ namespace bldc_serial class BldcSerial { public: + uint16_t motor_kp = 10; + uint16_t motor_ki = 0; + uint16_t motor_kd = 0; + BldcSerial(const char *port, int baud); ~BldcSerial(); bool connect(); void read(); void sendMotorCommand(int16_t motor_speed_cmd); + + void Send_KP( uint16_t motor_kp_value); + void Send_KI( uint16_t motor_kp_value); + void Send_KD( uint16_t motor_kp_value); private: std::vector splitString(const std::string& s, char delimeter); + void write(const char *data_ptr, int32_t size); void processFeedback(const char *data_ptr, int32_t size); void processStatus(const char *data_ptr, int32_t size); @@ -34,14 +46,13 @@ class BldcSerial bool extractData(std::vector& out, const char* data, int32_t size, const std::string som, int32_t n_values); - private: const char *port_; int baud_; serial::Serial *serial_; ros::NodeHandle nh_; ros::Publisher pub_status_, pub_feedback_; - + }; } // namespace bldc serial diff --git a/launch/2wd_robot.launch b/launch/2wd_robot.launch new file mode 100644 index 0000000..73ac7fc --- /dev/null +++ b/launch/2wd_robot.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/bldc_controller.cpp b/src/bldc_controller.cpp index 9bbb2c0..1aef2b1 100644 --- a/src/bldc_controller.cpp +++ b/src/bldc_controller.cpp @@ -1,22 +1,22 @@ -/** -Software License Agreement BSD -\file bldc_controller.cpp -\copyright Copyright (c) 2017, Chip Robotics, All rights reserved. -Redistribution and use in source and binary forms, with or without modification, are permitted provided that -the following conditions are met: -* Redistributions of source code must retain the above copyright notice, this list of conditions and the -following disclaimer. -* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the -following disclaimer in the documentation and/or other materials provided with the distribution. -* Neither the name of NAME nor the names of its contributors may be used to endorse or promote -products derived from this software without specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT -OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +/* + Software License Agreement BSD + \file bldc_controller.cpp + \copyright Copyright (c) 2017, Chip Robotics, All rights reserved. + Redistribution and use in source and binary forms, with or without modification, are permitted provided that + the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of NAME nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- + RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- + DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT + OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "chip_bldc_driver/bldc_controller.h" @@ -25,32 +25,95 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI namespace bldc_controller { -/** - * Constructs - */ -BldcController::BldcController(bldc_serial::BldcSerial *s) : serial(s), - nh_("~") -{ - sub_cmd_ = nh_.subscribe("/bldc_driver_node/Command", 1, &BldcController::motorCommand, this); - timeout_timer_ = nh_.createTimer(ros::Duration(2), &BldcController::timeoutCallback, this); - timeout_timer_.start(); -} + /** + * Constructs + */ + BldcController::BldcController(bldc_serial::BldcSerial *s) : serial(s), nh_("~") { + sub_cmd_ = nh_.subscribe("/bldc_driver_node/Command_left", 1, &BldcController::motorCommand, this); + sub_KP = nh_.subscribe( "/bldc_driver_node/Left_Motor_KP", 2, &BldcController::KP_Callback); // 2 is the queue size + sub_KI = nh_.subscribe( "/bldc_driver_node/Left_Motor_KI", 2, &BldcController::KI_Callback); // 2 is the queue size + sub_KD = nh_.subscribe( "/bldc_driver_node/Left_Motor_KD", 2, &BldcController::KD_Callback); // 2 is the queue size -BldcController::~BldcController() -{ -} + timeout_timer_ = nh_.createTimer(ros::Duration(2), &BldcController::timeoutCallback, this); + timeout_timer_.start(); + } -void BldcController::motorCommand(const chip_bldc_driver::Command &command) -{ - timeout_timer_.stop(); - timeout_timer_.start(); - serial->sendMotorCommand(command.motor_command); -} + BldcController::~BldcController() + { } -void BldcController::timeoutCallback(const ros::TimerEvent &) -{ - // In case the motorCommand function is not called the timeout will be called after 2 seconds to stop the motor. - serial->sendMotorCommand(0); -} + void BldcController::KP_Callback(const std_msgs::UInt16::ConstPtr& msg){ + ROS_INFO("Motor KP value is : [%d]", msg->data); + motor_kp = msg->data; + serial->Send_KP(motor_kp); // send kp to motor + } + void BldcController::KI_Callback(const std_msgs::UInt16::ConstPtr& msg){ + ROS_INFO("Motor KI value is : [%d]", msg->data); + motor_ki = msg->data; + serial->Send_KI(motor_ki); // send ki to motor + } + void BldcController::KD_Callback(const std_msgs::UInt16::ConstPtr& msg){ + ROS_INFO("Motor KD value is : [%d]", msg->data); + motor_kd = msg->data; + serial->Send_KD(motor_kd); // send kd to motor + } + + + void BldcController::motorCommand(const chip_bldc_driver::Command &command) { + timeout_timer_.stop(); + timeout_timer_.start(); + serial->sendMotorCommand(command.motor_command); + } + + void BldcController::timeoutCallback(const ros::TimerEvent &) + { + // In case the motorCommand function is not called the timeout will be called after 2 seconds to stop the motor. + serial->sendMotorCommand(0); + } + + /** + * Constructs for right + */ + BldcController_right::BldcController_right(bldc_serial::BldcSerial *s) : serial(s), nh_("~") + { + sub_cmd_ = nh_.subscribe("/bldc_driver_node/Command_right", 1, &BldcController_right::motorCommand, this); + sub_KP = nh_.subscribe( "/bldc_driver_node/Right_Motor_KP", 2, &BldcController_right::KP_Callback); // 2 is the queue size + sub_KI = nh_.subscribe( "/bldc_driver_node/Right_Motor_KI", 2, &BldcController_right::KI_Callback); // 2 is the queue size + sub_KD = nh_.subscribe( "/bldc_driver_node/Right_Motor_KD", 2, &BldcController_right::KD_Callback); // 2 is the queue size + + timeout_timer_ = nh_.createTimer(ros::Duration(2), &BldcController_right::timeoutCallback, this); + timeout_timer_.start(); + } + + BldcController_right::~BldcController_right() + { } + + void BldcController_right::KP_Callback(const std_msgs::UInt16::ConstPtr& msg){ + ROS_INFO("Motor KP value is : [%d]", msg->data); + motor_kp = msg->data; + serial->Send_KP(motor_kp); // send kp to motor + } + void BldcController_right::KI_Callback(const std_msgs::UInt16::ConstPtr& msg){ + ROS_INFO("Motor KI value is : [%d]", msg->data); + motor_ki = msg->data; + serial->Send_KI(motor_ki); // send ki to motor + } + void BldcController_right::KD_Callback(const std_msgs::UInt16::ConstPtr& msg){ + ROS_INFO("Motor KD value is : [%d]", msg->data); + motor_kd = msg->data; + serial->Send_KD(motor_kd); // send kd to motor + } + + void BldcController_right::motorCommand(const chip_bldc_driver::Command &command) + { + timeout_timer_.stop(); + timeout_timer_.start(); + serial->sendMotorCommand(command.motor_command); + } + + void BldcController_right::timeoutCallback(const ros::TimerEvent &) + { + // In case the motorCommand function is not called the timeout will be called after 2 seconds to stop the motor. + serial->sendMotorCommand(0); + } } // namespace bldc_controller \ No newline at end of file diff --git a/src/bldc_driver_node_right.cpp b/src/bldc_driver_node_right.cpp new file mode 100644 index 0000000..7deb57f --- /dev/null +++ b/src/bldc_driver_node_right.cpp @@ -0,0 +1,61 @@ +/** +Software License Agreement BSD +\file bldc_driver_node.cpp +\copyright Copyright (c) 2017, Chip Robotics, All rights reserved. +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: +* Redistributions of source code must retain the above copyright notice, this list of conditions and the +following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the +following disclaimer in the documentation and/or other materials provided with the distribution. +* Neither the name of NAME nor the names of its contributors may be used to endorse or promote +products derived from this software without specific prior written permission. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "chip_bldc_driver/bldc_controller.h" +#include "chip_bldc_driver/bldc_serial.h" +#include "ros/ros.h" + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "bldc_driver_node"); + ros::NodeHandle nh("~"); + + std::string port = "/dev/ttyUSB0"; + int32_t baud = 115200; + nh.param("port", port, port); + + bldc_serial::BldcSerial serial(port.c_str(), baud); + + bldc_controller::BldcController_right controller(&serial); + + while (ros::ok()) + { + ROS_INFO("Attempting connection to %s at %i baud.", port.c_str(), baud); + + if (serial.connect()) + { + ROS_INFO("Connection Succesful"); + ros::AsyncSpinner spinner(1); + spinner.start(); + while (ros::ok()) + { + serial.read(); + } + spinner.stop(); + } + else + { + ROS_WARN("Problem connecting to serial device, again after 1 second"); + sleep(1); + } + } + +} diff --git a/src/bldc_serial.cpp b/src/bldc_serial.cpp index b2618a5..b63bb92 100644 --- a/src/bldc_serial.cpp +++ b/src/bldc_serial.cpp @@ -30,11 +30,7 @@ namespace bldc_serial /** * Constructs */ -BldcSerial::BldcSerial(const char *port, int baud) : port_(port), - baud_(baud), - serial_(nullptr), - nh_("~") -{ +BldcSerial::BldcSerial(const char *port, int baud) : port_(port), baud_(baud), serial_(nullptr), nh_("~"){ pub_status_ = nh_.advertise("status", 1); pub_feedback_ = nh_.advertise("feedback", 1); } @@ -137,6 +133,60 @@ void BldcSerial::sendMotorCommand(int16_t motor_speed_cmd) write(message, msg_len); } +/** + * Transmit motor PID Parameters command through serial + * + * @param motor_PID_Values Motor PID values. Should be within the [0, 65535] range, otherwise it's ignored +*/ +void BldcSerial::Send_KP( uint16_t motor_kp_value){ + + constexpr int32_t MAX_CMD = 65535; + constexpr int32_t MIN_CMD = 0; + if (motor_kp_value < MIN_CMD || motor_kp_value > MAX_CMD) + { + ROS_WARN("The motor KP command %i exceed the range [%i, %i].", motor_kp_value, MIN_CMD, MAX_CMD); + return; + } + + constexpr int32_t MSG_SIZE = 16; + char message[MSG_SIZE]; + int32_t msg_len = sprintf(message, "$!MKP:%d\r\n", motor_kp_value); + write(message, msg_len); + ROS_INFO("Motor KP value updated "); +} +void BldcSerial::Send_KI( uint16_t motor_ki_value){ + constexpr int32_t MAX_CMD = 65535; + constexpr int32_t MIN_CMD = 0; + constexpr int32_t MSG_SIZE = 16; + if (motor_ki_value < MIN_CMD || motor_ki_value > MAX_CMD) + { + ROS_WARN("The motor KI command %i exceed the range [%i, %i].", motor_ki_value, MIN_CMD, MAX_CMD); + return; + } + + char message[MSG_SIZE]; + int32_t msg_len = sprintf(message, "$!MKI:%d\r\n", motor_ki_value); + write(message, msg_len); + ROS_INFO("Motor KI value updated "); +} +void BldcSerial::Send_KD( uint16_t motor_kd_value){ + + constexpr int32_t MAX_CMD = 65535; + constexpr int32_t MIN_CMD = 0; + constexpr int32_t MSG_SIZE = 16; + if (motor_kd_value < MIN_CMD || motor_kd_value > MAX_CMD) + { + ROS_WARN("The motor KD command %i exceed the range [%i, %i].", motor_kd_value, MIN_CMD, MAX_CMD); + return; + } + + char message[MSG_SIZE]; + int32_t msg_len = sprintf(message, "$!MKD:%d\r\n", motor_kd_value); + write(message, msg_len); + ROS_INFO("Motor KD value updated "); +} + + /** * Processes the status message and prints it * @param data_ptr Pointer to an array of bytes. It should be a complete message data