From a9bcaf326f1b179ba5027663ad05e768107d0ce2 Mon Sep 17 00:00:00 2001 From: iDddddd <90831551+iDddddd@users.noreply.github.com> Date: Fri, 22 Sep 2023 14:39:13 +0800 Subject: [PATCH 1/4] version3.5.17 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 删除任务队列 修改步进电机V5发送消息 修改can2发送逻辑 --- MDK-ARM/RM_Frame_C.uvoptx | 82 +++++++++---------- MDK-ARM/RM_Frame_C.uvprojx | 10 --- RM_Frame_C.ioc | 8 +- Src/dma.c | 2 +- Src/usart.c | 6 +- userCode/devices/Inc/ARMMotor.h | 5 +- userCode/devices/Inc/CommuType.h | 2 +- userCode/devices/Inc/ManiControl.h | 21 ++++- userCode/devices/Inc/StateMachine.h | 37 --------- userCode/devices/Src/ARMMotor.cpp | 108 +++++++++----------------- userCode/devices/Src/ChassisMotor.cpp | 2 +- userCode/devices/Src/CommuType.cpp | 50 +++++------- userCode/devices/Src/Device.cpp | 4 +- userCode/devices/Src/ManiControl.cpp | 34 +++++--- userCode/devices/Src/StateMachine.cpp | 74 ------------------ userCode/tasks/Inc/ArmTask.h | 14 +++- userCode/tasks/Inc/AutoTask.h | 29 ------- userCode/tasks/Inc/ControlTask.h | 1 - userCode/tasks/Src/ArmTask.cpp | 36 +++++---- userCode/tasks/Src/AutoTask.cpp | 33 -------- userCode/tasks/Src/ControlTask.cpp | 56 ++----------- 21 files changed, 186 insertions(+), 428 deletions(-) delete mode 100644 userCode/devices/Inc/StateMachine.h delete mode 100644 userCode/devices/Src/StateMachine.cpp delete mode 100644 userCode/tasks/Inc/AutoTask.h delete mode 100644 userCode/tasks/Src/AutoTask.cpp diff --git a/MDK-ARM/RM_Frame_C.uvoptx b/MDK-ARM/RM_Frame_C.uvoptx index d87ad85..ff79dff 100644 --- a/MDK-ARM/RM_Frame_C.uvoptx +++ b/MDK-ARM/RM_Frame_C.uvoptx @@ -162,6 +162,22 @@ 0 0 + 16 + 1 +
0
+ 0 + 0 + 0 + 0 + 0 + 0 + C:\Users\25396\Documents\GitHub\Swerve-chassis-control-frame-1\userCode\tasks\Src\AutoTask.cpp + + + + + 1 + 0 245 1
134226518
@@ -1164,18 +1180,6 @@ 0 0 0 - ..\userCode\tasks\Src\AutoTask.cpp - AutoTask.cpp - 0 - 0 - - - 6 - 55 - 8 - 0 - 0 - 0 ..\userCode\tasks\Src\Matrix.cpp Matrix.cpp 0 @@ -1183,7 +1187,7 @@ 6 - 56 + 55 8 0 0 @@ -1203,7 +1207,7 @@ 0 7 - 57 + 56 8 0 0 @@ -1215,7 +1219,7 @@ 7 - 58 + 57 8 0 0 @@ -1227,7 +1231,7 @@ 7 - 59 + 58 8 0 0 @@ -1239,7 +1243,7 @@ 7 - 60 + 59 8 0 0 @@ -1251,7 +1255,7 @@ 7 - 61 + 60 8 0 0 @@ -1263,7 +1267,7 @@ 7 - 62 + 61 8 0 0 @@ -1275,7 +1279,7 @@ 7 - 63 + 62 8 0 0 @@ -1287,7 +1291,7 @@ 7 - 64 + 63 8 0 0 @@ -1299,7 +1303,7 @@ 7 - 65 + 64 8 0 0 @@ -1311,7 +1315,7 @@ 7 - 66 + 65 8 0 0 @@ -1323,19 +1327,7 @@ 7 - 67 - 8 - 0 - 0 - 0 - ..\userCode\devices\Src\StateMachine.cpp - StateMachine.cpp - 0 - 0 - - - 7 - 68 + 66 8 0 0 @@ -1347,7 +1339,7 @@ 7 - 69 + 67 8 0 0 @@ -1359,7 +1351,7 @@ 7 - 70 + 68 8 0 0 @@ -1379,7 +1371,7 @@ 0 8 - 71 + 69 8 0 0 @@ -1391,7 +1383,7 @@ 8 - 72 + 70 8 0 0 @@ -1411,7 +1403,7 @@ 0 9 - 73 + 71 8 0 0 @@ -1431,7 +1423,7 @@ 0 10 - 74 + 72 8 0 0 @@ -1443,7 +1435,7 @@ 10 - 75 + 73 8 0 0 @@ -1463,7 +1455,7 @@ 0 11 - 76 + 74 8 0 0 @@ -1475,7 +1467,7 @@ 11 - 77 + 75 8 0 0 diff --git a/MDK-ARM/RM_Frame_C.uvprojx b/MDK-ARM/RM_Frame_C.uvprojx index 77a27e9..50d893b 100644 --- a/MDK-ARM/RM_Frame_C.uvprojx +++ b/MDK-ARM/RM_Frame_C.uvprojx @@ -726,11 +726,6 @@ 8 ..\userCode\tasks\Src\ServoTask.cpp - - AutoTask.cpp - 8 - ..\userCode\tasks\Src\AutoTask.cpp - Matrix.cpp 8 @@ -796,11 +791,6 @@ 8 ..\userCode\devices\Src\ManiControl.cpp - - StateMachine.cpp - 8 - ..\userCode\devices\Src\StateMachine.cpp - LED.cpp 8 diff --git a/RM_Frame_C.ioc b/RM_Frame_C.ioc index b11e743..14e777b 100644 --- a/RM_Frame_C.ioc +++ b/RM_Frame_C.ioc @@ -72,7 +72,7 @@ Dma.USART1_RX.5.MemInc=DMA_MINC_ENABLE Dma.USART1_RX.5.Mode=DMA_NORMAL Dma.USART1_RX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE Dma.USART1_RX.5.PeriphInc=DMA_PINC_DISABLE -Dma.USART1_RX.5.Priority=DMA_PRIORITY_MEDIUM +Dma.USART1_RX.5.Priority=DMA_PRIORITY_HIGH Dma.USART1_RX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode Dma.USART3_RX.0.Direction=DMA_PERIPH_TO_MEMORY Dma.USART3_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE @@ -92,7 +92,7 @@ Dma.USART6_RX.4.MemInc=DMA_MINC_ENABLE Dma.USART6_RX.4.Mode=DMA_NORMAL Dma.USART6_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE Dma.USART6_RX.4.PeriphInc=DMA_PINC_DISABLE -Dma.USART6_RX.4.Priority=DMA_PRIORITY_MEDIUM +Dma.USART6_RX.4.Priority=DMA_PRIORITY_HIGH Dma.USART6_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode File.Version=6 I2C3.I2C_Mode=I2C_Fast @@ -192,7 +192,7 @@ NVIC.CAN2_TX_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:false\:false\:true\:true -NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream1_IRQn=true\:1\:0\:true\:false\:true\:false\:true\:true NVIC.DMA2_Stream2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true NVIC.DMA2_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false @@ -213,7 +213,7 @@ NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false NVIC.TIM1_UP_TIM10_IRQn=true\:10\:0\:true\:false\:true\:true\:true\:true NVIC.TIM6_DAC_IRQn=true\:8\:0\:true\:false\:true\:true\:true\:true NVIC.TIM7_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true -NVIC.USART1_IRQn=true\:1\:0\:true\:false\:false\:true\:true\:true +NVIC.USART1_IRQn=true\:0\:0\:true\:false\:false\:true\:true\:true NVIC.USART3_IRQn=true\:0\:0\:true\:false\:false\:true\:true\:true NVIC.USART6_IRQn=true\:1\:0\:true\:false\:false\:true\:true\:true NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false diff --git a/Src/dma.c b/Src/dma.c index c19f89e..9cfb391 100644 --- a/Src/dma.c +++ b/Src/dma.c @@ -54,7 +54,7 @@ void MX_DMA_Init(void) HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn); /* DMA2_Stream1_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 1, 0); HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn); /* DMA2_Stream2_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 0, 0); diff --git a/Src/usart.c b/Src/usart.c index 1bd7a56..ade6d66 100644 --- a/Src/usart.c +++ b/Src/usart.c @@ -161,7 +161,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) hdma_usart1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; hdma_usart1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; hdma_usart1_rx.Init.Mode = DMA_NORMAL; - hdma_usart1_rx.Init.Priority = DMA_PRIORITY_MEDIUM; + hdma_usart1_rx.Init.Priority = DMA_PRIORITY_HIGH; hdma_usart1_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; if (HAL_DMA_Init(&hdma_usart1_rx) != HAL_OK) { @@ -171,7 +171,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart1_rx); /* USART1 interrupt Init */ - HAL_NVIC_SetPriority(USART1_IRQn, 1, 0); + HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspInit 1 */ @@ -253,7 +253,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) hdma_usart6_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; hdma_usart6_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; hdma_usart6_rx.Init.Mode = DMA_NORMAL; - hdma_usart6_rx.Init.Priority = DMA_PRIORITY_MEDIUM; + hdma_usart6_rx.Init.Priority = DMA_PRIORITY_HIGH; hdma_usart6_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; if (HAL_DMA_Init(&hdma_usart6_rx) != HAL_OK) { diff --git a/userCode/devices/Inc/ARMMotor.h b/userCode/devices/Inc/ARMMotor.h index 0f9c7a6..2f6f7c8 100644 --- a/userCode/devices/Inc/ARMMotor.h +++ b/userCode/devices/Inc/ARMMotor.h @@ -26,7 +26,6 @@ class SteppingMotor_v4 : public Motor, public CAN { void Handle() override; - void MoveTo(); void SetTargetPosition(float pos); @@ -57,8 +56,6 @@ class SteppingMotor_v5 : public Motor, public CAN { void Handle() override; - void MoveTo(); - void SetTargetPosition(float tarpos); ~SteppingMotor_v5(); @@ -69,7 +66,7 @@ class SteppingMotor_v5 : public Motor, public CAN { uint32_t Pulse{}; bool SendFlag = false; - uint8_t TxMessage[16]{0}; + uint8_t TxMessage[8]{0}; uint8_t TxMessageDLC{}; int32_t nowPos = 0; diff --git a/userCode/devices/Inc/CommuType.h b/userCode/devices/Inc/CommuType.h index 28a2625..4858dd4 100644 --- a/userCode/devices/Inc/CommuType.h +++ b/userCode/devices/Inc/CommuType.h @@ -25,7 +25,7 @@ typedef struct { uint32_t ID; uint8_t DLC; uint8_t canType; - uint8_t message[16]; + uint8_t message[8]; }DATA_t; typedef struct { DATA_t Data[MAX_MESSAGE_COUNT]; diff --git a/userCode/devices/Inc/ManiControl.h b/userCode/devices/Inc/ManiControl.h index b3eff80..8d74d7a 100644 --- a/userCode/devices/Inc/ManiControl.h +++ b/userCode/devices/Inc/ManiControl.h @@ -5,8 +5,8 @@ #ifndef RM_FRAME_C_MANICONTROL_H #define RM_FRAME_C_MANICONTROL_H #include "Device.h" -#include "StateMachine.h" -#include "AutoTask.h" + + #define BUFF_SIZE 40u #define CONTROL_LENGTH 0x10 #define MANI_LENGTH 0x0E @@ -18,7 +18,7 @@ typedef enum { CLAW, TRAY, MOVE_VEL, - + ARM_POS }TASK_FLAG_t; /*结构体定义--------------------------------------------------------------*/ @@ -30,6 +30,11 @@ typedef struct { f_u8_t Joint4Pos; f_u8_t Joint5Pos; } ARM_col_t; +typedef struct { + f_u8_t x; + f_u8_t y; + f_u8_t z; +} ARM_pos_t; typedef struct { f_u8_t x_Dis; f_u8_t y_Dis; @@ -42,6 +47,7 @@ typedef struct { } ChassisVel_col_t; typedef struct { ARM_col_t arm_col; + ARM_pos_t arm_pos; ChassisDis_col_t chassisDis_col; ChassisVel_col_t chassisVel_col; uint8_t TrayFlag; @@ -63,6 +69,15 @@ class ManiControl : public Device { void CompleteTask();//完成任务反馈函数,一般为向上位机发送数据0x01 uint8_t LRC_calc(uint8_t *data, uint8_t len);//LRC校验函数 +/*外部函数声明-------------------------------------------------------------*/ +extern void AutoChassisStop();//Realized in ChassisTask +extern void ChassisDistanceSet(float x, float y, float o);//Realized in ChassisTask +extern void ChassisVelocitySet(float x_vel, float y_vel, float w_vel);//Realized in ChassisTask +extern void ArmJointSet(float Joint1Pos, float Joint2Pos, float Joint3Pos, float Joint4Pos, float Joint5Pos); +extern void ArmPositionSet(float x, float y, float z);//Realized in ArmJointTask +//extern void AutoTraySet(uint8_t trayflag);//Realized in ArmJointTask +extern void ClawSet(uint8_t clawflag);//Realized in ArmJointTask + //以下为直接调用UART6中断函数 #ifdef __cplusplus diff --git a/userCode/devices/Inc/StateMachine.h b/userCode/devices/Inc/StateMachine.h deleted file mode 100644 index 532126b..0000000 --- a/userCode/devices/Inc/StateMachine.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// Created by 25396 on 2023/4/6. -// - -#ifndef RM_FRAME_C_STATEMACHINE_H -#define RM_FRAME_C_STATEMACHINE_H - -// 函数指针链表节点 -class FunctionNode { -public: - explicit FunctionNode(void (*f)()) : func(f), next(nullptr) {} - void (*func)(); - FunctionNode* next; -}; -// 函数指针链表 -class FunctionList { -public: - FunctionList() : head(nullptr) {} - void add(void (*f)()); - void remove(void (*f)()); - void call_all() ; -private: - FunctionNode* head; -}; -// 有限状态机 -class StateMachine { -public: - StateMachine(); - static void stateHandle() ; - static void add_function_to_state(void (*f)()); - static void remove_function_from_state(void (*f)()); -private: - static FunctionList function_lists; -}; - - -#endif //RM_FRAME_C_STATEMACHINE_H diff --git a/userCode/devices/Src/ARMMotor.cpp b/userCode/devices/Src/ARMMotor.cpp index d355eeb..fdd9248 100644 --- a/userCode/devices/Src/ARMMotor.cpp +++ b/userCode/devices/Src/ARMMotor.cpp @@ -38,44 +38,35 @@ void SteppingMotor_v4::CANMessageGenerate() { } void SteppingMotor_v4::Handle() { - TxMessage[0] = 0x36; - TxMessage[1] = 0x6B; - TxMessageDLC = 0x02; - - nowPos = (int32_t) (RxMessage[1] << 24u | RxMessage[2] << 16u | RxMessage[3] << 8u | RxMessage[4]); - NowPos = ((float) nowPos * 2.0f * PI) / 65536.0f / reductionRatio; - - CANMessageGenerate(); -} - -void SteppingMotor_v4::MoveTo() { - - if (stopFlag) { - Pulse = 0; - } else { - if (TarPos >= Position) { - Direction = 0x11; + if(SendFlag) { + if (stopFlag) { + Pulse = 0; } else { - Direction = 0x01; + if (TarPos >= Position) { + Direction = 0x11; + } else { + Direction = 0x01; + } + Pulse = (uint32_t) (abs(TarPos - Position) / 2.0f / PI * 200 * 16 * reductionRatio); + Position = TarPos; } - Pulse = (uint32_t) (abs(TarPos - Position) / 2.0f / PI * 200 * 16 * reductionRatio); - Position = TarPos; + TxMessageDLC = 0x08; + TxMessage[0] = 0xFD; + TxMessage[1] = Direction; + TxMessage[2] = 0xFF; + TxMessage[3] = 0x00; + TxMessage[4] = Pulse >> 16u; + TxMessage[5] = Pulse >> 8u; + TxMessage[6] = Pulse; + TxMessage[7] = 0x6B; + + CANMessageGenerate(); } - TxMessageDLC = 0x08; - TxMessage[0] = 0xFD; - TxMessage[1] = Direction; - TxMessage[2] = 0xFF; - TxMessage[3] = 0x00; - TxMessage[4] = Pulse >> 16u; - TxMessage[5] = Pulse >> 8u; - TxMessage[6] = Pulse; - TxMessage[7] = 0x6B; - - CANMessageGenerate(); } void SteppingMotor_v4::SetTargetPosition(float pos) { stopFlag = false; + SendFlag = true; TarPos = pos; } @@ -105,11 +96,6 @@ void SteppingMotor_v5::CANMessageGenerate() {; canQueue.Data[canQueue.rear].message[5] = TxMessage[5]; canQueue.Data[canQueue.rear].message[6] = TxMessage[6]; canQueue.Data[canQueue.rear].message[7] = TxMessage[7]; - canQueue.Data[canQueue.rear].message[8] = TxMessage[8]; - canQueue.Data[canQueue.rear].message[9] = TxMessage[9]; - canQueue.Data[canQueue.rear].message[10] = TxMessage[10]; - canQueue.Data[canQueue.rear].message[11] = TxMessage[11]; - canQueue.Data[canQueue.rear].message[12] = TxMessage[12]; canQueue.rear = (canQueue.rear + 1) % MAX_MESSAGE_COUNT; } else { @@ -136,12 +122,12 @@ void SteppingMotor_v5::Handle() { } else { if (TarPos >= 0) { - Direction = 0x01; - } else { Direction = 0x00; + } else { + Direction = 0x01; } Pulse = (uint32_t) (abs(TarPos) / 2.0f / PI * 200.0f * 16.0f * reductionRatio); - TxMessageDLC = 0x0D; + TxMessageDLC = 0x08; TxMessage[0] = 0xFD; TxMessage[1] = Direction; TxMessage[2] = 0x05; @@ -150,46 +136,22 @@ void SteppingMotor_v5::Handle() { TxMessage[5] = Pulse >> 24u; TxMessage[6] = Pulse >> 16u; TxMessage[7] = Pulse >> 8u; - TxMessage[8] = 0xFD; - TxMessage[9] = Pulse; - TxMessage[10] = 0x01; - TxMessage[11] = 0x00; - TxMessage[12] = 0x6B; + CANMessageGenerate(); + TxMessageDLC = 0x05; + TxMessage[0] = 0xFD; + TxMessage[1] = Pulse; + TxMessage[2] = 0x01; + TxMessage[3] = 0x00; + TxMessage[4] = 0x6B; + TxMessage[5] = 0x00; + TxMessage[6] = 0x00; + TxMessage[7] = 0x00; CANMessageGenerate(); SendFlag = false; } } } -void SteppingMotor_v5::MoveTo() { - if (stopFlag) { - Pulse = 0; - } else { - if (TarPos >= 0) { - Direction = 0x01; - } else { - Direction = 0x00; - } - Pulse = (uint32_t) (abs(TarPos) / 2.0f / PI * 200.0f * 16.0f * reductionRatio); - } - TxMessageDLC = 0x0D; - TxMessage[0] = 0xFD; - TxMessage[1] = Direction; - TxMessage[2] = 0x05; - TxMessage[3] = 0xDC; - TxMessage[4] = 0xC8; - TxMessage[5] = Pulse >> 24u; - TxMessage[6] = Pulse >> 16u; - TxMessage[7] = Pulse >> 8u; - TxMessage[8] = 0xFD; - TxMessage[9] = Pulse; - TxMessage[10] = 0x01; - TxMessage[11] = 0x00; - TxMessage[12] = 0x6B; - - CANMessageGenerate(); -} - void SteppingMotor_v5::SetTargetPosition(float tarpos) { stopFlag = false; SendFlag = true; diff --git a/userCode/devices/Src/ChassisMotor.cpp b/userCode/devices/Src/ChassisMotor.cpp index f220a8a..a273d73 100644 --- a/userCode/devices/Src/ChassisMotor.cpp +++ b/userCode/devices/Src/ChassisMotor.cpp @@ -155,7 +155,7 @@ void Motor_4010::Handle() { } - CANMessageGenerate(); + // CANMessageGenerate(); } void Motor_4010::MotorStateUpdate() { diff --git a/userCode/devices/Src/CommuType.cpp b/userCode/devices/Src/CommuType.cpp index 0601d0c..c8302ed 100644 --- a/userCode/devices/Src/CommuType.cpp +++ b/userCode/devices/Src/CommuType.cpp @@ -78,32 +78,19 @@ void CAN::CANPackageSend() { HAL_CAN_AddTxMessage(&hcan1, &txHeaderTypeDef, canQueue.Data[canQueue.front].message, &box); } else if (canQueue.Data[canQueue.front].canType == can2) { - if (canQueue.Data[canQueue.front].DLC <= 0x08) { - txHeaderTypeDef.ExtId = canQueue.Data[canQueue.front].ID;//从消息包中取出对应的ID - txHeaderTypeDef.DLC = canQueue.Data[canQueue.front].DLC;//数据长度 - txHeaderTypeDef.IDE = CAN_ID_EXT;//扩展帧 - txHeaderTypeDef.RTR = CAN_RTR_DATA;//数据帧 - txHeaderTypeDef.TransmitGlobalTime = DISABLE; - HAL_CAN_AddTxMessage(&hcan2, &txHeaderTypeDef, canQueue.Data[canQueue.front].message, &box); - } else { - txHeaderTypeDef.ExtId = canQueue.Data[canQueue.front].ID; - txHeaderTypeDef.DLC = 0x08; - txHeaderTypeDef.IDE = CAN_ID_EXT; - txHeaderTypeDef.RTR = CAN_RTR_DATA; - txHeaderTypeDef.TransmitGlobalTime = DISABLE; - HAL_CAN_AddTxMessage(&hcan2, &txHeaderTypeDef, canQueue.Data[canQueue.front].message, &box); - txHeaderTypeDef.ExtId = canQueue.Data[canQueue.front].ID + 1; - txHeaderTypeDef.DLC = canQueue.Data[canQueue.front].DLC - 0x08; - txHeaderTypeDef.IDE = CAN_ID_EXT; - txHeaderTypeDef.RTR = CAN_RTR_DATA; - txHeaderTypeDef.TransmitGlobalTime = DISABLE; - HAL_CAN_AddTxMessage(&hcan2, &txHeaderTypeDef, canQueue.Data[canQueue.front].message + 8, &box); - } + txHeaderTypeDef.StdId = canQueue.Data[canQueue.front].ID;//从消息包中取出对应的ID + txHeaderTypeDef.DLC = canQueue.Data[canQueue.front].DLC;//数据长度 + txHeaderTypeDef.IDE = CAN_ID_EXT;//标准帧 + txHeaderTypeDef.RTR = CAN_RTR_DATA;//数据帧 + txHeaderTypeDef.TransmitGlobalTime = DISABLE;//时间戳 + + HAL_CAN_AddTxMessage(&hcan2, &txHeaderTypeDef, canQueue.Data[canQueue.front].message, &box); } - memset(canQueue.Data[canQueue.front].message, 0 , sizeof(canQueue.Data[canQueue.front].message));//清空消息包中的数据 - canQueue.front = (canQueue.front + 1) % MAX_MESSAGE_COUNT;//消息队列头指针后移 + } + memset(canQueue.Data[canQueue.front].message, 0, sizeof(canQueue.Data[canQueue.front].message));//清空消息包中的数据 + canQueue.front = (canQueue.front + 1) % MAX_MESSAGE_COUNT;//消息队列头指针后移 } /** @@ -169,7 +156,7 @@ void RS485::RS485Init() { //内存缓冲区 2 hdma_usart1_rx.Instance->M1AR = (uint32_t) (rs485_rx_buff[1]); //数据长度 - hdma_usart1_rx.Instance->NDTR = RX_SIZE;//不确定需不需要 + hdma_usart1_rx.Instance->NDTR = RX_SIZE; //使能双缓冲区 CLEAR_BIT(hdma_usart1_rx.Instance->CR, DMA_SxCR_DBM); SET_BIT(hdma_usart1_rx.Instance->CR, DMA_SxCR_CIRC); @@ -182,7 +169,7 @@ void RS485::Rx_Handle() { { __HAL_UART_CLEAR_PEFLAG(&huart1); } else if (USART1->SR & UART_FLAG_IDLE) { - static uint16_t rx_len = 0; + static uint16_t uart1_rx_len = 0; __HAL_UART_CLEAR_PEFLAG(&huart1); if ((hdma_usart1_rx.Instance->CR & DMA_SxCR_CT) == RESET) { @@ -192,7 +179,7 @@ void RS485::Rx_Handle() { __HAL_DMA_DISABLE(&hdma_usart1_rx); //获取接收数据长度,长度 = 设定长度 - 剩余长度 - rx_len = RX_SIZE - hdma_usart1_rx.Instance->NDTR; + uart1_rx_len = RX_SIZE - hdma_usart1_rx.Instance->NDTR; //重新设定数据长度 hdma_usart1_rx.Instance->NDTR = RX_SIZE; @@ -204,16 +191,15 @@ void RS485::Rx_Handle() { __HAL_DMA_ENABLE(&hdma_usart1_rx); //将接收到的数据拷贝到字典中,则自动进入电机的RxMessage中 - if (rx_len == MOTOR_RX_SIZE) { - memcpy(dict_RS485[rs485_rx_buff[0][2] - 0x01], rs485_rx_buff[0], rx_len); + if (uart1_rx_len == MOTOR_RX_SIZE) { + memcpy(dict_RS485[rs485_rx_buff[0][2] - 0x01], rs485_rx_buff[0], uart1_rx_len); } } else { /* Current memory buffer used is Memory 1 */ //失效DMA __HAL_DMA_DISABLE(&hdma_usart1_rx); -0 //获取接收数据长度,长度 = 设定长度 - 剩余长度 - rx_len = RX_SIZE - hdma_usart1_rx.Instance->NDTR; + uart1_rx_len = RX_SIZE - hdma_usart1_rx.Instance->NDTR; //重新设定数据长度 hdma_usart1_rx.Instance->NDTR = RX_SIZE; @@ -223,8 +209,8 @@ void RS485::Rx_Handle() { //使能DMA __HAL_DMA_ENABLE(&hdma_usart1_rx); - if (rx_len == MOTOR_RX_SIZE) { - memcpy(dict_RS485[rs485_rx_buff[1][2] - 0x01], rs485_rx_buff[1], rx_len); + if (uart1_rx_len == MOTOR_RX_SIZE) { + memcpy(dict_RS485[rs485_rx_buff[1][2] - 0x01], rs485_rx_buff[1], uart1_rx_len); } } } diff --git a/userCode/devices/Src/Device.cpp b/userCode/devices/Src/Device.cpp index 224f9db..60e7361 100644 --- a/userCode/devices/Src/Device.cpp +++ b/userCode/devices/Src/Device.cpp @@ -119,7 +119,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { // ARMHandle(); // if(HAL_CAN_GetTxMailboxesFreeLevel(&hcan1)>0||HAL_CAN_GetTxMailboxesFreeLevel(&hcan2)>0) { - CAN::CANPackageSend(); + CAN::CANPackageSend(); //} /**只需关注该部分代码**/ @@ -214,7 +214,7 @@ int main() { MX_I2C3_Init(); MX_SPI1_Init(); MX_SPI2_Init(); - MX_IWDG_Init();//看门狗,若不使用遥控器需注释改行，否则程序不运行 + // MX_IWDG_Init();//看门狗,若不使用遥控器需注释改行，否则程序不运行 MX_USB_DEVICE_Init(); /* USER CODE BEGIN 2 */ diff --git a/userCode/devices/Src/ManiControl.cpp b/userCode/devices/Src/ManiControl.cpp index 6757b42..3208525 100644 --- a/userCode/devices/Src/ManiControl.cpp +++ b/userCode/devices/Src/ManiControl.cpp @@ -24,7 +24,7 @@ void ManiControl::Init() { //内存缓冲区 2 hdma_usart6_rx.Instance->M1AR = (uint32_t) (mani_rx_buff[1]); //数据长度 - hdma_usart6_rx.Instance->NDTR = BUFF_SIZE;//不确定需不需要 + hdma_usart6_rx.Instance->NDTR = BUFF_SIZE; //使能双缓冲区 CLEAR_BIT(hdma_usart6_rx.Instance->CR, DMA_SxCR_DBM); SET_BIT(hdma_usart6_rx.Instance->CR, DMA_SxCR_CIRC); @@ -38,7 +38,7 @@ void ManiControl::IT_Handle() { { __HAL_UART_CLEAR_PEFLAG(&huart6); } else if (USART6->SR & UART_FLAG_IDLE) { - static uint16_t rx_len = 0; + static uint16_t uart6_rx_len = 0; __HAL_UART_CLEAR_PEFLAG(&huart6); if ((hdma_usart6_rx.Instance->CR & DMA_SxCR_CT) == RESET) { @@ -48,7 +48,7 @@ void ManiControl::IT_Handle() { __HAL_DMA_DISABLE(&hdma_usart6_rx); //获取接收数据长度,长度 = 设定长度 - 剩余长度 - rx_len = BUFF_SIZE - hdma_usart6_rx.Instance->NDTR; + uart6_rx_len = BUFF_SIZE - hdma_usart6_rx.Instance->NDTR; //重新设定数据长度 hdma_usart6_rx.Instance->NDTR = BUFF_SIZE; @@ -59,7 +59,7 @@ void ManiControl::IT_Handle() { //使能DMA __HAL_DMA_ENABLE(&hdma_usart6_rx); /**只需关注该部分代码**/ - if (LRC_calc(mani_rx_buff[0], rx_len - 1) == mani_rx_buff[0][rx_len - 1] && (mani_rx_buff[0][0] == 0x7A)) { + if (LRC_calc(mani_rx_buff[0], uart6_rx_len - 1) == mani_rx_buff[0][uart6_rx_len - 1] && (mani_rx_buff[0][0] == 0x7A)) { GetData(0); } /**只需关注该部分代码**/ @@ -69,7 +69,7 @@ void ManiControl::IT_Handle() { __HAL_DMA_DISABLE(&hdma_usart6_rx); //获取接收数据长度,长度 = 设定长度 - 剩余长度 - rx_len = BUFF_SIZE - hdma_usart6_rx.Instance->NDTR; + uart6_rx_len = BUFF_SIZE - hdma_usart6_rx.Instance->NDTR; //重新设定数据长度 hdma_usart6_rx.Instance->NDTR = BUFF_SIZE; @@ -80,7 +80,7 @@ void ManiControl::IT_Handle() { //使能DMA __HAL_DMA_ENABLE(&hdma_usart6_rx); /**只需关注该部分代码**/ - if (LRC_calc(mani_rx_buff[1], rx_len - 1) == mani_rx_buff[1][rx_len - 1] && (mani_rx_buff[1][0] == 0x7A)) { + if (LRC_calc(mani_rx_buff[1], uart6_rx_len - 1) == mani_rx_buff[1][uart6_rx_len - 1] && (mani_rx_buff[1][0] == 0x7A)) { GetData(1); } /**只需关注该部分代码**/ @@ -99,7 +99,7 @@ void ManiControl::GetData(uint8_t bufIndex) { case 0x01: { TaskFlag = STOP; mc_ctrl.ChassisStopFlag = mani_rx_buff[bufIndex][4]; - StateMachine::add_function_to_state(ChassisStopTask); + AutoChassisStop(); break; } case 0x02: { @@ -108,7 +108,7 @@ void ManiControl::GetData(uint8_t bufIndex) { memcpy(&mc_ctrl.chassisDis_col.y_Dis, &mani_rx_buff[bufIndex][8], 4); memcpy(&mc_ctrl.chassisDis_col.Theta, &mani_rx_buff[bufIndex][12], 4); - StateMachine::add_function_to_state(Move_DisTask); + ChassisDistanceSet(mc_ctrl.chassisDis_col.x_Dis.f, mc_ctrl.chassisDis_col.y_Dis.f, mc_ctrl.chassisDis_col.Theta.f); break; } case 0x03: { @@ -119,21 +119,21 @@ void ManiControl::GetData(uint8_t bufIndex) { memcpy(&mc_ctrl.arm_col.Joint4Pos, &mani_rx_buff[bufIndex][16], 4); memcpy(&mc_ctrl.arm_col.Joint5Pos, &mani_rx_buff[bufIndex][20], 4); - StateMachine::add_function_to_state(ArmTask); + ArmJointSet(mc_ctrl.arm_col.Joint1Pos.f, mc_ctrl.arm_col.Joint2Pos.f, mc_ctrl.arm_col.Joint3Pos.f, mc_ctrl.arm_col.Joint4Pos.f, mc_ctrl.arm_col.Joint5Pos.f); break; } case 0x04: { TaskFlag = CLAW; mc_ctrl.ClawFlag = mani_rx_buff[bufIndex][5]; - StateMachine::add_function_to_state(ClawTask); + ClawSet(mc_ctrl.ClawFlag); break; } case 0x05: { TaskFlag = TRAY; mc_ctrl.TrayFlag = mani_rx_buff[bufIndex][5]; - StateMachine::add_function_to_state(TrayTask); + //AutoTraySet(mc_ctrl.TrayFlag); break; } case 0x07: { @@ -142,9 +142,19 @@ void ManiControl::GetData(uint8_t bufIndex) { memcpy(&mc_ctrl.chassisVel_col.y_Vel, &mani_rx_buff[bufIndex][8], 4); memcpy(&mc_ctrl.chassisVel_col.w_Vel, &mani_rx_buff[bufIndex][12], 4); - StateMachine::add_function_to_state(Move_VelTask); + ChassisVelocitySet(mc_ctrl.chassisVel_col.x_Vel.f, mc_ctrl.chassisVel_col.y_Vel.f, mc_ctrl.chassisVel_col.w_Vel.f); break; } + case 0x08:{ + TaskFlag = ARM_POS; + memcpy(&mc_ctrl.arm_pos.x, &mani_rx_buff[bufIndex][4], 4); + memcpy(&mc_ctrl.arm_pos.y, &mani_rx_buff[bufIndex][8], 4); + memcpy(&mc_ctrl.arm_pos.z, &mani_rx_buff[bufIndex][12], 4); + + ArmPositionSet(mc_ctrl.arm_pos.x.f,mc_ctrl.arm_pos.y.f,mc_ctrl.arm_pos.z.f); + + break; + } } } } diff --git a/userCode/devices/Src/StateMachine.cpp b/userCode/devices/Src/StateMachine.cpp deleted file mode 100644 index 313da01..0000000 --- a/userCode/devices/Src/StateMachine.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// -// Created by 25396 on 2023/4/6. -// - -#include "StateMachine.h" -//全局变量声明 -FunctionList StateMachine::function_lists = FunctionList(); - -// 函数指针链表 -void FunctionList::add(void (*f)()) { - //检查链表中是否已有该函数 - FunctionNode *curr = head; - while (curr) { - if (curr->func == f) { - return; - } - curr = curr->next; - } - //链表中没有该函数，添加 - if (!head) { - head = new FunctionNode(f); - } else { - curr = head; - while (curr->next) { - curr = curr->next; - } - curr->next = new FunctionNode(f); - } - -} - -void FunctionList::remove(void (*f)()) { - FunctionNode *curr = head; - FunctionNode *prev = nullptr; - while (curr) { - if (curr->func == f) { - if (prev) { - prev->next = curr->next; - } else { - head = curr->next; - } - delete curr; - break; - } - prev = curr; - curr = curr->next; - } -} - -void FunctionList::call_all() { - FunctionNode *curr = head; - while (curr) { - curr->func(); - curr = curr->next; - } -} - - -void StateMachine::stateHandle() { - function_lists.call_all(); // 触发当前状态对应的所有函数 -} - -void StateMachine::add_function_to_state(void (*f)()) { - function_lists.add(f); -} - -void StateMachine::remove_function_from_state(void (*f)()) { - function_lists.remove(f); -} - -StateMachine::StateMachine() { - function_lists = FunctionList();// 初始化函数指针链表 -} - diff --git a/userCode/tasks/Inc/ArmTask.h b/userCode/tasks/Inc/ArmTask.h index b98423d..3ca7e05 100644 --- a/userCode/tasks/Inc/ArmTask.h +++ b/userCode/tasks/Inc/ArmTask.h @@ -13,6 +13,18 @@ #include "Buzzer.h" #include "StepperMotor.h" -void ArmStop(); +#define l2 152.8f +#define l3 130.46f +#define l4 133.36f + +class ArmTask { +public: + static void ArmStop(); + static void ArmCalc(float x,float y,float z); + + + static float Angle[4]; + +}; #endif //RM_FRAME_C_ARMTASK_H diff --git a/userCode/tasks/Inc/AutoTask.h b/userCode/tasks/Inc/AutoTask.h deleted file mode 100644 index 742f85f..0000000 --- a/userCode/tasks/Inc/AutoTask.h +++ /dev/null @@ -1,29 +0,0 @@ -// -// Created by 25396 on 2023/4/6. -// - -#ifndef RM_FRAME_C_AUTOTASK_H -#define RM_FRAME_C_AUTOTASK_H -#include "Device.h" -#include "StateMachine.h" -#include "ManiControl.h" - - -static void AutoTask(); -//状态函数声明 -void ChassisStopTask(); -void Move_DisTask(); -void ArmTask(); -void TrayTask(); -void ClawTask(); -void Move_VelTask(); - -/*外部函数声明-------------------------------------------------------------*/ -extern void AutoChassisStop();//Realized in ChassisTask -extern void ChassisDistanceSet(float x, float y, float o);//Realized in ChassisTask -extern void ChassisVelocitySet(float x_vel, float y_vel, float w_vel);//Realized in ChassisTask -extern void ArmSet(float Joint1Pos, float Joint2Pos, float Joint3Pos, float Joint4Pos, float Joint5Pos); -//extern void AutoTraySet(uint8_t trayflag);//Realized in ArmTask -extern void ClawSet(uint8_t clawflag);//Realized in ArmTask - -#endif //RM_FRAME_C_AUTOTASK_H diff --git a/userCode/tasks/Inc/ControlTask.h b/userCode/tasks/Inc/ControlTask.h index 42fb21d..403f0d2 100644 --- a/userCode/tasks/Inc/ControlTask.h +++ b/userCode/tasks/Inc/ControlTask.h @@ -11,7 +11,6 @@ #include "RemoteControl.h" #include "ServoTask.h" #include "ArmTask.h" -#include "AutoTask.h" #include "ArmTask.h" /*枚举类型定义------------------------------------------------------------*/ diff --git a/userCode/tasks/Src/ArmTask.cpp b/userCode/tasks/Src/ArmTask.cpp index ce9975b..efc2df5 100644 --- a/userCode/tasks/Src/ArmTask.cpp +++ b/userCode/tasks/Src/ArmTask.cpp @@ -4,6 +4,9 @@ #include "ArmTask.h" +#include + +float ArmTask::Angle[4]{0}; MOTOR_INIT_t Joint1MotorInit = { .speedPIDp = nullptr, @@ -79,7 +82,7 @@ bool ArmStopFlag = true; bool ArmMoveFlag = false; static float joint1Angle, joint2Angle, joint3Angle, joint4Angle, joint5Angle; -void ArmStop() { +void ArmTask::ArmStop() { ArmStopFlag = true; Joint1Motor.Stop(); Joint2Motor.Stop(); @@ -89,7 +92,7 @@ void ArmStop() { ClawMotor.Stop(); } -void ArmSet(float Joint1Pos, float Joint2Pos, float Joint3Pos, float Joint4Pos, float Joint5Pos) { +void ArmJointSet(float Joint1Pos, float Joint2Pos, float Joint3Pos, float Joint4Pos, float Joint5Pos) { ArmStopFlag = false; Joint1Motor.SetTargetPosition(Joint1Pos); Joint2Motor.SetTargetPosition(Joint2Pos); @@ -108,19 +111,24 @@ void ClawSet(uint8_t clawflag) { } } -void ARMHandle() { - if (!ArmStopFlag) { - if (ArmMoveFlag) { - Joint1Motor.MoveTo(); - Joint2Motor.MoveTo(); - Joint3Motor.MoveTo(); - // Joint4Motor.MoveTo(); - // Joint5Motor.MoveTo(); - ArmMoveFlag = false; - } - } +void ArmPositionSet(float x, float y, float z){ + ArmTask::ArmCalc(x,y,z); + ArmJointSet(ArmTask::Angle[0], ArmTask::Angle[1], ArmTask::Angle[2], 0, 0); +} +void ArmTask::ArmCalc(float x,float y,float z){ + float d1 = sqrtf(x*x+y*y); + float d2 = sqrtf(d1*d1+(z+l4)*(z+l4)); + float angle1 = acosf((l3*l3-l2*l2-d2*d2)/(-2*l2*d2)); + float angle2 = acosf((d2*d2-l2*l2-l3*l3)/(-2*l2*l3)); + float angle3 = PI - angle1 - angle2; + float angle4 = atan2f(z+l4,d1); + float angle5 = PI/2 - angle4; + + Angle[0] = atan2f(x,y); + Angle[1] = PI/2 - angle1 - angle4; + Angle[2] = PI/2 - angle2; + Angle[3] = PI/2 - angle3 - angle5; } - diff --git a/userCode/tasks/Src/AutoTask.cpp b/userCode/tasks/Src/AutoTask.cpp deleted file mode 100644 index cdf569b..0000000 --- a/userCode/tasks/Src/AutoTask.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// -// Created by 25396 on 2023/4/6. -// - -#include "AutoTask.h" - - - -void ChassisStopTask(){ - AutoChassisStop(); - StateMachine::remove_function_from_state(ChassisStopTask); - CompleteTask(); -} -void Move_DisTask(){ - ChassisDistanceSet(ManiControl::mc_ctrl.chassisDis_col.x_Dis.f, ManiControl::mc_ctrl.chassisDis_col.y_Dis.f, ManiControl::mc_ctrl.chassisDis_col.Theta.f); - StateMachine::remove_function_from_state(Move_DisTask); -} -void Move_VelTask(){ - ChassisVelocitySet(ManiControl::mc_ctrl.chassisVel_col.x_Vel.f, ManiControl::mc_ctrl.chassisVel_col.y_Vel.f, ManiControl::mc_ctrl.chassisVel_col.w_Vel.f); - StateMachine::remove_function_from_state(Move_VelTask); -} -void ArmTask(){ - ArmSet(ManiControl::mc_ctrl.arm_col.Joint1Pos.f,ManiControl::mc_ctrl.arm_col.Joint2Pos.f,ManiControl::mc_ctrl.arm_col.Joint3Pos.f,ManiControl::mc_ctrl.arm_col.Joint4Pos.f,ManiControl::mc_ctrl.arm_col.Joint5Pos.f); - StateMachine::remove_function_from_state(ArmTask); -} -void TrayTask(){ - // AutoTraySet(ManiControl::mc_ctrl.TrayFlag); - StateMachine::remove_function_from_state(TrayTask); -} -void ClawTask(){ - ClawSet(ManiControl::mc_ctrl.ClawFlag); - StateMachine::remove_function_from_state(ClawTask); -} diff --git a/userCode/tasks/Src/ControlTask.cpp b/userCode/tasks/Src/ControlTask.cpp index f4f1ef4..43d7136 100644 --- a/userCode/tasks/Src/ControlTask.cpp +++ b/userCode/tasks/Src/ControlTask.cpp @@ -17,13 +17,16 @@ void autoImpulse(){ } void CtrlHandle() { - if (RemoteControl::rcInfo.sRight == DOWN_POS) {//右侧三档，急停模式 + + // StateMachine::stateHandle(); + + /* if (RemoteControl::rcInfo.sRight == DOWN_POS) {//右侧三档，急停模式 ChassisStop(); - ArmStop(); + ArmTask::ArmStop(); } -/* else if (RemoteControl::rcInfo.sRight == MID_POS){//右侧二档，脉冲模式 +*//* else if (RemoteControl::rcInfo.sRight == MID_POS){//右侧二档，脉冲模式 autoImpulse(); - }*/ + }*//* else {//其他正常模式 switch (RemoteControl::rcInfo.sLeft) { case UP_POS://左侧一档{ @@ -55,49 +58,6 @@ void CtrlHandle() { break; } - } + }*/ } -/* -void CtrlHandle() { - if (RemoteControl::rcInfo.sRight == DOWN_POS) {//右侧三档，急停模式 - ChassisStop(); - ArmStop(); - } else {//其他正常模式 - switch (RemoteControl::rcInfo.sLeft) { - case UP_POS://左侧一档{ - if (RemoteControl::rcInfo.sRight == UP_POS) { - ChassisSetVelocity(RemoteControl::rcInfo.right_col * 8, - RemoteControl::rcInfo.right_rol * 8, RemoteControl::rcInfo.left_rol*2); - //ArmSet(RemoteControl::rcInfo.left_col, RemoteControl::rcInfo.left_col * 90,RemoteControl::rcInfo.left_col); - AutoClawSet(0); - Headmemory(); - }else if (RemoteControl::rcInfo.sRight == MID_POS){ - ChassisSetVelocity(RemoteControl::rcInfo.right_col * 8, - RemoteControl::rcInfo.right_rol * 8, RemoteControl::rcInfo.left_rol*2); - AutoClawSet(1); - } - break; - case MID_POS://左侧二档 - if (RemoteControl::rcInfo.sRight == UP_POS) { - ArmSet(RemoteControl::rcInfo.right_rol * -1.57f, - RemoteControl::rcInfo.left_rol * -130,0); - AutoClawSet(0); - } else if (RemoteControl::rcInfo.sRight == MID_POS) { - ArmSet(RemoteControl::rcInfo.right_rol* -1.57f, - RemoteControl::rcInfo.left_rol * -130,0); - AutoClawSet(1); - } - break; - case DOWN_POS: - if (RemoteControl::rcInfo.sRight == UP_POS) { - AutoSetVelocity(); - } - break; - default: - break; - } - - } - -}*/ From 4b8a20b5e34a3513865e0a19498f1f05bf69b04a Mon Sep 17 00:00:00 2001 From: iDddddd <90831551+iDddddd@users.noreply.github.com> Date: Sun, 24 Sep 2023 23:15:20 +0800 Subject: [PATCH 2/4] verison3.5.18 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 机械臂调试正常 --- MDK-ARM/RM_Frame_C.uvoptx | 39 ++++++++++++++++++++++----- userCode/devices/Inc/ARMMotor.h | 5 +--- userCode/devices/Inc/CommuType.h | 2 +- userCode/devices/Src/ARMMotor.cpp | 27 +++++++------------ userCode/devices/Src/ChassisMotor.cpp | 2 +- userCode/devices/Src/CommuType.cpp | 7 +++-- userCode/devices/Src/ManiControl.cpp | 2 ++ userCode/tasks/Src/ArmTask.cpp | 8 +++--- 8 files changed, 54 insertions(+), 38 deletions(-) diff --git a/MDK-ARM/RM_Frame_C.uvoptx b/MDK-ARM/RM_Frame_C.uvoptx index ff79dff..e398f31 100644 --- a/MDK-ARM/RM_Frame_C.uvoptx +++ b/MDK-ARM/RM_Frame_C.uvoptx @@ -162,7 +162,7 @@ 0 0 - 16 + 87 1
0
0 @@ -171,25 +171,25 @@ 0 0 0 - C:\Users\25396\Documents\GitHub\Swerve-chassis-control-frame-1\userCode\tasks\Src\AutoTask.cpp + ..\userCode\devices\Src\CommuType.cpp 1 0 - 245 + 91 1 -
134226518
+
0
0 0 0 0 0 - 1 + 0 ..\userCode\devices\Src\CommuType.cpp - \\RM_Frame_C\../userCode/devices/Src/CommuType.cpp\245 + @@ -403,6 +403,11 @@ 1 state.data[4] + + 42 + 1 + Joint1Motor + @@ -430,6 +435,26 @@ 2 rx_buff + + 5 + 2 + autoMove + + + 6 + 2 + X + + + 7 + 2 + Y + + + 8 + 2 + RBL + @@ -1045,7 +1070,7 @@ Drivers/CMSIS - 0 + 1 0 0 0 diff --git a/userCode/devices/Inc/ARMMotor.h b/userCode/devices/Inc/ARMMotor.h index 2f6f7c8..7643c2e 100644 --- a/userCode/devices/Inc/ARMMotor.h +++ b/userCode/devices/Inc/ARMMotor.h @@ -31,7 +31,6 @@ class SteppingMotor_v4 : public Motor, public CAN { private: uint8_t Direction{}; - float Position{}; uint16_t Speed{1500}; uint32_t Pulse{0}; @@ -39,8 +38,6 @@ class SteppingMotor_v4 : public Motor, public CAN { uint8_t TxMessage[8]{0}; uint8_t TxMessageDLC{}; - int32_t nowPos = 0; - void CANMessageGenerate() override; }; @@ -51,7 +48,6 @@ class SteppingMotor_v5 : public Motor, public CAN { float NowPos = 0; float TarPos = 0; - SteppingMotor_v5(COMMU_INIT_t *commuInit, MOTOR_INIT_t *motorInit); void Handle() override; @@ -61,6 +57,7 @@ class SteppingMotor_v5 : public Motor, public CAN { ~SteppingMotor_v5(); private: + uint8_t Direction{}; uint16_t Speed{1500}; uint32_t Pulse{}; diff --git a/userCode/devices/Inc/CommuType.h b/userCode/devices/Inc/CommuType.h index 4858dd4..36b4f91 100644 --- a/userCode/devices/Inc/CommuType.h +++ b/userCode/devices/Inc/CommuType.h @@ -12,7 +12,7 @@ #define can1 1 #define can2 2 -#define MAX_MESSAGE_COUNT 20 +#define MAX_MESSAGE_COUNT 10 #define RX_SIZE 20 #define MOTOR_RX_SIZE 15u /*结构体定义--------------------------------------------------------------*/ diff --git a/userCode/devices/Src/ARMMotor.cpp b/userCode/devices/Src/ARMMotor.cpp index fdd9248..ce18e53 100644 --- a/userCode/devices/Src/ARMMotor.cpp +++ b/userCode/devices/Src/ARMMotor.cpp @@ -42,13 +42,13 @@ void SteppingMotor_v4::Handle() { if (stopFlag) { Pulse = 0; } else { - if (TarPos >= Position) { - Direction = 0x11; - } else { + if (TarPos >= NowPos) { Direction = 0x01; + } else { + Direction = 0x11; } - Pulse = (uint32_t) (abs(TarPos - Position) / 2.0f / PI * 200 * 16 * reductionRatio); - Position = TarPos; + Pulse = (uint32_t) (abs(TarPos - NowPos) / 2.0f / PI * 200 * 16 * reductionRatio); + NowPos = TarPos; } TxMessageDLC = 0x08; TxMessage[0] = 0xFD; @@ -61,6 +61,7 @@ void SteppingMotor_v4::Handle() { TxMessage[7] = 0x6B; CANMessageGenerate(); + SendFlag = false; } } @@ -78,6 +79,7 @@ SteppingMotor_v4::~SteppingMotor_v4() = default; SteppingMotor_v5::SteppingMotor_v5(COMMU_INIT_t *commuInit, MOTOR_INIT_t *motorInit) : Motor(motorInit, this), CAN(commuInit) { ID_Bind_Rx(RxMessage); + } SteppingMotor_v5::~SteppingMotor_v5() = default; @@ -105,17 +107,6 @@ void SteppingMotor_v5::CANMessageGenerate() {; } void SteppingMotor_v5::Handle() { -/* - TxMessage[0] = 0x36; - TxMessage[1] = 0x6B; - TxMessageDLC = 0x02; - - nowPos = (int32_t) (RxMessage[2] << 24u | RxMessage[3] << 16u | RxMessage[4] << 8u | RxMessage[5]); - if (RxMessage[1] == 0x01) { - nowPos = -nowPos; - } - NowPos = ((float) nowPos * 2.0f * PI) / 65536.0f / reductionRatio; -*/ if (SendFlag) { if (stopFlag) { Pulse = 0; @@ -137,6 +128,7 @@ void SteppingMotor_v5::Handle() { TxMessage[6] = Pulse >> 16u; TxMessage[7] = Pulse >> 8u; CANMessageGenerate(); + can_ID += 1; TxMessageDLC = 0x05; TxMessage[0] = 0xFD; TxMessage[1] = Pulse; @@ -147,8 +139,9 @@ void SteppingMotor_v5::Handle() { TxMessage[6] = 0x00; TxMessage[7] = 0x00; CANMessageGenerate(); - SendFlag = false; + can_ID -= 1; } + SendFlag = false; } } diff --git a/userCode/devices/Src/ChassisMotor.cpp b/userCode/devices/Src/ChassisMotor.cpp index a273d73..2d94cd9 100644 --- a/userCode/devices/Src/ChassisMotor.cpp +++ b/userCode/devices/Src/ChassisMotor.cpp @@ -119,7 +119,7 @@ void Motor_4010::CANMessageGenerate() { canQueue.Data[canQueue.rear].ID = can_ID; canQueue.Data[canQueue.rear].canType = canType; - canQueue.Data[canQueue.rear].DLC = 0x08; + canQueue.Data[canQueue.rear].DLC = 0x08; canQueue.Data[canQueue.rear].message[0] = 0xA1; canQueue.Data[canQueue.rear].message[1] = 0x00; canQueue.Data[canQueue.rear].message[2] = 0x00; diff --git a/userCode/devices/Src/CommuType.cpp b/userCode/devices/Src/CommuType.cpp index c8302ed..aaa0f92 100644 --- a/userCode/devices/Src/CommuType.cpp +++ b/userCode/devices/Src/CommuType.cpp @@ -78,7 +78,7 @@ void CAN::CANPackageSend() { HAL_CAN_AddTxMessage(&hcan1, &txHeaderTypeDef, canQueue.Data[canQueue.front].message, &box); } else if (canQueue.Data[canQueue.front].canType == can2) { - txHeaderTypeDef.StdId = canQueue.Data[canQueue.front].ID;//从消息包中取出对应的ID + txHeaderTypeDef.ExtId =canQueue.Data[canQueue.front].ID;//从消息包中取出对应的ID txHeaderTypeDef.DLC = canQueue.Data[canQueue.front].DLC;//数据长度 txHeaderTypeDef.IDE = CAN_ID_EXT;//标准帧 txHeaderTypeDef.RTR = CAN_RTR_DATA;//数据帧 @@ -87,10 +87,9 @@ void CAN::CANPackageSend() { HAL_CAN_AddTxMessage(&hcan2, &txHeaderTypeDef, canQueue.Data[canQueue.front].message, &box); } - + memset(canQueue.Data[canQueue.front].message, 0, sizeof(canQueue.Data[canQueue.front].message));//清空消息包中的数据 + canQueue.front = (canQueue.front + 1) % MAX_MESSAGE_COUNT;//消息队列头指针后移 } - memset(canQueue.Data[canQueue.front].message, 0, sizeof(canQueue.Data[canQueue.front].message));//清空消息包中的数据 - canQueue.front = (canQueue.front + 1) % MAX_MESSAGE_COUNT;//消息队列头指针后移 } /** diff --git a/userCode/devices/Src/ManiControl.cpp b/userCode/devices/Src/ManiControl.cpp index 3208525..a3d1ada 100644 --- a/userCode/devices/Src/ManiControl.cpp +++ b/userCode/devices/Src/ManiControl.cpp @@ -62,6 +62,7 @@ void ManiControl::IT_Handle() { if (LRC_calc(mani_rx_buff[0], uart6_rx_len - 1) == mani_rx_buff[0][uart6_rx_len - 1] && (mani_rx_buff[0][0] == 0x7A)) { GetData(0); } + memset(mani_rx_buff[0], 0, BUFF_SIZE); /**只需关注该部分代码**/ } else { /* Current memory buffer used is Memory 1 */ @@ -83,6 +84,7 @@ void ManiControl::IT_Handle() { if (LRC_calc(mani_rx_buff[1], uart6_rx_len - 1) == mani_rx_buff[1][uart6_rx_len - 1] && (mani_rx_buff[1][0] == 0x7A)) { GetData(1); } + memset(mani_rx_buff[1], 0, BUFF_SIZE); /**只需关注该部分代码**/ } } diff --git a/userCode/tasks/Src/ArmTask.cpp b/userCode/tasks/Src/ArmTask.cpp index efc2df5..11d2fd6 100644 --- a/userCode/tasks/Src/ArmTask.cpp +++ b/userCode/tasks/Src/ArmTask.cpp @@ -75,7 +75,7 @@ SteppingMotor_v5 Joint1Motor(&Joint1CommuInit, &Joint1MotorInit); SteppingMotor_v5 Joint2Motor(&Joint2CommuInit, &Joint2MotorInit); SteppingMotor_v5 Joint3Motor(&Joint3CommuInit, &Joint3MotorInit); //SteppingMotor_v4 Joint4Motor(&Joint4CommuInit, &Joint4MotorInit); -//SteppingMotor_v4 Joint5Motor(&Joint5CommuInit, &Joint5MotorInit); +SteppingMotor_v4 Joint5Motor(&Joint5CommuInit, &Joint5MotorInit); StepperMotor ClawMotor(&ClawMotorInit); bool ArmStopFlag = true; @@ -88,7 +88,7 @@ void ArmTask::ArmStop() { Joint2Motor.Stop(); Joint3Motor.Stop(); // Joint4Motor.Stop(); - // Joint5Motor.Stop(); + Joint5Motor.Stop(); ClawMotor.Stop(); } @@ -98,7 +98,7 @@ void ArmJointSet(float Joint1Pos, float Joint2Pos, float Joint3Pos, float Joint4 Joint2Motor.SetTargetPosition(Joint2Pos); Joint3Motor.SetTargetPosition(Joint3Pos); // Joint4Motor.SetTargetPosition(Joint4Pos); - // Joint5Motor.SetTargetPosition(Joint5Pos); + Joint5Motor.SetTargetPosition(Joint5Pos); ArmMoveFlag = true; } @@ -113,7 +113,7 @@ void ClawSet(uint8_t clawflag) { void ArmPositionSet(float x, float y, float z){ ArmTask::ArmCalc(x,y,z); - ArmJointSet(ArmTask::Angle[0], ArmTask::Angle[1], ArmTask::Angle[2], 0, 0); + ArmJointSet(ArmTask::Angle[0], ArmTask::Angle[1], ArmTask::Angle[2], 0, ArmTask::Angle[3]); } void ArmTask::ArmCalc(float x,float y,float z){ float d1 = sqrtf(x*x+y*y); From 9e75a4464c33375d55fe67a18c6a95b4dfbbd52e Mon Sep 17 00:00:00 2001 From: iDddddd <90831551+iDddddd@users.noreply.github.com> Date: Mon, 2 Oct 2023 22:19:24 +0800 Subject: [PATCH 3/4] version3.5.19 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 夹爪调试正常 --- Inc/main.h | 2 - Inc/spi.h | 3 - Inc/stm32f4xx_it.h | 2 - Inc/tim.h | 6 +- MDK-ARM/RM_Frame_C.uvoptx | 38 ++--- MDK-ARM/RM_Frame_C.uvprojx | 5 - RM_Frame_C.ioc | 191 +++++++++++-------------- Src/dma.c | 3 - Src/gpio.c | 35 ++++- Src/spi.c | 115 --------------- Src/stm32f4xx_it.c | 30 ---- Src/tim.c | 193 +++++++++++++------------- userCode/devices/Inc/Buzzer.h | 25 ---- userCode/devices/Src/Buzzer.cpp | 24 ---- userCode/devices/Src/Device.cpp | 5 +- userCode/devices/Src/ManiControl.cpp | 4 +- userCode/devices/Src/StepperMotor.cpp | 24 ++-- userCode/tasks/Inc/ArmTask.h | 1 - 18 files changed, 238 insertions(+), 468 deletions(-) delete mode 100644 userCode/devices/Inc/Buzzer.h delete mode 100644 userCode/devices/Src/Buzzer.cpp diff --git a/Inc/main.h b/Inc/main.h index 28ec64f..20d2fc8 100644 --- a/Inc/main.h +++ b/Inc/main.h @@ -109,8 +109,6 @@ void Error_Handler(void); #define OLED_RST_GPIO_Port GPIOB #define CS1_GYRO_Pin GPIO_PIN_0 #define CS1_GYRO_GPIO_Port GPIOB -#define OLED_DC_Pin GPIO_PIN_14 -#define OLED_DC_GPIO_Port GPIOB /* USER CODE BEGIN Private defines */ diff --git a/Inc/spi.h b/Inc/spi.h index beb378b..f40913c 100644 --- a/Inc/spi.h +++ b/Inc/spi.h @@ -34,14 +34,11 @@ extern "C" { extern SPI_HandleTypeDef hspi1; -extern SPI_HandleTypeDef hspi2; - /* USER CODE BEGIN Private defines */ /* USER CODE END Private defines */ void MX_SPI1_Init(void); -void MX_SPI2_Init(void); /* USER CODE BEGIN Prototypes */ diff --git a/Inc/stm32f4xx_it.h b/Inc/stm32f4xx_it.h index e5f16a8..bb270d2 100644 --- a/Inc/stm32f4xx_it.h +++ b/Inc/stm32f4xx_it.h @@ -75,12 +75,10 @@ void EXTI0_IRQHandler(void); void EXTI3_IRQHandler(void); void EXTI4_IRQHandler(void); void DMA1_Stream1_IRQHandler(void); -void DMA1_Stream4_IRQHandler(void); void CAN1_TX_IRQHandler(void); void CAN1_RX0_IRQHandler(void); void EXTI9_5_IRQHandler(void); void TIM1_UP_TIM10_IRQHandler(void); -void SPI2_IRQHandler(void); void TIM6_DAC_IRQHandler(void); void TIM7_IRQHandler(void); void DMA2_Stream1_IRQHandler(void); diff --git a/Inc/tim.h b/Inc/tim.h index 5cfcb28..a4270ae 100644 --- a/Inc/tim.h +++ b/Inc/tim.h @@ -32,8 +32,6 @@ extern "C" { /* USER CODE END Includes */ -extern TIM_HandleTypeDef htim4; - extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim6; @@ -42,15 +40,17 @@ extern TIM_HandleTypeDef htim7; extern TIM_HandleTypeDef htim10; +extern TIM_HandleTypeDef htim12; + /* USER CODE BEGIN Private defines */ /* USER CODE END Private defines */ -void MX_TIM4_Init(void); void MX_TIM5_Init(void); void MX_TIM6_Init(void); void MX_TIM7_Init(void); void MX_TIM10_Init(void); +void MX_TIM12_Init(void); void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); diff --git a/MDK-ARM/RM_Frame_C.uvoptx b/MDK-ARM/RM_Frame_C.uvoptx index e398f31..6aabf6b 100644 --- a/MDK-ARM/RM_Frame_C.uvoptx +++ b/MDK-ARM/RM_Frame_C.uvoptx @@ -171,7 +171,7 @@ 0 0 0 - ..\userCode\devices\Src\CommuType.cpp + startup_stm32f407xx.s @@ -187,7 +187,7 @@ 0 0 0 - ..\userCode\devices\Src\CommuType.cpp + startup_stm32f407xx.s @@ -1321,18 +1321,6 @@ 0 0 0 - ..\userCode\devices\Src\Buzzer.cpp - Buzzer.cpp - 0 - 0 - - - 7 - 64 - 8 - 0 - 0 - 0 ..\userCode\devices\Src\ARMMotor.cpp ARMMotor.cpp 0 @@ -1340,7 +1328,7 @@ 7 - 65 + 64 8 0 0 @@ -1352,7 +1340,7 @@ 7 - 66 + 65 8 0 0 @@ -1364,7 +1352,7 @@ 7 - 67 + 66 8 0 0 @@ -1376,7 +1364,7 @@ 7 - 68 + 67 8 0 0 @@ -1396,7 +1384,7 @@ 0 8 - 69 + 68 8 0 0 @@ -1408,7 +1396,7 @@ 8 - 70 + 69 8 0 0 @@ -1428,7 +1416,7 @@ 0 9 - 71 + 70 8 0 0 @@ -1448,7 +1436,7 @@ 0 10 - 72 + 71 8 0 0 @@ -1460,7 +1448,7 @@ 10 - 73 + 72 8 0 0 @@ -1480,7 +1468,7 @@ 0 11 - 74 + 73 8 0 0 @@ -1492,7 +1480,7 @@ 11 - 75 + 74 8 0 0 diff --git a/MDK-ARM/RM_Frame_C.uvprojx b/MDK-ARM/RM_Frame_C.uvprojx index 50d893b..f49023c 100644 --- a/MDK-ARM/RM_Frame_C.uvprojx +++ b/MDK-ARM/RM_Frame_C.uvprojx @@ -776,11 +776,6 @@ 8 ..\userCode\devices\Src\CommuType.cpp - - Buzzer.cpp - 8 - ..\userCode\devices\Src\Buzzer.cpp - ARMMotor.cpp 8 diff --git a/RM_Frame_C.ioc b/RM_Frame_C.ioc index 14e777b..78628be 100644 --- a/RM_Frame_C.ioc +++ b/RM_Frame_C.ioc @@ -30,10 +30,9 @@ CAN2.Prescaler=3 Dma.Request0=USART3_RX Dma.Request1=SPI1_RX Dma.Request2=SPI1_TX -Dma.Request3=SPI2_TX -Dma.Request4=USART6_RX -Dma.Request5=USART1_RX -Dma.RequestsNb=6 +Dma.Request3=USART6_RX +Dma.Request4=USART1_RX +Dma.RequestsNb=5 Dma.SPI1_RX.1.Direction=DMA_PERIPH_TO_MEMORY Dma.SPI1_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE Dma.SPI1_RX.1.Instance=DMA2_Stream0 @@ -54,26 +53,16 @@ Dma.SPI1_TX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE Dma.SPI1_TX.2.PeriphInc=DMA_PINC_DISABLE Dma.SPI1_TX.2.Priority=DMA_PRIORITY_HIGH Dma.SPI1_TX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode -Dma.SPI2_TX.3.Direction=DMA_MEMORY_TO_PERIPH -Dma.SPI2_TX.3.FIFOMode=DMA_FIFOMODE_DISABLE -Dma.SPI2_TX.3.Instance=DMA1_Stream4 -Dma.SPI2_TX.3.MemDataAlignment=DMA_MDATAALIGN_BYTE -Dma.SPI2_TX.3.MemInc=DMA_MINC_ENABLE -Dma.SPI2_TX.3.Mode=DMA_NORMAL -Dma.SPI2_TX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Dma.SPI2_TX.3.PeriphInc=DMA_PINC_DISABLE -Dma.SPI2_TX.3.Priority=DMA_PRIORITY_HIGH -Dma.SPI2_TX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode -Dma.USART1_RX.5.Direction=DMA_PERIPH_TO_MEMORY -Dma.USART1_RX.5.FIFOMode=DMA_FIFOMODE_DISABLE -Dma.USART1_RX.5.Instance=DMA2_Stream2 -Dma.USART1_RX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE -Dma.USART1_RX.5.MemInc=DMA_MINC_ENABLE -Dma.USART1_RX.5.Mode=DMA_NORMAL -Dma.USART1_RX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Dma.USART1_RX.5.PeriphInc=DMA_PINC_DISABLE -Dma.USART1_RX.5.Priority=DMA_PRIORITY_HIGH -Dma.USART1_RX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode +Dma.USART1_RX.4.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART1_RX.4.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART1_RX.4.Instance=DMA2_Stream2 +Dma.USART1_RX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART1_RX.4.MemInc=DMA_MINC_ENABLE +Dma.USART1_RX.4.Mode=DMA_NORMAL +Dma.USART1_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART1_RX.4.PeriphInc=DMA_PINC_DISABLE +Dma.USART1_RX.4.Priority=DMA_PRIORITY_HIGH +Dma.USART1_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode Dma.USART3_RX.0.Direction=DMA_PERIPH_TO_MEMORY Dma.USART3_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE Dma.USART3_RX.0.Instance=DMA1_Stream1 @@ -84,17 +73,18 @@ Dma.USART3_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE Dma.USART3_RX.0.PeriphInc=DMA_PINC_DISABLE Dma.USART3_RX.0.Priority=DMA_PRIORITY_VERY_HIGH Dma.USART3_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode -Dma.USART6_RX.4.Direction=DMA_PERIPH_TO_MEMORY -Dma.USART6_RX.4.FIFOMode=DMA_FIFOMODE_DISABLE -Dma.USART6_RX.4.Instance=DMA2_Stream1 -Dma.USART6_RX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE -Dma.USART6_RX.4.MemInc=DMA_MINC_ENABLE -Dma.USART6_RX.4.Mode=DMA_NORMAL -Dma.USART6_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Dma.USART6_RX.4.PeriphInc=DMA_PINC_DISABLE -Dma.USART6_RX.4.Priority=DMA_PRIORITY_HIGH -Dma.USART6_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode +Dma.USART6_RX.3.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART6_RX.3.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART6_RX.3.Instance=DMA2_Stream1 +Dma.USART6_RX.3.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART6_RX.3.MemInc=DMA_MINC_ENABLE +Dma.USART6_RX.3.Mode=DMA_NORMAL +Dma.USART6_RX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART6_RX.3.PeriphInc=DMA_PINC_DISABLE +Dma.USART6_RX.3.Priority=DMA_PRIORITY_HIGH +Dma.USART6_RX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode File.Version=6 +GPIO.groupedBy=Group By Peripherals I2C3.I2C_Mode=I2C_Fast I2C3.IPParameters=I2C_Mode IWDG.IPParameters=Prescaler,Reload @@ -105,19 +95,18 @@ Mcu.CPN=STM32F407IGH6 Mcu.Family=STM32F4 Mcu.IP0=ADC1 Mcu.IP1=ADC3 -Mcu.IP10=SPI2 -Mcu.IP11=SYS -Mcu.IP12=TIM4 -Mcu.IP13=TIM5 -Mcu.IP14=TIM6 -Mcu.IP15=TIM7 -Mcu.IP16=TIM10 -Mcu.IP17=USART1 -Mcu.IP18=USART3 -Mcu.IP19=USART6 +Mcu.IP10=SYS +Mcu.IP11=TIM5 +Mcu.IP12=TIM6 +Mcu.IP13=TIM7 +Mcu.IP14=TIM10 +Mcu.IP15=TIM12 +Mcu.IP16=USART1 +Mcu.IP17=USART3 +Mcu.IP18=USART6 +Mcu.IP19=USB_DEVICE Mcu.IP2=CAN1 -Mcu.IP20=USB_DEVICE -Mcu.IP21=USB_OTG_FS +Mcu.IP20=USB_OTG_FS Mcu.IP3=CAN2 Mcu.IP4=DMA Mcu.IP5=I2C3 @@ -125,7 +114,7 @@ Mcu.IP6=IWDG Mcu.IP7=NVIC Mcu.IP8=RCC Mcu.IP9=SPI1 -Mcu.IPNb=22 +Mcu.IPNb=21 Mcu.Name=STM32F407I(E-G)Hx Mcu.Package=UFBGA176 Mcu.Pin0=PB5 @@ -134,51 +123,50 @@ Mcu.Pin10=PC10 Mcu.Pin11=PA12 Mcu.Pin12=PG9 Mcu.Pin13=PD1 -Mcu.Pin14=PI2 -Mcu.Pin15=PA11 -Mcu.Pin16=PF0 -Mcu.Pin17=PA9 -Mcu.Pin18=PC9 -Mcu.Pin19=PA8 +Mcu.Pin14=PA11 +Mcu.Pin15=PF0 +Mcu.Pin16=PA9 +Mcu.Pin17=PC9 +Mcu.Pin18=PA8 +Mcu.Pin19=PH0-OSC_IN Mcu.Pin2=PB4 -Mcu.Pin20=PH0-OSC_IN -Mcu.Pin21=PH1-OSC_OUT -Mcu.Pin22=PF1 -Mcu.Pin23=PG6 -Mcu.Pin24=PF6 -Mcu.Pin25=PH12 -Mcu.Pin26=PG3 -Mcu.Pin27=PF10 -Mcu.Pin28=PH11 -Mcu.Pin29=PH10 +Mcu.Pin20=PH1-OSC_OUT +Mcu.Pin21=PF1 +Mcu.Pin22=PG6 +Mcu.Pin23=PF6 +Mcu.Pin24=PH12 +Mcu.Pin25=PG3 +Mcu.Pin26=PF10 +Mcu.Pin27=PH11 +Mcu.Pin28=PH10 +Mcu.Pin29=PD14 Mcu.Pin3=PB3 -Mcu.Pin30=PD14 -Mcu.Pin31=PA0-WKUP -Mcu.Pin32=PA4 -Mcu.Pin33=PC4 -Mcu.Pin34=PC5 -Mcu.Pin35=PB12 -Mcu.Pin36=PB13 -Mcu.Pin37=PA7 -Mcu.Pin38=PB0 -Mcu.Pin39=PB14 +Mcu.Pin30=PA0-WKUP +Mcu.Pin31=PA4 +Mcu.Pin32=PC4 +Mcu.Pin33=PC5 +Mcu.Pin34=PB12 +Mcu.Pin35=PB13 +Mcu.Pin36=PA7 +Mcu.Pin37=PB0 +Mcu.Pin38=PB14 +Mcu.Pin39=PB15 Mcu.Pin4=PA14 -Mcu.Pin40=PB15 -Mcu.Pin41=VP_ADC1_Vref_Input -Mcu.Pin42=VP_IWDG_VS_IWDG -Mcu.Pin43=VP_SYS_VS_Systick -Mcu.Pin44=VP_TIM4_VS_ClockSourceINT -Mcu.Pin45=VP_TIM5_VS_ClockSourceINT -Mcu.Pin46=VP_TIM6_VS_ClockSourceINT -Mcu.Pin47=VP_TIM7_VS_ClockSourceINT -Mcu.Pin48=VP_TIM10_VS_ClockSourceINT -Mcu.Pin49=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS +Mcu.Pin40=VP_ADC1_Vref_Input +Mcu.Pin41=VP_IWDG_VS_IWDG +Mcu.Pin42=VP_SYS_VS_Systick +Mcu.Pin43=VP_TIM5_VS_ClockSourceINT +Mcu.Pin44=VP_TIM6_VS_ClockSourceINT +Mcu.Pin45=VP_TIM7_VS_ClockSourceINT +Mcu.Pin46=VP_TIM10_VS_ClockSourceINT +Mcu.Pin47=VP_TIM12_VS_ClockSourceINT +Mcu.Pin48=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS Mcu.Pin5=PA13 Mcu.Pin6=PB7 Mcu.Pin7=PB6 Mcu.Pin8=PD0 Mcu.Pin9=PC11 -Mcu.PinsNb=50 +Mcu.PinsNb=49 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32F407IGHx @@ -190,7 +178,6 @@ NVIC.CAN1_TX_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true NVIC.CAN2_RX0_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true NVIC.CAN2_TX_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:false\:false\:true\:true NVIC.DMA2_Stream1_IRQn=true\:1\:0\:true\:false\:true\:false\:true\:true NVIC.DMA2_Stream2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true @@ -207,7 +194,6 @@ NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.OTG_FS_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SPI2_IRQn=true\:10\:0\:true\:false\:true\:true\:true\:true NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false NVIC.TIM1_UP_TIM10_IRQn=true\:10\:0\:true\:false\:true\:true\:true\:true @@ -262,18 +248,11 @@ PB12.Locked=true PB12.PinState=GPIO_PIN_SET PB12.Signal=GPIO_Output PB13.Locked=true -PB13.Mode=Full_Duplex_Master PB13.Signal=SPI2_SCK -PB14.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label -PB14.GPIO_Label=OLED_DC -PB14.GPIO_PuPd=GPIO_PULLUP -PB14.GPIO_Speed=GPIO_SPEED_FREQ_HIGH PB14.Locked=true -PB14.PinState=GPIO_PIN_SET PB14.Signal=GPIO_Output PB15.Locked=true -PB15.Mode=Full_Duplex_Master -PB15.Signal=SPI2_MOSI +PB15.Signal=S_TIM12_CH2 PB3.Mode=Full_Duplex_Master PB3.Signal=SPI1_SCK PB4.Mode=Full_Duplex_Master @@ -369,8 +348,6 @@ PH12.GPIO_PuPd=GPIO_NOPULL PH12.GPIO_Speed=GPIO_SPEED_FREQ_LOW PH12.Locked=true PH12.Signal=S_TIM5_CH3 -PI2.Mode=Full_Duplex_Master -PI2.Signal=SPI2_MISO PinOutPanel.CurrentBGAView=Top PinOutPanel.RotationAngle=0 ProjectManager.AskForMigrate=true @@ -400,7 +377,7 @@ ProjectManager.StackSize=0x800 ProjectManager.TargetToolchain=MDK-ARM V5 ProjectManager.ToolChainLocation= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_TIM5_Init-TIM5-false-HAL-true,5-MX_TIM4_Init-TIM4-false-HAL-true,6-MX_TIM10_Init-TIM10-false-HAL-true,7-MX_ADC1_Init-ADC1-false-HAL-true,8-MX_ADC3_Init-ADC3-false-HAL-true,9-MX_USART1_UART_Init-USART1-false-HAL-true,10-MX_USART6_UART_Init-USART6-false-HAL-true,11-MX_USART3_UART_Init-USART3-false-HAL-true,12-MX_CAN1_Init-CAN1-false-HAL-true,13-MX_CAN2_Init-CAN2-false-HAL-true,14-MX_I2C3_Init-I2C3-false-HAL-true,15-MX_SPI1_Init-SPI1-false-HAL-true,16-MX_IWDG_Init-IWDG-false-HAL-true,17-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,18-MX_TIM6_Init-TIM6-false-HAL-true,19-MX_TIM7_Init-TIM7-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true +ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_TIM5_Init-TIM5-false-HAL-true,5-MX_TIM4_Init-TIM4-false-HAL-true,5-MX_TIM10_Init-TIM10-false-HAL-true,6-MX_ADC1_Init-ADC1-false-HAL-true,7-MX_ADC3_Init-ADC3-false-HAL-true,8-MX_USART1_UART_Init-USART1-false-HAL-true,9-MX_USART6_UART_Init-USART6-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_CAN1_Init-CAN1-false-HAL-true,12-MX_CAN2_Init-CAN2-false-HAL-true,13-MX_I2C3_Init-I2C3-false-HAL-true,14-MX_SPI1_Init-SPI1-false-HAL-true,15-MX_IWDG_Init-IWDG-false-HAL-true,16-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,17-MX_TIM6_Init-TIM6-false-HAL-true,18-MX_TIM7_Init-TIM7-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true RCC.48MHZClocksFreq_Value=48000000 RCC.AHBFreq_Value=168000000 RCC.APB1CLKDivider=RCC_HCLK_DIV4 @@ -444,7 +421,9 @@ SH.GPXTI5.0=GPIO_EXTI5 SH.GPXTI5.ConfNb=1 SH.S_TIM10_CH1.0=TIM10_CH1,PWM Generation1 CH1 SH.S_TIM10_CH1.ConfNb=1 -SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3 +SH.S_TIM12_CH2.0=TIM12_CH2,PWM Generation2 CH2 +SH.S_TIM12_CH2.ConfNb=1 +SH.S_TIM4_CH3.0=TIM4_CH3 SH.S_TIM4_CH3.ConfNb=1 SH.S_TIM5_CH1.0=TIM5_CH1,PWM Generation1 CH1 SH.S_TIM5_CH1.ConfNb=1 @@ -460,18 +439,14 @@ SPI1.Direction=SPI_DIRECTION_2LINES SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,CLKPolarity,CLKPhase SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER -SPI2.CalculateBaudRate=21.0 MBits/s -SPI2.Direction=SPI_DIRECTION_2LINES -SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate -SPI2.Mode=SPI_MODE_MASTER -SPI2.VirtualType=VM_MASTER TIM10.Channel=TIM_CHANNEL_1 TIM10.IPParameters=Prescaler,Period,Channel TIM10.Period=1000-1 TIM10.Prescaler=168-1 -TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 -TIM4.IPParameters=Channel-PWM Generation3 CH3,Period -TIM4.Period=21000-1 +TIM12.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 +TIM12.IPParameters=Channel-PWM Generation2 CH2,Prescaler,Period +TIM12.Period=999 +TIM12.Prescaler=335 TIM5.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 TIM5.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 TIM5.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 @@ -508,8 +483,8 @@ VP_SYS_VS_Systick.Mode=SysTick VP_SYS_VS_Systick.Signal=SYS_VS_Systick VP_TIM10_VS_ClockSourceINT.Mode=Enable_Timer VP_TIM10_VS_ClockSourceINT.Signal=TIM10_VS_ClockSourceINT -VP_TIM4_VS_ClockSourceINT.Mode=Internal -VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT +VP_TIM12_VS_ClockSourceINT.Mode=Internal +VP_TIM12_VS_ClockSourceINT.Signal=TIM12_VS_ClockSourceINT VP_TIM5_VS_ClockSourceINT.Mode=Internal VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT VP_TIM6_VS_ClockSourceINT.Mode=Enable_Timer diff --git a/Src/dma.c b/Src/dma.c index 9cfb391..7373ba9 100644 --- a/Src/dma.c +++ b/Src/dma.c @@ -47,9 +47,6 @@ void MX_DMA_Init(void) /* DMA1_Stream1_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); - /* DMA1_Stream4_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn); /* DMA2_Stream0_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn); diff --git a/Src/gpio.c b/Src/gpio.c index 684e357..2b08ccd 100644 --- a/Src/gpio.c +++ b/Src/gpio.c @@ -38,6 +38,8 @@ * Output * EVENT_OUT * EXTI + PD14 ------> S_TIM4_CH3 + PB13 ------> SPI2_SCK */ void MX_GPIO_Init(void) { @@ -50,7 +52,6 @@ void MX_GPIO_Init(void) __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); - __HAL_RCC_GPIOI_CLK_ENABLE(); __HAL_RCC_GPIOF_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE(); @@ -64,7 +65,10 @@ void MX_GPIO_Init(void) HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOB, OLED_RST_Pin|CS1_GYRO_Pin|OLED_DC_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(GPIOB, OLED_RST_Pin|CS1_GYRO_Pin, GPIO_PIN_SET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); /*Configure GPIO pins : PFPin PFPin */ GPIO_InitStruct.Pin = DIR_Pin|STEP_Pin; @@ -86,6 +90,14 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(IST8310_DRDY_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : PD14 */ + GPIO_InitStruct.Pin = GPIO_PIN_14; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM4; + HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); + /*Configure GPIO pin : PtPin */ GPIO_InitStruct.Pin = KEY_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; @@ -105,13 +117,28 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - /*Configure GPIO pins : PBPin PBPin PBPin */ - GPIO_InitStruct.Pin = OLED_RST_Pin|CS1_GYRO_Pin|OLED_DC_Pin; + /*Configure GPIO pins : PBPin PBPin */ + GPIO_InitStruct.Pin = OLED_RST_Pin|CS1_GYRO_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + /*Configure GPIO pin : PB13 */ + GPIO_InitStruct.Pin = GPIO_PIN_13; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF5_SPI2; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /*Configure GPIO pin : PB14 */ + GPIO_InitStruct.Pin = GPIO_PIN_14; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + /* EXTI interrupt init*/ HAL_NVIC_SetPriority(EXTI0_IRQn, 10, 0); HAL_NVIC_EnableIRQ(EXTI0_IRQn); diff --git a/Src/spi.c b/Src/spi.c index d354083..8e75216 100644 --- a/Src/spi.c +++ b/Src/spi.c @@ -25,10 +25,8 @@ /* USER CODE END 0 */ SPI_HandleTypeDef hspi1; -SPI_HandleTypeDef hspi2; DMA_HandleTypeDef hdma_spi1_rx; DMA_HandleTypeDef hdma_spi1_tx; -DMA_HandleTypeDef hdma_spi2_tx; /* SPI1 init function */ void MX_SPI1_Init(void) @@ -61,38 +59,6 @@ void MX_SPI1_Init(void) /* USER CODE END SPI1_Init 2 */ -} -/* SPI2 init function */ -void MX_SPI2_Init(void) -{ - - /* USER CODE BEGIN SPI2_Init 0 */ - - /* USER CODE END SPI2_Init 0 */ - - /* USER CODE BEGIN SPI2_Init 1 */ - - /* USER CODE END SPI2_Init 1 */ - hspi2.Instance = SPI2; - hspi2.Init.Mode = SPI_MODE_MASTER; - hspi2.Init.Direction = SPI_DIRECTION_2LINES; - hspi2.Init.DataSize = SPI_DATASIZE_8BIT; - hspi2.Init.CLKPolarity = SPI_POLARITY_LOW; - hspi2.Init.CLKPhase = SPI_PHASE_1EDGE; - hspi2.Init.NSS = SPI_NSS_SOFT; - hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2; - hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB; - hspi2.Init.TIMode = SPI_TIMODE_DISABLE; - hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - hspi2.Init.CRCPolynomial = 10; - if (HAL_SPI_Init(&hspi2) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN SPI2_Init 2 */ - - /* USER CODE END SPI2_Init 2 */ - } void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle) @@ -169,61 +135,6 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle) /* USER CODE END SPI1_MspInit 1 */ } - else if(spiHandle->Instance==SPI2) - { - /* USER CODE BEGIN SPI2_MspInit 0 */ - - /* USER CODE END SPI2_MspInit 0 */ - /* SPI2 clock enable */ - __HAL_RCC_SPI2_CLK_ENABLE(); - - __HAL_RCC_GPIOI_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - /**SPI2 GPIO Configuration - PI2 ------> SPI2_MISO - PB13 ------> SPI2_SCK - PB15 ------> SPI2_MOSI - */ - GPIO_InitStruct.Pin = GPIO_PIN_2; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF5_SPI2; - HAL_GPIO_Init(GPIOI, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_15; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF5_SPI2; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - /* SPI2 DMA Init */ - /* SPI2_TX Init */ - hdma_spi2_tx.Instance = DMA1_Stream4; - hdma_spi2_tx.Init.Channel = DMA_CHANNEL_0; - hdma_spi2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; - hdma_spi2_tx.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_spi2_tx.Init.MemInc = DMA_MINC_ENABLE; - hdma_spi2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_spi2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - hdma_spi2_tx.Init.Mode = DMA_NORMAL; - hdma_spi2_tx.Init.Priority = DMA_PRIORITY_HIGH; - hdma_spi2_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - if (HAL_DMA_Init(&hdma_spi2_tx) != HAL_OK) - { - Error_Handler(); - } - - __HAL_LINKDMA(spiHandle,hdmatx,hdma_spi2_tx); - - /* SPI2 interrupt Init */ - HAL_NVIC_SetPriority(SPI2_IRQn, 10, 0); - HAL_NVIC_EnableIRQ(SPI2_IRQn); - /* USER CODE BEGIN SPI2_MspInit 1 */ - - /* USER CODE END SPI2_MspInit 1 */ - } } void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle) @@ -253,32 +164,6 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle) /* USER CODE END SPI1_MspDeInit 1 */ } - else if(spiHandle->Instance==SPI2) - { - /* USER CODE BEGIN SPI2_MspDeInit 0 */ - - /* USER CODE END SPI2_MspDeInit 0 */ - /* Peripheral clock disable */ - __HAL_RCC_SPI2_CLK_DISABLE(); - - /**SPI2 GPIO Configuration - PI2 ------> SPI2_MISO - PB13 ------> SPI2_SCK - PB15 ------> SPI2_MOSI - */ - HAL_GPIO_DeInit(GPIOI, GPIO_PIN_2); - - HAL_GPIO_DeInit(GPIOB, GPIO_PIN_13|GPIO_PIN_15); - - /* SPI2 DMA DeInit */ - HAL_DMA_DeInit(spiHandle->hdmatx); - - /* SPI2 interrupt Deinit */ - HAL_NVIC_DisableIRQ(SPI2_IRQn); - /* USER CODE BEGIN SPI2_MspDeInit 1 */ - - /* USER CODE END SPI2_MspDeInit 1 */ - } } /* USER CODE BEGIN 1 */ diff --git a/Src/stm32f4xx_it.c b/Src/stm32f4xx_it.c index c602eb9..77b7411 100644 --- a/Src/stm32f4xx_it.c +++ b/Src/stm32f4xx_it.c @@ -77,8 +77,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern CAN_HandleTypeDef hcan1; extern CAN_HandleTypeDef hcan2; extern DMA_HandleTypeDef hdma_spi1_tx; -extern DMA_HandleTypeDef hdma_spi2_tx; -extern SPI_HandleTypeDef hspi2; extern TIM_HandleTypeDef htim6; extern TIM_HandleTypeDef htim7; extern TIM_HandleTypeDef htim10; @@ -281,20 +279,6 @@ void DMA1_Stream1_IRQHandler(void) /* USER CODE END DMA1_Stream1_IRQn 1 */ } -/** - * @brief This function handles DMA1 stream4 global interrupt. - */ -void DMA1_Stream4_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream4_IRQn 0 */ - - /* USER CODE END DMA1_Stream4_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_spi2_tx); - /* USER CODE BEGIN DMA1_Stream4_IRQn 1 */ - - /* USER CODE END DMA1_Stream4_IRQn 1 */ -} - /** * @brief This function handles CAN1 TX interrupts. */ @@ -351,20 +335,6 @@ void TIM1_UP_TIM10_IRQHandler(void) /* USER CODE END TIM1_UP_TIM10_IRQn 1 */ } -/** - * @brief This function handles SPI2 global interrupt. - */ -void SPI2_IRQHandler(void) -{ - /* USER CODE BEGIN SPI2_IRQn 0 */ - - /* USER CODE END SPI2_IRQn 0 */ - HAL_SPI_IRQHandler(&hspi2); - /* USER CODE BEGIN SPI2_IRQn 1 */ - - /* USER CODE END SPI2_IRQn 1 */ -} - /** * @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts. */ diff --git a/Src/tim.c b/Src/tim.c index 2b4b397..e2fcaa9 100644 --- a/Src/tim.c +++ b/Src/tim.c @@ -24,66 +24,12 @@ /* USER CODE END 0 */ -TIM_HandleTypeDef htim4; TIM_HandleTypeDef htim5; TIM_HandleTypeDef htim6; TIM_HandleTypeDef htim7; TIM_HandleTypeDef htim10; +TIM_HandleTypeDef htim12; -/* TIM4 init function */ -void MX_TIM4_Init(void) -{ - - /* USER CODE BEGIN TIM4_Init 0 */ - - /* USER CODE END TIM4_Init 0 */ - - TIM_ClockConfigTypeDef sClockSourceConfig = {0}; - TIM_MasterConfigTypeDef sMasterConfig = {0}; - TIM_OC_InitTypeDef sConfigOC = {0}; - - /* USER CODE BEGIN TIM4_Init 1 */ - - /* USER CODE END TIM4_Init 1 */ - htim4.Instance = TIM4; - htim4.Init.Prescaler = 0; - htim4.Init.CounterMode = TIM_COUNTERMODE_UP; - htim4.Init.Period = 21000-1; - htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_Base_Init(&htim4) != HAL_OK) - { - Error_Handler(); - } - sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK) - { - Error_Handler(); - } - if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) - { - Error_Handler(); - } - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 0; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN TIM4_Init 2 */ - - /* USER CODE END TIM4_Init 2 */ - HAL_TIM_MspPostInit(&htim4); - -} /* TIM5 init function */ void MX_TIM5_Init(void) { @@ -254,22 +200,58 @@ void MX_TIM10_Init(void) HAL_TIM_MspPostInit(&htim10); } - -void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle) +/* TIM12 init function */ +void MX_TIM12_Init(void) { - if(tim_baseHandle->Instance==TIM4) - { - /* USER CODE BEGIN TIM4_MspInit 0 */ + /* USER CODE BEGIN TIM12_Init 0 */ - /* USER CODE END TIM4_MspInit 0 */ - /* TIM4 clock enable */ - __HAL_RCC_TIM4_CLK_ENABLE(); - /* USER CODE BEGIN TIM4_MspInit 1 */ + /* USER CODE END TIM12_Init 0 */ - /* USER CODE END TIM4_MspInit 1 */ + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_OC_InitTypeDef sConfigOC = {0}; + + /* USER CODE BEGIN TIM12_Init 1 */ + + /* USER CODE END TIM12_Init 1 */ + htim12.Instance = TIM12; + htim12.Init.Prescaler = 335; + htim12.Init.CounterMode = TIM_COUNTERMODE_UP; + htim12.Init.Period = 999; + htim12.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim12.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim12) != HAL_OK) + { + Error_Handler(); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim12, &sClockSourceConfig) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_Init(&htim12) != HAL_OK) + { + Error_Handler(); } - else if(tim_baseHandle->Instance==TIM5) + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 0; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + if (HAL_TIM_PWM_ConfigChannel(&htim12, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM12_Init 2 */ + + /* USER CODE END TIM12_Init 2 */ + HAL_TIM_MspPostInit(&htim12); + +} + +void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle) +{ + + if(tim_baseHandle->Instance==TIM5) { /* USER CODE BEGIN TIM5_MspInit 0 */ @@ -325,37 +307,27 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle) /* USER CODE END TIM10_MspInit 1 */ } + else if(tim_baseHandle->Instance==TIM12) + { + /* USER CODE BEGIN TIM12_MspInit 0 */ + + /* USER CODE END TIM12_MspInit 0 */ + /* TIM12 clock enable */ + __HAL_RCC_TIM12_CLK_ENABLE(); + /* USER CODE BEGIN TIM12_MspInit 1 */ + + /* USER CODE END TIM12_MspInit 1 */ + } } void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; - if(timHandle->Instance==TIM4) - { - /* USER CODE BEGIN TIM4_MspPostInit 0 */ - - /* USER CODE END TIM4_MspPostInit 0 */ - __HAL_RCC_GPIOD_CLK_ENABLE(); - /**TIM4 GPIO Configuration - PD14 ------> TIM4_CH3 - */ - GPIO_InitStruct.Pin = GPIO_PIN_14; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Alternate = GPIO_AF2_TIM4; - HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); - - /* USER CODE BEGIN TIM4_MspPostInit 1 */ - - /* USER CODE END TIM4_MspPostInit 1 */ - } - else if(timHandle->Instance==TIM5) + if(timHandle->Instance==TIM5) { /* USER CODE BEGIN TIM5_MspPostInit 0 */ /* USER CODE END TIM5_MspPostInit 0 */ - __HAL_RCC_GPIOH_CLK_ENABLE(); /**TIM5 GPIO Configuration PH12 ------> TIM5_CH3 @@ -394,24 +366,34 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle) /* USER CODE END TIM10_MspPostInit 1 */ } + else if(timHandle->Instance==TIM12) + { + /* USER CODE BEGIN TIM12_MspPostInit 0 */ + + /* USER CODE END TIM12_MspPostInit 0 */ + + __HAL_RCC_GPIOB_CLK_ENABLE(); + /**TIM12 GPIO Configuration + PB15 ------> TIM12_CH2 + */ + GPIO_InitStruct.Pin = GPIO_PIN_15; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF9_TIM12; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USER CODE BEGIN TIM12_MspPostInit 1 */ + + /* USER CODE END TIM12_MspPostInit 1 */ + } } void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle) { - if(tim_baseHandle->Instance==TIM4) - { - /* USER CODE BEGIN TIM4_MspDeInit 0 */ - - /* USER CODE END TIM4_MspDeInit 0 */ - /* Peripheral clock disable */ - __HAL_RCC_TIM4_CLK_DISABLE(); - /* USER CODE BEGIN TIM4_MspDeInit 1 */ - - /* USER CODE END TIM4_MspDeInit 1 */ - } - else if(tim_baseHandle->Instance==TIM5) + if(tim_baseHandle->Instance==TIM5) { /* USER CODE BEGIN TIM5_MspDeInit 0 */ @@ -464,6 +446,17 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle) /* USER CODE END TIM10_MspDeInit 1 */ } + else if(tim_baseHandle->Instance==TIM12) + { + /* USER CODE BEGIN TIM12_MspDeInit 0 */ + + /* USER CODE END TIM12_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM12_CLK_DISABLE(); + /* USER CODE BEGIN TIM12_MspDeInit 1 */ + + /* USER CODE END TIM12_MspDeInit 1 */ + } } /* USER CODE BEGIN 1 */ diff --git a/userCode/devices/Inc/Buzzer.h b/userCode/devices/Inc/Buzzer.h deleted file mode 100644 index 5026b3e..0000000 --- a/userCode/devices/Inc/Buzzer.h +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by 25396 on 2023/3/14. -// - -#ifndef RM_FRAME_C_BUZZER_H -#define RM_FRAME_C_BUZZER_H -#include "Device.h" - -#define BUZZER_CLOCK_FREQUENCY 84000000 -#define BUZZER_CLOCK htim4 -#define BUZZER_CLOCK_CHANNEL TIM_CHANNEL_3 -#define REFERENCE_MAX_VOL_CCR 5000 - -typedef struct { - uint8_t* f; - uint8_t* t; - uint16_t len; - uint16_t t_each; - uint16_t now_len; -}music_t; - -void bsp_BuzzerOn(float freq); -void bsp_BuzzerOff(); - -#endif //RM_FRAME_C_BUZZER_H diff --git a/userCode/devices/Src/Buzzer.cpp b/userCode/devices/Src/Buzzer.cpp deleted file mode 100644 index 7711226..0000000 --- a/userCode/devices/Src/Buzzer.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// -// Created by 25396 on 2023/3/14. -// - -#include "Buzzer.h" - - -bool buzzerWorkingFlag = false; - -void bsp_BuzzerOn(float freq) { - if (!buzzerWorkingFlag) { - buzzerWorkingFlag = 1; - HAL_TIM_Base_Start(&BUZZER_CLOCK); - HAL_TIM_PWM_Start(&BUZZER_CLOCK, BUZZER_CLOCK_CHANNEL); - __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, freq); - } -} - -void bsp_BuzzerOff() { - if (buzzerWorkingFlag) { - buzzerWorkingFlag = 0; - HAL_TIM_PWM_Stop(&BUZZER_CLOCK, BUZZER_CLOCK_CHANNEL); - } -} \ No newline at end of file diff --git a/userCode/devices/Src/Device.cpp b/userCode/devices/Src/Device.cpp index 60e7361..c6b1a35 100644 --- a/userCode/devices/Src/Device.cpp +++ b/userCode/devices/Src/Device.cpp @@ -6,7 +6,6 @@ #include "Motor.h" #include "IMU.h" #include "ArmMotor.h" -#include "Buzzer.h" #include "LED.h" #include "ControlTask.h" #include "KF.h" @@ -200,10 +199,10 @@ int main() { MX_GPIO_Init(); MX_DMA_Init(); MX_TIM5_Init(); - MX_TIM4_Init(); MX_TIM6_Init(); MX_TIM7_Init(); MX_TIM10_Init(); + MX_TIM12_Init(); MX_ADC1_Init(); MX_ADC3_Init(); MX_USART1_UART_Init(); @@ -213,7 +212,6 @@ int main() { MX_CAN2_Init(); MX_I2C3_Init(); MX_SPI1_Init(); - MX_SPI2_Init(); // MX_IWDG_Init();//看门狗,若不使用遥控器需注释改行，否则程序不运行 MX_USB_DEVICE_Init(); /* USER CODE BEGIN 2 */ @@ -224,6 +222,7 @@ int main() { HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_2); HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3); + //TODO adc校准？ RemoteControl::init();//遥控器通讯初始化，使用UART3串口 ManiControl::Init();//上位机通讯初始化，使用UART6串口 diff --git a/userCode/devices/Src/ManiControl.cpp b/userCode/devices/Src/ManiControl.cpp index a3d1ada..c824ba6 100644 --- a/userCode/devices/Src/ManiControl.cpp +++ b/userCode/devices/Src/ManiControl.cpp @@ -126,14 +126,14 @@ void ManiControl::GetData(uint8_t bufIndex) { } case 0x04: { TaskFlag = CLAW; - mc_ctrl.ClawFlag = mani_rx_buff[bufIndex][5]; + mc_ctrl.ClawFlag = mani_rx_buff[bufIndex][4]; ClawSet(mc_ctrl.ClawFlag); break; } case 0x05: { TaskFlag = TRAY; - mc_ctrl.TrayFlag = mani_rx_buff[bufIndex][5]; + mc_ctrl.TrayFlag = mani_rx_buff[bufIndex][4]; //AutoTraySet(mc_ctrl.TrayFlag); break; diff --git a/userCode/devices/Src/StepperMotor.cpp b/userCode/devices/Src/StepperMotor.cpp index 1b1c690..d6c4003 100644 --- a/userCode/devices/Src/StepperMotor.cpp +++ b/userCode/devices/Src/StepperMotor.cpp @@ -12,17 +12,13 @@ StepperMotor::StepperMotor(MOTOR_INIT_t *_init) : Motor(_init, this) { } void StepperMotor::Handle() { - static int step_count = 0; if (stopFlag) { - STEP = 0; + HAL_TIM_PWM_Stop(&htim12, TIM_CHANNEL_2); } - step_count++; - if(step_count>1) { - if (STEP > 0) { - HAL_GPIO_TogglePin(STEP_GPIO_Port, STEP_Pin); - STEP--; - } - step_count = 0; + if(STEP > 0){ + STEP--; + } else{ + HAL_TIM_PWM_Stop(&htim12, TIM_CHANNEL_2); } } @@ -31,13 +27,15 @@ void StepperMotor::Grab(bool _posflag) { stopFlag = false; if(posflag && !_posflag){ DIR = 0; - HAL_GPIO_WritePin(DIR_GPIO_Port, DIR_Pin, GPIO_PIN_RESET); - STEP = 300; + HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14, GPIO_PIN_RESET); + STEP = 1000; posflag = false; } else if(!posflag && _posflag){ DIR = 1; - HAL_GPIO_WritePin(DIR_GPIO_Port, DIR_Pin, GPIO_PIN_SET); - STEP = 300; + HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14, GPIO_PIN_SET); + STEP = 1000; posflag = true; } + HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_2); + __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_2, 499); } diff --git a/userCode/tasks/Inc/ArmTask.h b/userCode/tasks/Inc/ArmTask.h index 3ca7e05..fbe85c4 100644 --- a/userCode/tasks/Inc/ArmTask.h +++ b/userCode/tasks/Inc/ArmTask.h @@ -10,7 +10,6 @@ #include "Motor.h" #include "ArmMotor.h" //#include "Servo.h" -#include "Buzzer.h" #include "StepperMotor.h" #define l2 152.8f From 0289dd724f1df665d1d46a21241934206f77c646 Mon Sep 17 00:00:00 2001 From: iDddddd <90831551+iDddddd@users.noreply.github.com> Date: Fri, 6 Oct 2023 15:50:44 +0800 Subject: [PATCH 4/4] version3.5.20 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增机械臂重置命令； 增加机械臂解算错误保护 --- MDK-ARM/RM_Frame_C.uvoptx | 35 +--------------------------- MDK-ARM/RM_Frame_C.uvprojx | 9 +++++++ userCode/devices/Inc/ARMMotor.h | 4 ++-- userCode/devices/Inc/ManiControl.h | 7 ++++-- userCode/devices/Src/ARMMotor.cpp | 25 ++++++++++++++++++-- userCode/devices/Src/ManiControl.cpp | 6 +++++ userCode/tasks/Inc/ArmTask.h | 2 +- userCode/tasks/Src/ArmTask.cpp | 14 +++++++++++ 8 files changed, 61 insertions(+), 41 deletions(-) diff --git a/MDK-ARM/RM_Frame_C.uvoptx b/MDK-ARM/RM_Frame_C.uvoptx index 6aabf6b..f93de3a 100644 --- a/MDK-ARM/RM_Frame_C.uvoptx +++ b/MDK-ARM/RM_Frame_C.uvoptx @@ -158,40 +158,7 @@ -U00550061510000054E543652 -O10446 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) -WA0 -WE0 -WVCE4 -WS2710 -WM0 -WP2 - - - 0 - 0 - 87 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - startup_stm32f407xx.s - - - - - 1 - 0 - 91 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - startup_stm32f407xx.s - - - - + 0 diff --git a/MDK-ARM/RM_Frame_C.uvprojx b/MDK-ARM/RM_Frame_C.uvprojx index f49023c..2aec365 100644 --- a/MDK-ARM/RM_Frame_C.uvprojx +++ b/MDK-ARM/RM_Frame_C.uvprojx @@ -878,4 +878,13 @@ + + + + RM_Frame_C + 1 + + + + diff --git a/userCode/devices/Inc/ARMMotor.h b/userCode/devices/Inc/ARMMotor.h index 7643c2e..90cb585 100644 --- a/userCode/devices/Inc/ARMMotor.h +++ b/userCode/devices/Inc/ARMMotor.h @@ -25,7 +25,7 @@ class SteppingMotor_v4 : public Motor, public CAN { ~SteppingMotor_v4(); void Handle() override; - + void Reset(); void SetTargetPosition(float pos); @@ -54,6 +54,7 @@ class SteppingMotor_v5 : public Motor, public CAN { void SetTargetPosition(float tarpos); + void Reset(); ~SteppingMotor_v5(); private: @@ -69,7 +70,6 @@ class SteppingMotor_v5 : public Motor, public CAN { int32_t nowPos = 0; void CANMessageGenerate() override; - }; #endif //RM_FRAME_C_ARMMOTOR_H diff --git a/userCode/devices/Inc/ManiControl.h b/userCode/devices/Inc/ManiControl.h index 8d74d7a..ce2a350 100644 --- a/userCode/devices/Inc/ManiControl.h +++ b/userCode/devices/Inc/ManiControl.h @@ -18,7 +18,8 @@ typedef enum { CLAW, TRAY, MOVE_VEL, - ARM_POS + ARM_POS, + ARM_RESET }TASK_FLAG_t; /*结构体定义--------------------------------------------------------------*/ @@ -48,6 +49,7 @@ typedef struct { typedef struct { ARM_col_t arm_col; ARM_pos_t arm_pos; + uint8_t arm_reset; ChassisDis_col_t chassisDis_col; ChassisVel_col_t chassisVel_col; uint8_t TrayFlag; @@ -74,7 +76,8 @@ extern void AutoChassisStop();//Realized in ChassisTask extern void ChassisDistanceSet(float x, float y, float o);//Realized in ChassisTask extern void ChassisVelocitySet(float x_vel, float y_vel, float w_vel);//Realized in ChassisTask extern void ArmJointSet(float Joint1Pos, float Joint2Pos, float Joint3Pos, float Joint4Pos, float Joint5Pos); -extern void ArmPositionSet(float x, float y, float z);//Realized in ArmJointTask +extern void ArmPositionSet(float x, float y, float z);//Realized in ArmTask +void ArmReset();//Realized in ArmTask //extern void AutoTraySet(uint8_t trayflag);//Realized in ArmJointTask extern void ClawSet(uint8_t clawflag);//Realized in ArmJointTask diff --git a/userCode/devices/Src/ARMMotor.cpp b/userCode/devices/Src/ARMMotor.cpp index ce18e53..43b2d7f 100644 --- a/userCode/devices/Src/ARMMotor.cpp +++ b/userCode/devices/Src/ARMMotor.cpp @@ -38,7 +38,7 @@ void SteppingMotor_v4::CANMessageGenerate() { } void SteppingMotor_v4::Handle() { - if(SendFlag) { + if (SendFlag) { if (stopFlag) { Pulse = 0; } else { @@ -71,6 +71,12 @@ void SteppingMotor_v4::SetTargetPosition(float pos) { TarPos = pos; } +void SteppingMotor_v4::Reset() { + stopFlag = true; + TarPos = 0; + NowPos = 0; +} + SteppingMotor_v4::~SteppingMotor_v4() = default; @@ -84,7 +90,8 @@ SteppingMotor_v5::SteppingMotor_v5(COMMU_INIT_t *commuInit, MOTOR_INIT_t *motorI SteppingMotor_v5::~SteppingMotor_v5() = default; -void SteppingMotor_v5::CANMessageGenerate() {; +void SteppingMotor_v5::CANMessageGenerate() { + ; if ((canQueue.rear + 1) % MAX_MESSAGE_COUNT != canQueue.front) { canQueue.Data[canQueue.rear].ID = can_ID; @@ -151,4 +158,18 @@ void SteppingMotor_v5::SetTargetPosition(float tarpos) { TarPos = tarpos; } +void SteppingMotor_v5::Reset() { + stopFlag = true; + TxMessageDLC = 0x03; + TxMessage[0] = 0x0A; + TxMessage[1] = 0x6D; + TxMessage[2] = 0x6B; + TxMessage[3] = 0x00; + TxMessage[4] = 0x00; + TxMessage[5] = 0x00; + TxMessage[6] = 0x00; + TxMessage[7] = 0x00; + CANMessageGenerate(); +} + diff --git a/userCode/devices/Src/ManiControl.cpp b/userCode/devices/Src/ManiControl.cpp index c824ba6..8e03554 100644 --- a/userCode/devices/Src/ManiControl.cpp +++ b/userCode/devices/Src/ManiControl.cpp @@ -157,6 +157,12 @@ void ManiControl::GetData(uint8_t bufIndex) { break; } + case 0x09:{ + TaskFlag = ARM_RESET; + mc_ctrl.arm_reset = mani_rx_buff[bufIndex][4]; + + ArmReset(); + } } } } diff --git a/userCode/tasks/Inc/ArmTask.h b/userCode/tasks/Inc/ArmTask.h index fbe85c4..0017162 100644 --- a/userCode/tasks/Inc/ArmTask.h +++ b/userCode/tasks/Inc/ArmTask.h @@ -14,7 +14,7 @@ #define l2 152.8f #define l3 130.46f -#define l4 133.36f +#define l4 110.69 class ArmTask { public: diff --git a/userCode/tasks/Src/ArmTask.cpp b/userCode/tasks/Src/ArmTask.cpp index 11d2fd6..33a33aa 100644 --- a/userCode/tasks/Src/ArmTask.cpp +++ b/userCode/tasks/Src/ArmTask.cpp @@ -128,6 +128,20 @@ void ArmTask::ArmCalc(float x,float y,float z){ Angle[1] = PI/2 - angle1 - angle4; Angle[2] = PI/2 - angle2; Angle[3] = PI/2 - angle3 - angle5; + for(float & i : Angle) { + if(i != i){//判断是否为nan + i = 0; + } + } +} + +void ArmReset() { + Joint1Motor.Reset(); + Joint2Motor.Reset(); + Joint3Motor.Reset(); + // Joint4Motor.Reset(); + Joint5Motor.Reset(); + }