kobuki_driver : Added new protocol about custom PID gain setting. Issue

#249.

kobuki_driver/include/kobuki_driver/command.hpp

@@ -43,7 +43,7 @@ class kobuki_PUBLIC Command : public packet_handler::payloadBase
   enum Name
   {
     BaseControl = 1, Sound = 3, SoundSequence = 4, RequestExtra = 9, ChangeFrame = 10, RequestEeprom = 11,
-    SetDigitalOut = 12
+    SetDigitalOut = 12, SetController = 13, GetController = 14
   };
 
   enum VersionFlag
@@ -65,8 +65,9 @@ class kobuki_PUBLIC Command : public packet_handler::payloadBase
    */
   struct Data
   {
-    Data() :
-        command(BaseControl), speed(0), radius(0), request_flags(0), gp_out(0x00f0) // set all the power pins high, others low.
+    Data()
+      : command(BaseControl), speed(0), radius(0), request_flags(0), gp_out(0x00f0) // set all the power pins high, others low.
+      , type(0), p_gain(1000), i_gain(1000), d_gain(1000)
     {
     }
 
@@ -96,6 +97,15 @@ class kobuki_PUBLIC Command : public packet_handler::payloadBase
     // 0x00f0 - external power breakers (3.3V, 5V, 12V 12V1A) (0x0010, 0x0020, 0x0040, 0x0080)
     // 0x0f00 - led array (red1, green1, red2, green2) ( 0x0100, 0x0200, 0x0400, 0x0800)
     uint16_t gp_out;
+
+    // SetControllerGain
+    unsigned char type;
+    unsigned int p_gain;
+    unsigned int i_gain;
+    unsigned int d_gain;
+
+    // SetControllerGain
+    unsigned char reserved;
   };
 
   virtual ~Command() {}
@@ -107,6 +117,11 @@ class kobuki_PUBLIC Command : public packet_handler::payloadBase
   static Command GetVersionInfo();
   static Command SetVelocityControl(DiffDrive& diff_drive);
   static Command SetVelocityControl(const int16_t &speed, const int16_t &radius);
+  static Command SetControllerGain(const unsigned char &type,
+                                   const unsigned int &p_gain,
+                                   const unsigned int &d_gain,
+                                   const unsigned int &i_gain);
+  static Command GetControllerGain();
 
   Data data;
 

kobuki_driver/include/kobuki_driver/kobuki.hpp

@@ -129,6 +129,7 @@ class kobuki_PUBLIC Kobuki
   Inertia::Data getInertiaData() const { return inertia.data; }
   GpInput::Data getGpInputData() const { return gp_input.data; }
   ThreeAxisGyro::Data getRawInertiaData() const { return three_axis_gyro.data; }
+  ControllerInfo::Data getControllerInfoData() const { return controller_info.data; }
 
   /*********************
   ** Feedback
@@ -151,6 +152,9 @@ class kobuki_PUBLIC Kobuki
   void setDigitalOutput(const DigitalOutput &digital_output);
   void setExternalPower(const DigitalOutput &digital_output);
   void playSoundSequence(const enum SoundSequences &number);
+  void setControllerGain(const unsigned char &type, const unsigned int &p_gain,
+                         const unsigned int &i_gain, const unsigned int &d_gain);
+  void getControllerGain();
 
   /*********************
   ** Debugging
@@ -199,13 +203,15 @@ class kobuki_PUBLIC Kobuki
   Firmware firmware; // requestable
   UniqueDeviceID unique_device_id; // requestable
   ThreeAxisGyro three_axis_gyro;
+  ControllerInfo controller_info; // requestable
 
   ecl::Serial serial;
   PacketFinder packet_finder;
   PacketFinder::BufferType data_buffer;
   bool is_alive; // used as a flag set by the data stream watchdog
 
   int version_info_reminder;
+  int controller_info_reminder;
 
   /*********************
   ** Commands
@@ -228,7 +234,7 @@ class kobuki_PUBLIC Kobuki
   /*********************
   ** Signals
   **********************/
-  ecl::Signal<> sig_stream_data;
+  ecl::Signal<> sig_stream_data, sig_controller_info;
   ecl::Signal<const VersionInfo&> sig_version_info;
   ecl::Signal<const std::string&> sig_debug, sig_info, sig_warn, sig_error;
   ecl::Signal<Command::Buffer&> sig_raw_data_command; // should be const, but pushnpop is not fully realised yet for const args in the formatters.

kobuki_driver/include/kobuki_driver/packet_handler/payload_headers.hpp

@@ -35,7 +35,7 @@ class Header {
   // Service Payloads
   Hardware = 10, Firmware = 11, ThreeAxisGyro = 13, Eeprom = 15, GpInput = 16,
 
-  UniqueDeviceID = 19, Reserved = 20
+  UniqueDeviceID = 19, Reserved = 20, ControllerInfo = 21
   };
 };
 

kobuki_driver/include/kobuki_driver/packets.hpp

@@ -27,6 +27,7 @@
 #include "packets/hardware.hpp"
 #include "packets/unique_device_id.hpp"
 #include "packets/three_axis_gyro.hpp"
+#include "packets/controller_info.hpp"
 
 
 #endif /* KOBUKI_PACKETS_HPP_ */

kobuki_driver/include/kobuki_driver/packets/controller_info.hpp

@@ -0,0 +1,92 @@
+/**
+ * @file kobuki_driver/packets/controller_info.hpp
+ *
+ * @brief Docking infrared sensor packet payloads.
+ *
+ * License: BSD
+ *   https://raw.github.com/yujinrobot/kobuki/master/kobuki_driver/LICENSE
+ */
+/*****************************************************************************
+** Preprocessor
+*****************************************************************************/
+
+#ifndef KOBUKI_CONTROLLER_INFO_HPP__
+#define KOBUKI_CONTROLLER_INFO_HPP__
+
+/*****************************************************************************
+** Include
+*****************************************************************************/
+
+#include "../packet_handler/payload_base.hpp"
+#include "../packet_handler/payload_headers.hpp"
+
+/*****************************************************************************
+** Namespace
+*****************************************************************************/
+
+namespace kobuki
+{
+
+/*****************************************************************************
+** Interface
+*****************************************************************************/
+
+class ControllerInfo : public packet_handler::payloadBase
+{
+public:
+  ControllerInfo() : packet_handler::payloadBase(false, 13) {};
+  struct Data {
+    Data() : type(0), p_gain(100*1000), i_gain(100), d_gain(2*1000) {}
+    unsigned char type;
+    unsigned int p_gain; //default value: 100 * 1000
+    unsigned int i_gain; //default value: 0.1 * 1000
+    unsigned int d_gain; //default value:   2 * 1000
+  } data;
+
+  bool serialise(ecl::PushAndPop<unsigned char> & byteStream)
+  {
+    buildBytes(Header::ControllerInfo, byteStream);
+    buildBytes(length, byteStream);
+    buildBytes(data.type, byteStream);
+    buildBytes(data.p_gain, byteStream);
+    buildBytes(data.i_gain, byteStream);
+    buildBytes(data.d_gain, byteStream);
+    return true;
+  }
+
+  bool deserialise(ecl::PushAndPop<unsigned char> & byteStream)
+  {
+    if (!(byteStream.size() > length+2))
+    {
+      //std::cout << "kobuki_node: kobuki_controller_info: deserialise failed. not enough byte stream." << std::endl;
+      return false;
+    }
+
+    unsigned char header_id, length_packed;
+    buildVariable(header_id, byteStream);
+    buildVariable(length_packed, byteStream);
+    if( header_id != Header::ControllerInfo ) return false;
+    if( length_packed != length ) return false;
+
+    buildVariable(data.type, byteStream);
+    buildVariable(data.p_gain, byteStream);
+    buildVariable(data.i_gain, byteStream);
+    buildVariable(data.d_gain, byteStream);
+
+    //showMe();
+    return constrain();
+  }
+
+  bool constrain()
+  {
+    return true;
+  }
+
+  void showMe()
+  {
+  }
+};
+
+} // namespace kobuki
+
+#endif /* KOBUKI_IR_DATA_HPP__ */

kobuki_driver/src/driver/command.cpp

@@ -163,6 +163,7 @@ Command Command::SetVelocityControl(DiffDrive& diff_drive)
   outgoing.data.command = Command::BaseControl;
   return outgoing;
 }
+
 Command Command::SetVelocityControl(const int16_t &speed, const int16_t &radius)
 {
   Command outgoing;
@@ -172,6 +173,26 @@ Command Command::SetVelocityControl(const int16_t &speed, const int16_t &radius)
   return outgoing;
 }
 
+Command Command::SetControllerGain(const unsigned char &type, const unsigned int &p_gain,
+                                   const unsigned int &i_gain, const unsigned int &d_gain)
+{
+  Command outgoing;
+  outgoing.data.type = type;
+  outgoing.data.p_gain = p_gain;
+  outgoing.data.i_gain = i_gain;
+  outgoing.data.d_gain = d_gain;
+  outgoing.data.command = Command::SetController;
+  return outgoing;
+}
+
+Command Command::GetControllerGain()
+{
+  Command outgoing;
+  outgoing.data.command = Command::GetController;
+  outgoing.data.reserved = 0;
+  return outgoing;
+}
+
 /*****************************************************************************
 ** Implementation [Serialisation]
 *****************************************************************************/
@@ -232,6 +253,19 @@ bool Command::serialise(ecl::PushAndPop<unsigned char> & byteStream)
       buildBytes(data.gp_out, byteStream);
       break;
     }
+    case SetController:
+      buildBytes(cmd, byteStream);
+      buildBytes(length=13, byteStream);
+      buildBytes(data.type, byteStream);
+      buildBytes(data.p_gain, byteStream);
+      buildBytes(data.i_gain, byteStream);
+      buildBytes(data.d_gain, byteStream);
+      break;
+    case GetController:
+      buildBytes(cmd, byteStream);
+      buildBytes(length=1, byteStream);
+      buildBytes(data.reserved, byteStream);
+      break;
     default:
       return false;
       break;

kobuki_driver/src/driver/kobuki.cpp

@@ -53,6 +53,7 @@ Kobuki::Kobuki() :
     , is_connected(false)
     , is_alive(false)
     , version_info_reminder(0)
+    , controller_info_reminder(0)
     , heading_offset(0.0/0.0)
 {
 }
@@ -81,6 +82,7 @@ void Kobuki::init(Parameters &parameters) throw (ecl::StandardException)
 
   // connect signals
   sig_version_info.connect(sigslots_namespace + std::string("/version_info"));
+  sig_controller_info.connect(sigslots_namespace + std::string("/controller_info"));
   sig_stream_data.connect(sigslots_namespace + std::string("/stream_data"));
   sig_raw_data_command.connect(sigslots_namespace + std::string("/raw_data_command"));
   sig_raw_data_stream.connect(sigslots_namespace + std::string("/raw_data_stream"));
@@ -122,6 +124,14 @@ void Kobuki::init(Parameters &parameters) throw (ecl::StandardException)
    *******************************************/
   version_info_reminder = 10;
   sendCommand(Command::GetVersionInfo());
+
+  /******************************************
+   ** Get Controller Info Commands
+   *******************************************/
+  controller_info_reminder = 10;
+  sendCommand(Command::GetControllerGain());
+  //sig_controller_info.emit(); //emit default gain
+
   thread.start(&Kobuki::spin, *this);
 }
 
@@ -184,6 +194,7 @@ void Kobuki::spin()
         serial.block(4000); // blocks by default, but just to be clear!
         event_manager.update(is_connected, is_alive);
         version_info_reminder = 10;
+        controller_info_reminder = 10;
       }
       catch (const ecl::StandardException &e)
       {
@@ -213,6 +224,7 @@ void Kobuki::spin()
       {
         is_alive = false;
         version_info_reminder = 10;
+        controller_info_reminder = 10;
         sig_debug.emit("Timed out while waiting for incoming bytes.");
       }
       event_manager.update(is_connected, is_alive);
@@ -242,10 +254,10 @@ void Kobuki::spin()
       lockDataAccess();
       while (data_buffer.size() > 1/*size of etx*/)
       {
-//        std::cout << "header_id: " << (unsigned int)data_buffer[0] << " | ";
-//        std::cout << "remains: " << data_buffer.size() << " | ";
-//        std::cout << "local_buffer: " << local_buffer.size() << " | ";
-//        std::cout << std::endl;
+        std::cout << "header_id: " << (unsigned int)data_buffer[0] << " | ";
+        std::cout << "remains: " << data_buffer.size() << " | ";
+        std::cout << "local_buffer: " << local_buffer.size() << " | ";
+        std::cout << std::endl;
         switch (data_buffer[0])
         {
           // these come with the streamed feedback
@@ -329,6 +341,11 @@ void Kobuki::spin()
                                      + ". Firmware: " + VersionInfo::toString(firmware.data.version));
             version_info_reminder = 0;
             break;
+          case Header::ControllerInfo:
+            controller_info.deserialise(data_buffer);
+            sig_controller_info.emit();
+            controller_info_reminder = 0;
+            break;
           default:
             if (data_buffer.size() < 3 ) { /* minimum is 3, header_id, length, etx */
               sig_error.emit("malformed subpayload detected.");
@@ -364,6 +381,7 @@ void Kobuki::spin()
             break;
         }
       }
+      std::cout << "---" << std::endl;
       unlockDataAccess();
 
       is_alive = true;
@@ -372,6 +390,7 @@ void Kobuki::spin()
       sig_stream_data.emit();
       sendBaseControlCommand(); // send the command packet to mainboard;
       if( version_info_reminder/*--*/ > 0 ) sendCommand(Command::GetVersionInfo());
+      if( controller_info_reminder/*--*/ > 0 ) sendCommand(Command::GetControllerGain());
     }
     else
     {
@@ -466,6 +485,17 @@ void Kobuki::playSoundSequence(const enum SoundSequences &number)
   sendCommand(Command::PlaySoundSequence(number, kobuki_command.data));
 }
 
+void Kobuki::setControllerGain(const unsigned char &type, const unsigned int &p_gain,
+                               const unsigned int &i_gain, const unsigned int &d_gain)
+{
+  sendCommand(Command::SetControllerGain(type, p_gain, i_gain, d_gain));
+}
+
+void Kobuki::getControllerGain()
+{
+  sendCommand(Command::GetControllerGain());
+}
+
 void Kobuki::setBaseControl(const double &linear_velocity, const double &angular_velocity)
 {
   diff_drive.setVelocityCommands(linear_velocity, angular_velocity);