[WIP/Tracking] Roboclaw Appendage Complete Integration

.gitignore

@@ -88,6 +88,7 @@ Thumbs.db
 *.pyc
 
 build/
+build_armhf/
 doc/
 .idea/
 cmake-build-debug/

CMakeLists.txt

@@ -14,7 +14,13 @@ endif()
 # to correctly compile the codebase despite this line being in the
 # downstream CMake files.
 set (CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always")
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-psabi")
+
+option(GCC_DIAG_COLORS "Enable GCC Diagnostic Colors" ON)
+if (GCC_DIAG_COLORS)
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always")
+endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules)
 

appendages/include/appendages/exceptions.hpp

@@ -17,8 +17,8 @@
  *  \$$    $$   | $$   | $$  \$$\      | $$  | $$|   $$ \| $$
  *   \$$$$$$     \$$    \$$   \$$       \$$   \$$ \$$$$$$ \$$
  */
-#ifndef EXCEPTIONS_HPP
-#define EXCEPTIONS_HPP
+#ifndef APPENDAGES_EXCEPTIONS_HPP
+#define APPENDAGES_EXCEPTIONS_HPP
 #include <misc/exception_base.hpp>
 
 namespace rip
@@ -28,7 +28,9 @@ namespace rip
         NEW_EX(CommandNotFound)
         NEW_EX(AppendageWithoutType)
         NEW_EX(AppendageWithoutLabel)
-        NEW_EX(AppendageWithId)
+        NEW_EX(AppendageWithoutId)
+        NEW_EX(AppendageMissingField)
+        NEW_EX(AppendageNotImplemented)
     }
 }
-#endif // EXCEPTIONS_HPP
+#endif // APPENDAGES_EXCEPTIONS_HPP

appendages/include/appendages/roboclaw.hpp

@@ -0,0 +1,68 @@
+#ifndef ROBOCLAW_HPP
+#define ROBOCLAW_HPP
+
+#include <tuple>
+#include <misc/logger.hpp>
+#include <fmt/format.h>
+#include <cmd_messenger/command.hpp>
+
+#include "appendages/appendage.hpp"
+#include "appendages/appendage_factory.hpp"
+
+#include <json.hpp>
+
+namespace rip
+{
+	namespace appendages
+	{
+		class Roboclaw : public Appendage
+		{
+		public:
+			void SetSpeed(int32_t speed1, int32_t speed2);
+			void SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2);
+			std::tuple<uint32_t, uint32_t> ReadEncoders();
+			void SetDuty(int16_t duty1, int16_t duty2);
+			void SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps);
+
+			/**
+			* Stop
+			*/
+			virtual void stop() override;
+
+			virtual bool diagnostic() override;
+
+		protected:
+			friend class AppendageFactory;
+
+			/**
+			* Function wrapper for the constructor so it can be pointed to
+			*
+			* @param config The config from arduino gen
+			* @param command_map A map of the name of the commands to their enumerations
+			* @param device The connection to the device
+			*/
+			static std::shared_ptr<Appendage> create(const nlohmann::json& config, const std::map<std::string, int>& command_map, std::shared_ptr<cmdmessenger::Device> device);
+
+		private:
+			/**
+			* Constructor
+			*
+			* @param config The config from arduino gen
+			* @param command_map A map of the name of the commands to their enumerations
+			* @param device The connection to the device
+			*/
+			Roboclaw(const nlohmann::json& config, const std::map<std::string, int>& command_map, std::shared_ptr<cmdmessenger::Device> device);
+
+			uint8_t m_address;
+
+			std::shared_ptr<cmdmessenger::Command> m_set_speed;
+			std::shared_ptr<cmdmessenger::Command> m_set_speed_accel;
+			std::shared_ptr<cmdmessenger::Command> m_read_encoders;
+			std::shared_ptr<cmdmessenger::Command> m_read_encoders_result;
+			std::shared_ptr<cmdmessenger::Command> m_set_duty;
+			std::shared_ptr<cmdmessenger::Command> m_set_velocity_pid;
+		};
+	}
+}
+
+#endif // ROBOCLAW_HPP

appendages/include/appendages/servo.hpp

@@ -25,6 +25,8 @@ namespace rip
              */
             void write(int value);
 
+            void attach(bool state);
+
             /**
              * Stop
              */
@@ -55,6 +57,7 @@ namespace rip
             Servo(const nlohmann::json& config, const std::map<std::string, int>& command_map, std::shared_ptr<cmdmessenger::Device> device);
 
             std::shared_ptr<cmdmessenger::Command> m_write;
+            std::shared_ptr<cmdmessenger::Command> m_attach;
         };
     }
 }

appendages/json/roboclaw.json

@@ -0,0 +1,8 @@
+{
+    "serial":  "string",
+    "baudrate": "int",
+    "address": "int",
+    "core": [
+        "address"
+    ]
+}

appendages/json/servo.json

@@ -0,0 +1,4 @@
+{
+    "pin": "int",
+    "initialValue": "int"
+}

appendages/json/ultrasonic.json

@@ -0,0 +1,5 @@
+{
+    "triggerPin": "int",
+    "echoPin": "int",
+    "minSleepMs": "int"
+}

appendages/src/appendage.cpp

@@ -48,15 +48,23 @@ namespace rip
 
             if (config.find("label") == config.end())
             {
-                throw AppendageWithoutLabel(fmt::format("appendage missing label"));
+                throw AppendageWithoutLabel(fmt::format("appendage of type {} is missing label", m_type));
             }
             m_label = config["label"];
 
+            if (config.find("id") == config.end() && config.find("index") == config.end())
+            {
+                throw AppendageWithoutId(fmt::format("appendage labeled {} is missing an id or index", m_label));
+            }
+
             if (config.find("id") == config.end())
             {
-                throw AppendageWithId(fmt::format("appendage missing id"));
+              m_id = config["index"];
+            }
+            else
+            {
+              m_id = config["id"];
             }
-            m_id = config["id"];
         }
     }
 }

appendages/src/appendage_factory.cpp

@@ -5,6 +5,9 @@
 // Appendages
 #include "appendages/digital_input.hpp"
 #include "appendages/analog_input.hpp"
+#include "appendages/roboclaw.hpp"
+#include "appendages/servo.hpp"
+#include "appendages/ultrasonic.hpp"
 
 #include "appendages/exceptions.hpp"
 
@@ -29,8 +32,14 @@ namespace rip
             {
                 throw AppendageWithoutType(fmt::format("appendage missing type"));
             }
-
-            return m_constructors["type"](config, command_map, device);
+            std::string appendage_type = config["type"];
+            if (m_constructors.find(appendage_type) == m_constructors.end())
+            {
+                // we probably don't have this appendage type available to the factory
+                throw AppendageNotImplemented(fmt::format("Factory: no Constructor for appendage type {} !", appendage_type));
+            }
+            misc::Logger::getInstance()->debug(fmt::format("Factory: Constructing an appendage of type {} ...", appendage_type));
+            return m_constructors[appendage_type](config, command_map, device);
         }
 
         void AppendageFactory::registerAppendage(const std::string& type, std::function<std::shared_ptr<Appendage>(const nlohmann::json&,
@@ -45,6 +54,9 @@ namespace rip
         {
             registerAppendage("Digital Input", &DigitalInput::create);
             registerAppendage("Analog Input", &AnalogInput::create);
+            registerAppendage("Roboclaw", &Roboclaw::create);
+            registerAppendage("Servo", &Servo::create);
+            registerAppendage("Ultrasonic", &Ultrasonic::create);
         }
     }
 }

appendages/src/roboclaw.cpp

@@ -0,0 +1,183 @@
+#include "appendages/roboclaw.hpp"
+#include <ctime>
+#include <utility>
+#include <tuple>
+#include <memory>
+#include <chrono>
+#include <thread>
+#include <cmd_messenger/cmd_messenger.hpp>
+
+#include "appendages/exceptions.hpp"
+
+namespace rip
+{
+	namespace appendages
+	{
+		Roboclaw::Roboclaw(const nlohmann::json& config, const std::map<std::string, int>& command_map, std::shared_ptr<cmdmessenger::Device> device)
+			: Appendage(config, device),
+			m_set_speed(
+				createCommand(
+					"kSetSpeed",
+					command_map,
+					cmdmessenger::ArduinoCmdMessenger::makeArgumentString<
+					  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+					  cmdmessenger::ArduinoCmdMessenger::CharType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedLongType
+						>()
+					)
+				),
+			m_set_speed_accel(
+				createCommand(
+					"kSetSpeedAccel",
+					command_map,
+					cmdmessenger::ArduinoCmdMessenger::makeArgumentString<
+					  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+					  cmdmessenger::ArduinoCmdMessenger::CharType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedLongType
+					  >()
+					)
+				),
+			m_read_encoders(
+				createCommand(
+					"kReadEncoders",
+					command_map,
+					cmdmessenger::ArduinoCmdMessenger::makeArgumentString<
+					  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+					  cmdmessenger::ArduinoCmdMessenger::CharType
+						>()
+					)
+				),
+			m_read_encoders_result(createCommand(
+				"kReadEncodersResult",
+				command_map,
+				cmdmessenger::ArduinoCmdMessenger::makeArgumentString<
+					cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+					cmdmessenger::ArduinoCmdMessenger::UnsignedLongType
+					>()
+				)
+			),
+			m_set_duty(
+				createCommand(
+					"kSetDuty",
+					command_map,
+					cmdmessenger::ArduinoCmdMessenger::makeArgumentString<
+					  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+					  cmdmessenger::ArduinoCmdMessenger::CharType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>()
+					)
+				),
+			m_set_velocity_pid(
+				createCommand(
+					"kSetVelocityPID",
+					command_map,
+					cmdmessenger::ArduinoCmdMessenger::makeArgumentString<
+					  cmdmessenger::ArduinoCmdMessenger::CharType,
+						cmdmessenger::ArduinoCmdMessenger::CharType,
+						cmdmessenger::ArduinoCmdMessenger::FloatType,
+						cmdmessenger::ArduinoCmdMessenger::FloatType,
+						cmdmessenger::ArduinoCmdMessenger::FloatType,
+						cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>()
+					)
+				)
+		{
+			if (config.find("address") == config.end())
+			{
+				throw AppendageMissingField(fmt::format("Roboclaw: missing config field 'address'"));
+			}
+			m_address = config["address"];
+		}
+
+		std::shared_ptr<Appendage> Roboclaw::create(const nlohmann::json& config, const std::map<std::string, int>& command_map, std::shared_ptr<cmdmessenger::Device> device)
+		{
+			return std::dynamic_pointer_cast<Appendage>(std::shared_ptr<Roboclaw>(new Roboclaw(config, command_map, device)));
+		}
+
+		void Roboclaw::SetSpeed(int32_t speed1, int32_t speed2)
+		{
+			cmdmessenger::ArduinoCmdMessenger messenger;
+			messenger.send<
+			  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+			  cmdmessenger::ArduinoCmdMessenger::CharType,
+				cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+				cmdmessenger::ArduinoCmdMessenger::UnsignedLongType
+				>(m_device, m_set_speed, m_id, m_address, speed1, speed2);
+		}
+
+		void Roboclaw::SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2)
+		{
+			cmdmessenger::ArduinoCmdMessenger messenger;
+			messenger.send<
+			  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+			  cmdmessenger::ArduinoCmdMessenger::CharType,
+				cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+				cmdmessenger::ArduinoCmdMessenger::UnsignedLongType,
+				cmdmessenger::ArduinoCmdMessenger::UnsignedLongType
+				>(m_device, m_set_speed_accel, m_id, m_address, accel, speed1, speed2);
+		}
+
+		std::tuple<uint32_t, uint32_t> Roboclaw::ReadEncoders()
+		{
+			cmdmessenger::ArduinoCmdMessenger messenger;
+			messenger.send<
+			  cmdmessenger::ArduinoCmdMessenger::IntegerType,
+			  cmdmessenger::ArduinoCmdMessenger::CharType
+			>(
+				m_device,
+				m_read_encoders,
+				m_id,
+				m_address
+			);
+			return messenger.receive<cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_read_encoders_result);
+		}
+
+		void Roboclaw::SetDuty(int16_t duty1, int16_t duty2)
+		{
+			cmdmessenger::ArduinoCmdMessenger messenger;
+			messenger.send<cmdmessenger::ArduinoCmdMessenger::CharType, cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType, cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>(m_device, m_set_duty, m_id, m_address, duty1, duty2);
+		}
+
+		void Roboclaw::SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps)
+		{
+			cmdmessenger::ArduinoCmdMessenger messenger;
+			messenger.send<cmdmessenger::ArduinoCmdMessenger::CharType, cmdmessenger::ArduinoCmdMessenger::CharType, cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_velocity_pid, m_address, motor, Kp, Ki, Kd, qpps);
+		}
+
+		void Roboclaw::stop() {
+			misc::Logger::getInstance()->debug("Setting roboclaw duty to zero.");
+			SetDuty(0, 0);
+		}
+
+		bool Roboclaw::diagnostic()
+		{
+			std::chrono::time_point<std::chrono::system_clock> start_time = std::chrono::system_clock::now();
+			misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start");
+
+			misc::Logger::getInstance()->debug("Read encoders for 5s in a loop");
+			while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start_time).count() < 5000)
+			{
+				misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders())));
+			}
+			misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds");
+			misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders())));
+			SetDuty(16000, 16000);
+			start_time = std::chrono::system_clock::now();
+			while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start_time).count() < 5000)
+			{}
+			misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders())));
+			stop();
+			misc::Logger::getInstance()->debug("Setting speed accel drive (5s)");
+			misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders())));
+			SetSpeedAccel(12000, 12000, 12000);
+			start_time = std::chrono::system_clock::now();
+			while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start_time).count() < 5000)
+			{}
+			misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders())));
+			stop();
+			misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders())));
+		}
+	}
+}