Embedded_RD/YouSuanZhi
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
master
YouSuanZhi/Core/Src/StepMotor.c

636 lines
18 KiB
C
Raw Permalink Normal View History

TEST
2025-12-30 07:21:11 +00:00
#include "APPDEF.H"
已正常实验
2026-03-17 02:35:31 +00:00
#include "stm32f10x.h"
TEST
2025-12-30 07:21:11 +00:00
enum enumPumpSelect PumpSelect[pumpMax];
/******************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
*
*******************************************************************************/
void Motor_IECMD( enum enumPumpSelect Select, _Bool CMD )
{
switch( Select )
{
case M1:
已正常实验
2026-03-17 02:35:31 +00:00
TIM5_IECMD( CMD );
TEST
2025-12-30 07:21:11 +00:00
break;
case M2:
TIM3_IECMD( CMD );
break;
已正常实验
2026-03-17 02:35:31 +00:00
case M3:
TEST
2025-12-30 07:21:11 +00:00
default:
已正常实验
2026-03-17 02:35:31 +00:00
TIM2_IECMD( CMD );
TEST
2025-12-30 07:21:11 +00:00
break;
}
}
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************************/
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************************/
void Motor_PWM( enum enumPumpSelect Select, uint16_t Value )
{
switch( Select )
{
已正常实验
2026-03-17 02:35:31 +00:00
TEST
2025-12-30 07:21:11 +00:00
case M1:
已正常实验
2026-03-17 02:35:31 +00:00
TIM5_CH3_CTRL( Value, Value / 2 );
TEST
2025-12-30 07:21:11 +00:00
break;
case M2:
已正常实验
2026-03-17 02:35:31 +00:00
TIM3_CH1_CTRL( Value, Value / 2 );
break;
case M3:
TIM2_CH2_CTRL( Value, Value / 2 );
TEST
2025-12-30 07:21:11 +00:00
break;
default:
break;
}
}
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD>PWM<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************************/
void Motor_PWMCmd( enum enumPumpSelect Select, _Bool State )
{
switch( Select )
{
case M1:
已正常实验
2026-03-17 02:35:31 +00:00
TIM5_CH3_CMD( State );
TEST
2025-12-30 07:21:11 +00:00
break;
case M2:
已正常实验
2026-03-17 02:35:31 +00:00
TIM3_CH1_CMD( State );
break;
case M3:
TIM2_CH2_CMD( State );
TEST
2025-12-30 07:21:11 +00:00
break;
default:
break;
}
}
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>з<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************************/
void Motor_Dir( enum enumPumpSelect Select, _Bool State )
{
switch( Select )
{
case M1:
已正常实验
2026-03-17 02:35:31 +00:00
if(State)
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR3 );
else
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS3 );
break;
case M2:
if(State)
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR5 );
else
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS5 );
break;
case M3:
if(State)
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS0 );
else
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR0 );
TEST
2025-12-30 07:21:11 +00:00
break;
default:
break;
}
}
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD>ۿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
*********************************************************************************/
void Motor_En( enum enumPumpSelect Select, _Bool State )
{
switch( Select )
{
case M1:
break;
default:
break;
}
}
/******************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* TIMx <EFBFBD>жϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************************************************/
已正常实验
2026-03-17 02:35:31 +00:00
volatile uint32_t SetPluseCount[7u]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
volatile uint32_t PluseCount[7u]; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
volatile _Bool CountOver[7u] = {false}; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
volatile uint32_t SamplePumpSetPluseCount[7u]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
volatile uint32_t SamplePumpPluseCount[7u]; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
volatile _Bool SamplePumpCountOver[7u] = {false}; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
TEST
2025-12-30 07:21:11 +00:00
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************************/
已正常实验
2026-03-17 02:35:31 +00:00
//<2F>ϵ<EFBFBD>ǰ<EFBFBD><C7B0>Ҫ<EFBFBD>޸ĵİ<C4B5>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD>
/*
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD>򰴼<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
const int32_t FullCoordinate[pumpMax] = { 20500u, 65000u,125000u }; // <09><><EFBFBD><EFBFBD><EFBFBD>г<EFBFBD><D0B3><EFBFBD><EFBFBD><EFBFBD>
const float ShiftPluse[pumpMax]={ 100.0f, 1500.0f, 1500.0f }; // <09><>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٹ<EFBFBD><D9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
const float VelocityH[pumpMax]={ 10000.0f, 5000.0f, 20000.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>p/s
const float VelocityL[pumpMax]={ 10.0f, 1000.0f, 1000.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>p/s
//<2F>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸IJ<DEB8><C4B2><EFBFBD>
float SpeedSet[pumpMax]={ 20.0f, 1000.0f, 20000.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>p/s
int32_t CoordinateSet[pumpMax]; // Ŀ<><C4BF>λ<EFBFBD><CEBB>
TEST
2025-12-30 07:21:11 +00:00
_Bool TaskisRunning[pumpMax] = {0}; // <09><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
已正常实验
2026-03-17 02:35:31 +00:00
_Bool IdelEn[pumpMax] = { false,false,false}; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
TEST
2025-12-30 07:21:11 +00:00
已正常实验
2026-03-17 02:35:31 +00:00
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD>µIJ<C2B5><C4B2><EFBFBD>
int32_t CoordinatePosition[pumpMax]; // ʵʱ<CAB5><CAB1><EFBFBD><EFBFBD>ʵʱλ<CAB1><CEBB>
TEST
2025-12-30 07:21:11 +00:00
int32_t Differ[pumpMax]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
已正常实验
2026-03-17 02:35:31 +00:00
uint32_t Shifting[pumpMax] = { 500u, 500u, 500u }; // <09><><EFBFBD>ٹ<EFBFBD><D9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TEST
2025-12-30 07:21:11 +00:00
uint32_t period[pumpMax] = { 20000u, 2000u, 2000u }; // PWMʵʱ<CAB5><CAB1><EFBFBD><EFBFBD>
已正常实验
2026-03-17 02:35:31 +00:00
uint32_t CoordinateSetOld[pumpMax]; // <09>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
uint32_t Position[pumpMax]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
float Speeda[pumpMax] ={ 10000.0f, 10000.0f, 10000.0f }; // <09><><EFBFBD>ٶ<EFBFBD> p/(s*s)
float Velocity[pumpMax] = { 0.0f, 20.0f, 20.0f }; // ʵʱ<CAB5>ٶ<EFBFBD>
float VelocityMax[pumpMax] = { 10000.0f, 20.0f, 20.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
_Bool Direction[pumpMax] = { false,false,false}; // <09><><EFBFBD>з<EFBFBD><D0B7><EFBFBD>
TEST
2025-12-30 07:21:11 +00:00
/******************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD>ٶȼ<EFBFBD><EFBFBD>ٶ<EFBFBD>ת<EFBFBD><EFBFBD>
*******************************************************************************/
static uint16_t SpeedOld[pumpMax];
_Bool PumpShutDown[pumpMax] = {false, false}; // <09><>ͣ<EFBFBD><CDA3>־
_Bool DirectionSet[pumpMax]; // Ŀ<>귽<EFBFBD><EAB7BD>
_Bool ENSet[pumpMax] = {false,false,false}; // <09>Ƿ<EFBFBD>ʹ<EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD>
void SamplePumpSpeed_AcceleratedConver( enum enumPumpSelect Select, uint16_t Speed )
{
if( Speed != SpeedOld[Select] )
{
Speeda[Select] = (Speed * Speed) / (2u * ShiftPluse[Select]);
SpeedOld[Select] = Speed;
}
}
/******************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
*
*******************************************************************************/
已正常实验
2026-03-17 02:35:31 +00:00
//osSemaphoreId ArmRunFinish;
//osSemaphoreDef(ArmRunFinish);
//osSemaphoreId SampleRunFinish;
//osSemaphoreDef(SampleRunFinish);
TEST
2025-12-30 07:21:11 +00:00
_Bool SpeedMode[pumpMax];
_Bool PositionMode[pumpMax];
_Bool FindZero[pumpMax];
_Bool FindedZero[pumpMax];
void SamplePumpCtrl( enum enumPumpSelect Select )
{
// for(;;)
{
if((CoordinateSet[Select] != 0u) && (!TaskisRunning[Select]))
{
SamplePumpSpeed_AcceleratedConver(Select,SpeedSet[Select]);
TaskisRunning[Select] = true; // <09><>λ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
VelocityMax[Select] = 0;
period [Select] = 2000u;
Velocity[Select] = 6.0f;
Shifting[Select] = 100u;
PluseCount[Select] = 0u; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CountOver[Select] = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SetPluseCount[Select] = CoordinateSet[Select]; // <09><><EFBFBD>ñ<EFBFBD><C3B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Motor_IECMD( Select, true);
Motor_Dir ( Select, DirectionSet[Select] ); // ѡ<><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Motor_PWM ( Select, 100000.0f / VelocityL[Select] ); // <09><><EFBFBD><EFBFBD>pwmΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
osDelay( 50u );
Motor_En ( Select, true ); // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
Shifting[Select] = SetPluseCount[Select] / 2u;
if( Shifting[Select] > ShiftPluse[Select] )
Shifting[Select] = ShiftPluse[Select];
Motor_PWMCmd( Select, true ); // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
while( !CountOver[Select] ) // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if(PumpShutDown[Select])
{
PumpShutDown[Select] = false;
break;
}
if( PluseCount[Select] < Shifting[Select] ) // ǰShifting[Select]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
Velocity[Select] = sqrt( 2* PluseCount[Select] * (Speeda[Select]/* / 2u*/));
if( VelocityMax[Select] < Velocity[Select] )
VelocityMax[Select] = Velocity[Select];
if(Velocity[Select] > VelocityH[Select])
Velocity[Select] = VelocityH[Select];
if(Velocity[Select] < VelocityL[Select] )
Velocity[Select] = VelocityL[Select];
if( Velocity[Select] > SpeedOld[Select])
Velocity[Select] = SpeedOld[Select];
period[Select] = 100000.0f / Velocity[Select];
Motor_PWM( Select, period[Select] );
}
else // <09>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>
if( PluseCount[Select] <= (SetPluseCount[Select] - Shifting[Select]) )
{
period[Select] = 100000.0f / SpeedOld[Select];
Motor_PWM( Select, period[Select] );
while(CoordinateSet[Select] == 65535u)
{
PluseCount[Select] = 0;
osDelay( 5u );
}
if(CoordinateSet[Select] == 0u)
{
SetPluseCount[Select] = Shifting[Select]*2;
PluseCount[Select] = Shifting[Select]+1;
}
}
else // <09><>Shifting[Select]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// if( PluseCount[Select] >= (SetPluseCount[Select] - Shifting[Select]) )
{
Velocity[Select] = sqrt( 2* (SetPluseCount[Select] - (float)PluseCount[Select]) * (Speeda[Select] / 2u));
if(Velocity[Select] > VelocityH[Select])
Velocity[Select] = VelocityH[Select];
if(Velocity[Select] < VelocityL[Select] )
Velocity[Select] = VelocityL[Select];
period[Select] = 100000.0f / Velocity[Select];
Motor_PWM( Select, period[Select] );
}
osDelay( 5u );
}
// usRegHoldingBuf[20u+Select*5u] = 0u;
osDelay( 15u );
CoordinateSet[Select] = 0u;
Motor_En( Select, ENSet[Select] ); // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
TaskisRunning[Select] = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
CountOver[Select] = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
osDelay( 15u );
已正常实验
2026-03-17 02:35:31 +00:00
// osSemaphoreRelease(SampleRunFinish);
TEST
2025-12-30 07:21:11 +00:00
}
else
{
osDelay(15);
}
}
}
static bool ArmInitIsRuning[pumpMax] = { false,false,false};
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************************/
void ArmSoftCtrl( enum enumPumpSelect Select )
{
/*Vmin = 50p/s Vmax = 20000p/s At = 0.2s a = 100000p/s2 <20><>s = 2000p <20><>t = 0.001s P = 0.000001s */
//#define LowSpeed 40.0f
// for(;;)
{
if(
(CoordinateSet[Select] != CoordinateSetOld[Select]) // <09><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB><EFBFBD>뵱ǰλ<C7B0>ò<EFBFBD>ͬ
&& !TaskisRunning[Select] // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
&&!( ArmInitIsRuning[M3] // <09><><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC>
)
)
{
已正常实验
2026-03-17 02:35:31 +00:00
SamplePumpSpeed_AcceleratedConver(Select,SpeedSet[Select]);
TEST
2025-12-30 07:21:11 +00:00
TaskisRunning[Select] = true; // <09><>λ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
VelocityMax[Select] = 0;
if( CoordinateSet[Select] > FullCoordinate[Select])
{
/*usRegHoldingBuf[7] = */CoordinateSet[Select] = CoordinateSetOld[Select];
osDelay(15); // <09><><EFBFBD><EFBFBD>λ<EFBFBD>ñ<EFBFBD><C3B1><EFBFBD>
}
period [Select] = 20000u; //<2F><><EFBFBD><EFBFBD>
Velocity[Select] = 500.0f; //ʵʱ<CAB5>ٶ<EFBFBD>
Shifting[Select] = 1500u; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CoordinateSetOld[Select] = CoordinateSet[Select]; //
PluseCount[Select] = 0u; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CountOver[Select] = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Differ[Select] = CoordinateSet[Select] - CoordinatePosition[Select]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>뵱ǰλ<C7B0>õ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>
if(Differ[Select] != 0)
{
if( Differ[Select] > 0 )
Direction[Select] = true;
if( Differ[Select] < 0 )
Direction[Select] = false;
SetPluseCount[Select] = abs( Differ[Select]); // <09><><EFBFBD>ñ<EFBFBD><C3B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Position[Select] = CoordinatePosition[Select];
Motor_IECMD( Select, true);
Motor_Dir ( Select, ( Differ[Select] >= 0 ) ? false : true ); // ѡ<><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Motor_PWM ( Select, period[Select] ); // <09><><EFBFBD><EFBFBD>pwmΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
osDelay( 50u );
Shifting[Select] = abs(Differ[Select]) / 2u;
if( Shifting[Select] > ShiftPluse[Select] )
Shifting[Select] = ShiftPluse[Select];
Motor_PWMCmd( Select, true ); // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Motor_En ( Select, true ); // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
while( !CountOver[Select] ) // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
// ǰShifting[Select]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٲ<EFBFBD><D9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if( PluseCount[Select] <= (abs(Differ[Select]) - Shifting[Select]) )
{
if( Velocity[Select] < SpeedOld[Select])
{
Velocity[Select] = sqrt( 2* PluseCount[Select] * (Speeda[Select] / 2u));
if( Velocity[Select] > SpeedOld[Select])
Velocity[Select] = SpeedOld[Select];
if( VelocityMax[Select] < Velocity[Select] )
VelocityMax[Select] = Velocity[Select];
if(Velocity[Select] > VelocityH[Select])
Velocity[Select] = VelocityH[Select];
if(Velocity[Select] < VelocityL[Select] )
Velocity[Select] = VelocityL[Select];
period[Select] = 1000000.0f / Velocity[Select];
Motor_PWM( Select, period[Select] );
}
}
已正常实验
2026-03-17 02:35:31 +00:00
else if( PluseCount[Select] >= (abs(Differ[Select]) - Shifting[Select]) ) // <09><>Shifting[Select]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TEST
2025-12-30 07:21:11 +00:00
{
已正常实验
2026-03-17 02:35:31 +00:00
TEST
2025-12-30 07:21:11 +00:00
Velocity[Select] = sqrt( 2* (abs(Differ[Select]) - (float)PluseCount[Select]) * (Speeda[Select] / 2u));
if( VelocityMax[Select] < Velocity[Select] )
VelocityMax[Select] = Velocity[Select];
if(Velocity[Select] > VelocityH[Select])
Velocity[Select] = VelocityH[Select];
if(Velocity[Select] < VelocityL[Select] )
Velocity[Select] = VelocityL[Select];
period[Select] = 1000000.0f / Velocity[Select];
Motor_PWM( Select, period[Select] );
}
已正常实验
2026-03-17 02:35:31 +00:00
else
{
__NOP();
}
TEST
2025-12-30 07:21:11 +00:00
osDelay( 1u );
if( Differ[Select] >= 0 ) // <09><><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㵱ǰλ<C7B0><CEBB>
{
CoordinatePosition[Select] = Position[Select] + PluseCount[Select];
}
else
{
CoordinatePosition[Select] = Position[Select] - PluseCount[Select];
}
}
if( Differ[Select] >= 0 ) // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
CoordinatePosition[Select] = Position[Select] + PluseCount[Select];
}
else
{
CoordinatePosition[Select] = Position[Select] - PluseCount[Select];
}
}
Velocity[Select] = 0;
TaskisRunning[Select] = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
CountOver[Select] = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
osDelay( 5u );
已正常实验
2026-03-17 02:35:31 +00:00
// osSemaphoreRelease(ArmRunFinish);
TEST
2025-12-30 07:21:11 +00:00
}
else
{
osDelay(15);
}
}
}
//_Bool SpeedMode[pumpMax];
//_Bool PositionMode[pumpMax];
void MotorCtrl(enum enumPumpSelect Select)
{
// static _Bool SpeedModeOld[pumpMax];
// static _Bool PositionModeOld[pumpMax];
已正常实验
2026-03-17 02:35:31 +00:00
// ArmRunFinish = osSemaphoreNew( 1,0,NULL);
// SampleRunFinish = osSemaphoreNew( 1,0,NULL);
TEST
2025-12-30 07:21:11 +00:00
for(;;)
{
// if( SpeedModeOld[Select] != SpeedMode[Select])
// {
// SpeedModeOld[Select] = SpeedMode[Select];
// Motor_IECMD( Select, SpeedModeOld[Select]);
// }
// if( PositionModeOld[Select] != PositionMode[Select])
// {
// PositionModeOld[Select] = PositionMode[Select];
// }
// if( PositionMode[Select] )
// {
// SamplePumpCtrl(Select);
// }
switch(Select)
{
case M1:
case M2:
SamplePumpCtrl(Select);
break;
case M3:
ArmSoftCtrl(Select);
break;
default:
break;
}
}
}
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> ***********************************
* <EFBFBD>жϿ<EFBFBD><EFBFBD>ؿ<EFBFBD><EFBFBD><EFBFBD>
*******************************************************************************/
void EXTIx_IRQ_Enable( uint8_t EXTIx )
{
SET_BIT( EXTI->PR, 0x01u << EXTIx ); // д1<D0B4><31>λ<EFBFBD>жϱ<D0B6>־λ
SET_BIT( EXTI->IMR, 0x01u << EXTIx ); // <20><><EFBFBD><EFBFBD><EFBFBD>жϡ<D0B6>
}
void EXTIx_IRQ_Disable( uint8_t EXTIx )
{
CLEAR_BIT( EXTI->IMR, 0x01u << EXTIx ); // <20><>ֹ<EFBFBD>ж<EFBFBD>
SET_BIT( EXTI->PR, 0x01u << EXTIx ); // д1<D0B4><31>λ<EFBFBD>жϱ<D0B6>־λ
}
uint16_t iz = 0;
已正常实验
2026-03-17 02:35:31 +00:00
//void EXTI0_IRQHandler( void )
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
TEST
2025-12-30 07:21:11 +00:00
{
iz++;
SET_BIT( EXTI->PR, 0x01u << 1 ); // д1<D0B4><31>λ<EFBFBD>жϱ<D0B6>־λ
已正常实验
2026-03-17 02:35:31 +00:00
if(ArmInitIsRuning[M3] )
TEST
2025-12-30 07:21:11 +00:00
{
已正常实验
2026-03-17 02:35:31 +00:00
if( !READ_BIT( GPIOC->IDR, GPIO_IDR_IDR0 ) )
TEST
2025-12-30 07:21:11 +00:00
{
已正常实验
2026-03-17 02:35:31 +00:00
EXTIx_IRQ_Disable( 0 );
Motor_PWMCmd(M3, false );
CoordinateSet[M3] = 0u;
CoordinatePosition[M3] = 0;
TEST
2025-12-30 07:21:11 +00:00
}
}
else
{
已正常实验
2026-03-17 02:35:31 +00:00
if( READ_BIT( GPIOC->IDR, GPIO_IDR_IDR0 ) )
TEST
2025-12-30 07:21:11 +00:00
{
已正常实验
2026-03-17 02:35:31 +00:00
EXTIx_IRQ_Disable( 0 );
TEST
2025-12-30 07:21:11 +00:00
osDelay(400);
已正常实验
2026-03-17 02:35:31 +00:00
Motor_PWMCmd(M3, false );
if(CoordinateSet[M3] >0)
CoordinatePosition[M3] = 1;
TEST
2025-12-30 07:21:11 +00:00
else
已正常实验
2026-03-17 02:35:31 +00:00
CoordinatePosition[M3] = -1;
TEST
2025-12-30 07:21:11 +00:00
CoordinateSet[M1] = 0u;
}
}
}
_Bool Exti_IOStateRead( enum enumPumpSelect Select )
{
_Bool state = false;
switch (Select)
{
已正常实验
2026-03-17 02:35:31 +00:00
case M3:
state = READ_BIT( GPIOC->IDR, GPIO_IDR_IDR0);
TEST
2025-12-30 07:21:11 +00:00
break;
default:
break;
}
return state;
}
void Exti_IRQ_Ctrl( enum enumPumpSelect Select, _Bool State )
{
switch (Select)
{
已正常实验
2026-03-17 02:35:31 +00:00
case M3:
State? EXTIx_IRQ_Enable(0) : EXTIx_IRQ_Disable(0);
TEST
2025-12-30 07:21:11 +00:00
break;
default:
break;
}
}
void MotorZero_Init( enum enumPumpSelect Select)
{
FindedZero[Select] =false;
EXTIx_IRQ_Disable(0);
Motor_PWMCmd( Select, false );
ArmInitIsRuning[Select] = true;
Exti_IRQ_Ctrl(Select, false);
Motor_En( Select, true );
Motor_Dir( Select, true );
osDelay(10u);
Motor_IECMD( Select, false );
已正常实验
2026-03-17 02:35:31 +00:00
Motor_PWM( Select, 200 );
// DCMotor_PWM( Select, 55 );
TEST
2025-12-30 07:21:11 +00:00
if( !Exti_IOStateRead(Select) )
{
Motor_Dir( Select, false );
osDelay(10u);
Motor_PWMCmd( Select, true );
osDelay(600u);
Motor_PWMCmd( Select, false );
Motor_Dir( Select, true );
osDelay(150u);
}
Exti_IRQ_Ctrl(Select, true);
Motor_PWMCmd( Select, true );
CoordinateSet[Select] = 0u;
CoordinatePosition[Select] = 1u;
while( CoordinatePosition[Select] )
{
PluseCount[Select] = 0u;
osDelay( 20u );
}
PluseCount[Select] = 0u;
SetPluseCount[Select] = 0u;
CoordinateSet[Select] = 0u;
CoordinateSetOld[Select] = 0u;
Direction[Select] = false;
FindedZero[Select] =true;
// RegCoilsBufRst( (22u+Select*3u) );
已正常实验
2026-03-17 02:35:31 +00:00
/*usRegHoldingBuf[43] = 0;
TEST
2025-12-30 07:21:11 +00:00
usRegInputBuf[43] = 2;
已正常实验
2026-03-17 02:35:31 +00:00
usRegHoldingBuf[7] =*/ CoordinateSet[Select] = 0;
TEST
2025-12-30 07:21:11 +00:00
ArmInitIsRuning[Select] = FindZero[Select] = false;
}
BaseType_t temp1,temp2,temp3;
void _task_MotorCTRL( void * p_arg )
{
uint32_t arg = (uint32_t)p_arg;
MotorCtrl((enum enumPumpSelect)arg );
}
//osThreadDef( _task_MotorCTRL, osPriorityAboveNormal, 2u, 0u );
osThreadId_t MName[pumpMax];
TaskHandle_t xTaskHandles[pumpMax];
void MotorCTRLInitiate( void )
{
已正常实验
2026-03-17 02:35:31 +00:00
SamplePumpSpeed_AcceleratedConver(M1,20); //<2F>кͱ<D0BA>
SamplePumpSpeed_AcceleratedConver(M2,1000); // <20><>ȡ<EFBFBD><C8A1>
SamplePumpSpeed_AcceleratedConver(M3,20000); // ת<>̵<EFBFBD><CCB5><EFBFBD>
TEST
2025-12-30 07:21:11 +00:00
temp1 = xTaskCreate(_task_MotorCTRL, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
"TaskName1", // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ԣ<EFBFBD>
已正常实验
2026-03-17 02:35:31 +00:00
256, // <20><>ջ<EFBFBD><D5BB>С<EFBFBD><D0A1><EFBFBD>Զ<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>͵Ĵ<CDB5>СΪ<D0A1><CEAA>λ<EFBFBD><CEBB>
TEST
2025-12-30 07:21:11 +00:00
(void *)M1, // <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
osPriorityNormal, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
&xTaskHandles[M1]); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>
temp2 = xTaskCreate(_task_MotorCTRL, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
"TaskName2", // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ԣ<EFBFBD>
已正常实验
2026-03-17 02:35:31 +00:00
256, // <20><>ջ<EFBFBD><D5BB>С<EFBFBD><D0A1><EFBFBD>Զ<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>͵Ĵ<CDB5>СΪ<D0A1><CEAA>λ<EFBFBD><CEBB>
TEST
2025-12-30 07:21:11 +00:00
(void *)M2, // <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
osPriorityNormal, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
&xTaskHandles[M2]); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>
temp3 = xTaskCreate(_task_MotorCTRL, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
"TaskName3", // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ԣ<EFBFBD>
已正常实验
2026-03-17 02:35:31 +00:00
256, // <20><>ջ<EFBFBD><D5BB>С<EFBFBD><D0A1><EFBFBD>Զ<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>͵Ĵ<CDB5>СΪ<D0A1><CEAA>λ<EFBFBD><CEBB>
TEST
2025-12-30 07:21:11 +00:00
(void *)M3, // <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
osPriorityNormal, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
&xTaskHandles[M3]); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>
}
Reference in New Issue Copy Permalink