开始实际控制

2025-12-28 16:14:11 +08:00 · 2025-12-28 16:14:11 +08:00 · bef6f60c34
commit bef6f60c34
parent 0f6cf39492
26 changed files with 533 additions and 477 deletions
--- a/engineer/.mxproject
+++ b/engineer/.mxproject
--- a/engineer/CMakeLists.txt
+++ b/engineer/CMakeLists.txt
@ -50,7 +50,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
    User/bsp/mm.c
    User/bsp/time.c
    User/bsp/dwt.c
-    ./User/bsp/uart.c
+    # ./User/bsp/uart.c

    # User/component sources
    User/component/ahrs.c
@ -76,6 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
    # User/module sources
    User/module/config.c
    User/module/arm.c
+    ./User/module/kine.cpp

    # User/task sources
    User/task/control.c
--- a/engineer/Core/Inc/can.h
+++ b/engineer/Core/Inc/can.h
@ -34,11 +34,14 @@ extern "C" {

 extern CAN_HandleTypeDef hcan1;

+extern CAN_HandleTypeDef hcan2;
+
 /* USER CODE BEGIN Private defines */

 /* USER CODE END Private defines */

 void MX_CAN1_Init(void);
+void MX_CAN2_Init(void);

 /* USER CODE BEGIN Prototypes */

--- a/engineer/Core/Inc/stm32f4xx_hal_conf.h
+++ b/engineer/Core/Inc/stm32f4xx_hal_conf.h
@ -64,7 +64,7 @@
 /* #define HAL_MMC_MODULE_ENABLED */
 /* #define HAL_SPI_MODULE_ENABLED */
 #define HAL_TIM_MODULE_ENABLED
-#define HAL_UART_MODULE_ENABLED
+/* #define HAL_UART_MODULE_ENABLED */
 /* #define HAL_USART_MODULE_ENABLED */
 /* #define HAL_IRDA_MODULE_ENABLED */
 /* #define HAL_SMARTCARD_MODULE_ENABLED */
--- a/engineer/Core/Inc/stm32f4xx_it.h
+++ b/engineer/Core/Inc/stm32f4xx_it.h
@ -55,9 +55,10 @@ void DebugMon_Handler(void);
 void CAN1_TX_IRQHandler(void);
 void CAN1_RX0_IRQHandler(void);
 void CAN1_RX1_IRQHandler(void);
-void USART1_IRQHandler(void);
-void USART2_IRQHandler(void);
 void TIM8_TRG_COM_TIM14_IRQHandler(void);
+void CAN2_TX_IRQHandler(void);
+void CAN2_RX0_IRQHandler(void);
+void CAN2_RX1_IRQHandler(void);
 /* USER CODE BEGIN EFP */

 /* USER CODE END EFP */
--- a/engineer/Core/Inc/usart.h
+++ b/engineer/Core/Inc/usart.h
@ -1,55 +0,0 @@
-/* USER CODE BEGIN Header */
-/**
-  ******************************************************************************
-  * @file    usart.h
-  * @brief   This file contains all the function prototypes for
-  *          the usart.c file
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2025 STMicroelectronics.
-  * All rights reserved.
-  *
-  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
-  ******************************************************************************
-  */
-/* USER CODE END Header */
-/* Define to prevent recursive inclusion -------------------------------------*/
-#ifndef __USART_H__
-#define __USART_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Includes ------------------------------------------------------------------*/
-#include "main.h"
-
-/* USER CODE BEGIN Includes */
-
-/* USER CODE END Includes */
-
-extern UART_HandleTypeDef huart1;
-
-extern UART_HandleTypeDef huart2;
-
-/* USER CODE BEGIN Private defines */
-
-/* USER CODE END Private defines */
-
-void MX_USART1_UART_Init(void);
-void MX_USART2_UART_Init(void);
-
-/* USER CODE BEGIN Prototypes */
-
-/* USER CODE END Prototypes */
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __USART_H__ */
-
--- a/engineer/Core/Src/can.c
+++ b/engineer/Core/Src/can.c
@ -25,6 +25,7 @@
 /* USER CODE END 0 */

 CAN_HandleTypeDef hcan1;
+CAN_HandleTypeDef hcan2;

 /* CAN1 init function */
 void MX_CAN1_Init(void)
@ -58,6 +59,40 @@ void MX_CAN1_Init(void)
  /* USER CODE END CAN1_Init 2 */

 }
+/* CAN2 init function */
+void MX_CAN2_Init(void)
+{
+
+  /* USER CODE BEGIN CAN2_Init 0 */
+
+  /* USER CODE END CAN2_Init 0 */
+
+  /* USER CODE BEGIN CAN2_Init 1 */
+
+  /* USER CODE END CAN2_Init 1 */
+  hcan2.Instance = CAN2;
+  hcan2.Init.Prescaler = 3;
+  hcan2.Init.Mode = CAN_MODE_NORMAL;
+  hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ;
+  hcan2.Init.TimeSeg1 = CAN_BS1_6TQ;
+  hcan2.Init.TimeSeg2 = CAN_BS2_7TQ;
+  hcan2.Init.TimeTriggeredMode = DISABLE;
+  hcan2.Init.AutoBusOff = DISABLE;
+  hcan2.Init.AutoWakeUp = DISABLE;
+  hcan2.Init.AutoRetransmission = DISABLE;
+  hcan2.Init.ReceiveFifoLocked = DISABLE;
+  hcan2.Init.TransmitFifoPriority = DISABLE;
+  if (HAL_CAN_Init(&hcan2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN CAN2_Init 2 */
+
+  /* USER CODE END CAN2_Init 2 */
+
+}
+
+static uint32_t HAL_RCC_CAN1_CLK_ENABLED=0;

 void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
 {
@ -69,7 +104,10 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)

  /* USER CODE END CAN1_MspInit 0 */
    /* CAN1 clock enable */
-    __HAL_RCC_CAN1_CLK_ENABLE();
+    HAL_RCC_CAN1_CLK_ENABLED++;
+    if(HAL_RCC_CAN1_CLK_ENABLED==1){
+      __HAL_RCC_CAN1_CLK_ENABLE();
+    }

    __HAL_RCC_GPIOD_CLK_ENABLE();
    /**CAN1 GPIO Configuration
@ -94,6 +132,41 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)

  /* USER CODE END CAN1_MspInit 1 */
  }
+  else if(canHandle->Instance==CAN2)
+  {
+  /* USER CODE BEGIN CAN2_MspInit 0 */
+
+  /* USER CODE END CAN2_MspInit 0 */
+    /* CAN2 clock enable */
+    __HAL_RCC_CAN2_CLK_ENABLE();
+    HAL_RCC_CAN1_CLK_ENABLED++;
+    if(HAL_RCC_CAN1_CLK_ENABLED==1){
+      __HAL_RCC_CAN1_CLK_ENABLE();
+    }
+
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    /**CAN2 GPIO Configuration
+    PB5     ------> CAN2_RX
+    PB6     ------> CAN2_TX
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+    GPIO_InitStruct.Alternate = GPIO_AF9_CAN2;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    /* CAN2 interrupt Init */
+    HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(CAN2_TX_IRQn);
+    HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
+    HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
+  /* USER CODE BEGIN CAN2_MspInit 1 */
+
+  /* USER CODE END CAN2_MspInit 1 */
+  }
 }

 void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
@ -105,7 +178,10 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)

  /* USER CODE END CAN1_MspDeInit 0 */
    /* Peripheral clock disable */
-    __HAL_RCC_CAN1_CLK_DISABLE();
+    HAL_RCC_CAN1_CLK_ENABLED--;
+    if(HAL_RCC_CAN1_CLK_ENABLED==0){
+      __HAL_RCC_CAN1_CLK_DISABLE();
+    }

    /**CAN1 GPIO Configuration
    PD0     ------> CAN1_RX
@ -121,6 +197,32 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)

  /* USER CODE END CAN1_MspDeInit 1 */
  }
+  else if(canHandle->Instance==CAN2)
+  {
+  /* USER CODE BEGIN CAN2_MspDeInit 0 */
+
+  /* USER CODE END CAN2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_CAN2_CLK_DISABLE();
+    HAL_RCC_CAN1_CLK_ENABLED--;
+    if(HAL_RCC_CAN1_CLK_ENABLED==0){
+      __HAL_RCC_CAN1_CLK_DISABLE();
+    }
+
+    /**CAN2 GPIO Configuration
+    PB5     ------> CAN2_RX
+    PB6     ------> CAN2_TX
+    */
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
+
+    /* CAN2 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
+    HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
+    HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
+  /* USER CODE BEGIN CAN2_MspDeInit 1 */
+
+  /* USER CODE END CAN2_MspDeInit 1 */
+  }
 }

 /* USER CODE BEGIN 1 */
--- a/engineer/Core/Src/gpio.c
+++ b/engineer/Core/Src/gpio.c
@ -43,8 +43,8 @@ void MX_GPIO_Init(void)
 {

  /* GPIO Ports Clock Enable */
-  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();

--- a/engineer/Core/Src/main.c
+++ b/engineer/Core/Src/main.c
@ -20,7 +20,6 @@
 #include "main.h"
 #include "cmsis_os.h"
 #include "can.h"
-#include "usart.h"
 #include "gpio.h"

 /* Private includes ----------------------------------------------------------*/
@ -91,8 +90,7 @@ int main(void)
  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_CAN1_Init();
-  MX_USART1_UART_Init();
-  MX_USART2_UART_Init();
+  MX_CAN2_Init();
  /* USER CODE BEGIN 2 */

  /* USER CODE END 2 */
--- a/engineer/Core/Src/stm32f4xx_it.c
+++ b/engineer/Core/Src/stm32f4xx_it.c
@ -56,8 +56,7 @@

 /* External variables --------------------------------------------------------*/
 extern CAN_HandleTypeDef hcan1;
-extern UART_HandleTypeDef huart1;
-extern UART_HandleTypeDef huart2;
+extern CAN_HandleTypeDef hcan2;
 extern TIM_HandleTypeDef htim14;

 /* USER CODE BEGIN EV */
@ -204,34 +203,6 @@ void CAN1_RX1_IRQHandler(void)
  /* USER CODE END CAN1_RX1_IRQn 1 */
 }

-/**
-  * @brief This function handles USART1 global interrupt.
-  */
-void USART1_IRQHandler(void)
-{
-  /* USER CODE BEGIN USART1_IRQn 0 */
-
-  /* USER CODE END USART1_IRQn 0 */
-  HAL_UART_IRQHandler(&huart1);
-  /* USER CODE BEGIN USART1_IRQn 1 */
-
-  /* USER CODE END USART1_IRQn 1 */
-}
-
-/**
-  * @brief This function handles USART2 global interrupt.
-  */
-void USART2_IRQHandler(void)
-{
-  /* USER CODE BEGIN USART2_IRQn 0 */
-
-  /* USER CODE END USART2_IRQn 0 */
-  HAL_UART_IRQHandler(&huart2);
-  /* USER CODE BEGIN USART2_IRQn 1 */
-
-  /* USER CODE END USART2_IRQn 1 */
-}
-
 /**
  * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt.
  */
@ -246,6 +217,48 @@ void TIM8_TRG_COM_TIM14_IRQHandler(void)
  /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 1 */
 }

+/**
+  * @brief This function handles CAN2 TX interrupts.
+  */
+void CAN2_TX_IRQHandler(void)
+{
+  /* USER CODE BEGIN CAN2_TX_IRQn 0 */
+
+  /* USER CODE END CAN2_TX_IRQn 0 */
+  HAL_CAN_IRQHandler(&hcan2);
+  /* USER CODE BEGIN CAN2_TX_IRQn 1 */
+
+  /* USER CODE END CAN2_TX_IRQn 1 */
+}
+
+/**
+  * @brief This function handles CAN2 RX0 interrupts.
+  */
+void CAN2_RX0_IRQHandler(void)
+{
+  /* USER CODE BEGIN CAN2_RX0_IRQn 0 */
+
+  /* USER CODE END CAN2_RX0_IRQn 0 */
+  HAL_CAN_IRQHandler(&hcan2);
+  /* USER CODE BEGIN CAN2_RX0_IRQn 1 */
+
+  /* USER CODE END CAN2_RX0_IRQn 1 */
+}
+
+/**
+  * @brief This function handles CAN2 RX1 interrupt.
+  */
+void CAN2_RX1_IRQHandler(void)
+{
+  /* USER CODE BEGIN CAN2_RX1_IRQn 0 */
+
+  /* USER CODE END CAN2_RX1_IRQn 0 */
+  HAL_CAN_IRQHandler(&hcan2);
+  /* USER CODE BEGIN CAN2_RX1_IRQn 1 */
+
+  /* USER CODE END CAN2_RX1_IRQn 1 */
+}
+
 /* USER CODE BEGIN 1 */

 /* USER CODE END 1 */
--- a/engineer/Core/Src/usart.c
+++ b/engineer/Core/Src/usart.c
@ -1,196 +0,0 @@
-/* USER CODE BEGIN Header */
-/**
-  ******************************************************************************
-  * @file    usart.c
-  * @brief   This file provides code for the configuration
-  *          of the USART instances.
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2025 STMicroelectronics.
-  * All rights reserved.
-  *
-  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
-  ******************************************************************************
-  */
-/* USER CODE END Header */
-/* Includes ------------------------------------------------------------------*/
-#include "usart.h"
-
-/* USER CODE BEGIN 0 */
-
-/* USER CODE END 0 */
-
-UART_HandleTypeDef huart1;
-UART_HandleTypeDef huart2;
-
-/* USART1 init function */
-
-void MX_USART1_UART_Init(void)
-{
-
-  /* USER CODE BEGIN USART1_Init 0 */
-
-  /* USER CODE END USART1_Init 0 */
-
-  /* USER CODE BEGIN USART1_Init 1 */
-
-  /* USER CODE END USART1_Init 1 */
-  huart1.Instance = USART1;
-  huart1.Init.BaudRate = 115200;
-  huart1.Init.WordLength = UART_WORDLENGTH_8B;
-  huart1.Init.StopBits = UART_STOPBITS_1;
-  huart1.Init.Parity = UART_PARITY_NONE;
-  huart1.Init.Mode = UART_MODE_TX_RX;
-  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
-  if (HAL_UART_Init(&huart1) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN USART1_Init 2 */
-
-  /* USER CODE END USART1_Init 2 */
-
-}
-/* USART2 init function */
-
-void MX_USART2_UART_Init(void)
-{
-
-  /* USER CODE BEGIN USART2_Init 0 */
-
-  /* USER CODE END USART2_Init 0 */
-
-  /* USER CODE BEGIN USART2_Init 1 */
-
-  /* USER CODE END USART2_Init 1 */
-  huart2.Instance = USART2;
-  huart2.Init.BaudRate = 115200;
-  huart2.Init.WordLength = UART_WORDLENGTH_8B;
-  huart2.Init.StopBits = UART_STOPBITS_1;
-  huart2.Init.Parity = UART_PARITY_NONE;
-  huart2.Init.Mode = UART_MODE_TX_RX;
-  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
-  if (HAL_UART_Init(&huart2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN USART2_Init 2 */
-
-  /* USER CODE END USART2_Init 2 */
-
-}
-
-void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
-{
-
-  GPIO_InitTypeDef GPIO_InitStruct = {0};
-  if(uartHandle->Instance==USART1)
-  {
-  /* USER CODE BEGIN USART1_MspInit 0 */
-
-  /* USER CODE END USART1_MspInit 0 */
-    /* USART1 clock enable */
-    __HAL_RCC_USART1_CLK_ENABLE();
-
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-    /**USART1 GPIO Configuration
-    PB7     ------> USART1_RX
-    PB6     ------> USART1_TX
-    */
-    GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_6;
-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
-    GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
-    /* USART1 interrupt Init */
-    HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
-    HAL_NVIC_EnableIRQ(USART1_IRQn);
-  /* USER CODE BEGIN USART1_MspInit 1 */
-
-  /* USER CODE END USART1_MspInit 1 */
-  }
-  else if(uartHandle->Instance==USART2)
-  {
-  /* USER CODE BEGIN USART2_MspInit 0 */
-
-  /* USER CODE END USART2_MspInit 0 */
-    /* USART2 clock enable */
-    __HAL_RCC_USART2_CLK_ENABLE();
-
-    __HAL_RCC_GPIOD_CLK_ENABLE();
-    /**USART2 GPIO Configuration
-    PD6     ------> USART2_RX
-    PD5     ------> USART2_TX
-    */
-    GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_5;
-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
-    GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
-    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
-
-    /* USART2 interrupt Init */
-    HAL_NVIC_SetPriority(USART2_IRQn, 5, 0);
-    HAL_NVIC_EnableIRQ(USART2_IRQn);
-  /* USER CODE BEGIN USART2_MspInit 1 */
-
-  /* USER CODE END USART2_MspInit 1 */
-  }
-}
-
-void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
-{
-
-  if(uartHandle->Instance==USART1)
-  {
-  /* USER CODE BEGIN USART1_MspDeInit 0 */
-
-  /* USER CODE END USART1_MspDeInit 0 */
-    /* Peripheral clock disable */
-    __HAL_RCC_USART1_CLK_DISABLE();
-
-    /**USART1 GPIO Configuration
-    PB7     ------> USART1_RX
-    PB6     ------> USART1_TX
-    */
-    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_7|GPIO_PIN_6);
-
-    /* USART1 interrupt Deinit */
-    HAL_NVIC_DisableIRQ(USART1_IRQn);
-  /* USER CODE BEGIN USART1_MspDeInit 1 */
-
-  /* USER CODE END USART1_MspDeInit 1 */
-  }
-  else if(uartHandle->Instance==USART2)
-  {
-  /* USER CODE BEGIN USART2_MspDeInit 0 */
-
-  /* USER CODE END USART2_MspDeInit 0 */
-    /* Peripheral clock disable */
-    __HAL_RCC_USART2_CLK_DISABLE();
-
-    /**USART2 GPIO Configuration
-    PD6     ------> USART2_RX
-    PD5     ------> USART2_TX
-    */
-    HAL_GPIO_DeInit(GPIOD, GPIO_PIN_6|GPIO_PIN_5);
-
-    /* USART2 interrupt Deinit */
-    HAL_NVIC_DisableIRQ(USART2_IRQn);
-  /* USER CODE BEGIN USART2_MspDeInit 1 */
-
-  /* USER CODE END USART2_MspDeInit 1 */
-  }
-}
-
-/* USER CODE BEGIN 1 */
-
-/* USER CODE END 1 */
--- a/engineer/DM.ioc
+++ b/engineer/DM.ioc
@ -9,6 +9,13 @@ CAN1.CalculateTimeBit=1000
 CAN1.CalculateTimeQuantum=71.42857142857143
 CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2
 CAN1.Prescaler=3
+CAN2.BS1=CAN_BS1_6TQ
+CAN2.BS2=CAN_BS2_7TQ
+CAN2.CalculateBaudRate=1000000
+CAN2.CalculateTimeBit=1000
+CAN2.CalculateTimeQuantum=71.42857142857143
+CAN2.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2
+CAN2.Prescaler=3
 FREERTOS.IPParameters=Tasks01,configENABLE_FPU
 FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configENABLE_FPU=1
@ -18,28 +25,25 @@ KeepUserPlacement=false
 Mcu.CPN=STM32F407IGH6
 Mcu.Family=STM32F4
 Mcu.IP0=CAN1
-Mcu.IP1=FREERTOS
-Mcu.IP2=NVIC
-Mcu.IP3=RCC
-Mcu.IP4=SYS
-Mcu.IP5=USART1
-Mcu.IP6=USART2
-Mcu.IPNb=7
+Mcu.IP1=CAN2
+Mcu.IP2=FREERTOS
+Mcu.IP3=NVIC
+Mcu.IP4=RCC
+Mcu.IP5=SYS
+Mcu.IPNb=6
 Mcu.Name=STM32F407I(E-G)Hx
 Mcu.Package=UFBGA176
-Mcu.Pin0=PA14
-Mcu.Pin1=PA13
-Mcu.Pin10=VP_FREERTOS_VS_CMSIS_V2
-Mcu.Pin11=VP_SYS_VS_tim14
-Mcu.Pin2=PB7
+Mcu.Pin0=PB5
+Mcu.Pin1=PA14
+Mcu.Pin2=PA13
 Mcu.Pin3=PB6
-Mcu.Pin4=PD6
-Mcu.Pin5=PD0
-Mcu.Pin6=PD5
-Mcu.Pin7=PD1
-Mcu.Pin8=PH0-OSC_IN
-Mcu.Pin9=PH1-OSC_OUT
-Mcu.PinsNb=12
+Mcu.Pin4=PD0
+Mcu.Pin5=PD1
+Mcu.Pin6=PH0-OSC_IN
+Mcu.Pin7=PH1-OSC_OUT
+Mcu.Pin8=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin9=VP_SYS_VS_tim14
+Mcu.PinsNb=10
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407IGHx
@ -49,6 +53,9 @@ NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN2_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
@ -64,27 +71,21 @@ NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
 NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
 NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn
 NVIC.TimeBaseIP=TIM14
-NVIC.USART1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
-NVIC.USART2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 PA13.Mode=Serial_Wire
 PA13.Signal=SYS_JTMS-SWDIO
 PA14.Mode=Serial_Wire
 PA14.Signal=SYS_JTCK-SWCLK
-PB6.Mode=Asynchronous
-PB6.Signal=USART1_TX
-PB7.Mode=Asynchronous
-PB7.Signal=USART1_RX
+PB5.Mode=CAN_Activate
+PB5.Signal=CAN2_RX
+PB6.Mode=CAN_Activate
+PB6.Signal=CAN2_TX
 PD0.Locked=true
 PD0.Mode=CAN_Activate
 PD0.Signal=CAN1_RX
 PD1.Locked=true
 PD1.Mode=CAN_Activate
 PD1.Signal=CAN1_TX
-PD5.Mode=Asynchronous
-PD5.Signal=USART2_TX
-PD6.Mode=Asynchronous
-PD6.Signal=USART2_RX
 PH0-OSC_IN.Mode=HSE-External-Oscillator
 PH0-OSC_IN.Signal=RCC_OSC_IN
 PH1-OSC_OUT.Mode=HSE-External-Oscillator
@ -122,7 +123,7 @@ ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_CAN1_Init-CAN1-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_CAN1_Init-CAN1-false-HAL-true,4-MX_CAN2_Init-CAN2-false-HAL-true
 RCC.48MHZClocksFreq_Value=84000000
 RCC.AHBFreq_Value=168000000
 RCC.APB1CLKDivider=RCC_HCLK_DIV4
@ -156,10 +157,6 @@ RCC.VCOI2SOutputFreq_Value=384000000
 RCC.VCOInputFreq_Value=2000000
 RCC.VCOOutputFreq_Value=336000000
 RCC.VcooutputI2S=192000000
-USART1.IPParameters=VirtualMode
-USART1.VirtualMode=VM_ASYNC
-USART2.IPParameters=VirtualMode
-USART2.VirtualMode=VM_ASYNC
 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
 VP_SYS_VS_tim14.Mode=TIM14
--- a/engineer/User/bsp/bsp_config.yaml
+++ b/engineer/User/bsp/bsp_config.yaml
@ -2,6 +2,8 @@ can:
  devices:
  - instance: CAN1
    name: '1'
+  - instance: CAN2
+    name: '2'
  enabled: true
 dwt:
  enabled: true
--- a/engineer/User/bsp/can.c
+++ b/engineer/User/bsp/can.c
@ -66,6 +66,8 @@ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
    
    if (hcan->Instance == CAN1)
        return BSP_CAN_1;
+    else if (hcan->Instance == CAN2)
+        return BSP_CAN_2;
    else
        return BSP_CAN_ERR;
 }
@ -468,7 +470,24 @@ int8_t BSP_CAN_Init(void) {
    BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);

    // 激活CAN1中断
-    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY);
+    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | 
+                                        CAN_IT_TX_MAILBOX_EMPTY);  // 激活发送邮箱空中断
+
+    // 初始化 CAN2 - 使用 FIFO1
+    can1_filter.FilterBank = 14;
+    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1;
+    HAL_CAN_ConfigFilter(&hcan2, &can1_filter);  // 通过 CAN1 配置
+    HAL_CAN_Start(&hcan2);
+
+    // 自动注册CAN2接收回调函数
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
+
+    // 激活CAN2中断
+    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | 
+                                        CAN_IT_TX_MAILBOX_EMPTY);  // 激活发送邮箱空中断

    
    inited = true;
@ -484,6 +503,8 @@ CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
    switch (can) {
    case BSP_CAN_1:
      return &hcan1;
+    case BSP_CAN_2:
+      return &hcan2;
        default:
            return NULL;
    }
--- a/engineer/User/device/motor_dm.c
+++ b/engineer/User/device/motor_dm.c
@ -81,12 +81,12 @@ static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data
    motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12);  // (-12.0,12.0)
    motor->motor.feedback.temp = (float)(data[6]);

-    while (motor->motor.feedback.rotor_abs_angle < 0) {
-        motor->motor.feedback.rotor_abs_angle += M_2PI;
-    }
-    while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
-        motor->motor.feedback.rotor_abs_angle -= M_2PI;
-    }
+    // while (motor->motor.feedback.rotor_abs_angle < 0) {
+    //     motor->motor.feedback.rotor_abs_angle += M_2PI;
+    // }
+    // while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
+    //     motor->motor.feedback.rotor_abs_angle -= M_2PI;
+    // }

    if (motor->param.reverse) {
        motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
--- a/engineer/User/device/motor_lz.c
+++ b/engineer/User/device/motor_lz.c
@ -204,15 +204,15 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
    uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
    float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);

-    while (angle <0){
-        angle += M_2PI;
-    }
-    while (angle > M_2PI){
-        angle -= M_2PI;
-    }
+    // while (angle <0){
+    //     angle += M_2PI;
+    // }
+    // while (angle > M_2PI){
+    //     angle -= M_2PI;
+    // }
    // 自动反向
    if (motor->param.reverse) {
-        angle = M_2PI - angle;
+        angle =  - angle;
        velocity = -velocity;
        torque = -torque;
    }
--- a/engineer/User/module/arm.c
+++ b/engineer/User/module/arm.c
@ -1,59 +1,121 @@
 #include "module/arm.h"

-#include "module/config.h"
 #include "device/motor_dm.h"
 #include "device/motor_lz.h"
 #include "component/pid.h"
 #include "component/user_math.h"
 #include "bsp/dwt.h"
 #include "component/cmd.h"
-
-
+#include "module/config.h"





 //用于机械臂重力补偿 lxqx:linkx质心位置对qx的偏导数 由sympy符号计算库计算生成
-double compute_l5q2(double q1, double q2, double q3, double q4, double q5) {
-    return 0.063171000000000005*((-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + 1.0*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) - 1.0*cos(q2)*cos(q3))*sin(q5 + 1.5707963267948966) + 0.31590000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.31590000000000001*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
+// double compute_l5q2(double q1, double q2, double q3, double q4, double q5) {
+//     return 0.063171000000000005*((-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + 1.0*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) - 1.0*cos(q2)*cos(q3))*sin(q5 + 1.5707963267948966) + 0.31590000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.31590000000000001*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
+// }
+// double compute_l5q3(double q1, double q2, double q3, double q4, double q5) {
+//     return 0.063171000000000005*((-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + 1.0*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) - 1.0*cos(q2)*cos(q3))*sin(q5 + 1.5707963267948966) + 0.31590000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.31590000000000001*cos(q2)*cos(q3);
+// }
+// double compute_l5q4(double q1, double q2, double q3, double q4, double q5) {
+//     return 0.063171000000000005*(-(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + (-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*cos(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) - 6.123233995736766e-17*(-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*sin(q4 + 1.5707963267948966))*sin(q5 + 1.5707963267948966);
+// }
+// double compute_l5q5(double q1, double q2, double q3, double q4, double q5) {
+//   q3+=0.01;
+//     return -0.063171000000000005*((-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*sin(q4 + 1.5707963267948966))*sin(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*cos(q4 + 1.5707963267948966) - 6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3) + 3.749399456654644e-33)*cos(q5 + 1.5707963267948966);
+// }
+// double compute_l4q2(double q1, double q2, double q3, double q4, double q5) {
+//     return 0.22925000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.22925000000000001*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
+// }
+// double compute_l4q3(double q1, double q2, double q3, double q4, double q5) {
+//     return 0.22925000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.22925000000000001*cos(q2)*cos(q3);
+// }
+// double compute_l3q2(double q1, double q2, double q3, double q4, double q5) {
+//     return -0.0048250000000000003*sin(q2)*sin(q3) - 0.11803*sin(q2)*cos(q3) - 0.11803*sin(q3)*cos(q2) + 0.0048250000000000003*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
+// }
+// double compute_l3q3(double q1, double q2, double q3, double q4, double q5) {
+//
+//     return -0.0048250000000000003*sin(q2)*sin(q3) - 0.11803*sin(q2)*cos(q3) - 0.11803*sin(q3)*cos(q2) + 0.0048250000000000003*cos(q2)*cos(q3);
+// }
+// double compute_l2q2(double q1, double q2, double q3, double q4, double q5) {
+//     return 0.27905999999999997*cos(q2+0.07);
+// }
+
+//joint2
+double compute_l5q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.21817*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q2)*cos(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
 }
-double compute_l5q3(double q1, double q2, double q3, double q4, double q5) {
-    return 0.063171000000000005*((-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + 1.0*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) - 1.0*cos(q2)*cos(q3))*sin(q5 + 1.5707963267948966) + 0.31590000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.31590000000000001*cos(q2)*cos(q3);
+double compute_l4q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.60215*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.189273*sin(q2)*cos(q3 + 1.5707963267948966) - 0.189273*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
 }
-double compute_l5q4(double q1, double q2, double q3, double q4, double q5) {
-    return 0.063171000000000005*(-(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + (-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*cos(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) - 6.123233995736766e-17*(-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*sin(q4 + 1.5707963267948966))*sin(q5 + 1.5707963267948966);
+double compute_l3q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.72964*9.81*(-0.086979000000000001*sin(q2)*sin(q3 + 1.5707963267948966) + 0.086979000000000001*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
 }
-double compute_l5q5(double q1, double q2, double q3, double q4, double q5) {
-  q3+=0.01;
-    return -0.063171000000000005*((-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*sin(q4 + 1.5707963267948966))*sin(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*cos(q4 + 1.5707963267948966) - 6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3) + 3.749399456654644e-33)*cos(q5 + 1.5707963267948966);
+double compute_l2q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 1.1252*9.81*0.26277*cos(q2);
 }
-double compute_l4q2(double q1, double q2, double q3, double q4, double q5) {
-    return 0.22925000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.22925000000000001*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
+
+//joint3
+double compute_l5q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.21817*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q2)*cos(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966));
 }
-double compute_l4q3(double q1, double q2, double q3, double q4, double q5) {
-    return 0.22925000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.22925000000000001*cos(q2)*cos(q3);
+double compute_l4q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.60215*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.189273*sin(q2)*cos(q3 + 1.5707963267948966) - 0.189273*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966));
 }
-double compute_l3q2(double q1, double q2, double q3, double q4, double q5) {
-    return -0.0048250000000000003*sin(q2)*sin(q3) - 0.11803*sin(q2)*cos(q3) - 0.11803*sin(q3)*cos(q2) + 0.0048250000000000003*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
+double compute_l3q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.72964*9.81*(-0.086979000000000001*sin(q2)*sin(q3 + 1.5707963267948966) + 0.086979000000000001*cos(q2)*cos(q3 + 1.5707963267948966));
 }
-double compute_l3q3(double q1, double q2, double q3, double q4, double q5) {
-  
-    return -0.0048250000000000003*sin(q2)*sin(q3) - 0.11803*sin(q2)*cos(q3) - 0.11803*sin(q3)*cos(q2) + 0.0048250000000000003*cos(q2)*cos(q3);
+
+//joint4
+double compute_l5q4(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.21817*9.81*(-0.058000000000000003*(-(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*sin(q4) + (-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*cos(q4))*sin(q5) + 0.058000000000000003*(-6.123233995736766e-17*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) - 6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4))*cos(q5) + 3.5514757175273241e-18*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) + 3.5514757175273241e-18*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4));
 }
-double compute_l2q2(double q1, double q2, double q3, double q4, double q5) {
-    return 0.27905999999999997*cos(q2+0.07);
+
+//joint5
+double compute_l5q5(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
+    return 0.21817*9.81*(-0.058000000000000003*((1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) + (-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4))*cos(q5) - 0.058000000000000003*(-6.123233995736766e-17*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*sin(q4) + 6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*cos(q4) - 1.0*sin(q2)*sin(q3 + 1.5707963267948966) + 1.0*cos(q2)*cos(q3 + 1.5707963267948966) + 3.749399456654644e-33)*sin(q5));
 }


+double q2t(double q1, double q2, double q3, double q4, double q5,double q6,double q7)
+{
+  return \
+       // compute_l7q2(q1, q2, q3, q4, q5, q6-pi/2, q7)
+       // + compute_l6q2(q1, q2, q3, q4, q5, q6-pi/2, q7)
+       + compute_l5q2(q1, q2, q3, q4, q5, q6, q7)
+       + compute_l4q2(q1, q2, q3, q4, q5, q6, q7)
+        + compute_l3q2(q1, q2, q3, q4, q5, q6, q7)
+        + compute_l2q2(q1, q2, q3, q4, q5, q6, q7);
+}
+
+double q3t(double q1, double q2, double q3, double q4, double q5,double q6,double q7)
+{
+  return \
+        compute_l3q3(q1, q2, q3, q4, q5, q6, q7)
+        +compute_l4q3(q1, q2, q3, q4, q5, q6, q7)
+        +compute_l5q3(q1, q2, q3, q4, q5, q6, q7);
+        // +compute_l6q3(q1, q2, q3, q4, q5, q6-pi/2, q7);
+        // +compute_l7q3(q1, q2, q3, q4, q5, q6-pi/2, q7);
+}


-
-
+double q4t(double q1, double q2, double q3, double q4, double q5,double q6,double q7)
+{
+  return \
+        +compute_l5q4(q1, q2, q3, q4, q5, q6, q7);
+}
+double q5t(double q1, double q2, double q3, double q4, double q5,double q6,double q7)
+{
+  return \
+        +compute_l5q5(q1, q2, q3, q4, q5, q6, q7);
+}
 //机械臂初始化
 void arm_init(arm_t* arm,Config_RobotParam_t *cfg)
 {
    assert_param(!(cfg==NULL||arm==NULL));
+  MOTOR_LZ_Init();

  for (int i=0; i<JOINT_NUM; i++) {
    switch (cfg->arm_param.joint[i].motor.type) {
@ -73,6 +135,7 @@ void arm_init(arm_t* arm,Config_RobotParam_t *cfg)
    PID_Init(&arm->joint[i].controler.Vpid,KPID_MODE_NO_D,1000,&cfg->arm_param.joint[i].pid.Vpid);
    arm->joint[i].status.angle_limit[0]=cfg->arm_param.joint[i].angle_limit[0];
    arm->joint[i].status.angle_limit[1]=cfg->arm_param.joint[i].angle_limit[1];
+    arm->joint[i].status.init_angle=cfg->arm_param.joint[i].init_angle;


  arm->mass[i]=cfg->arm_param.mass[i];
@ -83,35 +146,14 @@ void arm_init(arm_t* arm,Config_RobotParam_t *cfg)
 void arm_gravity_calcu(arm_t* arm)
 {
    assert_param(!(arm==NULL));
-  arm->joint[4].ref.torque += -1 *
-               compute_l5q5(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-               0.403 * 9.81;
-  arm->joint[3].ref.torque += -1 *
-               compute_l5q4(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-               0.403 * 9.81;
-  arm->joint[2].ref.torque += compute_l5q3(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   0.403 * 9.81 +
-               compute_l4q3(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   0.720 * 9.81 +
-               compute_l3q3(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   1.410 * 9.81;
-  arm->joint[1].ref.torque += compute_l5q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   0.403 * 9.81 +
-               compute_l4q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   0.720 * 9.81 +
-               compute_l3q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   1.410 * 9.81 +
-               compute_l2q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
-                            arm->joint[5].feedback.position) *
-                   1.9889 * 9.81;
+  arm->joint[3].ref.torque += (float)q5t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
+                            arm->joint[3].feedback.position,0,0);
+  arm->joint[2].ref.torque += (float)q4t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
+                            arm->joint[3].feedback.position,0,0);
+  arm->joint[1].ref.torque += (float)q3t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
+                            arm->joint[3].feedback.position,0,0);
+  arm->joint[0].ref.torque += (float)q2t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
+                            arm->joint[3].feedback.position,0,0);
 }


@ -127,21 +169,22 @@ void arm_data_updata(arm_t* arm)
    float nspeed=0;
    nangle = (arm->joint[i].motor.type==DM_MOTOR)?((__dm_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_abs_angle):((__lz_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_abs_angle);
    nspeed = (arm->joint[i].motor.type==DM_MOTOR)?((__dm_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_speed):((__lz_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_speed);
-    if(nangle- arm->joint[i].status.last_angle >M_2PI/2)
-    {
-      arm->joint[i].status.circles++;
-    }
-    else if(nangle- arm->joint[i].status.last_angle < -M_2PI/2)
-    {
-      arm->joint[i].status.circles--;
-    }
+    // if(nangle- arm->joint[i].status.last_angle >M_2PI/2)
+    // {
+    //   arm->joint[i].status.circles++;
+    // }
+    // else if(nangle- arm->joint[i].status.last_angle < -M_2PI/2)
+    // {
+    //   arm->joint[i].status.circles--;
+    // }
    arm->joint[i].status.last_angle = nangle;
    arm->joint[i].feedback.velocity = nspeed;
-    arm->joint[i].feedback.position = arm->joint[i].status.circles*M_2PI+nangle;
+    // arm->joint[i].feedback.position = arm->joint[i].status.circles*M_2PI+nangle;
+    arm->joint[i].feedback.position = nangle - arm->joint[i].status.init_angle;
  }

-  arm->joint[2].feedback.velocity = arm->joint[2].feedback.velocity - arm->joint[1].feedback.velocity;
-  arm->joint[2].feedback.position = arm->joint[2].feedback.position - arm->joint[1].feedback.position;
+  arm->joint[1].feedback.velocity = arm->joint[1].feedback.velocity - arm->joint[0].feedback.velocity;
+  arm->joint[1].feedback.position = arm->joint[1].feedback.position - arm->joint[0].feedback.position;
  
 }

@ -163,6 +206,9 @@ void arm_control_apply(arm_t* arm)
    assert_param(!(arm==NULL));
  for (uint8_t i=0; i<JOINT_NUM; i++) {
    assert_param(!(arm->joint[i].motor.handle == NULL));
+
+arm->joint[i].ref.torque =0;
+
    if(arm->joint[i].feedback.position<arm->joint[i].status.angle_limit[0]||arm->joint[i].feedback.position>arm->joint[i].status.angle_limit[1])
    {
      arm->joint[i].ref.torque=0;
--- a/engineer/User/module/arm.h
+++ b/engineer/User/module/arm.h
@ -1,19 +1,18 @@
 #pragma once

-#ifdef __cplusplus
-extern "C" {
-#endif

 #include "main.h"
+
 #include "component/kinematics.h"
 #include "device/motor_dm.h"
 #include "device/motor_lz.h"
 #include "component/pid.h"

+
 #define ga 9.81
 #define __dm_handle(handle) (MOTOR_DM_t *)handle
 #define __lz_handle(handle) (MOTOR_LZ_t *)handle
-#define JOINT_NUM 6
+#define JOINT_NUM 5


 typedef enum{
@ -56,6 +55,7 @@ typedef struct {

  struct {
    float angle_limit[2];
+    float init_angle;
    float last_angle;
    int circles;
    float last_control_time;
@ -82,6 +82,7 @@ typedef struct{
      KPID_Params_t Vpid;
    }pid;
    float angle_limit[2];
+    float init_angle;
  }joint[JOINT_NUM];
  DH_parameter_t dh_param[6];
  float mass[6];
@ -106,11 +107,11 @@ typedef struct hand_t
    TAKE_MODE,
    POSITION_MODE
  }mode;
-
-  
-
 }arm_t;

-#ifdef __cplusplus
-}
-#endif
+
+typedef struct Config_RobotParam Config_RobotParam_t;
+void arm_init(arm_t* arm,Config_RobotParam_t *cfg);
+void arm_data_updata(arm_t* arm);
+void arm_control_apply(arm_t* arm);
+void arm_gravity_calcu(arm_t* arm);
--- a/engineer/User/module/config.c
+++ b/engineer/User/module/config.c
@ -0,0 +1,111 @@
+#include "config.h"
+#include "module/arm.h"
+
+
+
+
+static Config_RobotParam_t robot_config = {
+  .arm_param = { 
+    .joint[0]={
+      .angle_limit={0,3.0},
+      .init_angle = -1.384,
+      .motor.type = LZ_MOTOR,
+      .motor.param.lz_param={
+        .motor_id=2,.host_id=2,.can=BSP_CAN_1,.mode=MOTOR_LZ_MODE_MOTION,.module=MOTOR_LZ_RSO3,.reverse=1
+      },
+      .pid.Ppid={
+        .k=1,
+        .p=0,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100
+      }
+    },
+    .joint[1]={
+      .angle_limit={-6,0},
+      .init_angle = 1.668,
+      .motor.type = LZ_MOTOR,
+      .motor.param.lz_param={
+        .motor_id=3,.host_id=3,.can=BSP_CAN_1,.mode=MOTOR_LZ_MODE_MOTION,.module=MOTOR_LZ_RSO3,.reverse=0
+      },
+      .pid.Ppid={
+        .k=1,
+        .p=0,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100
+      }
+    },
+    .joint[2]={
+      .angle_limit={-3.14,3.14},
+      .init_angle = -2.38,
+      .motor.type = DM_MOTOR,
+      .motor.param.dm_param={
+        .can=BSP_CAN_2,.master_id=0x14,.can_id=0x04,.module=MOTOR_DM_J4310,.reverse=0
+      },
+      .pid.Ppid={
+        .k=1,
+        .p=0,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100
+      }
+    },
+
+    .joint[3]={
+      .angle_limit={-0.85,1.72},
+      .init_angle = 1.8214,
+      .motor.type = DM_MOTOR,
+      .motor.param.dm_param={
+        .can=BSP_CAN_2,.master_id=0x15,.can_id=0x05,.module=MOTOR_DM_J4310,.reverse=0
+      },
+      .pid.Ppid={
+        .k=1,
+        .p=0,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100
+      }
+    },
+
+    .joint[4]={
+      .angle_limit={-10,10},
+      .motor.type = DM_MOTOR,
+      .motor.param.dm_param={
+        .can=BSP_CAN_2,.master_id=0x16,.can_id=0x06,.module=MOTOR_DM_J4310,.reverse=0
+      },
+      .pid.Ppid={
+        .k=1,
+        .p=0,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100
+      }
+    },
+  }
+
+
+
+};
+
+
+
+
+
+/**
+ * @brief 获取机器人配置参数
+ * @return 机器人配置参数指针
+ */
+Config_RobotParam_t* Config_GetRobotParam(void) {
+    return &robot_config;
+}
--- a/engineer/User/module/config.h
+++ b/engineer/User/module/config.h
@ -2,6 +2,12 @@
 #include "main.h"
 #include "module/arm.h"

-typedef struct{
+typedef struct Config_RobotParam{
  arm_param_t arm_param;
 } Config_RobotParam_t;
+
+/**
+ * @brief 获取机器人配置参数
+ * @return 机器人配置参数指针
+ */
+Config_RobotParam_t* Config_GetRobotParam(void);
--- a/engineer/User/module/kine.cpp
+++ b/engineer/User/module/kine.cpp
@ -0,0 +1,17 @@
+#include "main.h"
+#include "./component/robotics.hpp"
+#include "kine.hpp"
+
+extern "C" void cesi()
+{
+  robotics::Link link[5];
+    link[0] = robotics::Link(0, 0     , 0      , PI / 2,  robotics::R, 0   ,  -100.57, 100.57);
+    link[1] = robotics::Link(0, 0     , 0.402  , 0,       robotics::R, 0   ,  -100.57, 100.57);
+    link[2] = robotics::Link(0, 0     , 0.1037 , -PI / 2, robotics::R, 1.57,  -100.57, 100.57);
+    link[3] = robotics::Link(0, 0.3159, 0,       PI / 2,  robotics::R,-1.57,  -100.57, 100.57);
+    link[4] = robotics::Link(0, 0     , 0,       -PI / 2, robotics::R, 0   ,  -100.57, 100.57);
+    // link[5] = robotics::Link(0, 0.05,   0,       0      , robotics::R, 0   ,  -100.57, 100.57);
+    static robotics::Serial_Link<5> p560(link);
+
+    Matrixf<4,4> t(p560.fkine((float[5]){0,0,0,0,0}));
+}
--- a/engineer/User/module/kine.hpp
+++ b/engineer/User/module/kine.hpp
@ -0,0 +1,9 @@
+#include "main.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+void cesi();
+#ifdef __cplusplus
+}
+#endif
--- a/engineer/User/task/control.c
+++ b/engineer/User/task/control.c
@ -11,6 +11,10 @@
 #include "device/motor_dm.h"
 #include "component/pid.h"
 #include "component/filter.h"
+#include "module/arm.h"
+#include "module/config.h"
+
+#include "module/kine.hpp"
 /* USER INCLUDE END */

 /* Private typedef ---------------------------------------------------------- */
@ -51,8 +55,8 @@ MOTOR_DM_t motor = {.param = {.can = BSP_CAN_1,
                              .reverse = false}};
 MOTOR_LZ_t lzmotor={
  .param ={.can = BSP_CAN_1,
-    .motor_id=126,
-    .host_id =128,
+    .motor_id=2,
+    .host_id =2,
    .module = MOTOR_LZ_RSO3,
    .reverse=0,
    .mode = MOTOR_LZ_MODE_MOTION
@ -92,6 +96,12 @@ LowPassFilter ff;


 MOTOR_LZ_t* a;
+
+
+
+
+
+arm_t arm;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
@ -102,9 +112,15 @@ void Task_control(void *argument) {
  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / CONTROL_FREQ;
-
  osDelay(CONTROL_INIT_DELAY); /* 延时一段时间再开启任务 */

+  BSP_CAN_Init();
+  Config_RobotParam_t* cfg_ptr = Config_GetRobotParam();
+  arm_init(&arm,cfg_ptr);
+//   MOTOR_LZ_Init();
+//   MOTOR_LZ_Register(&lzmotor.param);
+// MOTOR_LZ_Enable(&lzmotor.param);
+// a = MOTOR_LZ_GetMotor(&lzmotor.param);
  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */

@ -113,12 +129,17 @@ void Task_control(void *argument) {
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
-    
+
+    arm_gravity_calcu(&arm);
+    arm_control_apply(&arm);
+
+    arm_data_updata(&arm);
+
    // MOTOR_LZ_UpdateAll();
-    MOTOR_LZ_Update(&lzmotor.param);
-    MOTOR_LZ_MotionControl(&lzmotor.param, &control);
+    // MOTOR_LZ_Update(&lzmotor.param);
+    // MOTOR_LZ_MotionControl(&lzmotor.param, &control);
    // MOTOR_DM_Update(&motor.param);
-    //
+    // MOTOR_DM_UpdateAll();
    // MOTOR_DM_t *m = MOTOR_DM_GetMotor(&motor.param);
    // fb = m->motor.feedback;
    //
--- a/engineer/User/task/display.c
+++ b/engineer/User/task/display.c
@ -1,38 +0,0 @@
-/*
-    display Task
-    
-*/
-
-/* Includes ----------------------------------------------------------------- */
-#include "task/user_task.h"
-/* USER INCLUDE BEGIN */
-
-/* USER INCLUDE END */
-
-/* Private typedef ---------------------------------------------------------- */
-/* Private define ----------------------------------------------------------- */
-/* Private macro ------------------------------------------------------------ */
-/* Private variables -------------------------------------------------------- */
-/* USER STRUCT BEGIN */
-
-/* USER STRUCT END */
-
-/* Private function --------------------------------------------------------- */
-/* Exported functions ------------------------------------------------------- */
-void Task_display(void *argument) {
-  (void)argument; /* 未使用argument，消除警告 */
-
-  
-  osDelay(DISPLAY_INIT_DELAY); /* 延时一段时间再开启任务 */
-
-  /* USER CODE INIT BEGIN */
-
-  /* USER CODE INIT END */
-
-  while (1) {
-    /* USER CODE BEGIN */
-
-    /* USER CODE END */
-  }
-  
-}
--- a/engineer/User/task/init.c
+++ b/engineer/User/task/init.c
@ -7,6 +7,7 @@
 #include "task/user_task.h"

 /* USER INCLUDE BEGIN */
+#include "bsp/can.h"

 /* USER INCLUDE END */

@ -25,7 +26,6 @@
 void Task_Init(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
    /* USER CODE INIT BEGIN */
-
    /* USER CODE INIT END */
  osKernelLock(); /* 锁定内核，防止任务切换 */
  
--- a/engineer/cmake/stm32cubemx/CMakeLists.txt
+++ b/engineer/cmake/stm32cubemx/CMakeLists.txt
@ -26,7 +26,6 @@ set(MX_Application_Src
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c
-    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_it.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.c
@ -53,8 +52,7 @@ set(STM32_Drivers_Src
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c
-    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c
-    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c
 )

 # Drivers Midllewares