660 lines
20 KiB
C
660 lines
20 KiB
C
#include "APPDEF.H"
|
||
|
||
|
||
enum enumPumpSelect PumpSelect[pumpMax];
|
||
/******************************** 功能说明 *************************************
|
||
*
|
||
*******************************************************************************/
|
||
void Motor_IECMD( enum enumPumpSelect Select, _Bool CMD )
|
||
{
|
||
switch( Select )
|
||
{
|
||
case M1:
|
||
TIM4_IECMD( CMD );
|
||
break;
|
||
case M2:
|
||
TIM3_IECMD( CMD );
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂运行速度设置
|
||
*********************************************************************************/
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂运行速度设置
|
||
*********************************************************************************/
|
||
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;
|
||
}
|
||
}
|
||
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂PWM开关
|
||
*********************************************************************************/
|
||
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;
|
||
}
|
||
}
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂运行方向设置
|
||
*********************************************************************************/
|
||
void Motor_Dir( enum enumPumpSelect Select, _Bool State )
|
||
{
|
||
|
||
switch( Select )
|
||
{
|
||
case M1:
|
||
// TIM4_CH1_DIR( State);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂控制器使能
|
||
*********************************************************************************/
|
||
void Motor_En( enum enumPumpSelect Select, _Bool State )
|
||
{
|
||
switch( Select )
|
||
{
|
||
case M1:
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/******************************** 功能说明 *************************************
|
||
* TIMx 中断处理函数
|
||
*******************************************************************************/
|
||
volatile uint32_t SetPluseCount[7u]; // 设置运行脉冲数
|
||
volatile uint32_t PluseCount[7u]; // 实际运行脉冲数
|
||
volatile _Bool CountOver[7u] = {false}; // 计数器溢出标志
|
||
volatile uint32_t SamplePumpSetPluseCount[7u]; // 设置运行脉冲数
|
||
volatile uint32_t SamplePumpPluseCount[7u]; // 实际运行脉冲数
|
||
volatile _Bool SamplePumpCountOver[7u] = {false}; // 计数器溢出标志
|
||
|
||
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂运行任务
|
||
*********************************************************************************/
|
||
|
||
const uint32_t FullCoordinate[pumpMax] = { 20500u, 65000u,65000u };
|
||
int32_t CoordinateSet[pumpMax]; // 目标位置
|
||
int32_t CoordinatePosition[pumpMax]; // 实时更新实时位置
|
||
uint32_t Position[pumpMax]; // 本次运行起始位置
|
||
_Bool TaskisRunning[pumpMax] = {0}; // 防止重入标志
|
||
_Bool IdelEn[pumpMax] = { false,false,false}; // 电机空闲使能
|
||
int32_t DCMotorSpeed[pumpMax]; // 直流电机速度
|
||
|
||
uint32_t CoordinateSetOld[pumpMax]; // 上次所在位置
|
||
int32_t Differ[pumpMax]; // 本次所需运行脉冲数
|
||
float SpeedSet[pumpMax]={ 100.0f, 1000.0f, 5000.0f }; // 最高速度p/s
|
||
uint32_t period[pumpMax] = { 20000u, 2000u, 2000u }; // PWM实时周期
|
||
float Velocity[pumpMax] = { 0.0f, 20.0f, 20.0f }; // 实时速度
|
||
uint32_t Shifting[pumpMax] = { 500u, 500u, 500u }; // 加速过程脉冲数
|
||
float Speeda[pumpMax] ={ 10000.0f, 10000.0f, 10000.0f }; // 加速度 p/(s*s)
|
||
float VelocityH[pumpMax]={ 10000.0f, 5000.0f, 5000.0f }; // 最高速度p/s
|
||
float VelocityL[pumpMax]={ 10.0f, 1000.0f, 1000.0f }; // 最低速度p/s
|
||
float ShiftPluse[pumpMax]={ 100.0f, 1500.0f, 1500.0f }; // 标准最大加速过程脉冲数
|
||
float VelocityMax[pumpMax] = { 10000.0f, 20.0f, 20.0f }; // 最大速度
|
||
_Bool Direction[pumpMax] = { false,false,false}; // 运行方向
|
||
/******************************** 功能说明 *************************************
|
||
* 速度加速度转换
|
||
*******************************************************************************/
|
||
static uint16_t SpeedOld[pumpMax];
|
||
_Bool PumpShutDown[pumpMax] = {false, false}; // 急停标志
|
||
_Bool DirectionSet[pumpMax]; // 目标方向
|
||
_Bool ENSet[pumpMax] = {false,false,false}; // 是否使能电机
|
||
|
||
void SamplePumpSpeed_AcceleratedConver( enum enumPumpSelect Select, uint16_t Speed )
|
||
{
|
||
if( Speed != SpeedOld[Select] )
|
||
{
|
||
Speeda[Select] = (Speed * Speed) / (2u * ShiftPluse[Select]);
|
||
SpeedOld[Select] = Speed;
|
||
}
|
||
}
|
||
/******************************** 功能说明 *************************************
|
||
*
|
||
*******************************************************************************/
|
||
|
||
_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; // 置位 防重入标志
|
||
VelocityMax[Select] = 0;
|
||
period [Select] = 2000u;
|
||
Velocity[Select] = 6.0f;
|
||
Shifting[Select] = 100u;
|
||
|
||
PluseCount[Select] = 0u; // 清零计数脉冲
|
||
CountOver[Select] = false; // 清除计数器溢出
|
||
|
||
SetPluseCount[Select] = CoordinateSet[Select]; // 设置泵运行脉冲
|
||
Motor_IECMD( Select, true);
|
||
Motor_Dir ( Select, DirectionSet[Select] ); // 选择方向
|
||
Motor_PWM ( Select, 100000.0f / VelocityL[Select] ); // 重置pwm为最低输出
|
||
osDelay( 50u );
|
||
Motor_En ( Select, true ); // 步进电机控制器控制使能
|
||
|
||
Shifting[Select] = SetPluseCount[Select] / 2u;
|
||
if( Shifting[Select] > ShiftPluse[Select] )
|
||
Shifting[Select] = ShiftPluse[Select];
|
||
Motor_PWMCmd( Select, true ); // 开启脉冲输出
|
||
|
||
while( !CountOver[Select] ) // 计数器没有溢出
|
||
{
|
||
if(PumpShutDown[Select])
|
||
{
|
||
PumpShutDown[Select] = false;
|
||
break;
|
||
}
|
||
|
||
if( PluseCount[Select] < Shifting[Select] ) // 前Shifting[Select]脉冲加速
|
||
{
|
||
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 // 中间匀速
|
||
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 // 后Shifting[Select]脉冲减速
|
||
// 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] ); // 步进电机控制器控制使能
|
||
TaskisRunning[Select] = false; // 清除防重入标志
|
||
CountOver[Select] = false; // 清除计数器溢出
|
||
osDelay( 15u );
|
||
}
|
||
else
|
||
{
|
||
osDelay(15);
|
||
}
|
||
}
|
||
}
|
||
static bool ArmInitIsRuning[pumpMax] = { false,false,false};
|
||
/********************************** 功能说明 *************************************
|
||
* 机械臂运行任务
|
||
*********************************************************************************/
|
||
void ArmSoftCtrl( enum enumPumpSelect Select )
|
||
{
|
||
/*Vmin = 50p/s Vmax = 20000p/s At = 0.2s a = 100000p/s2 △s = 2000p △t = 0.001s P = 0.000001s */
|
||
|
||
//#define LowSpeed 40.0f
|
||
// for(;;)
|
||
{
|
||
|
||
if(
|
||
(CoordinateSet[Select] != CoordinateSetOld[Select]) // 如果目标位置与当前位置不同
|
||
&& !TaskisRunning[Select] // 如果此任务没有正在运行
|
||
&&!( ArmInitIsRuning[M3] // 且所有的任务没有正在初始化
|
||
)
|
||
)
|
||
{
|
||
|
||
TaskisRunning[Select] = true; // 置位 防重入标志
|
||
|
||
VelocityMax[Select] = 0;
|
||
|
||
if( CoordinateSet[Select] > FullCoordinate[Select])
|
||
{
|
||
/*usRegHoldingBuf[7] = */CoordinateSet[Select] = CoordinateSetOld[Select];
|
||
osDelay(15); // 极限位置保护
|
||
}
|
||
|
||
period [Select] = 20000u; //周期
|
||
Velocity[Select] = 500.0f; //实时速度
|
||
Shifting[Select] = 1500u; //加速脉冲
|
||
|
||
CoordinateSetOld[Select] = CoordinateSet[Select]; //
|
||
|
||
PluseCount[Select] = 0u; // 清零计数脉冲
|
||
CountOver[Select] = false; // 清除计数器溢出
|
||
|
||
Differ[Select] = CoordinateSet[Select] - CoordinatePosition[Select]; // 计算设置位置与当前位置的偏差和方向
|
||
if(Differ[Select] != 0)
|
||
{
|
||
if( Differ[Select] > 0 )
|
||
Direction[Select] = true;
|
||
if( Differ[Select] < 0 )
|
||
Direction[Select] = false;
|
||
SetPluseCount[Select] = abs( Differ[Select]); // 设置泵运行脉冲
|
||
Position[Select] = CoordinatePosition[Select];
|
||
|
||
Motor_IECMD( Select, true);
|
||
Motor_Dir ( Select, ( Differ[Select] >= 0 ) ? false : true ); // 选择方向
|
||
Motor_PWM ( Select, period[Select] ); // 重置pwm为最低输出
|
||
osDelay( 50u );
|
||
|
||
Shifting[Select] = abs(Differ[Select]) / 2u;
|
||
if( Shifting[Select] > ShiftPluse[Select] )
|
||
Shifting[Select] = ShiftPluse[Select];
|
||
Motor_PWMCmd( Select, true ); // 开启脉冲输出
|
||
Motor_En ( Select, true ); // 步进电机控制器控制使能
|
||
while( !CountOver[Select] ) // 计数器没有溢出
|
||
{
|
||
|
||
// 前Shifting[Select]脉冲加速至匀速并匀速运行
|
||
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]) ) // 后Shifting[Select]脉冲减速
|
||
{
|
||
|
||
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 ) // 根据方向偏差来计算当前位置
|
||
{
|
||
CoordinatePosition[Select] = Position[Select] + PluseCount[Select];
|
||
}
|
||
else
|
||
{
|
||
CoordinatePosition[Select] = Position[Select] - PluseCount[Select];
|
||
}
|
||
|
||
}
|
||
|
||
if( Differ[Select] >= 0 ) // 根据最终计数来计算最终位置
|
||
{
|
||
CoordinatePosition[Select] = Position[Select] + PluseCount[Select];
|
||
}
|
||
else
|
||
{
|
||
CoordinatePosition[Select] = Position[Select] - PluseCount[Select];
|
||
}
|
||
}
|
||
Velocity[Select] = 0;
|
||
TaskisRunning[Select] = false; // 清除防重入标志
|
||
CountOver[Select] = false; // 清除计数器溢出
|
||
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"
|
||
|
||
/******************************** 功能说明 *************************************
|
||
* 中断端口初始化使能
|
||
*******************************************************************************/
|
||
void BIOS_EXTI_Init( void )
|
||
{
|
||
|
||
// // 浮空输入模式
|
||
|
||
SET_BIT( RCC->APB2ENR, RCC_APB2ENR_IOPBEN );
|
||
|
||
GPIOB->BSRR |= 0x0002u; // 端口输出为1；
|
||
// GPIOF->BSRR |= 0x3800u; // 端口输出为1；
|
||
// GPIOE->BSRR |= 0x0040u; // 端口输出为1；
|
||
// SET_BIT( GPIOC->PUPDR, GPIO_PUPDR_PUPD0_0|GPIO_PUPDR_PUPD1_0|GPIO_PUPDR_PUPD2_0|GPIO_PUPDR_PUPD3_0 ); // IO口配置 上拉
|
||
// SET_BIT( GPIOF->PUPDR, GPIO_PUPDR_PUPD11_0|GPIO_PUPDR_PUPD12_0|GPIO_PUPDR_PUPD13_0 ); // IO口配置 上拉
|
||
CLEAR_BIT( GPIOB->CRL, GPIO_CRL_CNF1_0 );
|
||
SET_BIT( GPIOB->CRL, GPIO_CRL_CNF1_1 );
|
||
SET_BIT( GPIOB->ODR, GPIO_ODR_ODR1 ); // IO口配置 上拉
|
||
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE0, GPIO_MODER_MODE0_1 ); // IO口配置 复用输出
|
||
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE1, GPIO_MODER_MODE1_1 ); // IO口配置 复用输出
|
||
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE2, GPIO_MODER_MODE2_1 ); // IO口配置 复用输出
|
||
// MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE3, GPIO_MODER_MODE3_1 ); // IO口配置 复用输出
|
||
// MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE11, GPIO_MODER_MODE11_1 ); // IO口配置 复用输出
|
||
// MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE12, GPIO_MODER_MODE12_1 ); // IO口配置 复用输出
|
||
// MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE13, GPIO_MODER_MODE13_1 ); // IO口配置 复用输出
|
||
|
||
|
||
// SET_BIT(RCC->APB2ENR, RCC_APB2ENR_SYSCFGEN);
|
||
// // 开启中断复用
|
||
// 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 ); // 禁止中断
|
||
|
||
CLEAR_BIT( EXTI->EMR, 0xFFFFu ); // no event 无中断事件
|
||
// SET_BIT ( EXTI->RTSR, 0x0006u ); // rising edge trigger 上升沿触发
|
||
SET_BIT ( EXTI->FTSR, 0x0002u ); // falling edge trigger 下降沿触发
|
||
WRITE_REG( EXTI->PR, 0xFFFFu ); // 通过写1能复位触发的中断标志
|
||
|
||
// SET_BIT ( EXTI->IMR, 0x384Fu ); // 允许中断
|
||
// // IO中断
|
||
// 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 );
|
||
|
||
}
|
||
|
||
/********************************** 功能说明 ***********************************
|
||
* 中断开关控制
|
||
*******************************************************************************/
|
||
void EXTIx_IRQ_Enable( uint8_t EXTIx )
|
||
{
|
||
SET_BIT( EXTI->PR, 0x01u << EXTIx ); // 写1复位中断标志位
|
||
SET_BIT( EXTI->IMR, 0x01u << EXTIx ); // 允许中断。
|
||
}
|
||
|
||
void EXTIx_IRQ_Disable( uint8_t EXTIx )
|
||
{
|
||
CLEAR_BIT( EXTI->IMR, 0x01u << EXTIx ); // 禁止中断
|
||
SET_BIT( EXTI->PR, 0x01u << EXTIx ); // 写1复位中断标志位
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
uint16_t iz = 0;
|
||
void EXTI1_IRQHandler( void )
|
||
{
|
||
iz++;
|
||
SET_BIT( EXTI->PR, 0x01u << 1 ); // 写1复位中断标志位
|
||
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, // 任务函数
|
||
"TaskName1", // 任务名（用于调试）
|
||
512, // 堆栈大小（以堆栈类型的大小为单位）
|
||
(void *)M1, // 传递给任务的参数
|
||
osPriorityNormal, // 任务优先级（根据需要调整）
|
||
&xTaskHandles[M1]); // 任务句柄的地址（用于引用）
|
||
temp2 = xTaskCreate(_task_MotorCTRL, // 任务函数
|
||
"TaskName2", // 任务名（用于调试）
|
||
512, // 堆栈大小（以堆栈类型的大小为单位）
|
||
(void *)M2, // 传递给任务的参数
|
||
osPriorityNormal, // 任务优先级（根据需要调整）
|
||
&xTaskHandles[M2]); // 任务句柄的地址（用于引用）
|
||
temp3 = xTaskCreate(_task_MotorCTRL, // 任务函数
|
||
"TaskName3", // 任务名（用于调试）
|
||
512, // 堆栈大小（以堆栈类型的大小为单位）
|
||
(void *)M3, // 传递给任务的参数
|
||
osPriorityNormal, // 任务优先级（根据需要调整）
|
||
&xTaskHandles[M3]); // 任务句柄的地址（用于引用）
|
||
|
||
}
|