Adding class structure

src/robot/BUILD.bazel

@@ -19,7 +19,10 @@ py_library(
 
 cc_binary(
     name = "robot",
-    srcs = ["robot.cpp"],
+    srcs = [
+        "robot.cpp",
+        "robot.h",
+    ],
     deps = [
         "@eigen",
     ],

src/robot/dynamixel_motor.cpp

@@ -0,0 +1,50 @@
+#include "dynamixel_motor.h"
+
+DynamixelMotor::DynamixelMotor(int id, Eigen::Vector2d position, bool inverted)
+    : id_(id), position_(position), inverted_(inverted) {
+  DynamixelMemoryLayout offsets = {
+      24, // torque_enable
+      25, // led_enable
+      26, // d_gain
+      27, // i_gain
+      28, // p_gain
+      30, // goal_position
+      32, // moving_speed
+      34, // torque_limit
+      36, // present_position
+      38, // present_speed
+      40, // present_load
+      42, // present_input_voltage
+      43, // present_temperature
+      44, // registered
+      46, // moving
+      47, // lock
+      48, // punch
+      50, // realtime_tick
+      73, // goal_acceleration
+  };
+
+  DynamixelMemoryLayout sizes = {
+      1, // torque_enable
+      1, // led_enable
+      1, // d_gain
+      1, // i_gain
+      1, // p_gain
+      2, // goal_position
+      2, // moving_speed
+      2, // torque_limit
+      2, // present_position
+      2, // present_speed
+      2, // present_load
+      1, // present_input_voltage
+      1, // present_temperature
+      1, // registered
+      1, // moving
+      1, // lock
+      2, // punch
+      2, // realtime_tick
+      1, // goal_acceleration
+  };
+
+  config_ = {offsets, sizes};
+};

src/robot/dynamixel_motor.h

@@ -0,0 +1,81 @@
+#include "motor.h"
+#include <Eigen/Eigen>
+#include <string>
+
+struct DynamixelMemoryLayout {
+  int torque_enable;
+  int led_enable;
+  int d_gain;
+  int i_gain;
+  int p_gain;
+  int goal_position;
+  int moving_speed;
+  int torque_limit;
+  int present_position;
+  int present_speed;
+  int present_load;
+  int present_input_voltage;
+  int present_temperature;
+  int registered;
+  int moving;
+  int lock;
+  int punch;
+  int realtime_tick;
+  int goal_acceleration;
+};
+
+struct DynamixelConfig {
+  DynamixelMemoryLayout offsets;
+  DynamixelMemoryLayout sizes;
+};
+
+class DynamixelMotor : IMotor {
+private:
+  int id_;
+  Eigen::Vector2d position_;
+  bool inverted_;
+
+  DynamixelConfig config_;
+  bool initialized_;
+
+  bool ValidateCommand_();
+
+  bool WriteBytes_(int offset, int size, int value);
+
+  int ReadBytes_(int offset, int size);
+
+  int GetMemoryProperty_(std::string property);
+
+  void SetMemoryProperty_(std::string property, int value);
+
+public:
+  DynamixelMotor(int id, Eigen::Vector2d position, bool inverted = false);
+
+  bool Initialize();
+
+  bool Configure();
+
+  int GetSpeed();
+
+  void SetSpeed(int speed);
+
+  int GetPosition();
+
+  void SetPosition(int position);
+
+  int GetAcceleration();
+
+  void SetAcceleration(int acceleration);
+
+  PIDGain GetPIDGain();
+
+  void SetPIDGain(PIDGain pid_gain);
+
+  bool GetLED();
+
+  void SetLED(bool enabled);
+
+  bool GetEnabled();
+
+  void SetEnabled(bool enabled);
+};

src/robot/motor.h

@@ -0,0 +1,20 @@
+#include "pid.h"
+
+class IMotor {
+public:
+  IMotor(){};
+  virtual bool Initialize() = 0;
+  virtual bool Configure() = 0;
+  virtual int GetSpeed() = 0;
+  virtual void SetSpeed(int speed) = 0;
+  virtual int GetPosition() = 0;
+  virtual void SetPosition(int position) = 0;
+  virtual int GetAcceleration() = 0;
+  virtual void SetAcceleration(int acceleration) = 0;
+  virtual PIDGain GetPIDGain() = 0;
+  virtual void SetPIDGain(PIDGain pid_gain) = 0;
+  virtual bool GetLED() = 0;
+  virtual void SetLED(bool enabled) = 0;
+  virtual bool GetEnabled() = 0;
+  virtual void SetEnabled(bool enabled) = 0;
+};

src/robot/pid.h

@@ -0,0 +1,5 @@
+struct PIDGain {
+  int kP;
+  int kI;
+  int kD;
+};

src/robot/robot.cpp

@@ -1,14 +1,13 @@
-#include <iostream>
 #include <Eigen/Dense>
+#include <iostream>
 
 using Eigen::MatrixXd;
 
-int main()
-{
-    MatrixXd m(2, 2);
-    m(0, 0) = 3;
-    m(1, 0) = 2.5;
-    m(0, 1) = -1;
-    m(1, 1) = m(1, 0) + m(0, 1);
-    std::cout << m << std::endl;
+int main() {
+  MatrixXd m(2, 2);
+  m(0, 0) = 3;
+  m(1, 0) = 2.5;
+  m(0, 1) = -1;
+  m(1, 1) = m(1, 0) + m(0, 1);
+  std::cout << m << std::endl;
 }

src/robot/robot.h

@@ -0,0 +1,4 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+class