add motor_task.c

.vscode/c_cpp_properties.json

@@ -0,0 +1,19 @@
+{
+    "configurations": [
+        {
+            "name": "Win32",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [
+                "_DEBUG",
+                "UNICODE",
+                "_UNICODE"
+            ],
+            "cStandard": "c17",
+            "intelliSenseMode": "windows-msvc-x64",
+            "configurationProvider": "ms-vscode.makefile-tools"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -26,6 +26,9 @@
         "can_receive.h": "c",
         "can_process.h": "c",
         "can.h": "c",
-        "stdlib.h": "c"
+        "stdlib.h": "c",
+        "lk9025.h": "c",
+        "dji_motor.h": "c",
+        "ht04.h": "c"
     }
 }

Src/stm32f4xx_it.c

@@ -199,6 +199,7 @@ void DMA1_Stream1_IRQHandler(void)
   /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
 
   /* USER CODE END DMA1_Stream1_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_usart3_rx);
   /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
 
   /* USER CODE END DMA1_Stream1_IRQn 1 */

basic_framework.ioc

@@ -154,7 +154,7 @@ MxDb.Version=DB.6.0.60
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.CAN1_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true
 NVIC.CAN2_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true
-NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:false\:true
+NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
 NVIC.DMA2_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
 NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA2_Stream6_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
@@ -309,8 +309,8 @@ ProjectManager.MainLocation=Src
 ProjectManager.NoMain=false
 ProjectManager.PreviousToolchain=
 ProjectManager.ProjectBuild=false
-ProjectManager.ProjectFileName=abstract_test.ioc
+ProjectManager.ProjectFileName=basic_framework.ioc
-ProjectManager.ProjectName=abstract_test
+ProjectManager.ProjectName=basic_framework
 ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x4000
 ProjectManager.TargetToolchain=Makefile

modules/motor/HT04.c

@@ -1,7 +1,19 @@
 #include "HT04.h"
 #include "memory.h"
 
-joint_instance joint_motor_info[HT_MOTOR_CNT];
+joint_instance* joint_motor_info[HT_MOTOR_CNT];
+
+
+void HTMotorInit(joint_instance* _instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id)
+{
+    static uint8_t idx;
+    _instance->motor_can_instace.can_handle=_hcan;
+    _instance->motor_can_instace.tx_id=tx_id;
+    _instance->motor_can_instace.rx_id=rx_id;
+    _instance->motor_can_instace.can_module_callback=DecodeJoint;
+    CANRegister(&_instance->motor_can_instace);
+    joint_motor_info[idx++]=_instance;
+}
 
 static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
 {
@@ -24,7 +36,7 @@ void JointControl(joint_instance* _instance,float current)
     tmp = float_to_uint(current, T_MIN, T_MAX, 12);
     _instance->motor_can_instace.rx_buff[6] = tmp>>8;
     _instance->motor_can_instace.rx_buff[7] = tmp&0xff;
-    CANTransmit(_instance);
+    CANTransmit(&_instance->motor_can_instace);
 }
 
 void SetJointMode(joint_mode cmd,joint_instance* _instance)
@@ -40,15 +52,15 @@ void DecodeJoint(can_instance* motor_instance)
     uint16_t tmp;
     for (size_t i = 0; i < HT_MOTOR_CNT; i++)
     {
-        if(&joint_motor_info[i].motor_can_instace==motor_instance)
+        if(&joint_motor_info[i]->motor_can_instace==motor_instance)
         {
             tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
-            joint_motor_info[i].last_ecd=joint_motor_info[i].ecd;
+            joint_motor_info[i]->last_ecd=joint_motor_info[i]->ecd;
-            joint_motor_info[i].ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
+            joint_motor_info[i]->ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
             tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
-            joint_motor_info[i].speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
+            joint_motor_info[i]->speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
             tmp=((motor_instance->rx_buff[4]&0xf)<<8) | motor_instance->rx_buff[5];
-            joint_motor_info[i].given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
+            joint_motor_info[i]->given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
             break;
         }
     }

modules/motor/HT04.h

@@ -3,6 +3,7 @@
 
 #include "struct_typedef.h"
 #include "bsp_can.h"
+#include "controller.h"
 
 #define HT_MOTOR_CNT 4
 
@@ -19,6 +20,8 @@ typedef struct //HT04
     float ecd;
     float speed_rpm;
     float given_current;
+
+    PID_t pid;
     can_instance motor_can_instace;
 } joint_instance;
 
@@ -29,6 +32,7 @@ typedef enum
     CMD_ZERO_POSITION = 0xfe
 } joint_mode;
 
+void HTMotorInit(joint_instance* _instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id);
 
 void JointControl(joint_instance* _instance,float current);
 

modules/motor/LK9025.c

@@ -1,26 +1,43 @@
 #include"LK9025.h"
 
-static driven_instance driven_motor_info[2];
+static driven_instance* driven_motor_info[LK_MOTOR_CNT];
 
-void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan)
+static void DecodeDriven(can_instance* _instance)
+{
+    for (size_t i = 0; i < LK_MOTOR_CNT; i++)
+    {
+        if(&driven_motor_info[i]->motor_can_instance==_instance)
+        {
+            driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd;                                   
+            driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7]<<8) | _instance->rx_buff[6]);             
+            driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);      
+            driven_motor_info[i]->given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);  
+            driven_motor_info[i]->temperate = _instance->rx_buff[1];
+            break;
+        }
+    }
+}
+
+void LKMotroInit(driven_instance* instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id)
 {
     static uint8_t idx;
-    driven_motor_info[idx].motor_can_instance.can_handle=hcan;
+    instance->motor_can_instance.can_module_callback=DecodeDriven;
-    driven_motor_info[idx].motor_can_instance.can_module_callback=DecodeDriven;
+    instance->motor_can_instance.can_handle=_hcan;
-    driven_motor_info[idx].motor_can_instance.rx_id=rx_id;
+    instance->motor_can_instance.tx_id=tx_id;
-    driven_motor_info[idx].motor_can_instance.tx_id=motor_id;
+    instance->motor_can_instance.rx_id=rx_id;
-    CANRegister(&driven_motor_info[idx].motor_can_instance);
+    CANRegister(&instance->motor_can_instance);
+    driven_motor_info[idx++]=instance;
 }
 
 void DrivenControl(int16_t motor1_current,int16_t motor2_current)
 {
-    // LIMIT_MIN_MAX(motor1_current,  I_MIN,  I_MAX);
+    LIMIT_MIN_MAX(motor1_current,  I_MIN,  I_MAX);
-    // LIMIT_MIN_MAX(motor2_current,  I_MIN,  I_MAX);
+    LIMIT_MIN_MAX(motor2_current,  I_MIN,  I_MAX);
-    driven_motor_info[0].motor_can_instance.tx_buff[0] = motor1_current;
+    driven_motor_info[0]->motor_can_instance.tx_buff[0] = motor1_current;
-    driven_motor_info[0].motor_can_instance.tx_buff[1] = motor1_current>>8;
+    driven_motor_info[0]->motor_can_instance.tx_buff[1] = motor1_current>>8;
-    driven_motor_info[0].motor_can_instance.tx_buff[2] = motor2_current;
+    driven_motor_info[0]->motor_can_instance.tx_buff[2] = motor2_current;
-    driven_motor_info[0].motor_can_instance.tx_buff[3] = motor2_current>>8;
+    driven_motor_info[0]->motor_can_instance.tx_buff[3] = motor2_current>>8;
-    CANTransmit(&driven_motor_info[0].motor_can_instance);
+    CANTransmit(&driven_motor_info[0]->motor_can_instance);
 }
 
 void SetDrivenMode(driven_mode cmd,uint16_t motor_id)
@@ -31,18 +48,3 @@ void SetDrivenMode(driven_mode cmd,uint16_t motor_id)
     // CANTransmit(driven_mode)
 }
 
-void DecodeDriven(can_instance* _instance)
-{
-    for (size_t i = 0; i < LK_MOTOR_CNT; i++)
-    {
-        if(&driven_motor_info[i].motor_can_instance==_instance)
-        {
-            driven_motor_info[i].last_ecd = driven_motor_info[i].ecd;                                   
-            driven_motor_info[i].ecd = (uint16_t)((_instance->rx_buff[7]<<8) | _instance->rx_buff[6]);             
-            driven_motor_info[i].speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);      
-            driven_motor_info[i].given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);  
-            driven_motor_info[i].temperate = _instance->rx_buff[1];
-            break;
-        }
-    }
-}

modules/motor/LK9025.h

@@ -3,6 +3,7 @@
 
 #include "struct_typedef.h"
 #include "bsp_can.h"
+#include "controller.h"
 
 #define LK_MOTOR_CNT 2
 #define I_MIN -2000
@@ -17,7 +18,10 @@ typedef struct //9025
     int16_t speed_rpm;
     int16_t given_current;
     uint8_t temperate;
+
+    PID_t pid;
     can_instance motor_can_instance;
+
 } driven_instance;
 
 typedef enum
@@ -25,12 +29,10 @@ typedef enum
     unused = 0,
 } driven_mode;
 
-void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan);
+void LKMotroInit(driven_instance* instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id);
 
 void DrivenControl(int16_t motor1_current,int16_t motor2_current);
 
 void SetDrivenMode(driven_mode cmd,uint16_t motor_id);
 
-void DecodeDriven(can_instance* _instance);
-
 #endif // LK9025_H

modules/motor/dji_motor.c

@@ -0,0 +1,36 @@
+#include "dji_motor.h"
+
+static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
+
+
+static void DecodeDJIMotor(can_instance* _instance)
+{
+    for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
+    {
+        if(&dji_motor_info[i]->motor_can_instance==_instance)
+        {
+            dji_motor_info[i]->last_ecd = dji_motor_info[i]->ecd;                                   
+            dji_motor_info[i]->ecd = (uint16_t)(_instance->rx_buff[0] << 8 | _instance->rx_buff[1]);            
+            dji_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[2] << 8 | _instance->rx_buff[3]);      
+            dji_motor_info[i]->given_current = (uint16_t)(_instance->rx_buff[4] << 8 | _instance->rx_buff[5]);  
+            dji_motor_info[i]->temperate = _instance->rx_buff[6];
+            break;
+        }
+    }
+}
+
+void DJIMotorInit(dji_motor_instance* motor_instance,CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id)
+{
+    static uint8_t idx;
+    motor_instance->motor_can_instance.can_handle=_hcan;
+    motor_instance->motor_can_instance.tx_id=tx_id;
+    motor_instance->motor_can_instance.rx_id=rx_id;
+    motor_instance->motor_can_instance.can_module_callback=DecodeDJIMotor;
+    CANRegister(&motor_instance->motor_can_instance);
+    dji_motor_info[idx++]=motor_instance;
+}
+
+void DJIMotorControl()
+{
+    
+}

modules/motor/dji_motor.h

@@ -0,0 +1,29 @@
+#ifndef DJI_MOTOR_H
+#define DJI_MOTOR_H
+
+#define DJI_MOTOR_CNT 8
+
+#include "bsp_can.h"
+#include "controller.h"
+
+typedef struct
+{
+    uint16_t ecd;
+    int16_t speed_rpm;
+    int16_t given_current;
+    uint8_t temperate;
+    int16_t last_ecd;
+
+    PID_t motor_pid;
+    can_instance motor_can_instance;
+
+} dji_motor_instance;
+
+void DJIMotorInit(dji_motor_instance* motor_instace,CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id);
+
+void DJIMotorControl();
+
+
+
+
+#endif // !DJI_MOTOR_H

modules/motor/motor_task.c

@@ -0,0 +1,31 @@
+#include "motor_task.h"
+
+static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
+static joint_instance* joint_motor_info[HT_MOTOR_CNT];
+static driven_instance* driven_motor_info[LK_MOTOR_CNT];
+
+
+void RegisterMotor(Motor_Type_e motor_type,void* motor_instance)
+{
+    static uint8_t dji_idx,joint_idx,driven_idx;
+    switch (motor_type)
+    {
+    case GM6020:
+    case M3508:
+    case M2006:
+        dji_motor_info[dji_idx++]=(dji_motor_instance*)motor_instance;
+        break;
+    case LK9025:
+        driven_motor_info[driven_idx++]=(driven_instance*)motor_instance;
+        break;
+    case HT04:
+        joint_motor_info[joint_idx++]=(joint_instance*)motor_instance;
+        break;
+    }
+}
+
+void MotorControlTask()
+{
+    
+}
+

modules/motor/motor_task.h

@@ -0,0 +1,22 @@
+#ifndef MOTOR_TASK_H
+#define MOTOR_TASK_H
+
+#include "LK9025.h"
+#include "HT04.h"
+#include "dji_motor.h"
+
+typedef enum 
+{
+    GM6020=0,
+    M3508=1,
+    M2006=2,
+    LK9025=3,
+    HT04=4
+} Motor_Type_e;
+
+void MotorControlTask();
+
+void RegisterMotor(Motor_Type_e motor_type,void* motor_instance);
+
+#endif // !MOTOR_TASK_H
+