chiprobotics · Jatin1o1 · Jan 6, 2022
diff --git 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
@@ -7,6 +7,9 @@
 #include <vector>
 #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<std::string> 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<std::string>& 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
@@ -0,0 +1,18 @@
+<launch>
+
+  <node pkg="chip_bldc_driver" type="bldc_driver_node_left" name="motor_left" output="screen">
+    <param name="~port" value="/dev/motorLeft"/>
+    <param name="~baud" value="115200" />
+  </node>
+
+
+  <node pkg="chip_bldc_driver" type="bldc_driver_node_right" name="motor_right" output="screen">
+    <param name="~port" value="/dev/motorRight"/>
+    <param name="~baud" value="115200" />
+  </node>
+
+
+  <!-- <node pkg="differential_drive" type="twist_to_motors.py" name="d" output="screen"></node> -->
+
+
+</launch>
diff --git 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
diff --git a/src/bldc_driver_node_right.cpp 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<std::string>("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);
+    }
+  }
+
+}