Damiao Motor Interface.

CMakeLists.txt

@@ -396,6 +396,17 @@ target_include_directories(ut_steer_chassis PRIVATE dev/application/unit_tests/u
 target_compile_definitions(ut_steer_chassis PRIVATE ut_steer_chassis)
 target_link_libraries(ut_steer_chassis ${AHRS_LIB})
 
+add_executable(ut_damiao_motor
+        dev/application/unit_tests/ut_damiao_motor/main.cpp
+        dev/interface/damiao_motor/damiao_motor_interface.cpp
+        dev/interface/damiao_motor/damiao_motor_controller.cpp
+        dev/interface/damiao_motor/damiao_motor_feedback.cpp
+        dev/interface/can/can_interface.cpp)
+target_include_directories(ut_damiao_motor PRIVATE dev/application/unit_tests/ut_damiao_motor
+        dev/interface/damiao_motor
+        dev/interface/can)
+target_compile_definitions(ut_damiao_motor PRIVATE ut_damiao_motor)
+
 # --------------------------------------- Param Adjusts ---------------------------------------
 
 

dev/application/unit_tests/ut_damiao_motor/damiao_motor_config.h

@@ -0,0 +1,59 @@
+//
+// Created by Wu Feiyang on 7/13/23.
+//
+
+#ifndef META_DAMIAO_MOTOR_CFG_H
+#define META_DAMIAO_MOTOR_CFG_H
+
+/**
+ * Damiao 4310 motor.
+ */
+#include "hal.h"
+#define can_channel_1 &CAND1
+#define can_channel_2 &CAND2
+
+typedef enum motor_mode{
+    MIT_MODE,
+    POS_VEL_MODE,
+    VEL_MODE,
+}motor_mode_t;
+
+class DamiaoMotorBase{
+public:
+    CANDriver* can_driver;
+    int        masterID;
+    int        slaveID;
+    float      mitKp;
+    float      mitKd;
+    float      V_max;   // maximum rotation speed. Unit is Rad/s.
+    float      P_max;   // maximum Position. Unit is Rad.
+    float      T_max;   // maximum Torque. Unit is N*m.
+    float      initial_encoder_angle;
+    motor_mode_t mode;
+    float      kp_min;
+    float      kp_max;
+    float      kd_min;
+    float      kd_max;
+};
+
+class DamiaoMotorCFG{
+public:
+    enum MotorName{
+        YAW,
+        PITCH,
+        MOTOR_COUNT,
+    }motor_usage_t;
+    /***
+     * @brief Motor Configuration
+     * The unit of all velocity is Rad/s, unit of P_MAX must be exactly identical to the value with that of the Damiao Offical tool,
+     * whose unit is by defualt Rad. All unit of torque is N * m. The unit of @param initial_encoder_angle is degree.
+     */
+    static constexpr DamiaoMotorBase motorCfg[MOTOR_COUNT] = {
+            {can_channel_1,0x00,0x01,1.0,0.3,30,3.141953,10.0,
+             55.26,POS_VEL_MODE,0.0,500.0,0.0,5.0},
+            {can_channel_2,0x00,0x01,0.0,0.0,30,3.141593,10.0,
+             0.0,VEL_MODE,0.0,500.0,0.0,5.0}
+    };
+};
+
+#endif //META_DAMIAO_MOTOR_CFG_H

dev/application/unit_tests/ut_damiao_motor/hardware_conf.h

@@ -0,0 +1,33 @@
+//
+// Created by 钱晨 on 3/16/22.
+// This file contains some feature enabling masks.
+//
+
+#ifndef META_INFANTRY_HARDWARE_CONF_H
+#define META_INFANTRY_HARDWARE_CONF_H
+
+#if !defined(ENABLE_VISION) || defined(__DOXYGEN__)
+#define ENABLE_VISION                   FALSE
+#endif
+
+#if !defined(ENABLE_REFEREE) || defined(__DOXYGEN__)
+#define ENABLE_REFEREE                  TRUE
+#endif
+
+#if !defined(ENABLE_AHRS) || defined(__DOXYGEN__)
+#define ENABLE_AHRS                     TRUE
+#endif
+
+#if !defined(ENABLE_SUBPITCH) || defined(__DOXYGEN__)
+#define ENABLE_SUBPITCH                 FALSE
+#endif
+
+#if !defined(ENABLE_CAPACITOR) || defined(__DOXYGEN__)
+#define ENABLE_CAPACITOR                TRUE
+#endif
+
+#if !defined(ENABLE_USB_SHELL) || defined(__DOXYGEN__)
+#define ENABLE_USB_SHELL                TRUE
+#endif
+
+#endif //META_INFANTRY_HARDWARE_CONF_H

dev/application/unit_tests/ut_damiao_motor/main.cpp

@@ -0,0 +1,58 @@
+//
+// Created by Wu Feiyang on 2023/7/23.
+//
+
+#include "hal.h"
+#include "ch.hpp"
+#include "can_interface.h"
+#include "shell.h"
+#include "damiao_motor_feedback.h"
+#include "damiao_motor_controller.h"
+
+CANInterface can1(&CAND1);
+CANInterface can2(&CAND2);
+
+static CANConfig can_cfg = {
+        CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
+        CAN_BTR_SJW(0) | CAN_BTR_TS2(3) |
+        CAN_BTR_TS1(8) | CAN_BTR_BRP(2)
+};
+
+
+static CANTxFrame can_tx_frame[2];
+
+static uint8_t ctr_id;
+static uint8_t err;
+static uint16_t pos;
+static uint16_t vel;
+static uint16_t torque;
+static uint8_t mos_avg_tempr;
+static uint8_t rotor_avg_tempr;
+
+int main(){
+    halInit();
+    chibios_rt::System::init();
+    can1.start(HIGHPRIO);
+    can2.start(HIGHPRIO-1);
+    Shell::start(NORMALPRIO+2);
+//    print_thread.start(NORMALPRIO+4);
+    DamiaoMotorController::start(NORMALPRIO+1,NORMALPRIO+2,&can1,&can2);
+    DamiaoMotorController::motor_enable(DamiaoMotorCFG::YAW);
+//    DamiaoMotorController::set_target_vel(DamiaoMotorCFG::YAW,5);.
+//    DamiaoMotorController::set_target_angle(DamiaoMotorCFG::YAW,50);
+//    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::YAW,20,2);
+    DamiaoMotorController::shell_display(DamiaoMotorCFG::YAW,true);
+//    DamiaoMotorController::set_target_MIT(DamiaoMotorCFG::YAW,2.5,0.9,0.9);
+    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::YAW,0,2);
+    chThdSleepSeconds(5);
+    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::YAW,90,2);
+    chThdSleepSeconds(5);
+    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::YAW,-90,2);
+    chThdSleepSeconds(5);
+    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::YAW,0,2);
+    chThdSleepSeconds(5);
+    DamiaoMotorController::motor_disable(DamiaoMotorCFG::YAW);
+
+
+
+}

dev/interface/damiao_motor/README.md

@@ -0,0 +1,109 @@
+# USE Damiao Motor controller
+
+## Initiate
+
+### Set up `damiao_motor_config`
+
+1. Editing CMake target. Ensure `damiao_motor_config.h` and `damiao_motor_config.cpp` is in your `target_include_directories`
+2. Editing the `damiao_motor_config.h` and `damiao_motor_config.cpp`
+Please create a c++ class called `DamiaoMotorCFG` in the `.h` file.
+a class template is provided below:
+   ```c++
+   class DamiaoMotorBase{
+   public:
+   CANDriver* can_driver;
+   int        masterID;
+   int        slaveID;
+   float      mitKp;
+   float      mitKd;
+   float      V_max;   // maximum rotation speed. Unit is Rad/s.
+   float      P_max;   // maximum Position. Unit is Rad.
+   float      T_max;   // maximum Torque. Unit is N*m.
+   float      initial_encoder_angle;
+   motor_mode_t mode;
+   float      kp_min;
+   float      kp_max;
+   float      kd_min;
+   float      kd_max;
+   };
+   static constexpr DamiaoMotorBase motorCfg[MOTOR_COUNT] = {
+            {can_channel_1,0x00,0x01,1.0,0.3,30,3.141593,10.0,
+             0.0,MIT_MODE,0.0,500.0,0.0,5.0},
+            {can_channel_2,0x00,0x01,0.0,0.0,30,3.141593,10.0,
+             0.0,VEL_MODE,0.0,500.0,0.0,5.0}
+    };
+   ```
+   The `DamiaoMotorCFG` is a structure that describe CAN motors' hardware information, include
+   their SID(Identifier field-ID), CAN channes, etc.
+   
+   Remember that the parameters, like `kp_min`,`kp_max`,`masterID`,etc. are set by the damiao offical
+   tool and can't be edited by the embedded program, but the program needs to use these parameters to calculate
+   the correct value of the velocity, position etc. because the data on the CAN Bus are adjusted based on
+   these parameters. It is important to check the parameters on the official tool before filling
+   `damiao_motor_cfg.h`.
+   
+   
+### Set up CAN-BUS
+Outside `main()` function, create two `CANInterface` classes with parameters `&CAND1` and `&CAND2`.
+   ```c++
+   CANInterface can1(&CAND1);
+   CANInterface can2(&CAND2)
+   ```
+   In `main()` function, initiate two `CANInterface` by calling
+   ```c++
+   can1.start(<CAN1_RX_PRIO>);
+   can2.start(<CAN2_RX_PRIO>);
+   ```
+   where `<CAN1_RX_PRIO>` and `<CAN1_RX_PRIO>` should be replaced with your own thread priorities
+### Setup interface and motor controller
+   Initiate `DamiaoMotorController` by calling
+   ```c++
+   DamiaoMotorController::start(<MOTOR_SKD_PRIO>, <FEEDBACK_SKD_PRIO>, &can1, &can2);
+   ```
+   where `<MOTOR_SKD_PRIO>` and `<FEEDBACK_SKD_PRIO>` should be replaced with your own thread priorities.
+   With this command, `DamiaoMotorIF` will be automatically set up. Then the Damiao motor must be initialized
+   by sending the fixed startup command, we can do this by calling:
+   ```c++
+   DamiaoMotorController::motor_enable(<MOTOR_NAME>);
+   ```
+   To stop the motor, just use `motor_disable` function.
+   ### Damiao Motor Contorl
+   There are three modes to control the damiao motor, by translation, is MIT mode, position velocity mode and velocity mode.
+   The CAN ID for the MIT is mode is the same as the slave ID of the motor, while the CAN ID of position velocity mode is motor
+   plus 0x100, and velocity  mode is plusing 0x200. Please refer the Chinese version of Damiao motor document for more details.
+
+   If you want to use the MIT mode, just call the function:
+   ```c++
+   void DamiaoMotorController::set_target_MIT(DamiaoMotorCFG::MotorName name,float pos,float vel,float torque)
+   ```
+   And for velocity mode:
+   ```c++
+   void DamiaoMotorController::set_target_VEL(DamiaoMotorCFG::MotorName name, float vel)
+   ```
+   And it is the same for position velocity mode.
+
+
+
+## Included functions
+See the inline DOXYGEN document in files for detailed description.
+
+**PLEASE READ INLINE DOXYGEN DOCUMENT OF CAN MOTOR INTERFACE!**
+It will help you understand the communication DJI motors' communication protocol and how CAN-BUS works.
+
+### Basic functions for `CANMotorController` and `CANMotorIF`
+
+`DamiaoMotorIF` is the interface for Robomaster motors that use *Control Area Network* (CAN) to communicate with our board.
+You can get feedback of motors from the interface, by accessing `DamiaoMotorIF::motor_feedback`.
+
+`CANMotorController` is aimed at controlling the motors. You could set angle, angular velocity or torque current through 
+this controller. 
+
+
+
+
+## Further optimization of motor controller
+Please finish the requirements below in the future updates.
+
+Current program use dual-loop PID controllers to achieve angle and angular velocity control. In the future, we planned to
+modularize controllers and add a configuration file to describe the controller network. A Simulink like UI could be created.
+