Merge pull request #1 from tinymovr/develCAN

Devel can
tinymovr · Sep 19, 2023 · 251d880 · 251d880
2 parents 0c61876 + d938705
commit 251d880
Show file tree

Hide file tree

Showing 42 changed files with 669 additions and 342 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -148,7 +148,7 @@ set(TINYMOVR_SOURCES
 #   src/${PROJECT_NAME}/tinymovr_ros.cpp
 # )
 
-add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp.cpp src/tinymovr_can.cpp src/tm_joint_iface.cpp)
+add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp/socketcan_cpp.cpp src/tm_joint_iface.cpp)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries

diff --git a/config/hardware.yaml b/config/hardware.yaml
@@ -4,6 +4,7 @@ tinymovr_joint_iface:
       # hardware ID of the actuator
       id: 2
       offset: 3.14159265359
+      delay_us: 1000
       # offset to be added, in radians, to the position of an actuator
       #max-speed: 4.0 # in rad
       command_interface: velocity
diff --git a/config/velocity_controller.yaml b/config/velocity_controller.yaml
@@ -0,0 +1,4 @@
+arm_controller:
+  type: "velocity_controllers/JointVelocityController"
+  joint: arm_1
+  pid: {p: 1.0, i: 0.0, d: 0.0}
diff --git a/include/socketcan_cpp/socketcan_cpp.hpp b/include/socketcan_cpp/socketcan_cpp.hpp
@@ -60,4 +60,4 @@ namespace scpp
         std::string m_interface;
         SocketMode m_socket_mode;
     };
-}
+}
diff --git a/include/tinymovr/can.hpp b/include/tinymovr/can.hpp
@@ -14,8 +14,8 @@ class Can_ : Node
 {
     public:
 
-        Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint32_t get_rate(void);
         void set_rate(uint32_t value);
         uint32_t get_id(void);

diff --git a/include/tinymovr/comms.hpp b/include/tinymovr/comms.hpp
@@ -15,9 +15,9 @@ class Comms_ : Node
 {
     public:
 
-        Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         Can_ can;
 
 };
diff --git a/include/tinymovr/controller.hpp b/include/tinymovr/controller.hpp
@@ -18,12 +18,12 @@ class Controller_ : Node
 {
     public:
 
-        Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint8_t get_state(void);
         void set_state(uint8_t value);
         uint8_t get_mode(void);

diff --git a/include/tinymovr/current.hpp b/include/tinymovr/current.hpp
@@ -14,8 +14,8 @@ class Current_ : Node
 {
     public:
 
-        Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_Iq_setpoint(void);
         void set_Iq_setpoint(float value);
         float get_Id_setpoint(void);

diff --git a/include/tinymovr/encoder.hpp b/include/tinymovr/encoder.hpp
@@ -14,8 +14,8 @@ class Encoder_ : Node
 {
     public:
 
-        Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_position_estimate(void);
         float get_velocity_estimate(void);
         uint8_t get_type(void);

diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp
@@ -21,8 +21,12 @@
 #include "Arduino.h"
 #endif
 
-#define EP_BITS (6)
-#define RECV_DELAY_US (160.0f)
+#define CAN_EP_SIZE (12)
+#define CAN_EP_MASK ((1 << CAN_EP_SIZE) - 1)
+#define CAN_SEQ_SIZE (9)
+#define CAN_SEQ_MASK (((1 << CAN_SEQ_SIZE) - 1) << CAN_EP_SIZE)
+#define CAN_DEV_SIZE (8)
+#define CAN_DEV_MASK (((1 << CAN_DEV_SIZE) - 1) << (CAN_EP_SIZE + CAN_SEQ_SIZE))
 
 typedef void (*send_callback)(uint32_t arbitration_id, uint8_t *data, uint8_t dlc, bool rtr);
 typedef bool (*recv_callback)(uint32_t *arbitration_id, uint8_t *data, uint8_t *dlc);
@@ -31,26 +35,28 @@ typedef void (*delay_us_callback)(uint32_t us);
 class Node {
     public:
 
-    Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-        can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb) {}
+    Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+        can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb), delay_us_value(_delay_us_value) {}
 
     protected:
     uint8_t can_node_id;
     send_callback send_cb;
     recv_callback recv_cb;
     delay_us_callback delay_us_cb;
+    uint32_t delay_us_value;
     uint8_t _data[8];
     uint8_t _dlc;
-    uint8_t get_arbitration_id(uint8_t cmd_id) {
-        return this->can_node_id << EP_BITS | cmd_id;
+    uint32_t get_arbitration_id(uint32_t cmd_id)
+    {
+        return ((this->can_node_id << (CAN_EP_SIZE + CAN_SEQ_SIZE)) & CAN_DEV_MASK) | (cmd_id & CAN_EP_MASK);
     }
-    void send(uint8_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr)
+    void send(uint32_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr)
     {
-        const uint8_t arb_id = this->get_arbitration_id(cmd_id);
+        const uint32_t arb_id = this->get_arbitration_id(cmd_id);
         this->send_cb(arb_id, data, data_size, rtr);
     }
 
-    bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us)
+    bool recv(uint32_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us)
     {
         uint32_t _arbitration_id;
         uint8_t _data[8];
@@ -62,7 +68,7 @@ class Node {
         {
            this->delay_us_cb(delay_us);
         }
-        const uint8_t arb_id = this->get_arbitration_id(cmd_id);
+        const uint32_t arb_id = this->get_arbitration_id(cmd_id);
         while (this->recv_cb(&_arbitration_id, _data, &_data_size))
         {
             if (_arbitration_id == arb_id)
@@ -218,8 +224,6 @@ inline size_t read_le<uint64_t>(uint64_t* value, const uint8_t* buffer) {
 
 template<>
 inline size_t read_le<float>(float* value, const uint8_t* buffer) {
-    //static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
-    //static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
     return read_le(reinterpret_cast<uint32_t*>(value), buffer);
 }
 

diff --git a/include/tinymovr/homing.hpp b/include/tinymovr/homing.hpp
@@ -0,0 +1,31 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#pragma once
+
+#include <helpers.hpp>
+#include <stall_detect.hpp>
+
+class Homing_ : Node
+{
+    public:
+
+        Homing_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , stall_detect(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
+        float get_velocity(void);
+        void set_velocity(float value);
+        float get_max_homing_t(void);
+        void set_max_homing_t(float value);
+        float get_retract_dist(void);
+        void set_retract_dist(float value);
+        uint8_t get_warnings(void);
+        Stall_detect_ stall_detect;
+        void home();
+
+};
diff --git a/include/tinymovr/motor.hpp b/include/tinymovr/motor.hpp
@@ -14,8 +14,8 @@ class Motor_ : Node
 {
     public:
 
-        Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_R(void);
         void set_R(float value);
         float get_L(void);

diff --git a/include/tinymovr/position.hpp b/include/tinymovr/position.hpp
@@ -14,8 +14,8 @@ class Position_ : Node
 {
     public:
 
-        Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_setpoint(void);
         void set_setpoint(float value);
         float get_p_gain(void);

diff --git a/include/tinymovr/scheduler.hpp b/include/tinymovr/scheduler.hpp
@@ -14,10 +14,8 @@ class Scheduler_ : Node
 {
     public:
 
-        Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
-        uint32_t get_total(void);
-        uint32_t get_busy(void);
+        Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint8_t get_errors(void);
 
 };
diff --git a/include/tinymovr/stall_detect.hpp b/include/tinymovr/stall_detect.hpp
@@ -0,0 +1,26 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#pragma once
+
+#include <helpers.hpp>
+
+class Stall_detect_ : Node
+{
+    public:
+
+        Stall_detect_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
+        float get_velocity(void);
+        void set_velocity(float value);
+        float get_delta_pos(void);
+        void set_delta_pos(float value);
+        float get_t(void);
+        void set_t(float value);
+
+};
diff --git a/include/tinymovr/tinymovr.hpp b/include/tinymovr/tinymovr.hpp
@@ -15,15 +15,21 @@
 #include <motor.hpp>
 #include <encoder.hpp>
 #include <traj_planner.hpp>
+#include <homing.hpp>
 #include <watchdog.hpp>
 
-static uint32_t avlos_proto_hash = 3273002564;
+static uint32_t avlos_proto_hash = 4118115615;
 
 enum errors_flags
 {
     ERRORS_NONE = 0,
     ERRORS_UNDERVOLTAGE = (1 << 0), 
-    ERRORS_DRIVER_FAULT = (1 << 1)
+    ERRORS_DRIVER_FAULT = (1 << 1), 
+    ERRORS_CHARGE_PUMP_FAULT_STAT = (1 << 2), 
+    ERRORS_CHARGE_PUMP_FAULT = (1 << 3), 
+    ERRORS_DRV10_DISABLE = (1 << 4), 
+    ERRORS_DRV32_DISABLE = (1 << 5), 
+    ERRORS_DRV54_DISABLE = (1 << 6)
 };
 
 enum scheduler_errors_flags
@@ -68,21 +74,42 @@ enum traj_planner_errors_flags
     TRAJ_PLANNER_ERRORS_VCRUISE_OVER_LIMIT = (1 << 1)
 };
 
+enum homing_warnings_flags
+{
+    HOMING_WARNINGS_NONE = 0,
+    HOMING_WARNINGS_HOMING_TIMEOUT = (1 << 0)
+};
+
+enum motor_type_options
+{
+    MOTOR_TYPE_HIGH_CURRENT = 0, 
+    MOTOR_TYPE_GIMBAL = 1
+};
+
+enum encoder_type_options
+{
+    ENCODER_TYPE_INTERNAL = 0, 
+    ENCODER_TYPE_HALL = 1
+};
+
 class Tinymovr : Node
 {
     public:
 
-        Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , homing(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint32_t get_protocol_hash(void);
         uint32_t get_uid(void);
+        void get_fw_version(char out_value[]);
+        uint32_t get_hw_revision(void);
         float get_Vbus(void);
         float get_Ibus(void);
         float get_power(void);
@@ -92,12 +119,14 @@ class Tinymovr : Node
         void save_config();
         void erase_config();
         void reset();
+        void enter_dfu();
         Scheduler_ scheduler;
         Controller_ controller;
         Comms_ comms;
         Motor_ motor;
         Encoder_ encoder;
         Traj_planner_ traj_planner;
+        Homing_ homing;
         Watchdog_ watchdog;
 
 };
diff --git a/include/tinymovr/traj_planner.hpp b/include/tinymovr/traj_planner.hpp
@@ -14,14 +14,20 @@ class Traj_planner_ : Node
 {
     public:
 
-        Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_max_accel(void);
         void set_max_accel(float value);
         float get_max_decel(void);
         void set_max_decel(float value);
         float get_max_vel(void);
         void set_max_vel(float value);
+        float get_t_accel(void);
+        void set_t_accel(float value);
+        float get_t_decel(void);
+        void set_t_decel(float value);
+        float get_t_total(void);
+        void set_t_total(float value);
         void move_to(float pos_setpoint);
         void move_to_tlimit(float pos_setpoint);
         uint8_t get_errors(void);

diff --git a/include/tinymovr/velocity.hpp b/include/tinymovr/velocity.hpp
@@ -14,8 +14,8 @@ class Velocity_ : Node
 {
     public:
 
-        Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_setpoint(void);
         void set_setpoint(float value);
         float get_limit(void);

diff --git a/include/tinymovr/voltage.hpp b/include/tinymovr/voltage.hpp
@@ -14,8 +14,8 @@ class Voltage_ : Node
 {
     public:
 
-        Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_Vq_setpoint(void);
 
 };
-Original file line number
+Diff line change
@@ Expand Up / @@ -60,4 +60,4 @@ namespace scpp @@
             std::string m_interface;
             SocketMode m_socket_mode;
         };
-    }
+    }