From ce8d9a6e66e111cd1663b7f37a7f5658d5542ba0 Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@westonrobot.com>
Date: Tue, 13 Jul 2021 18:58:54 +0800
Subject: [PATCH] scout mini works

---
 demo/scout_demo/scout_robot_demo.cpp          | 23 +++++++++----------
 .../protocol_v1/protocol_v1_parser.hpp        |  2 +-
 .../ugv_sdk/details/robot_base/scout_base.hpp |  3 +++
 include/ugv_sdk/mobile_robot/scout_robot.hpp  |  3 ++-
 src/mobile_robot/scout_robot.cpp              | 18 +++++++++++----
 5 files changed, 30 insertions(+), 19 deletions(-)

diff --git a/demo/scout_demo/scout_robot_demo.cpp b/demo/scout_demo/scout_robot_demo.cpp
index ac7ea2c..3f96891 100644
--- a/demo/scout_demo/scout_robot_demo.cpp
+++ b/demo/scout_demo/scout_robot_demo.cpp
@@ -34,11 +34,11 @@ int main(int argc, char **argv) {
 
   std::unique_ptr<ScoutRobot> scout;
   if (protocol_version == "v1") {
-    scout =
-        std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolVersion::AGX_V1));
+    scout = std::unique_ptr<ScoutRobot>(
+        new ScoutRobot(ProtocolVersion::AGX_V1, true));
   } else if (protocol_version == "v2") {
-    scout =
-        std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolVersion::AGX_V2));
+    scout = std::unique_ptr<ScoutRobot>(
+        new ScoutRobot(ProtocolVersion::AGX_V2, true));
   } else {
     std::cout << "Error: invalid protocol version string" << std::endl;
     return -1;
@@ -66,19 +66,18 @@ int main(int argc, char **argv) {
   std::cout << "Light: breath" << std::endl;
   scout->SetLightCommand(BREATH, 0, BREATH, 0);
   sleep(3);
-  std::cout << "Light: custom 90-80" << std::endl;
-  scout->SetLightCommand(CUSTOM, 90, CUSTOM, 80);
+  std::cout << "Light: custom 30-80" << std::endl;
+  scout->SetLightCommand(CUSTOM, 30, CUSTOM, 80);
   sleep(3);
-  std::cout << "Light: diabled cmd control" << std::endl;
-  scout->DisableLightControl();
+  //   std::cout << "Light: diabled cmd control" << std::endl;
+  //   scout->DisableLightControl();
+  scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
 
   int count = 0;
   while (true) {
     // motion control
-    // if (count < 100) {
-    std::cout << "Motor: 0.2, 0" << std::endl;
-    scout->SetMotionCommand(0.2, 0.0);
-    // }
+    std::cout << "Motor: 1.0, 0" << std::endl;
+    scout->SetMotionCommand(1.0, 0.0);
 
     // get robot state
     auto state = scout->GetRobotState();
diff --git a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp
index d8a1eb9..4cc395c 100644
--- a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp
+++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp
@@ -61,7 +61,7 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
 };
 
 using ScoutProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
-using ScoutMiniProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
+using ScoutMiniProtocolV1Parser = ProtocolV1Parser<ScoutMiniLimits>;
 
 using HunterProtocolV1Parser = ProtocolV1Parser<HunterV1Limits>;
 
diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp
index 2b20baf..51f2890 100644
--- a/include/ugv_sdk/details/robot_base/scout_base.hpp
+++ b/include/ugv_sdk/details/robot_base/scout_base.hpp
@@ -125,7 +125,10 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
 
 namespace westonrobot {
 using ScoutBaseV1 = ScoutBase<ScoutProtocolV1Parser>;
+using ScoutMiniBaseV1 = ScoutBase<ScoutMiniProtocolV1Parser>;
+
 using ScoutBaseV2 = ScoutBase<ProtocolV2Parser>;
+using ScoutMiniBaseV2 = ScoutBase<ProtocolV2Parser>;
 }  // namespace westonrobot
 
 #endif /* SCOUT_BASE_HPP */
diff --git a/include/ugv_sdk/mobile_robot/scout_robot.hpp b/include/ugv_sdk/mobile_robot/scout_robot.hpp
index f6b2bf4..1e8b0a2 100644
--- a/include/ugv_sdk/mobile_robot/scout_robot.hpp
+++ b/include/ugv_sdk/mobile_robot/scout_robot.hpp
@@ -18,7 +18,8 @@
 namespace westonrobot {
 class ScoutRobot : public RobotInterface, public ScoutInterface {
  public:
-  ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2);
+  ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2,
+             bool is_mini_model = false);
   ~ScoutRobot();
 
   void Connect(std::string can_name) override;
diff --git a/src/mobile_robot/scout_robot.cpp b/src/mobile_robot/scout_robot.cpp
index 7f3e0d7..5a05950 100644
--- a/src/mobile_robot/scout_robot.cpp
+++ b/src/mobile_robot/scout_robot.cpp
@@ -11,11 +11,19 @@
 #include "ugv_sdk/details/robot_base/scout_base.hpp"
 
 namespace westonrobot {
-ScoutRobot::ScoutRobot(ProtocolVersion protocol) {
-  if (protocol == ProtocolVersion::AGX_V1) {
-    robot_ = new ScoutBaseV1();
-  } else if (protocol == ProtocolVersion::AGX_V2) {
-    robot_ = new ScoutBaseV2();
+ScoutRobot::ScoutRobot(ProtocolVersion protocol, bool is_mini_model) {
+  if (!is_mini_model) {
+    if (protocol == ProtocolVersion::AGX_V1) {
+      robot_ = new ScoutBaseV1();
+    } else if (protocol == ProtocolVersion::AGX_V2) {
+      robot_ = new ScoutBaseV2();
+    }
+  } else {
+    if (protocol == ProtocolVersion::AGX_V1) {
+      robot_ = new ScoutMiniBaseV1();
+    } else if (protocol == ProtocolVersion::AGX_V2) {
+      robot_ = new ScoutMiniBaseV2();
+    }
   }
 }