660 lines
20 KiB
C
660 lines
20 KiB
C
|
#include "APPDEF.H"
|
|||
|
|
|
|||
|
|
|
|||
|
|
enum enumPumpSelect PumpSelect[pumpMax];
|
|||
|
|
/******************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
|
|||
|
|
*
|
|||
|
|
*******************************************************************************/
|
|||
|
|
void Motor_IECMD( enum enumPumpSelect Select, _Bool CMD )
|
|||
|
|
{
|
|||
|
|
switch( Select )
|
|||
|
|
{
|
|||
|
|
case M1:
|
|||
|
|
TIM4_IECMD( CMD );
|
|||
|
|
break;
|
|||
|
|
case M2:
|
|||
|
|
TIM3_IECMD( CMD );
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
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 )
|
|||
|
|
{
|
|||
|
|
case M1:
|
|||
|
|
TIM4_CH1_CTRL( Value, Value / 2 );
|
|||
|
|
break;
|
|||
|
|
case M2:
|
|||
|
|
TIM3_CH2_CTRL( Value, Value / 2 );
|
|||
|
|
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:
|
|||
|
|
TIM4_CH1_CMD( State );
|
|||
|
|
break;
|
|||
|
|
case M2:
|
|||
|
|
TIM3_CH2_CMD( State );
|
|||
|
|
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:
|
|||
|
|
// TIM4_CH1_DIR( State);
|
|||
|
|
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>
|
|||
|
|
*******************************************************************************/
|
|||
|
|
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>־
|
|||
|
|
|
|||
|
|
|
|||
|
|
/********************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
|
|||
|
|
* <EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*********************************************************************************/
|
|||
|
|
|
|||
|
|
const uint32_t FullCoordinate[pumpMax] = { 20500u, 65000u,65000u };
|
|||
|
|
int32_t CoordinateSet[pumpMax]; // Ŀ<><C4BF>λ<EFBFBD><CEBB>
|
|||
|
|
int32_t CoordinatePosition[pumpMax]; // ʵʱ<CAB5><CAB1><EFBFBD><EFBFBD>ʵʱλ<CAB1><CEBB>
|
|||
|
|
uint32_t Position[pumpMax]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
|
|||
|
|
_Bool TaskisRunning[pumpMax] = {0}; // <09><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
|
|||
|
|
_Bool IdelEn[pumpMax] = { false,false,false}; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
|
|||
|
|
int32_t DCMotorSpeed[pumpMax]; // ֱ<><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
|||
|
|
|
|||
|
|
uint32_t CoordinateSetOld[pumpMax]; // <09>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
|
int32_t Differ[pumpMax]; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float SpeedSet[pumpMax]={ 100.0f, 1000.0f, 5000.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>p/s
|
|||
|
|
uint32_t period[pumpMax] = { 20000u, 2000u, 2000u }; // PWMʵʱ<CAB5><CAB1><EFBFBD><EFBFBD>
|
|||
|
|
float Velocity[pumpMax] = { 0.0f, 20.0f, 20.0f }; // ʵʱ<CAB5>ٶ<EFBFBD>
|
|||
|
|
uint32_t Shifting[pumpMax] = { 500u, 500u, 500u }; // <09><><EFBFBD>ٹ<EFBFBD><D9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float Speeda[pumpMax] ={ 10000.0f, 10000.0f, 10000.0f }; // <09><><EFBFBD>ٶ<EFBFBD> p/(s*s)
|
|||
|
|
float VelocityH[pumpMax]={ 10000.0f, 5000.0f, 5000.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>p/s
|
|||
|
|
float VelocityL[pumpMax]={ 10.0f, 1000.0f, 1000.0f }; // <09><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>p/s
|
|||
|
|
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>
|
|||
|
|
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>
|
|||
|
|
/******************************** <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> *************************************
|
|||
|
|
*
|
|||
|
|
*******************************************************************************/
|
|||
|
|
|
|||
|
|
_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 );
|
|||
|
|
}
|
|||
|
|
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>
|
|||
|
|
)
|
|||
|
|
)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
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] );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
else//if( PluseCount[Select] >= (abs(Differ) - Shifting[Select]) ) // <09><>Shifting[Select]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
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] );
|
|||
|
|
}
|
|||
|
|
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 );
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
osDelay(15);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//_Bool SpeedMode[pumpMax];
|
|||
|
|
//_Bool PositionMode[pumpMax];
|
|||
|
|
|
|||
|
|
void MotorCtrl(enum enumPumpSelect Select)
|
|||
|
|
{
|
|||
|
|
// static _Bool SpeedModeOld[pumpMax];
|
|||
|
|
// static _Bool PositionModeOld[pumpMax];
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
#include "stm32f10x.h"
|
|||
|
|
|
|||
|
|
/******************************** <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5> *************************************
|
|||
|
|
* <EFBFBD>ж϶˿ڳ<EFBFBD>ʼ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
|
|||
|
|
*******************************************************************************/
|
|||
|
|
void BIOS_EXTI_Init( void )
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
// // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
|
|||
|
|
|
|||
|
|
SET_BIT( RCC->APB2ENR, RCC_APB2ENR_IOPBEN );
|
|||
|
|
|
|||
|
|
GPIOB->BSRR |= 0x0002u; // <20>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD>Ϊ1<CEAA><31>
|
|||
|
|
// GPIOF->BSRR |= 0x3800u; // <20>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD>Ϊ1<CEAA><31>
|
|||
|
|
// GPIOE->BSRR |= 0x0040u; // <20>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD>Ϊ1<CEAA><31>
|
|||
|
|
// SET_BIT( GPIOC->PUPDR, GPIO_PUPDR_PUPD0_0|GPIO_PUPDR_PUPD1_0|GPIO_PUPDR_PUPD2_0|GPIO_PUPDR_PUPD3_0 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD>
|
|||
|
|
// SET_BIT( GPIOF->PUPDR, GPIO_PUPDR_PUPD11_0|GPIO_PUPDR_PUPD12_0|GPIO_PUPDR_PUPD13_0 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD>
|
|||
|
|
CLEAR_BIT( GPIOB->CRL, GPIO_CRL_CNF1_0 );
|
|||
|
|
SET_BIT( GPIOB->CRL, GPIO_CRL_CNF1_1 );
|
|||
|
|
SET_BIT( GPIOB->ODR, GPIO_ODR_ODR1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE0, GPIO_MODER_MODE0_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE1, GPIO_MODER_MODE1_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE2, GPIO_MODER_MODE2_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE3, GPIO_MODER_MODE3_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE11, GPIO_MODER_MODE11_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE12, GPIO_MODER_MODE12_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE13, GPIO_MODER_MODE13_1 ); // IO<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
|
|||
|
|
// SET_BIT(RCC->APB2ENR, RCC_APB2ENR_SYSCFGEN);
|
|||
|
|
// // <09><><EFBFBD><EFBFBD><EFBFBD>жϸ<D0B6><CFB8><EFBFBD>
|
|||
|
|
// SYSCFG->EXTICR[0] =
|
|||
|
|
// SYSCFG_EXTICR1_EXTI1_PC
|
|||
|
|
// |SYSCFG_EXTICR1_EXTI1_PC
|
|||
|
|
// |SYSCFG_EXTICR1_EXTI2_PC
|
|||
|
|
// |SYSCFG_EXTICR1_EXTI3_PC
|
|||
|
|
;
|
|||
|
|
// SYSCFG->EXTICR[1] =
|
|||
|
|
// SYSCFG_EXTICR2_EXTI6_PF;
|
|||
|
|
// SYSCFG->EXTICR[2] =
|
|||
|
|
// SYSCFG_EXTICR3_EXTI11_PF;
|
|||
|
|
// SYSCFG->EXTICR[3] =
|
|||
|
|
// SYSCFG_EXTICR4_EXTI12_PF
|
|||
|
|
// |SYSCFG_EXTICR4_EXTI13_PF
|
|||
|
|
// ;
|
|||
|
|
SET_BIT( RCC->APB2ENR, RCC_APB2ENR_AFIOEN );
|
|||
|
|
|
|||
|
|
AFIO->EXTICR[0] = AFIO_EXTICR1_EXTI1_PB
|
|||
|
|
// | AFIO_EXTICR1_EXTI1_PB
|
|||
|
|
// | AFIO_EXTICR1_EXTI2_PA
|
|||
|
|
// | AFIO_EXTICR1_EXTI3_PA
|
|||
|
|
// ;
|
|||
|
|
// AFIO->EXTICR[1] = AFIO_EXTICR2_EXTI4_PA
|
|||
|
|
// | AFIO_EXTICR2_EXTI5_PA
|
|||
|
|
// | AFIO_EXTICR2_EXTI6_PA
|
|||
|
|
// | AFIO_EXTICR2_EXTI7_PA
|
|||
|
|
;
|
|||
|
|
|
|||
|
|
|
|||
|
|
CLEAR_BIT( EXTI->IMR, 0xFFFFu ); // <20><>ֹ<EFBFBD>ж<EFBFBD>
|
|||
|
|
|
|||
|
|
CLEAR_BIT( EXTI->EMR, 0xFFFFu ); // no event <20><><EFBFBD>ж<EFBFBD><D0B6>¼<EFBFBD>
|
|||
|
|
// SET_BIT ( EXTI->RTSR, 0x0006u ); // rising edge trigger <20><><EFBFBD><EFBFBD><EFBFBD>ش<EFBFBD><D8B4><EFBFBD>
|
|||
|
|
SET_BIT ( EXTI->FTSR, 0x0002u ); // falling edge trigger <20>½<EFBFBD><C2BD>ش<EFBFBD><D8B4><EFBFBD>
|
|||
|
|
WRITE_REG( EXTI->PR, 0xFFFFu ); // ͨ<><CDA8>д1<D0B4>ܸ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<D0B6>־
|
|||
|
|
|
|||
|
|
// SET_BIT ( EXTI->IMR, 0x384Fu ); // <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
|
|||
|
|
// // IO<49>ж<EFBFBD>
|
|||
|
|
// NVIC_EnableIRQ( EXTI0_IRQn );
|
|||
|
|
NVIC_EnableIRQ( EXTI1_IRQn );
|
|||
|
|
// NVIC_EnableIRQ( EXTI2_IRQn );
|
|||
|
|
// NVIC_EnableIRQ( EXTI3_IRQn );
|
|||
|
|
// NVIC_EnableIRQ( EXTI9_5_IRQn );
|
|||
|
|
// NVIC_EnableIRQ( EXTI15_10_IRQn );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************************** <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;
|
|||
|
|
void EXTI1_IRQHandler( void )
|
|||
|
|
{
|
|||
|
|
iz++;
|
|||
|
|
SET_BIT( EXTI->PR, 0x01u << 1 ); // д1<D0B4><31>λ<EFBFBD>жϱ<D0B6>־λ
|
|||
|
|
if(ArmInitIsRuning[M1] )
|
|||
|
|
{
|
|||
|
|
if( !READ_BIT( GPIOB->IDR, GPIO_Pin_1 ) )
|
|||
|
|
{
|
|||
|
|
EXTIx_IRQ_Disable( 1 );
|
|||
|
|
Motor_PWMCmd(M1, false );
|
|||
|
|
CoordinateSet[M1] = 0u;
|
|||
|
|
CoordinatePosition[M1] = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if( READ_BIT( GPIOB->IDR, GPIO_Pin_1 ) )
|
|||
|
|
{
|
|||
|
|
EXTIx_IRQ_Disable( 1 );
|
|||
|
|
osDelay(400);
|
|||
|
|
Motor_PWMCmd(M1, false );
|
|||
|
|
if(CoordinateSet[M1] >0)
|
|||
|
|
CoordinatePosition[M1] = 1;
|
|||
|
|
else
|
|||
|
|
CoordinatePosition[M1] = -1;
|
|||
|
|
|
|||
|
|
CoordinateSet[M1] = 0u;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
_Bool Exti_IOStateRead( enum enumPumpSelect Select )
|
|||
|
|
{
|
|||
|
|
_Bool state = false;
|
|||
|
|
switch (Select)
|
|||
|
|
{
|
|||
|
|
case M1:
|
|||
|
|
state = READ_BIT( GPIOB->IDR, GPIO_IDR_IDR1);
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
return state;
|
|||
|
|
}
|
|||
|
|
void Exti_IRQ_Ctrl( enum enumPumpSelect Select, _Bool State )
|
|||
|
|
{
|
|||
|
|
switch (Select)
|
|||
|
|
{
|
|||
|
|
case M1:
|
|||
|
|
State? EXTIx_IRQ_Enable(1) : EXTIx_IRQ_Disable(1);
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
void MotorZero_Init( enum enumPumpSelect Select)
|
|||
|
|
{
|
|||
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS5 );
|
|||
|
|
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 );
|
|||
|
|
DCMotor_PWM( Select, 55 );
|
|||
|
|
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) );
|
|||
|
|
usRegHoldingBuf[43] = 0;
|
|||
|
|
usRegInputBuf[43] = 2;
|
|||
|
|
usRegHoldingBuf[7] = CoordinateSet[Select] = 0;
|
|||
|
|
ArmInitIsRuning[Select] = FindZero[Select] = false;
|
|||
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR5 );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
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 )
|
|||
|
|
{
|
|||
|
|
SamplePumpSpeed_AcceleratedConver(M1,100);
|
|||
|
|
SamplePumpSpeed_AcceleratedConver(M2,1000);
|
|||
|
|
SamplePumpSpeed_AcceleratedConver(M3,100);
|
|||
|
|
|
|||
|
|
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>
|
|||
|
|
512, // <20><>ջ<EFBFBD><D5BB>С<EFBFBD><D0A1><EFBFBD>Զ<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>͵Ĵ<CDB5>СΪ<D0A1><CEAA>λ<EFBFBD><CEBB>
|
|||
|
|
(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>
|
|||
|
|
512, // <20><>ջ<EFBFBD><D5BB>С<EFBFBD><D0A1><EFBFBD>Զ<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>͵Ĵ<CDB5>СΪ<D0A1><CEAA>λ<EFBFBD><CEBB>
|
|||
|
|
(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>
|
|||
|
|
512, // <20><>ջ<EFBFBD><D5BB>С<EFBFBD><D0A1><EFBFBD>Զ<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>͵Ĵ<CDB5>СΪ<D0A1><CEAA>λ<EFBFBD><CEBB>
|
|||
|
|
(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>
|
|||
|
|
|
|||
|
|
}
|