hehehe/engineer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

开始实际控制

Browse Source
This commit is contained in:
shentou 2025-12-28 16:14:11 +08:00
parent 0f6cf39492
commit bef6f60c34
26 changed files with 533 additions and 477 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
engineer/.mxproject
View File

File diff suppressed because one or more lines are too long

3
engineer/CMakeLists.txt
View File

@ -50,7 +50,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/bsp/mm.c User/bsp/mm.c
User/bsp/time.c User/bsp/time.c
User/bsp/dwt.c User/bsp/dwt.c
./User/bsp/uart.c # ./User/bsp/uart.c
# User/component sources # User/component sources
User/component/ahrs.c User/component/ahrs.c
@ -76,6 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# User/module sources # User/module sources
User/module/config.c User/module/config.c
User/module/arm.c User/module/arm.c
./User/module/kine.cpp
# User/task sources # User/task sources
User/task/control.c User/task/control.c

3
engineer/Core/Inc/can.h
View File

@ -34,11 +34,14 @@ extern "C" {
extern CAN_HandleTypeDef hcan1; extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
/* USER CODE BEGIN Private defines */ /* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */ /* USER CODE END Private defines */
void MX_CAN1_Init(void); void MX_CAN1_Init(void);
void MX_CAN2_Init(void);
/* USER CODE BEGIN Prototypes */ /* USER CODE BEGIN Prototypes */

2
engineer/Core/Inc/stm32f4xx_hal_conf.h
View File

@ -64,7 +64,7 @@
/* #define HAL_MMC_MODULE_ENABLED */ /* #define HAL_MMC_MODULE_ENABLED */
/* #define HAL_SPI_MODULE_ENABLED */ /* #define HAL_SPI_MODULE_ENABLED */
#define HAL_TIM_MODULE_ENABLED #define HAL_TIM_MODULE_ENABLED
#define HAL_UART_MODULE_ENABLED /* #define HAL_UART_MODULE_ENABLED */
/* #define HAL_USART_MODULE_ENABLED */ /* #define HAL_USART_MODULE_ENABLED */
/* #define HAL_IRDA_MODULE_ENABLED */ /* #define HAL_IRDA_MODULE_ENABLED */
/* #define HAL_SMARTCARD_MODULE_ENABLED */ /* #define HAL_SMARTCARD_MODULE_ENABLED */

5
engineer/Core/Inc/stm32f4xx_it.h
View File

@ -55,9 +55,10 @@ void DebugMon_Handler(void);
void CAN1_TX_IRQHandler(void); void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void); void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void); void CAN1_RX1_IRQHandler(void);
void USART1_IRQHandler(void);
void USART2_IRQHandler(void);
void TIM8_TRG_COM_TIM14_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 BEGIN EFP */
/* USER CODE END EFP */ /* USER CODE END EFP */

55
engineer/Core/Inc/usart.h
View File

@ -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__ */

106
engineer/Core/Src/can.c
View File

@ -25,6 +25,7 @@
/* USER CODE END 0 */ /* USER CODE END 0 */
CAN_HandleTypeDef hcan1; CAN_HandleTypeDef hcan1;
CAN_HandleTypeDef hcan2;
/* CAN1 init function */ /* CAN1 init function */
void MX_CAN1_Init(void) void MX_CAN1_Init(void)
@ -58,6 +59,40 @@ void MX_CAN1_Init(void)
/* USER CODE END CAN1_Init 2 */ /* 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) void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
{ {
@ -69,7 +104,10 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
/* USER CODE END CAN1_MspInit 0 */ /* USER CODE END CAN1_MspInit 0 */
/* CAN1 clock enable */ /* 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(); __HAL_RCC_GPIOD_CLK_ENABLE();
/**CAN1 GPIO Configuration /**CAN1 GPIO Configuration
@ -94,6 +132,41 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
/* USER CODE END CAN1_MspInit 1 */ /* 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) void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
@ -105,7 +178,10 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
/* USER CODE END CAN1_MspDeInit 0 */ /* USER CODE END CAN1_MspDeInit 0 */
/* Peripheral clock disable */ /* 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 /**CAN1 GPIO Configuration
PD0 ------> CAN1_RX PD0 ------> CAN1_RX
@ -121,6 +197,32 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
/* USER CODE END CAN1_MspDeInit 1 */ /* 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 */ /* USER CODE BEGIN 1 */

2
engineer/Core/Src/gpio.c
View File

@ -43,8 +43,8 @@ void MX_GPIO_Init(void)
{ {
/* GPIO Ports Clock Enable */ /* GPIO Ports Clock Enable */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE();

4
engineer/Core/Src/main.c
View File

@ -20,7 +20,6 @@
#include "main.h" #include "main.h"
#include "cmsis_os.h" #include "cmsis_os.h"
#include "can.h" #include "can.h"
#include "usart.h"
#include "gpio.h" #include "gpio.h"
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
@ -91,8 +90,7 @@ int main(void)
/* Initialize all configured peripherals */ /* Initialize all configured peripherals */
MX_GPIO_Init(); MX_GPIO_Init();
MX_CAN1_Init(); MX_CAN1_Init();
MX_USART1_UART_Init(); MX_CAN2_Init();
MX_USART2_UART_Init();
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
/* USER CODE END 2 */ /* USER CODE END 2 */

73
engineer/Core/Src/stm32f4xx_it.c
View File

@ -56,8 +56,7 @@
/* External variables --------------------------------------------------------*/ /* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1; extern CAN_HandleTypeDef hcan1;
extern UART_HandleTypeDef huart1; extern CAN_HandleTypeDef hcan2;
extern UART_HandleTypeDef huart2;
extern TIM_HandleTypeDef htim14; extern TIM_HandleTypeDef htim14;
/* USER CODE BEGIN EV */ /* USER CODE BEGIN EV */
@ -204,34 +203,6 @@ void CAN1_RX1_IRQHandler(void)
/* USER CODE END CAN1_RX1_IRQn 1 */ /* 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. * @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 */ /* 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 BEGIN 1 */
/* USER CODE END 1 */ /* USER CODE END 1 */

196
engineer/Core/Src/usart.c
View File

@ -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 */

65
engineer/DM.ioc
View File

@ -9,6 +9,13 @@ CAN1.CalculateTimeBit=1000
CAN1.CalculateTimeQuantum=71.42857142857143 CAN1.CalculateTimeQuantum=71.42857142857143
CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2 CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2
CAN1.Prescaler=3 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.IPParameters=Tasks01,configENABLE_FPU
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
FREERTOS.configENABLE_FPU=1 FREERTOS.configENABLE_FPU=1
@ -18,28 +25,25 @@ KeepUserPlacement=false
Mcu.CPN=STM32F407IGH6 Mcu.CPN=STM32F407IGH6
Mcu.Family=STM32F4 Mcu.Family=STM32F4
Mcu.IP0=CAN1 Mcu.IP0=CAN1
Mcu.IP1=FREERTOS Mcu.IP1=CAN2
Mcu.IP2=NVIC Mcu.IP2=FREERTOS
Mcu.IP3=RCC Mcu.IP3=NVIC
Mcu.IP4=SYS Mcu.IP4=RCC
Mcu.IP5=USART1 Mcu.IP5=SYS
Mcu.IP6=USART2 Mcu.IPNb=6
Mcu.IPNb=7
Mcu.Name=STM32F407I(E-G)Hx Mcu.Name=STM32F407I(E-G)Hx
Mcu.Package=UFBGA176 Mcu.Package=UFBGA176
Mcu.Pin0=PA14 Mcu.Pin0=PB5
Mcu.Pin1=PA13 Mcu.Pin1=PA14
Mcu.Pin10=VP_FREERTOS_VS_CMSIS_V2 Mcu.Pin2=PA13
Mcu.Pin11=VP_SYS_VS_tim14
Mcu.Pin2=PB7
Mcu.Pin3=PB6 Mcu.Pin3=PB6
Mcu.Pin4=PD6 Mcu.Pin4=PD0
Mcu.Pin5=PD0 Mcu.Pin5=PD1
Mcu.Pin6=PD5 Mcu.Pin6=PH0-OSC_IN
Mcu.Pin7=PD1 Mcu.Pin7=PH1-OSC_OUT
Mcu.Pin8=PH0-OSC_IN Mcu.Pin8=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin9=PH1-OSC_OUT Mcu.Pin9=VP_SYS_VS_tim14
Mcu.PinsNb=12 Mcu.PinsNb=10
Mcu.ThirdPartyNb=0 Mcu.ThirdPartyNb=0
Mcu.UserConstants= Mcu.UserConstants=
Mcu.UserName=STM32F407IGHx 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_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_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.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.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.ForceEnableDMAVector=true NVIC.ForceEnableDMAVector=true
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false 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.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn
NVIC.TimeBaseIP=TIM14 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 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
PA13.Mode=Serial_Wire PA13.Mode=Serial_Wire
PA13.Signal=SYS_JTMS-SWDIO PA13.Signal=SYS_JTMS-SWDIO
PA14.Mode=Serial_Wire PA14.Mode=Serial_Wire
PA14.Signal=SYS_JTCK-SWCLK PA14.Signal=SYS_JTCK-SWCLK
PB6.Mode=Asynchronous PB5.Mode=CAN_Activate
PB6.Signal=USART1_TX PB5.Signal=CAN2_RX
PB7.Mode=Asynchronous PB6.Mode=CAN_Activate
PB7.Signal=USART1_RX PB6.Signal=CAN2_TX
PD0.Locked=true PD0.Locked=true
PD0.Mode=CAN_Activate PD0.Mode=CAN_Activate
PD0.Signal=CAN1_RX PD0.Signal=CAN1_RX
PD1.Locked=true PD1.Locked=true
PD1.Mode=CAN_Activate PD1.Mode=CAN_Activate
PD1.Signal=CAN1_TX 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.Mode=HSE-External-Oscillator
PH0-OSC_IN.Signal=RCC_OSC_IN PH0-OSC_IN.Signal=RCC_OSC_IN
PH1-OSC_OUT.Mode=HSE-External-Oscillator PH1-OSC_OUT.Mode=HSE-External-Oscillator
@ -122,7 +123,7 @@ ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath= ProjectManager.UAScriptBeforePath=
ProjectManager.UnderRoot=false 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.48MHZClocksFreq_Value=84000000
RCC.AHBFreq_Value=168000000 RCC.AHBFreq_Value=168000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4 RCC.APB1CLKDivider=RCC_HCLK_DIV4
@ -156,10 +157,6 @@ RCC.VCOI2SOutputFreq_Value=384000000
RCC.VCOInputFreq_Value=2000000 RCC.VCOInputFreq_Value=2000000
RCC.VCOOutputFreq_Value=336000000 RCC.VCOOutputFreq_Value=336000000
RCC.VcooutputI2S=192000000 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.Mode=CMSIS_V2
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
VP_SYS_VS_tim14.Mode=TIM14 VP_SYS_VS_tim14.Mode=TIM14

2
engineer/User/bsp/bsp_config.yaml
View File

@ -2,6 +2,8 @@ can:
devices: devices:
- instance: CAN1 - instance: CAN1
name: '1' name: '1'
- instance: CAN2
name: '2'
enabled: true enabled: true
dwt: dwt:
enabled: true enabled: true

23
engineer/User/bsp/can.c
View File

@ -66,6 +66,8 @@ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
if (hcan->Instance == CAN1) if (hcan->Instance == CAN1)
return BSP_CAN_1; return BSP_CAN_1;
else if (hcan->Instance == CAN2)
return BSP_CAN_2;
else else
return BSP_CAN_ERR; 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); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN1中断 // 激活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; inited = true;
@ -484,6 +503,8 @@ CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
switch (can) { switch (can) {
case BSP_CAN_1: case BSP_CAN_1:
return &hcan1; return &hcan1;
case BSP_CAN_2:
return &hcan2;
default: default:
return NULL; return NULL;
} }

12
engineer/User/device/motor_dm.c
View File

@ -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.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]); motor->motor.feedback.temp = (float)(data[6]);
while (motor->motor.feedback.rotor_abs_angle < 0) { // while (motor->motor.feedback.rotor_abs_angle < 0) {
motor->motor.feedback.rotor_abs_angle += M_2PI; // motor->motor.feedback.rotor_abs_angle += M_2PI;
} // }
while (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; // motor->motor.feedback.rotor_abs_angle -= M_2PI;
} // }
if (motor->param.reverse) { if (motor->param.reverse) {
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle; motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;

14
engineer/User/device/motor_lz.c
View File

@ -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]); uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM); float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
while (angle <0){ // while (angle <0){
angle += M_2PI; // angle += M_2PI;
} // }
while (angle > M_2PI){ // while (angle > M_2PI){
angle -= M_2PI; // angle -= M_2PI;
} // }
// 自动反向 // 自动反向
if (motor->param.reverse) { if (motor->param.reverse) {
angle = M_2PI - angle; angle = - angle;
velocity = -velocity; velocity = -velocity;
torque = -torque; torque = -torque;
} }

176
engineer/User/module/arm.c
View File

@ -1,59 +1,121 @@
#include "module/arm.h" #include "module/arm.h"
#include "module/config.h"
#include "device/motor_dm.h" #include "device/motor_dm.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
#include "component/pid.h" #include "component/pid.h"
#include "component/user_math.h" #include "component/user_math.h"
#include "bsp/dwt.h" #include "bsp/dwt.h"
#include "component/cmd.h" #include "component/cmd.h"
#include "module/config.h"
//用于机械臂重力补偿 lxqx:linkx质心位置对qx的偏导数 由sympy符号计算库计算生成 //用于机械臂重力补偿 lxqx:linkx质心位置对qx的偏导数 由sympy符号计算库计算生成
double compute_l5q2(double q1, double q2, double q3, double q4, double q5) { // 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); // 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) { double compute_l4q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
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); 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) { double compute_l3q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
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); 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) { double compute_l2q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
q3+=0.01; return 1.1252*9.81*0.26277*cos(q2);
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); //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) { double compute_l4q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
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); 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) { double compute_l3q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
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); 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) {
//joint4
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_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) void arm_init(arm_t* arm,Config_RobotParam_t *cfg)
{ {
assert_param(!(cfg==NULL||arm==NULL)); assert_param(!(cfg==NULL||arm==NULL));
MOTOR_LZ_Init();
for (int i=0; i<JOINT_NUM; i++) { for (int i=0; i<JOINT_NUM; i++) {
switch (cfg->arm_param.joint[i].motor.type) { 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); 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[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.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]; 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) void arm_gravity_calcu(arm_t* arm)
{ {
assert_param(!(arm==NULL)); assert_param(!(arm==NULL));
arm->joint[4].ref.torque += -1 * arm->joint[3].ref.torque += (float)q5t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
compute_l5q5(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position, arm->joint[3].feedback.position,0,0);
arm->joint[5].feedback.position) * arm->joint[2].ref.torque += (float)q4t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
0.403 * 9.81; arm->joint[3].feedback.position,0,0);
arm->joint[3].ref.torque += -1 * arm->joint[1].ref.torque += (float)q3t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
compute_l5q4(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position, arm->joint[3].feedback.position,0,0);
arm->joint[5].feedback.position) * arm->joint[0].ref.torque += (float)q2t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
0.403 * 9.81; arm->joint[3].feedback.position,0,0);
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;
} }
@ -127,21 +169,22 @@ void arm_data_updata(arm_t* arm)
float nspeed=0; 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); 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); 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) // if(nangle- arm->joint[i].status.last_angle >M_2PI/2)
{ // {
arm->joint[i].status.circles++; // arm->joint[i].status.circles++;
} // }
else if(nangle- arm->joint[i].status.last_angle < -M_2PI/2) // else if(nangle- arm->joint[i].status.last_angle < -M_2PI/2)
{ // {
arm->joint[i].status.circles--; // arm->joint[i].status.circles--;
} // }
arm->joint[i].status.last_angle = nangle; arm->joint[i].status.last_angle = nangle;
arm->joint[i].feedback.velocity = nspeed; 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[1].feedback.velocity = arm->joint[1].feedback.velocity - arm->joint[0].feedback.velocity;
arm->joint[2].feedback.position = arm->joint[2].feedback.position - arm->joint[1].feedback.position; 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)); assert_param(!(arm==NULL));
for (uint8_t i=0; i<JOINT_NUM; i++) { for (uint8_t i=0; i<JOINT_NUM; i++) {
assert_param(!(arm->joint[i].motor.handle == NULL)); 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]) 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; arm->joint[i].ref.torque=0;

21
engineer/User/module/arm.h
View File

@ -1,19 +1,18 @@
#pragma once #pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h" #include "main.h"
#include "component/kinematics.h" #include "component/kinematics.h"
#include "device/motor_dm.h" #include "device/motor_dm.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
#include "component/pid.h" #include "component/pid.h"
#define ga 9.81 #define ga 9.81
#define __dm_handle(handle) (MOTOR_DM_t *)handle #define __dm_handle(handle) (MOTOR_DM_t *)handle
#define __lz_handle(handle) (MOTOR_LZ_t *)handle #define __lz_handle(handle) (MOTOR_LZ_t *)handle
#define JOINT_NUM 6 #define JOINT_NUM 5
typedef enum{ typedef enum{
@ -56,6 +55,7 @@ typedef struct {
struct { struct {
float angle_limit[2]; float angle_limit[2];
float init_angle;
float last_angle; float last_angle;
int circles; int circles;
float last_control_time; float last_control_time;
@ -82,6 +82,7 @@ typedef struct{
KPID_Params_t Vpid; KPID_Params_t Vpid;
}pid; }pid;
float angle_limit[2]; float angle_limit[2];
float init_angle;
}joint[JOINT_NUM]; }joint[JOINT_NUM];
DH_parameter_t dh_param[6]; DH_parameter_t dh_param[6];
float mass[6]; float mass[6];
@ -106,11 +107,11 @@ typedef struct hand_t
TAKE_MODE, TAKE_MODE,
POSITION_MODE POSITION_MODE
}mode; }mode;
}arm_t; }arm_t;
#ifdef __cplusplus
} typedef struct Config_RobotParam Config_RobotParam_t;
#endif 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);

111
engineer/User/module/config.c
View File

@ -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;
}

8
engineer/User/module/config.h
View File

@ -2,6 +2,12 @@
#include "main.h" #include "main.h"
#include "module/arm.h" #include "module/arm.h"
typedef struct{ typedef struct Config_RobotParam{
arm_param_t arm_param; arm_param_t arm_param;
} Config_RobotParam_t; } Config_RobotParam_t;
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t* Config_GetRobotParam(void);

17
engineer/User/module/kine.cpp Normal file
View File

@ -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}));
}

9
engineer/User/module/kine.hpp Normal file
View File

@ -0,0 +1,9 @@
#include "main.h"
#ifdef __cplusplus
extern "C" {
#endif
void cesi();
#ifdef __cplusplus
}
#endif

35
engineer/User/task/control.c
View File

@ -11,6 +11,10 @@
#include "device/motor_dm.h" #include "device/motor_dm.h"
#include "component/pid.h" #include "component/pid.h"
#include "component/filter.h" #include "component/filter.h"
#include "module/arm.h"
#include "module/config.h"
#include "module/kine.hpp"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -51,8 +55,8 @@ MOTOR_DM_t motor = {.param = {.can = BSP_CAN_1,
.reverse = false}}; .reverse = false}};
MOTOR_LZ_t lzmotor={ MOTOR_LZ_t lzmotor={
.param ={.can = BSP_CAN_1, .param ={.can = BSP_CAN_1,
.motor_id=126, .motor_id=2,
.host_id =128, .host_id =2,
.module = MOTOR_LZ_RSO3, .module = MOTOR_LZ_RSO3,
.reverse=0, .reverse=0,
.mode = MOTOR_LZ_MODE_MOTION .mode = MOTOR_LZ_MODE_MOTION
@ -92,6 +96,12 @@ LowPassFilter ff;
MOTOR_LZ_t* a; MOTOR_LZ_t* a;
arm_t arm;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -102,9 +112,15 @@ void Task_control(void *argument) {
/* 计算任务运行到指定频率需要等待的tick数 */ /* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / CONTROL_FREQ; const uint32_t delay_tick = osKernelGetTickFreq() / CONTROL_FREQ;
osDelay(CONTROL_INIT_DELAY); /* 延时一段时间再开启任务 */ 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(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
@ -113,12 +129,17 @@ void Task_control(void *argument) {
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
arm_gravity_calcu(&arm);
arm_control_apply(&arm);
arm_data_updata(&arm);
// MOTOR_LZ_UpdateAll(); // MOTOR_LZ_UpdateAll();
MOTOR_LZ_Update(&lzmotor.param); // MOTOR_LZ_Update(&lzmotor.param);
MOTOR_LZ_MotionControl(&lzmotor.param, &control); // MOTOR_LZ_MotionControl(&lzmotor.param, &control);
// MOTOR_DM_Update(&motor.param); // MOTOR_DM_Update(&motor.param);
// // MOTOR_DM_UpdateAll();
// MOTOR_DM_t *m = MOTOR_DM_GetMotor(&motor.param); // MOTOR_DM_t *m = MOTOR_DM_GetMotor(&motor.param);
// fb = m->motor.feedback; // fb = m->motor.feedback;
// //

38
engineer/User/task/display.c
View File

@ -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 */
}
}

2
engineer/User/task/init.c
View File

@ -7,6 +7,7 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/can.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
@ -25,7 +26,6 @@
void Task_Init(void *argument) { void Task_Init(void *argument) {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
/* USER CODE INIT END */ /* USER CODE INIT END */
osKernelLock(); /* 锁定内核，防止任务切换 */ osKernelLock(); /* 锁定内核，防止任务切换 */

4
engineer/cmake/stm32cubemx/CMakeLists.txt
View File

@ -26,7 +26,6 @@ set(MX_Application_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.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_it.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.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_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_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.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_exti.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
) )
# Drivers Midllewares # Drivers Midllewares