damiao motor on dart is on CAN2 and is working now. Enable damiao mot…

…or method modified.

CMakeLists.txt

@@ -280,8 +280,13 @@ add_executable(DART
         dev/application/vehicles/dart/can_motor_config.cpp
         dev/application/vehicles/dart/main_dart.cpp
         dev/application/vehicles/dart/user_dart.cpp
+
+        dev/interface/damiao_motor/damiao_motor_feedback.cpp
+        dev/interface/damiao_motor/damiao_motor_interface.cpp
+        dev/interface/damiao_motor/damiao_motor_controller.cpp
         )
-target_include_directories(DART PRIVATE dev/application/vehicles/dart)
+target_include_directories(DART PRIVATE dev/application/vehicles/dart
+        dev/interface/damiao_motor)
 target_compile_definitions(DART PRIVATE DART)
 
 # ======================================= Unit Tests Programs =======================================

dev/application/vehicles/dart/damiao_motor_config.h

@@ -0,0 +1,56 @@
+//
+// 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{
+        DART_LOADER,
+        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_2,0x00,0x01,1.0,0.3,30,3.141953,10.0,
+             -124.74,POS_VEL_MODE,0.0,500.0,0.0,5.0},
+    };
+};
+
+#endif //META_DAMIAO_MOTOR_CFG_H

dev/application/vehicles/dart/main_dart.cpp

@@ -10,6 +10,7 @@
 #include "user_dart.h"
 #include "led.h"
 #include "referee_interface.h"
+#include "damiao_motor_controller.h"
 
 CANInterface can1(&CAND1);
 CANInterface can2(&CAND2);
@@ -34,16 +35,21 @@ int main(){
     Remote::start();
 
     LED::led_on(1); // led 1 turned on now
-    can1.start(NORMALPRIO+3);
-    can2.start(NORMALPRIO+4);
+    can1.start(HIGHPRIO-1);
+    can2.start(HIGHPRIO-2);
     LED::led_on(2);//  led 2 turned on now
 
     /// Setup Referee
     Referee::init();
 
     UserDart::start(NORMALPRIO-1);
-    CANMotorController::start(HIGHPRIO-1,HIGHPRIO-2,&can1,&can2);
-    chThdSleepSeconds(5);
+    CANMotorController::start(HIGHPRIO-3,HIGHPRIO-4,&can1,&can2);
+    DamiaoMotorController::start(NORMALPRIO+3,NORMALPRIO+2,&can1,&can2);
+    DamiaoMotorController::motor_enable(DamiaoMotorCFG::DART_LOADER);
+    DamiaoMotorController::shell_display(DamiaoMotorCFG::DART_LOADER,true);
+    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::DART_LOADER,0,2);
+//
+//    DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::DART_LOADER,0,2);
 
 #if CH_CFG_NO_IDLE_THREAD // see chconf.h for what this #define means
     // ChibiOS idle thread has been disabled, main() should implement infinite loop

dev/application/vehicles/dart/user_dart.cpp

@@ -2,6 +2,7 @@
 // Created by Wu Feiyang on 2023/7/11.
 //
 #include "user_dart.h"
+#include "damiao_motor_controller.h"
 UserDart::UserThread UserDart::userThread;
 bool UserDart::UserThread::start_flag;
 bool UserDart::timer_started;
@@ -43,7 +44,7 @@ void UserDart::UserThread::main(){
 
 
     while(!shouldTerminate()){
-
+//        DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::DART_LOADER,90,2);
         if(Remote::rc.s1 == Remote::S_UP){                  // Forced Relax Mode
             chSysLock();
             for(uint i=0;i<CANMotorCFG::MOTOR_COUNT;i++){
@@ -105,12 +106,16 @@ void UserDart::UserThread::main(){
                         CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,0);
                         CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,0);
 
-                    }else if(current_time -start_time >= 4000 && current_time - start_time < 6000){
+                    }else if(current_time - start_time >= 4000 && current_time -start_time < 8000){
+                        DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::DART_LOADER,90,3);
+                        chThdSleepSeconds(1);
+                        DamiaoMotorController::set_target_POSVEL(DamiaoMotorCFG::DART_LOADER,0,3);
+                    }else if(current_time -start_time >= 8000 && current_time - start_time < 10000){
                         rudder1.set_rudder_angle(0);
                         chThdSleepMilliseconds(900);
                         rudder1.set_rudder_angle(180);
                         chThdSleepMilliseconds(900);
-                    }else if(current_time > 8000){
+                    }else if(current_time > 10000){
                         start_flag = false;
                         timer_started = false;
                     }

dev/interface/damiao_motor/damiao_motor_controller.h

@@ -6,7 +6,7 @@
  * @file can_motor_scheduler.h
  * @brief Class to drive can motors.
  *
- * @addtogroup CAN Driver
+ * @addtogroup Damiao Driver
  * @{
  */
 

dev/interface/damiao_motor/damiao_motor_feedback.h

@@ -15,7 +15,7 @@
 
 //#include "damiao_motor_interface.h"
 #include "common_macro.h"
-#include "application/unit_tests/ut_damiao_motor/damiao_motor_config.h"
+#include "damiao_motor_config.h"
 
 /**
  * @author  Wu Feiyang

dev/interface/damiao_motor/damiao_motor_interface.cpp

@@ -40,18 +40,11 @@ void DamiaoMotorIF::init(CANInterface *can1_, CANInterface *can2_) {
 void DamiaoMotorIF::start(DamiaoMotorCFG::MotorName motorProfile) {
     DamiaoMotorIF::set_mode(motorProfile,DamiaoMotorCFG::motorCfg[motorProfile].mode);
     chThdSleepMilliseconds(1500);
-    for(int i=10;i > 0;i--){
-        chSysLock();
-        motors_can_tx_frame[motorProfile].DLC = 0x08;
-        motors_can_tx_frame[motorProfile].data64[0] = start_cmd;
-        chSysUnlock();
-        if(DamiaoMotorCFG::motorCfg[motorProfile].can_driver == &CAND1){
-            can1->send_msg(&motors_can_tx_frame[motorProfile]);
-        }else if(DamiaoMotorCFG::motorCfg[motorProfile].can_driver == &CAND2){
-            can2->send_msg(&motors_can_tx_frame[motorProfile]);
-        }
-        chThdSleepMilliseconds(10);
-    }
+    chSysLock();
+    motors_can_tx_frame[motorProfile].DLC = 0x08;
+    motors_can_tx_frame[motorProfile].data64[0] = start_cmd;
+    chSysUnlock();
+    chThdSleepMilliseconds(500);
     chSysLock();
     if(motors_mode[motorProfile] != VEL_MODE){
         motors_can_tx_frame[motorProfile].DLC = 0x08;