diff --git a/2.0.ver b/2.0.ver new file mode 100644 index 00000000..e69de29b diff --git a/driver/inc/Driver_Motor.h b/driver/inc/Driver_Motor.h index 177f0690..53f6192f 100644 --- a/driver/inc/Driver_Motor.h +++ b/driver/inc/Driver_Motor.h @@ -24,7 +24,7 @@ typedef struct { int16_t speed; // 转子转速值, RPM float actualCurrent; //电流 int16_t temperature; //温度 - uint16_t updatedAt; // 更新电机数据时的tickCount + uint32_t updatedAt; // 更新电机数据时的tickCount uint8_t online; // 电机是否离线 int8_t angleEnabled; // 连续角度启用 @@ -67,4 +67,11 @@ void Motor_Update(Motor_Type *motor, uint8_t data[8]); */ void Motor_Set_Angle_Bias(Motor_Type *motor, float angleBias); +/** + * @brief 校准连续角度为输入量 + * + * @param calibratedAngle 校准角度 + */ +void Motor_Calibrate(Motor_Type *motor, float calibratedAngle); + #endif diff --git a/driver/inc/Driver_Protocol.h b/driver/inc/Driver_Protocol.h index 789d7427..8a2ff9ac 100644 --- a/driver/inc/Driver_Protocol.h +++ b/driver/inc/Driver_Protocol.h @@ -63,8 +63,8 @@ typedef struct { // 接收相关 uint16_t receive; // 是否接收并解包该协议 uint16_t receiveCount; // 接收计数 - uint16_t receiveSeq; // 接收包序号 - uint16_t lastReceiveSeq; // (仅供用户使用)上次接收序号 + int16_t receiveSeq; // 接收包序号 + int16_t lastReceiveSeq; // (仅供用户使用)上次接收序号 TickType_t receiveTime; // 接收时间 // 发送相关 void * node; // 发送节点 diff --git a/driver/src/Driver_Motor.c b/driver/src/Driver_Motor.c index 397878f8..a531b0ec 100644 --- a/driver/src/Driver_Motor.c +++ b/driver/src/Driver_Motor.c @@ -7,6 +7,7 @@ void Motor_Init(Motor_Type *motor, float reductionRate, int8_t angleEnabled, int motor->positionBias = -1; // -1为未赋值状态 motor->angleBias = -1; // -1为未赋值状态 motor->lastPosition = -1; // -1为未赋值状态 + motor->online = 1; motor->reductionRate = reductionRate; motor->angleEnabled = angleEnabled; motor->inputEnabled = inputEnabled; @@ -18,8 +19,12 @@ void Motor_Update(Motor_Type *motor, uint8_t data[8]) { int16_t actualCurrent = data[4] << 8 | data[5]; int16_t temperature = data[6]; float angle = 0; - motor->updatedAt = xTaskGetTickCount(); - motor->online = 1; + if (motor->online) { + motor->updatedAt = xTaskGetTickCount(); + } else if (xTaskGetTickCount() - motor->updatedAt >= 6000) { + motor->updatedAt = xTaskGetTickCount(); + motor->online = 1; + } // 更新转子信息 motor->position = position; @@ -47,7 +52,7 @@ void Motor_Update(Motor_Type *motor, uint8_t data[8]) { if (motor->positionBias != -1) { motor->angleBias = motor->positionBias / 8192.0f * 360; // 向下兼容 } else { - motor->angleBias = angle; // 将当前位置设为 + motor->angleBias = angle; // 将当前位置设为零点 } motor->angleBiasInit = 1; } @@ -63,3 +68,9 @@ void Motor_Set_Angle_Bias(Motor_Type *motor, float angleBias) { motor->angleBiasInit = 1; motor->lastPosition = -1; } + +void Motor_Calibrate(Motor_Type *motor, float calibratedAngle) { + motor->angleBias = motor->angle + motor->angleBias - calibratedAngle; + motor->angle = calibratedAngle; + motor->angleBiasInit = 1; +} diff --git a/mdk/gold-miner.uvoptx b/mdk/gold-miner.uvoptx new file mode 100644 index 00000000..9a699d8e --- /dev/null +++ b/mdk/gold-miner.uvoptx @@ -0,0 +1,1115 @@ + + + + 1.0 + +
### uVision Project, (C) Keil Software
+ + + *.c + *.s*; *.src; *.a* + *.obj + *.lib + *.txt; *.h; *.inc + *.plm + *.cpp + 0 + + + + 0 + 0 + + + + Target 1 + 0x4 + ARM-ADS + + 12000000 + + 1 + 1 + 0 + 1 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + .\ + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 1 + 0 + 1 + + 18 + + + 0 + STM32F429I-Discovery Quick Start Guide (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\32F429IDISCOVERY_QSG.pdf + + + 1 + Getting Started (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\DM00092920.pdf + + + 2 + User Manual (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\DM00093903.pdf + + + 3 + STM32F429I-Discovery: Blinky Lab (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\STMicroelectronics_Blinky_Lab.pdf + + + 4 + Bill of Materials (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\stm32f429i-disco_bom.zip + + + 5 + Gerber Files (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\stm32f429i-disco_gerber.zip + + + 6 + Schematics (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\stm32f429i-disco_sch.zip + + + 7 + STM32F429I-Discovery Web Page (STM32F429I-Discovery) + http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1199/PF259090 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 3 + + + + + + + + + + + Segger\JL2CM3.dll + + + + 0 + ARMRTXEVENTFLAGS + -L70 -Z18 -C0 -M0 -T1 + + + 0 + DLGTARM + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) + + + 0 + ARMDBGFLAGS + + + + 0 + JL2CM3 + -U59400616 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32F427II$CMSIS\Flash\STM32F4xx_2048.FLM) + + + 0 + UL2CM3 + UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32F427II$CMSIS\Flash\STM32F4xx_2048.FLM)) + + + + + 0 + 0 + 107 + 1 +
134229784
+ 0 + 0 + 0 + 0 + 0 + 1 + C:\Codes\RV-CornerStone\user\template\interrupt.c + + + + + 1 + 0 + 186 + 1 +
134234324
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 2 + 0 + 184 + 1 +
134234314
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 3 + 0 + 174 + 1 +
134234276
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 4 + 0 + 166 + 1 +
134234228
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 5 + 0 + 162 + 1 +
134234200
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 6 + 0 + 37 + 1 +
134232692
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Motor.c + + + + + 7 + 0 + 15 + 1 +
134232410
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Motor.c + + + + + 8 + 0 + 118 + 1 +
134224354
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Bridge.c + + \\Belief_exp2\../driver/src/Driver_Bridge.c\118 + + + 9 + 0 + 119 + 1 +
134224374
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Bridge.c + + \\Belief_exp2\../driver/src/Driver_Bridge.c\119 + + + + + 0 + 1 + LastWakeTime + + + 1 + 1 + position + + + 2 + 1 + speed + + + 3 + 1 + temperature + + + 4 + 1 + actualCurrent + + + 5 + 1 + motor + + + 6 + 1 + protocolInfo + + + 7 + 1 + ProtocolInfoList + + + 8 + 1 + taskName + + + 9 + 1 + protocolInfo + + + + 0 + + + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 + + + 0 + + 1 + 0 + 2 + 10000000 + + + + + + bsp + 0 + 0 + 0 + 0 + + + + core + 0 + 0 + 0 + 0 + + 2 + 1 + 2 + 0 + 0 + 0 + ..\core\startup_stm32f427xx.s + startup_stm32f427xx.s + 0 + 0 + + + + + library + 0 + 0 + 0 + 0 + + 3 + 2 + 1 + 0 + 0 + 0 + ..\library\std\src\misc.c + misc.c + 0 + 0 + + + 3 + 3 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_adc.c + stm32f4xx_adc.c + 0 + 0 + + + 3 + 4 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_can.c + stm32f4xx_can.c + 0 + 0 + + + 3 + 5 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_dma.c + stm32f4xx_dma.c + 0 + 0 + + + 3 + 6 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_dma2d.c + stm32f4xx_dma2d.c + 0 + 0 + + + 3 + 7 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_exti.c + stm32f4xx_exti.c + 0 + 0 + + + 3 + 8 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_flash.c + stm32f4xx_flash.c + 0 + 0 + + + 3 + 9 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_gpio.c + stm32f4xx_gpio.c + 0 + 0 + + + 3 + 10 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_rcc.c + stm32f4xx_rcc.c + 0 + 0 + + + 3 + 11 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_spi.c + stm32f4xx_spi.c + 0 + 0 + + + 3 + 12 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_syscfg.c + stm32f4xx_syscfg.c + 0 + 0 + + + 3 + 13 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_tim.c + stm32f4xx_tim.c + 0 + 0 + + + 3 + 14 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_usart.c + stm32f4xx_usart.c + 0 + 0 + + + 3 + 15 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_wwdg.c + stm32f4xx_wwdg.c + 0 + 0 + + + 3 + 16 + 1 + 0 + 0 + 0 + ..\library\freertos\croutine.c + croutine.c + 0 + 0 + + + 3 + 17 + 1 + 0 + 0 + 0 + ..\library\freertos\event_groups.c + event_groups.c + 0 + 0 + + + 3 + 18 + 1 + 0 + 0 + 0 + ..\library\freertos\list.c + list.c + 0 + 0 + + + 3 + 19 + 1 + 0 + 0 + 0 + ..\library\freertos\queue.c + queue.c + 0 + 0 + + + 3 + 20 + 1 + 0 + 0 + 0 + ..\library\freertos\tasks.c + tasks.c + 0 + 0 + + + 3 + 21 + 1 + 0 + 0 + 0 + ..\library\freertos\timers.c + timers.c + 0 + 0 + + + 3 + 22 + 1 + 0 + 0 + 0 + ..\library\freertos\portable\MemMang\heap_4.c + heap_4.c + 0 + 0 + + + 3 + 23 + 1 + 0 + 0 + 0 + ..\library\freertos\portable\RVDS\ARM_CM4F\port.c + port.c + 0 + 0 + + + 3 + 24 + 1 + 0 + 0 + 0 + ..\library\atom\delay\delay.c + delay.c + 0 + 0 + + + 3 + 25 + 1 + 0 + 0 + 0 + ..\library\atom\sys\sys.c + sys.c + 0 + 0 + + + 3 + 26 + 1 + 0 + 0 + 0 + ..\library\mpu6500\MadgwickAHRS.c + MadgwickAHRS.c + 0 + 0 + + + 3 + 27 + 1 + 0 + 0 + 0 + ..\library\mpu6500\mpu6500_driver.c + mpu6500_driver.c + 0 + 0 + + + 3 + 28 + 1 + 0 + 0 + 0 + ..\library\mpu6500\mpu6500_i2c.c + mpu6500_i2c.c + 0 + 0 + + + 3 + 29 + 1 + 0 + 0 + 0 + ..\library\atom\debug\beep.c + beep.c + 0 + 0 + + + 3 + 30 + 1 + 0 + 0 + 0 + ..\library\atom\debug\key.c + key.c + 0 + 0 + + + 3 + 31 + 1 + 0 + 0 + 0 + ..\library\atom\debug\led.c + led.c + 0 + 0 + + + 3 + 32 + 1 + 0 + 0 + 0 + ..\library\vegmath\vegmath.c + vegmath.c + 0 + 0 + + + 3 + 33 + 1 + 0 + 0 + 0 + ..\library\oled\oled.c + oled.c + 0 + 0 + + + 3 + 34 + 1 + 0 + 0 + 0 + ..\library\atom\usart\usart.c + usart.c + 0 + 0 + + + + + driver + 1 + 0 + 0 + 0 + + 4 + 35 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_CAN.c + Driver_CAN.c + 0 + 0 + + + 4 + 36 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_DBUS.c + Driver_DBUS.c + 0 + 0 + + + 4 + 37 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_PID.c + Driver_PID.c + 0 + 0 + + + 4 + 38 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Magic.c + Driver_Magic.c + 0 + 0 + + + 4 + 39 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Chassis.c + Driver_Chassis.c + 0 + 0 + + + 4 + 40 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Motor.c + Driver_Motor.c + 0 + 0 + + + 4 + 41 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Filter.c + Driver_Filter.c + 0 + 0 + + + 4 + 42 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Gyroscope.c + Driver_Gyroscope.c + 0 + 0 + + + 4 + 43 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_BSP.c + Driver_BSP.c + 0 + 0 + + + 4 + 44 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Protocol.c + Driver_Protocol.c + 0 + 0 + + + 4 + 45 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Fsm.c + Driver_Fsm.c + 0 + 0 + + + 4 + 46 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Bridge.c + Driver_Bridge.c + 0 + 0 + + + + + user_common + 0 + 0 + 0 + 0 + + 5 + 47 + 5 + 0 + 0 + 0 + ..\user\common\macro.h + macro.h + 0 + 0 + + + 5 + 48 + 1 + 0 + 0 + 0 + ..\user\common\system_stm32f4xx.c + system_stm32f4xx.c + 0 + 0 + + + 5 + 49 + 5 + 0 + 0 + 0 + ..\user\common\interrupt.h + interrupt.h + 0 + 0 + + + + + user_gold_miner + 1 + 0 + 0 + 0 + + 6 + 50 + 1 + 0 + 0 + 0 + ..\user\gold-miner\interrupt.c + interrupt.c + 0 + 0 + + + 6 + 51 + 1 + 0 + 0 + 0 + ..\user\gold-miner\main.c + main.c + 0 + 0 + + + 6 + 52 + 1 + 0 + 0 + 0 + ..\user\gold-miner\tasks.c + tasks.c + 0 + 0 + + + + diff --git a/mdk/gold-miner.uvprojx b/mdk/gold-miner.uvprojx new file mode 100644 index 00000000..c27101a6 --- /dev/null +++ b/mdk/gold-miner.uvprojx @@ -0,0 +1,669 @@ + + + + 2.1 + +
### uVision Project, (C) Keil Software
+ + + + Target 1 + 0x4 + ARM-ADS + 5060183::V5.06 update 2 (build 183)::ARMCC + + + STM32F427II + STMicroelectronics + Keil.STM32F4xx_DFP.2.15.0 + http://www.keil.com/pack/ + IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE + + + UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32F427II$Flash\STM32F4xx_2048.FLM)) + 0 + $$Device:STM32F427II$Device\Include\stm32f4xx.h + + + + + + + + + + $$Device:STM32F427II$SVD\STM32F427x.svd + 0 + 0 + + + + + + + 0 + 0 + 0 + 0 + 1 + + .\Objects\ + Belief exp2 + 1 + 0 + 1 + 1 + 1 + .\ + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + 0 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 1 + + + SARMCM3.DLL + -REMAP -MPU + DCM.DLL + -pCM4 + SARMCM3.DLL + -MPU + TCM.DLL + -pCM4 + + + + 1 + 0 + 0 + 0 + 16 + + + + + 1 + 0 + 0 + 1 + 1 + 4096 + + 1 + BIN\UL2CM3.DLL + "" () + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M4" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 2 + 1 + 0 + 8 + 0 + 0 + 0 + 0 + 3 + 3 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x30000 + + + 1 + 0x8000000 + 0x200000 + + + 0 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8000000 + 0x200000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x30000 + + + 0 + 0x10000000 + 0x10000 + + + + + + 1 + 1 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 2 + 0 + 0 + 0 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + + --gnu + STM32F427_437xx,USE_STDPERIPH_DRIVER + + ..\core;..\bsp\inc;..\driver\inc;..\library\std\inc;..\user;..\library\freertos\include;..\library\freertos\portable\RVDS\ARM_CM4F;..\user\common;..\library\atom\delay;..\library\atom\sys;..\library\atom\usart;..\library\atom\debug;..\library\mpu6500;..\library\vegmath;..\user\gold-miner;..\library\oled + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + 1 + 0 + 0 + 0 + 1 + 0 + 0x08000000 + 0x20000000 + + + + + + + + + + + + + bsp + + + core + + + startup_stm32f427xx.s + 2 + ..\core\startup_stm32f427xx.s + + + + + library + + + misc.c + 1 + ..\library\std\src\misc.c + + + stm32f4xx_adc.c + 1 + ..\library\std\src\stm32f4xx_adc.c + + + stm32f4xx_can.c + 1 + ..\library\std\src\stm32f4xx_can.c + + + stm32f4xx_dma.c + 1 + ..\library\std\src\stm32f4xx_dma.c + + + stm32f4xx_dma2d.c + 1 + ..\library\std\src\stm32f4xx_dma2d.c + + + stm32f4xx_exti.c + 1 + ..\library\std\src\stm32f4xx_exti.c + + + stm32f4xx_flash.c + 1 + ..\library\std\src\stm32f4xx_flash.c + + + stm32f4xx_gpio.c + 1 + ..\library\std\src\stm32f4xx_gpio.c + + + stm32f4xx_rcc.c + 1 + ..\library\std\src\stm32f4xx_rcc.c + + + stm32f4xx_spi.c + 1 + ..\library\std\src\stm32f4xx_spi.c + + + stm32f4xx_syscfg.c + 1 + ..\library\std\src\stm32f4xx_syscfg.c + + + stm32f4xx_tim.c + 1 + ..\library\std\src\stm32f4xx_tim.c + + + stm32f4xx_usart.c + 1 + ..\library\std\src\stm32f4xx_usart.c + + + stm32f4xx_wwdg.c + 1 + ..\library\std\src\stm32f4xx_wwdg.c + + + croutine.c + 1 + ..\library\freertos\croutine.c + + + event_groups.c + 1 + ..\library\freertos\event_groups.c + + + list.c + 1 + ..\library\freertos\list.c + + + queue.c + 1 + ..\library\freertos\queue.c + + + tasks.c + 1 + ..\library\freertos\tasks.c + + + timers.c + 1 + ..\library\freertos\timers.c + + + heap_4.c + 1 + ..\library\freertos\portable\MemMang\heap_4.c + + + port.c + 1 + ..\library\freertos\portable\RVDS\ARM_CM4F\port.c + + + delay.c + 1 + ..\library\atom\delay\delay.c + + + sys.c + 1 + ..\library\atom\sys\sys.c + + + MadgwickAHRS.c + 1 + ..\library\mpu6500\MadgwickAHRS.c + + + mpu6500_driver.c + 1 + ..\library\mpu6500\mpu6500_driver.c + + + mpu6500_i2c.c + 1 + ..\library\mpu6500\mpu6500_i2c.c + + + beep.c + 1 + ..\library\atom\debug\beep.c + + + key.c + 1 + ..\library\atom\debug\key.c + + + led.c + 1 + ..\library\atom\debug\led.c + + + vegmath.c + 1 + ..\library\vegmath\vegmath.c + + + oled.c + 1 + ..\library\oled\oled.c + + + usart.c + 1 + ..\library\atom\usart\usart.c + + + + + driver + + + Driver_CAN.c + 1 + ..\driver\src\Driver_CAN.c + + + Driver_DBUS.c + 1 + ..\driver\src\Driver_DBUS.c + + + Driver_PID.c + 1 + ..\driver\src\Driver_PID.c + + + Driver_Magic.c + 1 + ..\driver\src\Driver_Magic.c + + + Driver_Chassis.c + 1 + ..\driver\src\Driver_Chassis.c + + + Driver_Motor.c + 1 + ..\driver\src\Driver_Motor.c + + + Driver_Filter.c + 1 + ..\driver\src\Driver_Filter.c + + + Driver_Gyroscope.c + 1 + ..\driver\src\Driver_Gyroscope.c + + + Driver_BSP.c + 1 + ..\driver\src\Driver_BSP.c + + + Driver_Protocol.c + 1 + ..\driver\src\Driver_Protocol.c + + + Driver_Fsm.c + 1 + ..\driver\src\Driver_Fsm.c + + + Driver_Bridge.c + 1 + ..\driver\src\Driver_Bridge.c + + + + + user_common + + + macro.h + 5 + ..\user\common\macro.h + + + system_stm32f4xx.c + 1 + ..\user\common\system_stm32f4xx.c + + + interrupt.h + 5 + ..\user\common\interrupt.h + + + + + user_gold_miner + + + interrupt.c + 1 + ..\user\gold-miner\interrupt.c + + + main.c + 1 + ..\user\gold-miner\main.c + + + tasks.c + 1 + ..\user\gold-miner\tasks.c + + + + + + + + diff --git a/user/common/default_config.h b/user/common/default_config.h index 2ae4be0c..c11e323e 100644 --- a/user/common/default_config.h +++ b/user/common/default_config.h @@ -4,7 +4,7 @@ // 总线设置 #define CAN_DEVICE_ID 0x501 // 发送数据时CAN标准帧中的ID 0x501-0x503 -#define CAN_TIMEOUT 100 // 电机超过该时间(ms)无数据将报警 +#define CAN_TIMEOUT 1000 // 电机超过该时间(ms)无数据将报警 // 调试 #define DEBUG_ENABLED 0 // 调试开关 diff --git a/user/common/macro.h b/user/common/macro.h index 7bcad2f7..febba4bf 100644 --- a/user/common/macro.h +++ b/user/common/macro.h @@ -8,6 +8,11 @@ // clang-format off +// 电机减速比 +#define MOTOR_REDUCTION_RATE_3508 19.2f +#define MOTOR_REDUCTION_RATE_6020 1 +#define MOTOR_REDUCTION_RATE_2006 36.0f + // 单位换算 #define RPM2RPS 0.10471975f // 2 * 3.1415926f / 60.0f, round/min->rad/s #define DPS2RPS 0.01745329f // 3.1415926f / 180.0f, degree/s ->rad/s diff --git a/user/gold-miner/config.h b/user/gold-miner/config.h new file mode 100644 index 00000000..7732c8c9 --- /dev/null +++ b/user/gold-miner/config.h @@ -0,0 +1,28 @@ +/** + * @brief 机器人参数 + * @note 默认参数及参数列表见 default_config.h + */ + +#include "default_config.h" + +// 总线设置 +#define CAN_DEVICE_ID 0x501 // 发送数据时CAN标准帧中的ID 0x501-0x503 + +// 调试 +#define DEBUG_ENABLED 0 // 调试开关 +#define SERIAL_DEBUG_PORT USART6 // 串口调试端口 + +// 陀螺仪 +#define BOARD_FRONT_IS_UP 1 // 板子正面朝上 +#define BOARD_SHORT_SIDE_IS_PARALLEL_TO_PITCH 1 // 板子短边朝下 +#define GYROSCOPE_YAW_FILTER_THRESHOLD 0.003f // 零飘修正阈值 +#define GYROSCOPE_LSB 16.384f // 陀螺仪敏感度 2^16/4000 +#define ACCELERATE_LSB 4096.0f // 加速度计敏感度 2^16/16 + +// 底盘 +#define CHASSIS_MOTOR_REDUCTION_RATE 19.2f // 底盘电机减速比 +#define CHASSIS_SIZE_K 0.385f // 测量值, 机器人中心点到XY边缘的距离之和 +#define CHASSIS_INVERSE_WHEEL_RADIUS 13.16f // 测量值, 麦克纳姆轮半径的倒数 + +// DMA +#define DMA_BUFFER_LENGTH 128 // DMA发送接收长度 \ No newline at end of file diff --git a/user/gold-miner/handle.h b/user/gold-miner/handle.h new file mode 100644 index 00000000..64fbc36d --- /dev/null +++ b/user/gold-miner/handle.h @@ -0,0 +1,87 @@ +#ifndef __HANDLE_H +#define __HANDLE_H + +#include "sys.h" +#include "delay.h" +#include "led.h" +#include "beep.h" +#include "key.h" +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" +#include "vegmath.h" +#include "config.h" +#include "oled.h" +#include "Driver_BSP.h" +#include "Driver_Filter.h" +#include "Driver_PID.h" +#include "Driver_DBUS.h" +#include "Driver_CAN.h" +#include "Driver_Motor.h" +#include "Driver_Chassis.h" +#include "mpu6500_driver.h" +#include "Driver_Gyroscope.h" +#include "Driver_Protocol.h" +#include "Driver_Bridge.h" +#include "Driver_Magic.h" +#include "Driver_Fsm.h" + +#ifdef __HANDLE_GLOBALS +#define __HANDLE_EXT +#else +#define __HANDLE_EXT extern +#endif + +// Stone ID +__HANDLE_EXT uint8_t Board_Id, Robot_Id; + +// 功能开关 +__HANDLE_EXT uint8_t SafetyMode; +__HANDLE_EXT uint8_t JointCalibrated; + +// TIM +__HANDLE_EXT volatile uint32_t ulHighFrequencyTimerTicks; + +// 底盘电机 +__HANDLE_EXT Motor_Type Motor_LF, Motor_RF, Motor_RB, Motor_LB; + +// 机械臂 +__HANDLE_EXT Motor_Type Motor_BaseJoint, Motor_ShoulderJoint, Motor_ElbowJoint, Motor_WristJoint1, Motor_WristJoint2; +__HANDLE_EXT PID_Type PID_BaseJointSpeed, PID_BaseJointAngle, PID_ShoulderJointSpeed, PID_ShoulderJointAngle, PID_ElbowSpeed, PID_ElbowAngle, + PID_WristJoint1Speed, PID_WristJoint1Angle, PID_WristJoint2Speed, PID_WristJoint2Angle; + +// 遥控器 +__HANDLE_EXT uint8_t remoteBuffer[DBUS_LENGTH + DBUS_BACK_LENGTH]; +__HANDLE_EXT Remote_Type remoteData; +__HANDLE_EXT Keyboard_Type keyboardData; +__HANDLE_EXT Mouse_Type mouseData; + +// 陀螺仪 +__HANDLE_EXT volatile ImuData_Type ImuData; +__HANDLE_EXT volatile GyroscopeData_Type Gyroscope_EulerData; + +// 调试数据 +__HANDLE_EXT DebugData_Type DebugData; + +// 底盘 +__HANDLE_EXT ChassisData_Type ChassisData; +__HANDLE_EXT Motor_Type Motor_LF, Motor_RF, Motor_RB, Motor_LB; +__HANDLE_EXT PID_Type PID_LFCM, PID_LBCM, PID_RBCM, PID_RFCM, PID_YawAngle, PID_YawSpeed; + +// 通讯协议 +__HANDLE_EXT ProtocolData_Type ProtocolData; +__HANDLE_EXT Node_Type Node_Judge, Node_Host, Node_Board; + +// PWM +__HANDLE_EXT PWM_Type PWM_Test; + +// 总线 +__HANDLE_EXT Bridge_Type BridgeData; + +/** + * @brief 初始化结构体 + * @note 该函数将在所有硬件及任务初始化之前执行 + */ +void Handle_Init(void); + +#endif diff --git a/user/gold-miner/interrupt.c b/user/gold-miner/interrupt.c new file mode 100644 index 00000000..186b4846 --- /dev/null +++ b/user/gold-miner/interrupt.c @@ -0,0 +1,176 @@ +/** + * @brief 中断服务函数根据地 + */ +#include "handle.h" + +// EXTI9_5 陀螺仪中断 +void EXTI9_5_IRQHandler(void) { + uint8_t suc; + if (EXTI_GetITStatus(EXTI_Line8) != RESET) { + EXTI_ClearFlag(EXTI_Line8); + EXTI_ClearITPendingBit(EXTI_Line8); + Gyroscope_Update(&Gyroscope_EulerData); + } +} + +// DBus空闲中断(USART1) +void USART1_IRQHandler(void) { + uint8_t UARTtemp; + + UARTtemp = USART1->DR; + UARTtemp = USART1->SR; + + // disabe DMA + DMA_Disable(USART1_Rx); + + //数据量正确 + if (DMA_Get_Stream(USART1_Rx)->NDTR == DBUS_BACK_LENGTH) { + DBus_Update(&remoteData, &keyboardData, &mouseData, remoteBuffer); //解码 + } + + // enable DMA + DMA_Enable(USART1_Rx, DBUS_LENGTH + DBUS_BACK_LENGTH); +} + +/** + * @brief USART3 串口中断 + */ +void USART3_IRQHandler(void) { + uint8_t tmp; + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 3); +} + +/** + * @brief USART6 串口中断 + */ +void USART6_IRQHandler(void) { + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 6); +} + +/** + * @brief UART7 串口中断 + */ +void UART7_IRQHandler(void) { + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 7); +} + +/** + * @brief UART8 串口中断 + */ +void UART8_IRQHandler(void) { + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 8); +} + +// CAN1数据接收中断服务函数 +void CAN1_RX0_IRQHandler(void) { + Bridge_Receive_CAN(&BridgeData, CAN1_BRIDGE); +} + +// CAN2数据接收中断服务函数 +void CAN2_RX0_IRQHandler(void) { + Bridge_Receive_CAN(&BridgeData, CAN2_BRIDGE); +} + +// void CAN1_SCE_IRQHandler(void) { +// RED_LIGHT_ON; +// CAN_ClearITPendingBit(CAN1, CAN_IT_EWG | CAN_IT_EPV | CAN_IT_BOF | CAN_IT_LEC | CAN_IT_ERR); +// } + +// TIM2 高频计数器 +extern volatile uint32_t ulHighFrequencyTimerTicks; + +void TIM2_IRQHandler(void) { + if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) { + ulHighFrequencyTimerTicks++; + TIM_ClearITPendingBit(TIM2, TIM_IT_Update); + TIM_ClearFlag(TIM2, TIM_FLAG_Update); + } +} + +/** + * @brief This function handles NMI exception. + * @param None + * @return None + */ + +void NMI_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Hard Fault exception. + * @param None + * @return None + */ +void HardFault_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Memory Manage exception. + * @param None + * @return None + */ +void MemManage_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Bus Fault exception. + * @param None + * @return None + */ +void BusFault_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Usage Fault exception. + * @param None + * @return None + */ +void UsageFault_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Debug Monitor exception. + * @param None + * @return None + */ +void DebugMon_Handler(void) { + while (1) { + } +} + +// /** +// * @brief This function handles SVCall exception. +// * @param None +// * @return None +// */ +// void SVC_Handler(void) { +// //while(1){} +// } + +// /** +// * @brief This function handles PendSVC exception. +// * @param None +// * @return None +// */ +// void PendSV_Handler(void) { +// //while(1){} +// } + +// /** +// * @brief This function handles SysTick Handler. +// * @param None +// * @return None +// */ +// void SysTick_Handler(void) { +// //while(1){} +// } \ No newline at end of file diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c new file mode 100644 index 00000000..df0812d4 --- /dev/null +++ b/user/gold-miner/main.c @@ -0,0 +1,120 @@ +#define __HANDLE_GLOBALS + +#include "config.h" +#include "macro.h" +#include "handle.h" +#include "FreeRTOS.h" +#include "task.h" +#include "tasks.h" +// #include "queue.h" + +int main(void) { + + //设置中断优先级位数 + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + //调试相关 + Delay_Init(180); // 初始化延时 + LED_Init(); // 初始化LED + Beep_Init(); // 初始化蜂鸣器 + + /******************************************************************************* + * 硬件初始化 * + *******************************************************************************/ + + // 获得设备ID + BSP_Stone_Id_Init(&Board_Id, &Robot_Id); + + // 底盘电机 + Motor_Init(&Motor_LF, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + Motor_Init(&Motor_LB, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + Motor_Init(&Motor_RB, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + Motor_Init(&Motor_RF, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + + // 机械臂电机 + Motor_Init(&Motor_BaseJoint, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); + Motor_Init(&Motor_ShoulderJoint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); + Motor_Init(&Motor_ElbowJoint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); + Motor_Init(&Motor_WristJoint1, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); + Motor_Init(&Motor_WristJoint2, MOTOR_REDUCTION_RATE_3508 * 30, ENABLE, ENABLE); + + // 遥控器数据初始化 + DBUS_Init(&remoteData, &keyboardData, &mouseData); + + // 通讯协议初始化 + Protocol_Init(&Node_Judge, &ProtocolData); + Protocol_Init(&Node_Host, &ProtocolData); + Protocol_Init(&Node_Board, &ProtocolData); + + // 硬件配置 + BSP_CAN_Init(); + BSP_DBUS_Init(remoteBuffer); + BSP_TIM2_Init(); + BSP_IMU_Init(); + BSP_Laser_Init(); + BSP_Beep_Init(); + BSP_LED_Init(); + BSP_User_Power_Init(); + // BSP_OLED_Init(); + + // USART + // BSP_USART3_Init(115200, USART_IT_IDLE); // Rx故障 + // BSP_USART6_Init(115200, USART_IT_IDLE); // Tx故障 + BSP_UART7_Init(115200, USART_IT_IDLE); + BSP_UART8_Init(115200, USART_IT_IDLE); + + // PWM + // BSP_PWM_Set_Port(&PWM_Test, PWM_PD12); + // BSP_PWM_Init(&PWM_Test, 9000, 200, TIM_OCPolarity_Low); + + // ADC + BSP_ADC1_Init(1, ADC_Channel6, 0); + + // 总线设置 + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x201, &Motor_LF); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x202, &Motor_LB); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x203, &Motor_RB); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x204, &Motor_RF); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x205, &Motor_BaseJoint); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x201, &Motor_ShoulderJoint); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_ElbowJoint); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x206, &Motor_WristJoint1); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x203, &Motor_WristJoint2); + Bridge_Bind(&BridgeData, USART_BRIDGE, 7, &Node_Host); + Bridge_Bind(&BridgeData, USART_BRIDGE, 8, &Node_Judge); + + // 陀螺仪 + Gyroscope_Set_Bias(&ImuData, 0, 0, 0); // 设置静态误差 + Gyroscope_Init(&Gyroscope_EulerData, 300); // 初始化 + + /******************************************************************************* + * 任务初始化 * + *******************************************************************************/ + + // 低优先级任务 + xTaskCreate(Task_Blink, "Task_Blink", 400, NULL, 3, NULL); // 跑马灯/呼吸灯任务 + // xTaskCreate(Task_OLED, "Task_OLED", 400, NULL, 3, NULL);// OLED 菜单任务 + // xTaskCreate(Task_Startup_Music, "Task_Startup_Music", 400, NULL, 3, NULL);// 开机音乐任务 + + // 等待遥控器开启 + while (!remoteData.state) { + } + + // 高优先级任务 + xTaskCreate(Task_Control, "Task_Control", 400, NULL, 9, NULL); //模式切换任务 + // xTaskCreate(Task_Chassis, "Task_Chassis", 400, NULL, 5, NULL); // 底盘运动任务 + xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 + xTaskCreate(Task_Communication, "Task_Communication", 500, NULL, 6, NULL); // 通讯测试任务 + xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 + + // 定义协议发送频率 + Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 + Bridge_Send_Protocol(&Node_Host, 0x405, 50); // 机械臂数据 + + //启动调度,开始执行任务 + vTaskStartScheduler(); + + //系统启动失败:定时器任务或者空闲任务的heap空间不足 + while (1) { + } +} diff --git a/user/protocol.h b/user/gold-miner/protocol.h similarity index 85% rename from user/protocol.h rename to user/gold-miner/protocol.h index 8a8f6aee..996ac3ec 100644 --- a/user/protocol.h +++ b/user/gold-miner/protocol.h @@ -11,8 +11,8 @@ * 裁判系统 0xF100 - 0xF1FF * 车间通讯 0xF200 - 0xF2FF * @version judge.yml v1.0 - * @version host.yml v0.1 - * @version user.yml v0.1 + * @version host.yml v1.3 + * @version user.yml v1.0 */ /********************************************************************** @@ -416,6 +416,25 @@ typedef union { }; } dbus_data_t; +typedef union { + struct { + uint8_t state_code; + int16_t base_joint_position; + int16_t base_joint_speed; + int16_t shoulder_joint_position; + int16_t shoulder_joint_speed; + int16_t elbow_joint_position; + int16_t elbow_joint_speed; + int16_t wrist_joint_1_position; + int16_t wrist_joint_1_speed; + int16_t wrist_joint_2_position; + int16_t wrist_joint_2_speed; + }; + struct { + uint8_t data[21]; + }; +} joint_info_t; + typedef union { struct { uint16_t data_cmd_id; @@ -449,11 +468,15 @@ typedef union { #define PROTOCOL_INFO_LIST \ { \ - {0X0001, 11, 1}, {0X0002, 1, 1}, {0X0003, 32, 1}, {0X0004, 3, 1}, {0X0101, 4, 1}, {0X0102, 4, 1}, {0X0104, 2, 1}, {0X0105, 1, 1}, {0X0201, 27, 1}, \ - {0X0202, 16, 1}, {0X0203, 16, 1}, {0X0204, 1, 1}, {0X0205, 1, 1}, {0X0206, 1, 1}, {0X0207, 7, 1}, {0X0208, 6, 1}, {0X0209, 4, 1}, {0X020A, 12, 1}, \ - {0XF100, 8, 0}, {0XF101, 21, 0}, {0XF102, 36, 0}, {0XF103, 81, 0}, {0XF110, 51, 0}, {0XF104, 111, 0}, {0X1024, 32, 0}, {0X6666, 24, 0}, \ - {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ - 0X0502, 16, 1 \ + {0X0001, 11, 1, 0, -1, -1}, {0X0002, 1, 1, 0, -1, -1}, {0X0003, 32, 1, 0, -1, -1}, {0X0004, 3, 1, 0, -1, -1}, {0X0101, 4, 1, 0, -1, -1}, \ + {0X0102, 4, 1, 0, -1, -1}, {0X0104, 2, 1, 0, -1, -1}, {0X0105, 1, 1, 0, -1, -1}, {0X0201, 27, 1, 0, -1, -1}, {0X0202, 16, 1, 0, -1, -1}, \ + {0X0203, 16, 1, 0, -1, -1}, {0X0204, 1, 1, 0, -1, -1}, {0X0205, 1, 1, 0, -1, -1}, {0X0206, 1, 1, 0, -1, -1}, {0X0207, 7, 1, 0, -1, -1}, \ + {0X0208, 6, 1, 0, -1, -1}, {0X0209, 4, 1, 0, -1, -1}, {0X020A, 12, 1, 0, -1, -1}, {0XF100, 8, 0, 0, -1, -1}, {0XF101, 21, 0, 0, -1, -1}, \ + {0XF102, 36, 0, 0, -1, -1}, {0XF103, 81, 0, 0, -1, -1}, {0XF110, 51, 0, 0, -1, -1}, {0XF104, 111, 0, 0, -1, -1}, {0X1024, 32, 0, 0, -1, -1}, \ + {0X6666, 24, 0, 0, -1, -1}, {0X0120, 0, 1, 0, -1, -1}, {0X0401, 9, 1, 0, -1, -1}, {0X0402, 12, 1, 0, -1, -1}, {0X0403, 12, 0, 0, -1, -1}, \ + {0X0404, 18, 0, 0, -1, -1}, {0X0405, 21, 1, 0, -1, -1}, {0X0406, 21, 1, 0, -1, -1}, {0X0407, 21, 1, 0, -1, -1}, {0XF301, 22, 1, 0, -1, -1}, \ + {0X0501, 16, 1, 0, -1, -1}, { \ + 0X0502, 16, 1, 0, -1, -1 \ } \ } @@ -493,13 +516,16 @@ typedef union { autoaim_data_t autoaimData; // 0X0401 云台控制 chassis_data_t chassisData; // 0X0402 底盘控制 gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 - dbus_data_t dbusData; // 0X0404 遥控器及键鼠数据 + dbus_data_t dbugData; // 0X0404 遥控器及键鼠数据 + joint_info_t jointState; // 0X0405 工程机械臂数据 + joint_info_t jointControl; // 0X0406 工程机械臂数据 + joint_info_t jointCalibration; // 0X0407 工程机械臂数据 robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 board_interactive_data_t boardBeta; // 0X0502 主控板间通信 }; struct { - uint8_t data[618]; + uint8_t data[681]; }; } ProtocolData_Type; diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c new file mode 100644 index 00000000..7fd790a6 --- /dev/null +++ b/user/gold-miner/tasks.c @@ -0,0 +1,284 @@ +/** + * @brief 任务 + */ +#include "tasks.h" +#include "config.h" +#include "macro.h" +#include "handle.h" + +void Task_Control(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); + float interval = 0.01; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + while (1) { + // SafetyMode = (RIGHT_SWITCH_BOTTOM && LEFT_SWITCH_BOTTOM); + SafetyMode = SafetyMode ? 1 : (ABS(remoteData.rx) > 440); + vTaskDelayUntil(&LastWakeTime, intervalms); + } + vTaskDelete(NULL); +} + +void Task_Chassis(void *Parameters) { + // 任务 + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.005; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + + // 运动模式 + int mode = 1; // 底盘运动模式,1直线,2转弯 + int lastMode = 2; // 上一次的运动模式 + float yawAngleTarget = 0; // 目标值 + float yawAngle, yawSpeed; // 反馈值 + + // 初始化麦轮角速度PID + PID_Init(&PID_LFCM, 15, 0.3, 0, 4000, 2000); + PID_Init(&PID_LBCM, 15, 0.3, 0, 4000, 2000); + PID_Init(&PID_RBCM, 15, 0.3, 0, 4000, 2000); + PID_Init(&PID_RFCM, 15, 0.3, 0, 4000, 2000); + + // 初始化航向角角度PID和角速度PID + PID_Init(&PID_YawAngle, 10, 0, 0, 1000, 1000); + PID_Init(&PID_YawSpeed, 2, 0, 0, 4000, 1000); + + // 初始化底盘 + Chassis_Init(&ChassisData); + + while (1) { + + // 更新运动模式 + mode = ABS(remoteData.rx) < 5 ? 1 : 2; + + // 设置反馈值 + yawAngle = Gyroscope_EulerData.yaw; // 航向角角度反馈 + yawSpeed = -1 * ImuData.gz / GYROSCOPE_LSB; // 逆时针为正 + + // 切换运动模式 + if (mode != lastMode) { + PID_YawAngle.output_I = 0; // 清空角度PID积分 + PID_YawSpeed.output_I = 0; // 清空角速度PID积分 + yawAngleTarget = yawAngle; // 更新角度PID目标值 + lastMode = mode; // 更新lastMode + } + + // 根据运动模式计算PID + if (mode == 1) { + PID_Calculate(&PID_YawAngle, yawAngleTarget, yawAngle); // 计算航向角角度PID + PID_Calculate(&PID_YawSpeed, PID_YawAngle.output, yawSpeed); // 计算航向角角速度PID + } else { + PID_Calculate(&PID_YawSpeed, -remoteData.rx, yawSpeed); // 计算航向角角速度PID + } + + // 设置底盘总体移动速度 + Chassis_Update(&ChassisData, (float) -remoteData.lx / 660.0f, (float) remoteData.ly / 660.0f, (float) PID_YawSpeed.output / 660.0f); + + // 麦轮解算 + Chassis_Calculate_Rotor_Speed(&ChassisData); + + // 设置转子速度上限 (rad/s) + Chassis_Limit_Rotor_Speed(&ChassisData, 300); + + // 计算输出电流PID + PID_Calculate(&PID_LFCM, ChassisData.rotorSpeed[0], Motor_LF.speed * RPM2RPS); + PID_Calculate(&PID_LBCM, ChassisData.rotorSpeed[1], Motor_LB.speed * RPM2RPS); + PID_Calculate(&PID_RBCM, ChassisData.rotorSpeed[2], Motor_RB.speed * RPM2RPS); + PID_Calculate(&PID_RFCM, ChassisData.rotorSpeed[3], Motor_RF.speed * RPM2RPS); + + // 输出电流值到电调(安全起见默认注释此行) + Motor_LF.input = PID_LFCM.output; + Motor_LB.input = PID_LBCM.output; + Motor_RB.input = PID_RBCM.output; + Motor_RF.input = PID_RFCM.output; + + // 底盘运动更新频率 + vTaskDelayUntil(&LastWakeTime, intervalms); + } + + vTaskDelete(NULL); +} + +void Task_Communication(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.002; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + extern DMA_Type DMA_Table[10]; + + float positionFactor = 1.0 / 360 * 8192; + + ProtocolInfo_Type *protocolInfo; + + while (1) { + // 校准 + protocolInfo = Protocol_Get_Info_Handle(0x407); + if (!JointCalibrated && protocolInfo->receiveCount > 0) { + JointCalibrated = 1; + // Motor_Calibrate(&Motor_BaseJoint, ProtocolData.jointCalibration.base_joint_position / positionFactor); + // Motor_Calibrate(&Motor_ShoulderJoint, ProtocolData.jointCalibration.shoulder_joint_position / positionFactor); + // Motor_Calibrate(&Motor_ElbowJoint, ProtocolData.jointCalibration.elbow_joint_position / positionFactor); + // PID_ElbowAngle.target = ProtocolData.jointCalibration.elbow_joint_position / positionFactor + PID_ElbowAngle.error; + Motor_Set_Angle_Bias(&Motor_WristJoint1, 92.724); + PID_WristJoint2Angle.target = ProtocolData.jointCalibration.wrist_joint_2_position / positionFactor; + Motor_Calibrate(&Motor_WristJoint2, ProtocolData.jointCalibration.wrist_joint_2_position / positionFactor); + } + DebugData.debug4 = protocolInfo->receiveSeq; + + // 设置目标 + protocolInfo = Protocol_Get_Info_Handle(0x406); + if (protocolInfo->receiveSeq != protocolInfo->lastReceiveSeq) { + // PID_BaseJointAngle.target = ProtocolData.jointControl.base_joint_position / positionFactor; + // PID_ShoulderJointAngle.target = ProtocolData.jointControl.shoulder_joint_position / positionFactor; + // PID_ElbowAngle.target = ProtocolData.jointControl.elbow_joint_position / positionFactor; + PID_WristJoint1Angle.target = ProtocolData.jointControl.wrist_joint_1_position / positionFactor; + // PID_WristJoint2Angle.target = ProtocolData.jointControl.wrist_joint_2_position / positionFactor; + protocolInfo->lastReceiveSeq = protocolInfo->receiveSeq; + } + + // 修改数据 + ProtocolData.jointState.base_joint_position = Motor_BaseJoint.angle * positionFactor; + ProtocolData.jointState.base_joint_speed = Motor_BaseJoint.speed; + ProtocolData.jointState.shoulder_joint_position = Motor_ShoulderJoint.angle * positionFactor; + ProtocolData.jointState.shoulder_joint_speed = Motor_ShoulderJoint.speed; + ProtocolData.jointState.elbow_joint_position = Motor_ElbowJoint.angle * positionFactor; + ProtocolData.jointState.elbow_joint_speed = Motor_ElbowJoint.speed; + ProtocolData.jointState.wrist_joint_1_position = Motor_WristJoint1.angle * positionFactor; + ProtocolData.jointState.wrist_joint_1_speed = Motor_WristJoint1.speed; + ProtocolData.jointState.wrist_joint_2_position = Motor_WristJoint2.angle * positionFactor; + ProtocolData.jointState.wrist_joint_2_speed = Motor_WristJoint2.speed; + // 调试信息 + DebugData.debug1 = Motor_ElbowJoint.angle * 1000; + DebugData.debug2 = PID_WristJoint2Angle.target * 1000; + DebugData.debug3 = PID_WristJoint2Angle.feedback * 1000; + + // 发送频率 + vTaskDelayUntil(&LastWakeTime, intervalms); + } + vTaskDelete(NULL); +} + +void Task_Manipulator(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.01; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + + // 反馈值 + float base_Jointangle, base_Jointspeed, shoulder_Jointangle, shoulder_Jointspeed, elbow_Jointangle, elbow_Jointspeed; + float wrist_Joint_1angle, wrist_Joint_1speed, wrist_Joint_2angle, wrist_Joint_2speed; + + // PID_Init(&PID_BaseJointAngle, 40, 3, 0, 16384, 16384); + // PID_Init(&PID_BaseJointSpeed, 0, 0, 0, 16384, 16384); + // PID_Init(&PID_ShoulderJointAngle, 40, 3, 0, 16384, 16384); + // PID_Init(&PID_ShoulderJointSpeed, 0, 0, 0, 16384, 16384); + PID_Init(&PID_ElbowAngle, 40, 3, 0, 16384, 16384); + PID_Init(&PID_ElbowSpeed, 0, 0, 0, 16384, 16384); + PID_Init(&PID_WristJoint1Angle, 0.3, 0, 0, 16384, 16384); + PID_Init(&PID_WristJoint1Speed, 4000, 100, 0, 16384, 4000); + PID_Init(&PID_WristJoint2Angle, 80, 0, 0, 400, 0); + PID_Init(&PID_WristJoint2Speed, 30, 0, 0, 15000, 3000); // 30, 0.75 + + JointCalibrated = 0; + + while (1) { + + // 测试输入 + // if (remoteData.lx > 100) { + // PID_ElbowSpeed.target = remoteData.lx / 300 * 50; + // } else if (remoteData.lx < -100) { + // PID_ElbowSpeed.target = ((remoteData.lx / 300) - 1) * 50; + // } else { + // PID_ElbowSpeed.target = 0; + // } + + // PID_ElbowSpeed.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 50 : 0; + // PID_ElbowSpeed.p = CHOOSEL(7, 21, 70) * CHOOSER(0.1, 1, 10); + + // PID_WristJoint1Angle.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 45 : PID_WristJoint1Angle.target; + // PID_WristJoint2Angle.target = ABS(remoteData.ly) > 100 ? remoteData.ly / 300 * 15 : PID_WristJoint2Angle.target; + // PID_WristJoint1Angle.target += ABS(remoteData.lx) > 100 ? (remoteData.lx / 660.0f * 45 * interval) : 0; + // PID_WristJoint2Angle.target += ABS(remoteData.ly) > 100 ? (remoteData.ly / 660.0f * 15 * interval) : 0; + + // 测试电流环 + // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointSpeed.target, Motor_BaseJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointSpeed.target, Motor_ShoulderJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); + // PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Speed.target, Motor_WristJoint1.speed * RPM2RPS); + // PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Speed.target, Motor_WristJoint2.speed * RPM2RPS); + + // 测试角度环 + // PID_Calculate(&PID_BaseJointAngle, PID_BaseJointAngle.target, Motor_BaseJoint.angle); + // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointAngle.output, Motor_BaseJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ShoulderJointAngle, PID_ShoulderJointAngle.target, Motor_ShoulderJoint.angle); + // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointAngle.output, Motor_ShoulderJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ElbowAngle, PID_ElbowAngle.target, Motor_ElbowJoint.angle); + PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); + PID_Calculate(&PID_WristJoint1Angle, PID_WristJoint1Angle.target, Motor_WristJoint1.angle); + PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Angle.output, Motor_WristJoint1.speed * RPM2RPS); + PID_Calculate(&PID_WristJoint2Angle, PID_WristJoint2Angle.target, Motor_WristJoint2.angle); + if (ABS(remoteData.ry) > 100) { + // PID_WristJoint2Angle.target = 0; + PID_WristJoint2Angle.output = remoteData.ry / 330 * 5 * 30; + Motor_WristJoint2.positionBias = Motor_WristJoint2.position; + Motor_WristJoint2.round = 0; + } + PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Angle.output, Motor_WristJoint2.speed * RPM2RPS); + + //输出电流到电调 + Motor_BaseJoint.input = PID_BaseJointSpeed.output; + Motor_ShoulderJoint.input = PID_ShoulderJointSpeed.output; + Motor_ElbowJoint.input = PID_ElbowSpeed.output; + Motor_WristJoint1.input = PID_WristJoint1Speed.output; + Motor_WristJoint2.input = PID_WristJoint2Speed.output; + + // 调试信息 + // DebugData.debug1 = PID_ElbowSpeed.target * 1000; + // DebugData.debug2 = PID_ElbowSpeed.feedback * 1000; + // DebugData.debug3 = PID_ElbowSpeed.output; + + vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 + } + vTaskDelete(NULL); +} + +void Task_Can_Send(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.01; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + + while (1) { + Bridge_Send_Motor(&BridgeData, SafetyMode); + vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 + } + vTaskDelete(NULL); +} + +void Task_Blink(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); + while (1) { + LED_Run_Horse_XP(); // XP开机动画,建议延时200ms + // LED_Run_Horse(); // 跑马灯,建议延时20ms + vTaskDelayUntil(&LastWakeTime, 200); + } + + vTaskDelete(NULL); +} + +void Task_Startup_Music(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); + while (1) { + if (KTV_Play(Music_Soul)) break; + vTaskDelayUntil(&LastWakeTime, 60); + } + vTaskDelete(NULL); +} + +void Task_OLED(void *Parameters) { + uint16_t JoystickValue = -1; + TickType_t LastWakeTime = xTaskGetTickCount(); + oled_init(); + while (1) { + JoystickValue = ADC_GetConversionValue(ADC1); + oled_clear(Pen_Clear); + oled_menu(JoystickValue); + oled_refresh_gram(); + vTaskDelayUntil(&LastWakeTime, 125); + } + vTaskDelete(NULL); +} diff --git a/user/gold-miner/tasks.h b/user/gold-miner/tasks.h new file mode 100644 index 00000000..07562945 --- /dev/null +++ b/user/gold-miner/tasks.h @@ -0,0 +1,14 @@ +#ifndef __TASKS_H +#define __TASKS_H + +void Task_Control(void *Parameters); +void Task_Communication(void *Parameters); +void Task_Manipulator(void *Parameters); +void Task_Blink(void *Parameters); +void Task_Chassis(void *Parameters); +void Task_Host(void *Parameters); +void Task_Can_Send(void *Parameters); +void Task_Startup_Music(void *Parameters); +void Task_OLED(void *Parameters); + +#endif