2192 lines
43 KiB
C
2192 lines
43 KiB
C
//-------------------------------------------------------------------------------
|
||
// File Name: sewhead.c
|
||
// Brief:
|
||
// Version: 1.0.0
|
||
// Create Date: 2024/10/11
|
||
// Create by: W.X
|
||
// Copyright:
|
||
// TIANJIN GETONAGAIN TECHNOLOGY CO., LIMITED
|
||
// All rights reserved.
|
||
//
|
||
// Modify by: W.X
|
||
// Modify Date: 2024/10/17
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#define _IN_HEADBOARD_C
|
||
|
||
#include "sewhead.h"
|
||
#if (BOARD_USE_FOR == SEW_HEAD)
|
||
|
||
#include "trigger.h"
|
||
#include "inout.h"
|
||
#include "debug.h"
|
||
#include "workctrl.h"
|
||
#include "shell.h"
|
||
#include "delay.h"
|
||
#include "modbus_def.h"
|
||
#include "modbus_s.h"
|
||
#include "modbus_m.h"
|
||
#include "canwork.h"
|
||
#include "para.h"
|
||
|
||
typedef void (*PwmFunc)(void);
|
||
PwmFunc D29PwmProc = NULL;
|
||
|
||
void RefreshOutputSta(void);
|
||
void OutputCtrl(u8 num, u8 onof);
|
||
void PwmCtrlTask(void);
|
||
void InitSTM32PwmTimer(void);
|
||
void InitSoftPwmCtrl(PwmFunc proc);
|
||
void SoftPwmIntProc(void);
|
||
void InOutStaRefreshTask(void);
|
||
|
||
#if(CONTROL_BUS == COMM_485)
|
||
void InOutWorkTask(Rs485Cmd * pCmd);
|
||
#endif
|
||
|
||
void ModBusCmdProcTask(Rs485Cmd * pCmd);
|
||
void RefreshElasticMoto(void);
|
||
void TopThreadBreakCheck(void);
|
||
void ModBusReceiveProc(Rs485Cmd *cmd);
|
||
int MotoMoveAsOfst(int mode, int ofst, spdCtrl *spd, int block);
|
||
void FootMotoDis(void);
|
||
void FootMotoEn(void);
|
||
void FollowEcdOutPulseOn(void);
|
||
void FollowEcdOutPulseOff(void);
|
||
void FollowEcdOutDirOn(void);
|
||
void FollowEcdOutDirOff(void);
|
||
void FootMotoToZero(void);
|
||
void CheckParasValue(void);
|
||
void PFootCtrlUp(void);
|
||
void PFootCtrlDown(void);
|
||
void PFootCtrlBack(void);
|
||
void SetMotoPosValue(u16 pos,u16 value);
|
||
|
||
void TestAction(char * para1, char * para2);
|
||
|
||
#define CHK_TIME_GAP 10 // 发送时间间隔, 单位 100us
|
||
#define MODRSV_TIMEOUT 820 // 读取超时时间, 单位为 CHK_TIME_GAP * 100us
|
||
#define MODSEND_GAP 50 // 发送和读取的间隔, 单位为 CHK_TIME_GAP * 100us
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#define BUF_TAB_SIZE 1000 // 缓冲区大小
|
||
|
||
typedef struct
|
||
{
|
||
u8 boardId;
|
||
u8 outputSta;
|
||
u8 inputSta;
|
||
u8 outputCtrl;
|
||
u8 pwmOutputFlag;
|
||
u8 softLockSta;
|
||
// u16 d29cmdReg[MODBUS_REG_MAX-MODBUS_REG_MIN];
|
||
|
||
u16 pwmCtrl; // 输出占空比 0~100
|
||
u16 pwmFreq; // 定时器PWM频率
|
||
u16 pwmCount;
|
||
u32 timeCount;
|
||
u32 pwmPulsTime;
|
||
u32 startTime; // 启动时间
|
||
u32 chgTime; // 变化次数
|
||
u32 cmprbuf; // 分频系数缓存
|
||
int workingSta;
|
||
|
||
u32 tabIdx; // 缓存表索引
|
||
u32 chgTimeTab[BUF_TAB_SIZE]; // 变更次数缓存表
|
||
u32 curTimeTab[BUF_TAB_SIZE]; // 变更次数缓存表
|
||
u8 pwmTab[BUF_TAB_SIZE]; // PWM缓存缓存表
|
||
|
||
u8 outsta;
|
||
u8 outOtFlag;
|
||
u8 workFlag;
|
||
|
||
u16 enable;
|
||
u16 adcvalue;
|
||
u16 thrdBreakFlag;
|
||
u16 mtWkSta[8];
|
||
u32 outTime;
|
||
|
||
u32 enflag;
|
||
u32 stopAngle;
|
||
u32 encoderValue;
|
||
s32 footMtPos;
|
||
|
||
u32 followRunNum;
|
||
int followStartPPos;
|
||
int followStartNPos;
|
||
u32 followConPAng;
|
||
u32 followConNAng;
|
||
|
||
u32 pfootFlLowPos;
|
||
|
||
u32 followEcdEn;
|
||
u32 followCountr;
|
||
u32 followStepNum;
|
||
u32 followNumLow;
|
||
u32 followNumHigh;
|
||
int followTableIdx;
|
||
int followEcdDir;
|
||
int followEcdCheck;
|
||
int followChangeFlag;
|
||
u32 followChangeNum;
|
||
int setParasFlag;
|
||
u32 setParasTime;
|
||
int pfootSta;
|
||
|
||
u32 pfootRunHighPos;
|
||
u32 pfootFlowPAng; // 压脚跟踪上升位
|
||
u32 pfootFlowNAng; // 压脚跟踪下降位
|
||
u32 pfootFlowPuls; // 压脚跟踪脉冲数
|
||
u32 pfootRunLowPos; // 压脚运行低位
|
||
u32 pfootRunBackPos; // 压脚运行回针位
|
||
s32 ecOfstValue; // 编码器差值
|
||
u32 pfootRunConAng; // 跟随持续角度
|
||
//------------
|
||
void (*FollowMotoPulseOn)(void);
|
||
void (*FollowMotoPulseOff)(void);
|
||
void (*FollowMotoSignOn)(void);
|
||
void (*FollowMotoSignOff)(void);
|
||
//------------
|
||
|
||
u32 thrdBreakCount;
|
||
u32 motocoordinate;
|
||
u16 headCmdReg[MODBUS_REG_MAX-MODBUS_REG_MIN];
|
||
}HeadCtrl;
|
||
|
||
HeadCtrl g_headCtrl;
|
||
|
||
//-------------------------------------------------------------------------------
|
||
u8 GetBoardSwitchValue(void)
|
||
{
|
||
u8 val = 0;
|
||
if(GetSwitch1Status() == SENSOR_ON)
|
||
{
|
||
val |= 0x01;
|
||
}
|
||
if(GetSwitch2Status() == SENSOR_ON)
|
||
{
|
||
val |= 0x02;
|
||
}
|
||
if(GetSwitch3Status() == SENSOR_ON)
|
||
{
|
||
val |= 0x04;
|
||
}
|
||
if(GetSwitch4Status() == SENSOR_ON)
|
||
{
|
||
val |= 0x08;
|
||
}
|
||
return val;
|
||
}
|
||
|
||
// 初始化
|
||
void InitSewHeadBoard(void)
|
||
{
|
||
memset(&g_headCtrl, 0, sizeof(HeadCtrl));
|
||
g_headCtrl.boardId = GetBoardSwitchValue();
|
||
|
||
if((g_headCtrl.boardId & 0x04) == 0)
|
||
{
|
||
InitSoftPwmCtrl(PwmCtrlTask);
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
g_headCtrl.pwmOutputFlag = 1;
|
||
#endif
|
||
printf("initsoftpwmctrl.");
|
||
}
|
||
|
||
#if(CONTROL_BUS == COMM_485)
|
||
#if(COM_485_RTU == 1)
|
||
InitModbusS(COMM_USART2, B115200, '8', 'N', '1','R'); // 初始化485
|
||
#else
|
||
if((g_headCtrl.boardId & 0x08) != 0)
|
||
{
|
||
InitModbusS(COMM_USART2, B115200, '8', 'N', '1','R'); // 初始化485
|
||
}
|
||
else
|
||
{
|
||
InitModbusS(COMM_USART2, B115200, '8', 'N', '1','A'); // 初始化485
|
||
}
|
||
#endif
|
||
//InitModbusS(COMM_USART1,B115200, '9', 'E', '1', 'A'); // 串口3,115200波特率
|
||
InitModbusReg(MODBUS_REG_MIN, &g_headCtrl.d29cmdReg[0], (MODBUS_REG_MAX-MODBUS_REG_MIN));
|
||
InitModbusSCtrl(D29_BOARD_ID,30); // 配置modbus(收包超时时间:2ms)
|
||
|
||
RegModbusSCmdProc(ModBusCmdProcTask); // 外部函数
|
||
|
||
#endif
|
||
InitModbus_m(0,COMM_USART3, B115200, '8', 'N', '1','R'); // 初始化485
|
||
InitRs485Ctrl(MODRSV_TIMEOUT);
|
||
RegRsCmdProc(ModBusReceiveProc);
|
||
|
||
CheckParasValue();
|
||
g_headCtrl.pfootFlLowPos = g_headCtrl.pfootRunLowPos;
|
||
g_headCtrl.enflag = EN_FALG;
|
||
g_headCtrl.FollowMotoPulseOn = FollowEcdOutPulseOn;
|
||
g_headCtrl.FollowMotoPulseOff = FollowEcdOutPulseOff;
|
||
g_headCtrl.FollowMotoSignOn = FollowEcdOutDirOn;
|
||
g_headCtrl.FollowMotoSignOff = FollowEcdOutDirOff;
|
||
DelayRef(500);
|
||
FootMotoToZero();
|
||
SetEn485OutOff();
|
||
|
||
AddShellCmd("KEY", "test action", TestAction);
|
||
}
|
||
|
||
|
||
void InitSTM32PwmTimer(void)
|
||
{
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
|
||
#else
|
||
NVIC_InitTypeDef NVIC_InitStructure;
|
||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||
TIM_OCInitTypeDef TIM_TimeOCStructure;
|
||
/* 中断配置 */
|
||
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; /* 通道 */
|
||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; /* 抢占式中断优先级 */
|
||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; /* 响应式中断优先级 */
|
||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; /* 使能中断 */
|
||
NVIC_Init(&NVIC_InitStructure);
|
||
/*IO配置*/
|
||
GPIO_InitTypeDef GPIO_InitStructure;
|
||
RCC_APB2PeriphClockCmd(OUT7_RCC_APB2Periph, ENABLE);
|
||
|
||
GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3,ENABLE);
|
||
|
||
GPIO_InitStructure.GPIO_Pin = OUT6_GPIO_Pin | OUT7_GPIO_Pin;
|
||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
||
|
||
// GPIO_InitStructure.GPIO_Pin = OUT7_GPIO_Pin;
|
||
// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
||
|
||
GPIO_Init(OUT7_GPIO_PORT, &GPIO_InitStructure);
|
||
|
||
///* 定时器配置 */
|
||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); /* 启动定时器时钟 */
|
||
TIM_DeInit(TIM3); /* 重新将Timer设置为缺省值 */
|
||
///* 时基配置 */
|
||
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
||
TIM_TimeBaseStructure.TIM_Prescaler = (72-1); /* 预分频 */
|
||
TIM_TimeBaseStructure.TIM_Period = (3334-1); /* 自动装载值 500us中断*/
|
||
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; /* 采样时钟 */
|
||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /* 向上计数模式 */
|
||
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); /* 初始化TIMx */
|
||
|
||
TIM_TimeOCStructure.TIM_OCMode = TIM_OCMode_PWM2;
|
||
TIM_TimeOCStructure.TIM_OCPolarity = TIM_OCPolarity_High;
|
||
TIM_TimeOCStructure.TIM_OutputState = TIM_OutputState_Enable;
|
||
TIM_TimeOCStructure.TIM_Pulse = 0;
|
||
|
||
TIM_OC1Init(TIM3,&TIM_TimeOCStructure);
|
||
TIM_OC2Init(TIM3,&TIM_TimeOCStructure);
|
||
|
||
TIM_CtrlPWMOutputs(TIM3,DISABLE);
|
||
|
||
TIM_OC1PreloadConfig(TIM3,ENABLE);
|
||
TIM_OC2PreloadConfig(TIM3,ENABLE);
|
||
TIM_ARRPreloadConfig(TIM3,ENABLE);
|
||
|
||
//TIM_ClearFlag(TIM3, TIM_FLAG_Update); /* 清除溢出中断标志 */
|
||
//TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); /* 开启中断 */
|
||
TIM_Cmd(TIM3, ENABLE); /* 开启定时器 */
|
||
|
||
#endif
|
||
}
|
||
|
||
void SoftPwmIntProc(void)
|
||
{
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
if (D29PwmProc != NULL)
|
||
{
|
||
D29PwmProc();
|
||
}
|
||
#else
|
||
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
|
||
{
|
||
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
|
||
if (D29PwmProc != NULL)
|
||
{
|
||
D29PwmProc();
|
||
}
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void InitSoftPwmCtrl(PwmFunc proc)
|
||
{
|
||
// InitSTM32PwmTimer();
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
// Tim1IntProc = SoftPwmIntProc;
|
||
D29PwmProc = proc;
|
||
g_headCtrl.pwmFreq = 100;
|
||
g_headCtrl.pwmCount = 0;
|
||
g_headCtrl.pwmCtrl = 0; // 初始占空比为0
|
||
#else
|
||
Tim3IntProc = SoftPwmIntProc;
|
||
D29PwmProc = proc;
|
||
g_headCtrl.pwmCtrl = 0;
|
||
g_headCtrl.pwmFreq = 3334;
|
||
#endif
|
||
}
|
||
|
||
void PwmCtrlTask(void)
|
||
{
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
if(g_headCtrl.pwmCount < g_headCtrl.pwmCtrl)
|
||
{
|
||
g_headCtrl.pwmCount ++;
|
||
// if((g_headCtrl.pwmIOSta & 0x20) != 0)
|
||
{
|
||
SetOutputPWM1On();
|
||
SetOutputPWM2On();
|
||
}
|
||
}
|
||
else if(g_headCtrl.pwmCount < g_headCtrl.pwmFreq)
|
||
{
|
||
g_headCtrl.pwmCount ++;
|
||
if(g_headCtrl.pwmCount >= g_headCtrl.pwmFreq)
|
||
{
|
||
g_headCtrl.pwmCount = 0;
|
||
if(g_headCtrl.timeCount != 0)
|
||
{
|
||
if(g_headCtrl.timeCount == 1)
|
||
{
|
||
g_headCtrl.pwmPulsTime = GetUsSoftTimer();
|
||
}
|
||
|
||
g_headCtrl.timeCount ++;
|
||
if(g_headCtrl.timeCount >= 11)
|
||
{
|
||
g_headCtrl.timeCount = 0;
|
||
// printf("puls Freq:%d.\r\n",10000000/(GetUsSoftTimer() - g_headCtrl.pwmPulsTime));
|
||
}
|
||
}
|
||
}
|
||
SetOutputPWM1Off();
|
||
SetOutputPWM2Off();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.pwmCount = 0;
|
||
// SetOutput6Off();
|
||
// SetOutput7Off();
|
||
}
|
||
|
||
// g_headCtrl.timeCount ++;
|
||
// if(g_headCtrl.timeCount > 20000)
|
||
// {
|
||
// u32 ntime = 0;
|
||
// ntime = GetMsSoftTimer();
|
||
// g_headCtrl.timeCount = 0;
|
||
//
|
||
// printf("timer init 1s,pwmCount = %d,time:%d.\r\n",g_headCtrl.pwmCount,ntime);
|
||
// }
|
||
#else
|
||
// g_headCtrl.timeCount ++;
|
||
// if(g_headCtrl.timeCount > 1000000/g_headCtrl.pwmFreq)
|
||
// {
|
||
// u32 ntime = 0;
|
||
// ntime = GetMsSoftTimer();
|
||
// g_headCtrl.timeCount = 0;
|
||
//
|
||
// printf("timer 1s,time:%d,pwm频率 = %dHz,pwm占空比 %d.\r\n",ntime,1000000/g_headCtrl.pwmFreq,g_headCtrl.pwmCtrl);
|
||
// }
|
||
#endif
|
||
}
|
||
|
||
|
||
void SysLedTask(void)
|
||
{
|
||
static u32 ledsta = 0;
|
||
static u32 ledtime = 0;
|
||
if(GetMsSoftTimer10() - ledtime >= 1000)
|
||
{
|
||
if(ledsta == 0)
|
||
{
|
||
SetSysLedOn();
|
||
ledsta = 1;
|
||
}
|
||
else
|
||
{
|
||
SetSysLedOff();
|
||
ledsta = 0;
|
||
}
|
||
ledtime = GetMsSoftTimer10();
|
||
}
|
||
}
|
||
|
||
|
||
void SewHeadBoardTask(void)
|
||
{
|
||
//SetParasTask();
|
||
Rs485Task();
|
||
#if(CONTROL_BUS == COMM_485)
|
||
ModbusSTask();
|
||
#endif
|
||
InOutStaRefreshTask();
|
||
SysLedTask();
|
||
}
|
||
|
||
void ModBusReceiveProc(Rs485Cmd *cmd)
|
||
{
|
||
if (cmd == NULL)
|
||
{
|
||
return;
|
||
}
|
||
|
||
memcpy(g_headCtrl.mtWkSta,cmd->rddat,16);
|
||
|
||
}
|
||
|
||
void SendMotoCtrlCmd(u16 cmd,u16 act)
|
||
{
|
||
Rs485Cmd ccmd;
|
||
memset(&ccmd,0,sizeof(Rs485Cmd));
|
||
|
||
ccmd.index = 10;
|
||
ccmd.cmd = MODBUS_WR;
|
||
ccmd.addr = cmd;
|
||
ccmd.wrdat[0] = act;
|
||
ccmd.wlen = 1;
|
||
ccmd.rlen = 0;
|
||
|
||
AddRs485Cmd(&ccmd);
|
||
}
|
||
|
||
void FootMotoCmdRst(void)
|
||
{
|
||
SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_CMD_RST);
|
||
}
|
||
|
||
void FootMotoToZero(void)
|
||
{
|
||
SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_TO_ZERO);
|
||
// SetSTM32Moto1RealPos(0);
|
||
}
|
||
|
||
void FootMotoDis(void)
|
||
{
|
||
SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_DIS);
|
||
}
|
||
|
||
void FootMotoEn(void)
|
||
{
|
||
SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_EN);
|
||
}
|
||
|
||
void FootMotoReset(void)
|
||
{
|
||
SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_RESET);
|
||
}
|
||
|
||
void OutputStepOn(void)
|
||
{
|
||
if(g_headCtrl.followEcdDir == 1)
|
||
{
|
||
g_headCtrl.footMtPos ++;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.footMtPos --;
|
||
}
|
||
SetMoto1StepOn();
|
||
}
|
||
|
||
#define FOLLOW_TAB_NUM 6
|
||
|
||
int FOLLOW_TABLE[FOLLOW_TAB_NUM] =
|
||
{
|
||
20,
|
||
16,
|
||
12,
|
||
8,
|
||
6,
|
||
3,
|
||
};
|
||
|
||
void GetFollowTableValue(u32 angle,u32 puls)
|
||
{
|
||
int i, temp,value,followec,sum = 0;
|
||
followec = angle*1000/360;
|
||
value = followec/puls;
|
||
|
||
for(i = 0; i<FOLLOW_TAB_NUM-1; i++)
|
||
{
|
||
FOLLOW_TABLE[i] = value + (4*(FOLLOW_TAB_NUM -2 - i));
|
||
sum += FOLLOW_TABLE[i];
|
||
}
|
||
temp = puls-((FOLLOW_TAB_NUM-1)*2);
|
||
FOLLOW_TABLE[FOLLOW_TAB_NUM-1] = (followec - sum*2)/temp;
|
||
}
|
||
|
||
void FollowEcdOutPulseOn(void)
|
||
{
|
||
if (g_headCtrl.followEcdEn != 0)
|
||
{
|
||
g_headCtrl.followCountr++;
|
||
|
||
if (g_headCtrl.followCountr >= g_headCtrl.followStepNum)
|
||
{
|
||
if(g_headCtrl.followNumHigh > g_headCtrl.followRunNum)
|
||
{
|
||
g_headCtrl.followNumHigh = 0;
|
||
g_headCtrl.followNumLow = 0;
|
||
g_headCtrl.followEcdEn = 0;
|
||
return;
|
||
}
|
||
|
||
OutputStepOn();
|
||
g_headCtrl.followCountr = 0;
|
||
g_headCtrl.followNumHigh++;
|
||
|
||
// 加速
|
||
if (g_headCtrl.followTableIdx < FOLLOW_TAB_NUM && g_headCtrl.followNumHigh < FOLLOW_TAB_NUM)
|
||
{
|
||
g_headCtrl.followTableIdx++; // 跟随运动的索引
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[g_headCtrl.followTableIdx]; // 装载新的值
|
||
}
|
||
|
||
// 降速
|
||
if(g_headCtrl.followNumHigh >= g_headCtrl.followRunNum - FOLLOW_TAB_NUM)
|
||
{
|
||
if(g_headCtrl.followTableIdx <= 0)
|
||
{
|
||
g_headCtrl.followTableIdx = 0;
|
||
}
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[g_headCtrl.followTableIdx]; // 装载新的值
|
||
g_headCtrl.followTableIdx--; // 跟随运动的索引
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void FollowEcdOutPulseOff(void)
|
||
{
|
||
if (g_headCtrl.followEcdEn != 0)
|
||
{
|
||
if (g_headCtrl.followCountr * 2 + 1 >= g_headCtrl.followStepNum)
|
||
{
|
||
OutputStepOff();
|
||
g_headCtrl.followNumLow++;
|
||
}
|
||
}
|
||
}
|
||
|
||
void FollowEcdOutDirOn(void)
|
||
{
|
||
if (g_headCtrl.followEcdCheck != 0)
|
||
{
|
||
if (g_headCtrl.followEcdDir == 1)
|
||
{
|
||
OutputDirOn();
|
||
}
|
||
else
|
||
{
|
||
OutputDirOff();
|
||
}
|
||
}
|
||
}
|
||
void FollowEcdOutDirOff(void)
|
||
{
|
||
if (g_headCtrl.followEcdCheck != 0)
|
||
{
|
||
if (g_headCtrl.followEcdDir == 1)
|
||
{
|
||
OutputDirOff();
|
||
}
|
||
else
|
||
{
|
||
OutputDirOn();
|
||
}
|
||
}
|
||
}
|
||
|
||
void CheckParasValue(void)
|
||
{
|
||
g_headCtrl.followRunNum = g_headCtrl.pfootFlowPuls;
|
||
if(g_headCtrl.followRunNum == 0)
|
||
{
|
||
g_headCtrl.followRunNum = 40;
|
||
}
|
||
g_headCtrl.followStepNum = 3;
|
||
g_headCtrl.followStartPPos = g_headCtrl.pfootFlowPAng*1000/360 + g_headCtrl.ecOfstValue;
|
||
if(g_headCtrl.followStartPPos >= 1000)
|
||
{
|
||
g_headCtrl.followStartPPos -= 1000;
|
||
}
|
||
if(g_headCtrl.followStartPPos < 0)
|
||
{
|
||
g_headCtrl.followStartPPos += 1000;
|
||
}
|
||
if(g_headCtrl.followStartPPos == 0)
|
||
{
|
||
g_headCtrl.followStartPPos = 100;
|
||
}
|
||
g_headCtrl.followStartNPos = g_headCtrl.pfootFlowNAng*1000/360 + g_headCtrl.ecOfstValue;
|
||
if(g_headCtrl.followStartNPos >= 1000)
|
||
{
|
||
g_headCtrl.followStartNPos -= 1000;
|
||
}
|
||
if(g_headCtrl.followStartNPos < 0)
|
||
{
|
||
g_headCtrl.followStartNPos += 1000;
|
||
}
|
||
if(g_headCtrl.followStartNPos == 0)
|
||
{
|
||
g_headCtrl.followStartNPos = 600;
|
||
}
|
||
g_headCtrl.pfootRunLowPos = g_headCtrl.pfootRunLowPos;
|
||
if(g_headCtrl.pfootRunLowPos == 0)
|
||
{
|
||
g_headCtrl.pfootRunLowPos = 10;
|
||
}
|
||
g_headCtrl.pfootRunBackPos = g_headCtrl.pfootRunBackPos;
|
||
if(g_headCtrl.pfootRunBackPos == 0)
|
||
{
|
||
g_headCtrl.pfootRunBackPos = 10;
|
||
}
|
||
g_headCtrl.pfootRunHighPos = g_headCtrl.pfootRunLowPos + g_headCtrl.followRunNum;
|
||
if(g_headCtrl.pfootRunHighPos == 0)
|
||
{
|
||
g_headCtrl.pfootRunHighPos = 50;
|
||
}
|
||
|
||
g_headCtrl.followConPAng = g_headCtrl.pfootRunConAng;
|
||
if(g_headCtrl.followConPAng == 0)
|
||
{
|
||
g_headCtrl.followConPAng = 140;
|
||
}
|
||
else if(g_headCtrl.followConPAng >160)
|
||
{
|
||
g_headCtrl.followConPAng = 160;
|
||
}
|
||
|
||
GetFollowTableValue(g_headCtrl.followConPAng,g_headCtrl.followRunNum);
|
||
//GetFollowTableValue(g_headCtrl.followConNAng,g_headCtrl.followRunNum);
|
||
}
|
||
|
||
void SetMotoPosValue(u16 pos,u16 value)
|
||
{
|
||
SendMotoCtrlCmd(pos,value);
|
||
}
|
||
|
||
void PFootCtrlUp(void)
|
||
{
|
||
//MotoMoveAsOfst(1,100,NULL,0);
|
||
//SendMotoCtrlCmd(MOTO_CTRL_TOPOS,PFOOT_HIGH_POS);
|
||
SendMotoCtrlCmd(MOTO_CTRL_TOZPOS,g_headCtrl.pfootRunHighPos);
|
||
g_headCtrl.pfootSta = 1;
|
||
}
|
||
|
||
void PFootCtrlDown(void)
|
||
{
|
||
//MotoMoveAsOfst(1,30,NULL,0);
|
||
//SendMotoCtrlCmd(MOTO_CTRL_TOPOS,PFOOT_LOW_POS);
|
||
SendMotoCtrlCmd(MOTO_CTRL_TOZPOS,g_headCtrl.pfootRunLowPos);
|
||
g_headCtrl.pfootSta = 2;
|
||
g_headCtrl.followRunNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunLowPos;
|
||
// g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunLowPos;
|
||
// if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
|
||
// {
|
||
// g_headCtrl.followChangeFlag = 1;
|
||
// g_headCtrl.pfootFlLowPos = g_headCtrl.pfootRunLowPos;
|
||
// }
|
||
}
|
||
|
||
void PFootCtrlBack(void)
|
||
{
|
||
//MotoMoveAsOfst(1,g_headCtrl.pfootRunBackPos,NULL,0);
|
||
SendMotoCtrlCmd(MOTO_CTRL_TOZPOS,g_headCtrl.pfootRunBackPos);
|
||
g_headCtrl.pfootSta = 2;
|
||
g_headCtrl.followRunNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunBackPos;
|
||
// g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunBackPos;
|
||
// if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
|
||
// {
|
||
// g_headCtrl.followChangeFlag = 1;
|
||
// g_headCtrl.pfootFlLowPos = g_headCtrl.pfootRunBackPos;
|
||
// }
|
||
}
|
||
|
||
#if(CONTROL_BUS == COMM_485)
|
||
void ModBusCmdProcTask(Rs485Cmd * pCmd)
|
||
{
|
||
Rs485Cmd rCmd;
|
||
|
||
if (pCmd == NULL)
|
||
{
|
||
return;
|
||
}
|
||
memcpy(&rCmd, pCmd, sizeof(Rs485Cmd));
|
||
|
||
InOutWorkTask(pCmd);
|
||
if (rCmd.cmd == MODBUS_06WR)
|
||
{
|
||
switch (rCmd.addr)
|
||
{
|
||
case 0:
|
||
{
|
||
break;
|
||
}
|
||
|
||
case 1:
|
||
{
|
||
if (rCmd.wrdat[0] == 1)
|
||
{
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[0];
|
||
g_headCtrl.followEcdCheck = 1;
|
||
g_headCtrl.followEcdDir = 1;
|
||
g_headCtrl.footMtPos = GetSTM32Moto1RealPos();
|
||
|
||
g_headCtrl.followEcdEn = 0;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.followEcdCheck = 0;
|
||
//g_headCtrl.pfootSta = 0;
|
||
}
|
||
break;
|
||
}
|
||
case 5:
|
||
{
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[0];
|
||
g_headCtrl.followEcdCheck = 1;
|
||
g_headCtrl.followEcdDir = 1;
|
||
g_headCtrl.footMtPos = GetSTM32Moto1RealPos();
|
||
//if(g_headCtrl.pfootSta == 0)
|
||
{
|
||
PFootCtrlDown();
|
||
}
|
||
g_headCtrl.followEcdEn = 0;
|
||
break;
|
||
}
|
||
case 2: // 压脚升降
|
||
{
|
||
if (rCmd.wrdat[0] == 1)
|
||
{
|
||
PFootCtrlUp();
|
||
}
|
||
else if (rCmd.wrdat[0] == 2)
|
||
{
|
||
PFootCtrlDown();
|
||
}
|
||
else if (rCmd.wrdat[0] == 3)
|
||
{
|
||
PFootCtrlBack();
|
||
}
|
||
g_headCtrl.followEcdEn = 0;
|
||
break;
|
||
}
|
||
|
||
case 3:
|
||
{
|
||
g_headCtrl.followStartNPos = rCmd.wrdat[0];
|
||
break;
|
||
}
|
||
case 4:
|
||
{
|
||
g_headCtrl.followStartPPos = rCmd.wrdat[0];
|
||
break;
|
||
}
|
||
case 11:
|
||
{
|
||
MotoMoveAsOfst(0,(s16)rCmd.wrdat[0],NULL,0);
|
||
break;
|
||
}
|
||
case 12:
|
||
{
|
||
MotoMoveAsOfst(1,(s16)rCmd.wrdat[0],NULL,0);
|
||
break;
|
||
}
|
||
case 20:
|
||
{
|
||
if(rCmd.wrdat[0] == 0)
|
||
{
|
||
FootMotoToZero();
|
||
}
|
||
else if(rCmd.wrdat[0] == 1)
|
||
{
|
||
FootMotoDis();
|
||
}
|
||
else if(rCmd.wrdat[0] == 2)
|
||
{
|
||
FootMotoEn();
|
||
}
|
||
else if(rCmd.wrdat[0] == 10)
|
||
{
|
||
FootMotoReset();
|
||
}
|
||
else
|
||
{
|
||
FootMotoCmdRst();
|
||
}
|
||
break;
|
||
}
|
||
case 50: // 设定压脚工作下位
|
||
{
|
||
if(rCmd.wrdat[0] == 0)
|
||
{
|
||
g_headCtrl.followChangeNum = g_headCtrl.followRunNum;
|
||
//g_headCtrl.followChangeFlag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - rCmd.wrdat[0];
|
||
g_headCtrl.pfootFlLowPos = rCmd.wrdat[0];
|
||
if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
|
||
{
|
||
g_headCtrl.followChangeFlag = 1;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
case 100: // 校正编码器位置
|
||
{
|
||
int lpos;
|
||
lpos = rCmd.wrdat[0]*1000/36000;
|
||
g_headCtrl.ecOfstValue = g_headCtrl.encoderValue - lpos;
|
||
CheckParasValue();
|
||
SaveD32McPara();
|
||
break;
|
||
}
|
||
case 200: // 校正电机位置
|
||
{
|
||
|
||
break;
|
||
}
|
||
case 1000: // 配置参数
|
||
{
|
||
|
||
break;
|
||
}
|
||
|
||
case 10:
|
||
{// 输出
|
||
if (rCmd.wrdat[0] == 1)
|
||
{
|
||
SetOutput1On();
|
||
}
|
||
else// if (rCmd.wrdat[0] == 2)
|
||
{
|
||
SetOutput1Off();
|
||
}
|
||
|
||
if (rCmd.wrdat[0] == 1)
|
||
{
|
||
SetOutput1On();
|
||
}
|
||
else// if (rCmd.wrdat[0] == 2)
|
||
{
|
||
SetOutput1Off();
|
||
}
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
else if (rCmd.cmd == MODBUS_10WR)
|
||
{
|
||
g_headCtrl.pfootFlowPAng = rCmd.wrdat[0];
|
||
g_headCtrl.pfootFlowNAng = rCmd.wrdat[1];
|
||
g_headCtrl.pfootFlowPuls = rCmd.wrdat[2];
|
||
g_headCtrl.pfootRunLowPos = rCmd.wrdat[3];
|
||
g_headCtrl.pfootRunConAng = rCmd.wrdat[4];
|
||
g_headCtrl.pfootRunBackPos = rCmd.wrdat[5];
|
||
CheckParasValue();
|
||
//g_headCtrl.setParasFlag = 1;
|
||
//SaveD32McPara();
|
||
}
|
||
}
|
||
#elif (CONTROL_BUS == COMM_CAN)
|
||
//-------------------------------------------------------------------------------
|
||
// CAN 命令处理
|
||
void CanCmdProc(void)
|
||
{
|
||
u32 exid;
|
||
int dev, tid, sid, cmd;
|
||
|
||
if (g_workCtrl.curBdCtrl.cmdFlag != 0)
|
||
{
|
||
g_workCtrl.curBdCtrl.cmdFlag = 0;
|
||
exid = g_workCtrl.curBdCtrl.ctrlCmd.cancomm.exId.id;
|
||
|
||
dev = GET_PRT_FROM_EXID(exid);
|
||
tid = GET_TID_FROM_EXID(exid);
|
||
sid = GET_SID_FROM_EXID(exid);
|
||
cmd = GET_CMD_FROM_EXID(exid);
|
||
|
||
if (cmd == BN_CMD_TRAVERSE) // 遍历命令
|
||
{
|
||
if (dev != BN_PROTOCOL_ALL || tid != BN_ID_BROADCAST)
|
||
{
|
||
return;
|
||
}
|
||
g_workCtrl.hostId = sid;
|
||
|
||
// ......功能待开发
|
||
}
|
||
else if (cmd == BN_CMD_EXCTRL) // 控制命令
|
||
{
|
||
SewHeadCommand * pCmd = (SewHeadCommand *)(&(g_workCtrl.curBdCtrl.ctrlCmd.cancomm.data.buff));
|
||
// printf("exctrl cmd=0x%x\r\n", pCmd->headCtrl.cmd);
|
||
switch(pCmd->headCtrl.cmd)
|
||
{
|
||
#if (BOARD_USE_FOR == SEW_HEAD)
|
||
case CTRL_CMD_SEW_HEAD_CTRL:
|
||
#endif
|
||
{
|
||
switch(pCmd->headCtrl.subcmd)
|
||
{
|
||
case CTRL_SUBCMD_SEW_DISABLE : // 机头控制功能关闭
|
||
{
|
||
printf("ctrl func close\r\n");
|
||
g_headCtrl.softLockSta = 1;
|
||
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_ENABLE : // 机头控制功能打开
|
||
{
|
||
printf("ctrl func en\r\n");
|
||
g_headCtrl.softLockSta = 0;
|
||
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_WORK_STA: // 工作状态刷新
|
||
{
|
||
u8 pwmval;
|
||
pwmval = pCmd->headCtrl.para[0];
|
||
#if (PWM_TIMER_CTRL == 1)
|
||
if(pwmval > 0 && pwmval <= 100)
|
||
{
|
||
if(g_headCtrl.pwmCtrl != pwmval)
|
||
{
|
||
if(pwmval <= 10 && g_headCtrl.workingSta == 1)
|
||
{
|
||
printf("little clip level in working.lv:%d\r\n",pwmval);
|
||
return;
|
||
}
|
||
|
||
g_headCtrl.pwmCount = 0;
|
||
g_headCtrl.pwmCtrl = pwmval;
|
||
|
||
// if (g_headCtrl.cmprbuf != pwmval)
|
||
// {// 打印是否丢包
|
||
// g_headCtrl.chgTime++;
|
||
// g_headCtrl.cmprbuf = pwmval;
|
||
// printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval);
|
||
//
|
||
// // 将变化次数加入到缓冲区中
|
||
// g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
|
||
// g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
|
||
// g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
|
||
// g_headCtrl.pwmTab[g_headCtrl.tabIdx] = pwmval;
|
||
// g_headCtrl.tabIdx++;
|
||
// }
|
||
}
|
||
}
|
||
#endif
|
||
u8 sta = pCmd->headCtrl.para[1];
|
||
OutputCtrl(0,sta);
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_OUTPUT_CTRL: // IO输出控制
|
||
{
|
||
u8 ionum,sta;
|
||
ionum = pCmd->headCtrl.para[0];
|
||
//sta = rCmd.wrdat[1];
|
||
sta = pCmd->headCtrl.para[1];
|
||
OutputCtrl(ionum,sta);
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_ELCLIP_CTRL: // 电子夹线控制
|
||
{
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
if(pCmd->headCtrl.para[0] > 0 && pCmd->headCtrl.para[0] <= g_headCtrl.pwmFreq)
|
||
{
|
||
g_headCtrl.pwmCtrl = pCmd->headCtrl.para[0];
|
||
g_headCtrl.pwmCount = 0;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.pwmCtrl = 0;
|
||
SetOutputPWM1Off();
|
||
SetOutputPWM2Off();
|
||
}
|
||
#endif
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_IN_WORK: // 进入工作状态
|
||
{
|
||
if(pCmd->headCtrl.para[0] == 0)
|
||
{
|
||
g_headCtrl.workingSta = 0;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.workingSta = 1;
|
||
}
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_PFOOT_CTRL: // 压脚电机控制
|
||
{
|
||
u8 pfoot_cmd= pCmd->headCtrl.para[0];
|
||
if(pfoot_cmd == CTRL_PFOOT_CMD_TO_ZERO)
|
||
{
|
||
FootMotoToZero();
|
||
}
|
||
else if(pfoot_cmd == CTRL_PFOOT_CMD_DIS)
|
||
{
|
||
FootMotoDis();
|
||
}
|
||
else if(pfoot_cmd == CTRL_PFOOT_CMD_EN)
|
||
{
|
||
FootMotoEn();
|
||
}
|
||
else if(pfoot_cmd == CTRL_PFOOT_CMD_RESET)
|
||
{
|
||
FootMotoReset();
|
||
}
|
||
else if (pfoot_cmd == CTRL_PFOOT_CMD_UP)
|
||
{
|
||
PFootCtrlUp();
|
||
g_headCtrl.followEcdEn = 0;
|
||
}
|
||
else if (pfoot_cmd == CTRL_PFOOT_CMD_DOWN)
|
||
{
|
||
PFootCtrlDown();
|
||
g_headCtrl.followEcdEn = 0;
|
||
}
|
||
else if (pfoot_cmd == CTRL_PFOOT_CMD_BACK)
|
||
{
|
||
PFootCtrlBack();
|
||
g_headCtrl.followEcdEn = 0;
|
||
}
|
||
else
|
||
{
|
||
FootMotoCmdRst();
|
||
}
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_PFOOT_FOLLOW_CTRL: // 压脚电机跟随
|
||
{
|
||
if (pCmd->headCtrl.para[0] == 1)
|
||
{
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[0];
|
||
g_headCtrl.followEcdCheck = 1;
|
||
g_headCtrl.followEcdDir = 1;
|
||
// g_headCtrl.footMtPos = GetSTM32Moto1RealPos();
|
||
|
||
if (pCmd->headCtrl.para[1] != 0)
|
||
{
|
||
PFootCtrlDown();
|
||
}
|
||
|
||
g_headCtrl.followEcdEn = 0;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.followEcdCheck = 0;
|
||
//g_headCtrl.pfootSta = 0;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case CTRL_SUBCMD_SEW_PFOOT_SET_POS: // 设定压脚工作下位
|
||
{
|
||
if(pCmd->headCtrl.para[0] == 0)
|
||
{
|
||
g_headCtrl.followChangeNum = g_headCtrl.followRunNum;
|
||
//g_headCtrl.followChangeFlag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - pCmd->headCtrl.para[0];
|
||
g_headCtrl.pfootFlLowPos = pCmd->headCtrl.para[0];
|
||
if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
|
||
{
|
||
g_headCtrl.followChangeFlag = 1;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
case CTRL_SUBCMD_SEW_PFOOT_SET_ECD: // 校正编码器位置
|
||
{
|
||
int lpos;
|
||
lpos = pCmd->headCtrl.para[0]*1000/36000;
|
||
g_headCtrl.ecOfstValue = g_headCtrl.encoderValue - lpos;
|
||
CheckParasValue();
|
||
break;
|
||
}
|
||
default:break;
|
||
}
|
||
break;
|
||
}
|
||
case CTRL_CMD_SEW_HEAD_PARA: // 缝纫机头板参数设置
|
||
{
|
||
// g_pHeadPara->pfootFlowPAng = pCmd->headPara.para[0];
|
||
// g_pHeadPara->pfootFlowNAng = pCmd->headPara.para[1];
|
||
// g_pHeadPara->pfootFlowPuls = pCmd->headPara.para[2];
|
||
// g_pHeadPara->pfootRunLowPos = pCmd->headPara.para[3];
|
||
// g_pHeadPara->pfootRunConAng = pCmd->headPara.para[4];
|
||
// g_pHeadPara->pfootRunBackPos = pCmd->headPara.para[5];
|
||
g_HeadPara.buf[pCmd->headPara.pataId] = pCmd->headPara.paraVal;
|
||
if (pCmd->headPara.cmd != 0)
|
||
{
|
||
CheckParasValue();
|
||
}
|
||
}
|
||
default:break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
#if (0)
|
||
int MotoNormalStop(u32 para1, u32 para2)
|
||
{
|
||
int rslt = 0;
|
||
|
||
|
||
return rslt;
|
||
}
|
||
|
||
int MotoQuickStop(u32 para1, u32 para2)
|
||
{
|
||
int rslt = 0;
|
||
|
||
|
||
return rslt;
|
||
}
|
||
|
||
int MotoMoveAsOfst(int mode, int ofst, spdCtrl *spd, int block)
|
||
{
|
||
int rslt;
|
||
MotoCtrl mtcfg;
|
||
rslt = 0;
|
||
|
||
memset(&mtcfg, 0, sizeof(MotoCtrl));
|
||
|
||
if(mode == 1)
|
||
{
|
||
ofst = ofst - GetSTM32Moto1RealPos();
|
||
}
|
||
// 速度
|
||
if(spd == NULL)
|
||
{
|
||
mtcfg.startPPS = 100;
|
||
mtcfg.runPPS = 200;
|
||
mtcfg.addPPSG = 10;
|
||
mtcfg.brkPPSG = 50;
|
||
}
|
||
else
|
||
{
|
||
mtcfg.startPPS = spd->stspd;
|
||
mtcfg.runPPS = spd->runspd;
|
||
mtcfg.addPPSG = spd->add;
|
||
mtcfg.brkPPSG = spd->brkadd;
|
||
}
|
||
|
||
mtcfg.movement = ofst;// 位移量
|
||
mtcfg.GetNormalStop = MotoNormalStop;
|
||
mtcfg.GetQuickStop = MotoQuickStop;
|
||
mtcfg.condPara1 = 0;
|
||
mtcfg.condPara2 = 0;
|
||
mtcfg.blockRunflag = block;
|
||
mtcfg.funTestTime = 1;
|
||
mtcfg.ExecWhenRun = NULL;
|
||
mtcfg.ExecWhenStart = NULL;
|
||
mtcfg.ExecWhenStop = NULL;
|
||
|
||
STM32Moto1Stop();
|
||
|
||
DelayRef(50);
|
||
|
||
STM32Moto1Start(&mtcfg);
|
||
|
||
return rslt;
|
||
}
|
||
#endif
|
||
// 得到主轴角度，单位0.01度
|
||
int GetAngleFromEncoder(void)
|
||
{
|
||
int ang = g_headCtrl.encoderValue * 36000 / ENCODER_PNUM + g_headCtrl.stopAngle;
|
||
|
||
// 转化为 0 -- 35999
|
||
while (ang >= 36000)
|
||
{
|
||
ang -= 36000;
|
||
}
|
||
|
||
while (ang <= -36000)
|
||
{
|
||
ang += 36000;
|
||
}
|
||
return ang;
|
||
}
|
||
|
||
|
||
#if(CONTROL_BUS == COMM_485)
|
||
// 输入输出工作任务
|
||
void InOutWorkTask(Rs485Cmd * pCmd)
|
||
{
|
||
Rs485Cmd rCmd;
|
||
int sendstacmd = 0;
|
||
|
||
if (pCmd == NULL)
|
||
{
|
||
return;
|
||
}
|
||
|
||
memcpy(&rCmd, pCmd, sizeof(Rs485Cmd));
|
||
|
||
rCmd.index &= 0x7f;
|
||
if (rCmd.index != D29_BOARD_ID && rCmd.index != D29_ALL_BOARD_ID)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (rCmd.cmd == MODBUS_WR)
|
||
{
|
||
if(rCmd.addr != 0)
|
||
{
|
||
printf("get modbus cmd:%d,data:%d.\r\n",rCmd.addr,rCmd.wrdat[0]);
|
||
}
|
||
|
||
if(rCmd.addr == 0) // 多IO控制
|
||
{
|
||
u8 ionum,sta;
|
||
ionum = 0;
|
||
|
||
#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
|
||
if (D29_BOARD_ID == 1)
|
||
{// 同步刷新PWM占空比
|
||
u8 pwmval;
|
||
pwmval = (rCmd.wrdat[0] & 0xFF00) >> 8;
|
||
if ((pwmval & 0x80) == 0x80) // 收到PWM占空比标志
|
||
{
|
||
pwmval = pwmval & 0x7F;
|
||
#if (PWM_TIMER_CTRL == 1)
|
||
if(pwmval > 0 && pwmval <= 100)
|
||
{
|
||
if(g_headCtrl.pwmCtrl != pwmval)
|
||
{
|
||
if(pwmval <= 10 && g_headCtrl.workingSta == 1)
|
||
{
|
||
printf("little clip level in working.lv:%d\r\n",pwmval);
|
||
return;
|
||
}
|
||
|
||
//g_headCtrl.pwmCtrl = g_headCtrl.pwmFreq;
|
||
g_headCtrl.pwmCount = 0;
|
||
// TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE); /* 开启中断 */
|
||
// TIM_Cmd(TIM1, ENABLE);
|
||
g_headCtrl.pwmCtrl = pwmval;
|
||
|
||
if (g_headCtrl.cmprbuf != pwmval)
|
||
{// 打印是否丢包
|
||
g_headCtrl.chgTime++;
|
||
g_headCtrl.cmprbuf = pwmval;
|
||
printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval);
|
||
|
||
// 将变化次数加入到缓冲区中
|
||
g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
|
||
g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
|
||
g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
|
||
g_headCtrl.pwmTab[g_headCtrl.tabIdx] = pwmval;
|
||
g_headCtrl.tabIdx++;
|
||
}
|
||
}
|
||
}
|
||
//printf("P:%d\tF:%d\t",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
||
#else
|
||
#if (0)
|
||
if (pwmval >= 0 && pwmval <= 100)
|
||
{
|
||
PWMoutputTask(pwmval,1);
|
||
}
|
||
#else
|
||
if ((pwmval >= 0 && pwmval <= 100) &&
|
||
//(g_headCtrl.pwmCtrl != pwmval) &&
|
||
1 )
|
||
{
|
||
u32 cmpr;
|
||
if(g_headCtrl.pwmCtrl != pwmval)
|
||
{
|
||
if(pwmval <= 10 && g_headCtrl.workingSta == 1)
|
||
{
|
||
printf("little clip level in working.lv:%d\r\n",pwmval);
|
||
return;
|
||
}
|
||
cmpr = g_headCtrl.pwmFreq * pwmval / 100;
|
||
TIM_SetCompare1(TIM3,cmpr);
|
||
TIM_SetCompare2(TIM3,cmpr);
|
||
g_headCtrl.pwmCtrl = pwmval;
|
||
}
|
||
|
||
if (g_headCtrl.cmprbuf != cmpr)
|
||
{// 打印是否丢包
|
||
g_headCtrl.chgTime++;
|
||
g_headCtrl.cmprbuf = cmpr;
|
||
printf("[%d][%d]\tpwm:%d\tcmpr:%d (0)\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval,cmpr);
|
||
|
||
// 将变化次数加入到缓冲区中
|
||
g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
|
||
g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
|
||
g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
|
||
g_headCtrl.pwmTab[g_headCtrl.tabIdx] = pwmval;
|
||
g_headCtrl.tabIdx++;
|
||
}
|
||
//printf("P:%d\tF:%d\t",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
||
}
|
||
#endif
|
||
#endif
|
||
}
|
||
}
|
||
#endif
|
||
sta = (rCmd.wrdat[0]>>(8*(D29_BOARD_ID-1))) & 0x00FF;
|
||
OutputCtrl(ionum,sta);
|
||
}
|
||
else if(rCmd.addr == 1) // IO控制ON
|
||
{
|
||
u8 ionum,sta;
|
||
ionum = rCmd.wrdat[0];
|
||
//sta = rCmd.wrdat[1];
|
||
sta = 1;
|
||
OutputCtrl(ionum,sta);
|
||
}
|
||
else if(rCmd.addr == 2) // IO控制OFF
|
||
{
|
||
u8 ionum,sta;
|
||
ionum = rCmd.wrdat[0];
|
||
//sta = rCmd.wrdat[1];
|
||
sta = 0;
|
||
OutputCtrl(ionum,sta);
|
||
}
|
||
#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
|
||
else if(rCmd.addr == 3 || rCmd.addr == 30) //PWM控制
|
||
{
|
||
#if(PWM_TIMER_CTRL == 1)
|
||
if(rCmd.wrdat[0] > 0 && rCmd.wrdat[0] <= g_headCtrl.pwmFreq)
|
||
{
|
||
g_headCtrl.pwmCtrl = rCmd.wrdat[0];
|
||
g_headCtrl.pwmCount = 0;
|
||
//TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE); /* 开启中断 */
|
||
//TIM_Cmd(TIM1, ENABLE);
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.pwmCtrl = 0;
|
||
//TIM_ITConfig(TIM1, TIM_IT_Update, DISABLE); /* 开启中断 */
|
||
//TIM_Cmd(TIM1, DISABLE); // 关闭定时器
|
||
SetOutput6Off();
|
||
SetOutput7Off();
|
||
//SetOutput8Off();
|
||
}
|
||
#else
|
||
if (rCmd.wrdat[0] >= 0 && rCmd.wrdat[0] <= 100)
|
||
{
|
||
#if (0)
|
||
PWMoutputTask(pwmval,1);
|
||
#else
|
||
u32 cmpr;
|
||
u16 pwmval;
|
||
pwmval = rCmd.wrdat[0];
|
||
if(g_headCtrl.pwmCtrl != pwmval)
|
||
{
|
||
if(pwmval <= 10 && g_headCtrl.workingSta == 1)
|
||
{
|
||
printf("little clip level in working.lv:%d\r\n",pwmval);
|
||
return;
|
||
}
|
||
cmpr = g_headCtrl.pwmFreq * pwmval / 100;
|
||
if(rCmd.addr == 3)
|
||
{
|
||
TIM_SetCompare1(TIM3,cmpr);
|
||
}
|
||
TIM_SetCompare2(TIM3,cmpr);
|
||
g_headCtrl.pwmCtrl = pwmval;
|
||
}
|
||
|
||
if (g_headCtrl.cmprbuf != cmpr)
|
||
{// 打印是否丢包
|
||
g_headCtrl.chgTime++;
|
||
g_headCtrl.cmprbuf = cmpr;
|
||
printf("[%d][%d]\tpwm:%d\tcmpr:%d (3)\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),rCmd.wrdat[0],cmpr);
|
||
|
||
// 将变化次数加入到缓冲区中
|
||
g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
|
||
g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
|
||
g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
|
||
g_headCtrl.pwmTab[g_headCtrl.tabIdx] = rCmd.wrdat[0];
|
||
g_headCtrl.tabIdx++;
|
||
}
|
||
|
||
g_headCtrl.pwmCtrl = rCmd.wrdat[0];
|
||
|
||
//printf("pwm:%d,Freq:%dHz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
||
//printf("占空比：%d,频率：%d Hz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
||
#endif
|
||
}
|
||
else
|
||
{// 通讯异常,不响应
|
||
|
||
}
|
||
#endif
|
||
}
|
||
#endif
|
||
else if(rCmd.addr == 4) // 计时器复位
|
||
{
|
||
printf("startTime reset\r\n");
|
||
|
||
g_headCtrl.chgTime = 0;
|
||
g_headCtrl.startTime = GetMsSoftTimer10();
|
||
}
|
||
else if(rCmd.addr == 5) // 进入工作状态
|
||
{
|
||
if(rCmd.wrdat[0] == 0)
|
||
{
|
||
g_headCtrl.workingSta = 0;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.workingSta = 1;
|
||
}
|
||
}
|
||
else if(rCmd.addr == 100) // 更改IO配置
|
||
{
|
||
if(rCmd.wrdat[0] == 1)
|
||
{
|
||
TIM_Cmd(TIM3, DISABLE); // 关闭定时器
|
||
GPIO_InitTypeDef GPIO_InitStructure;
|
||
OUT_OD_CONFIG(OUT6);
|
||
OUT_OD_CONFIG(OUT7);
|
||
g_headCtrl.pwmOutputFlag = 0;
|
||
}
|
||
else
|
||
{
|
||
InitSTM32PwmTimer();
|
||
g_headCtrl.pwmOutputFlag = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// RefreshOutputSta();
|
||
// sendstacmd = 1;
|
||
}
|
||
}
|
||
|
||
if(sendstacmd == 1)
|
||
{
|
||
|
||
}
|
||
}
|
||
#endif
|
||
// 刷新输出状态
|
||
void RefreshOutputSta(void)
|
||
{
|
||
if(GetOutput1Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x01;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x01;
|
||
}
|
||
|
||
if(GetOutput2Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x02;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x02;
|
||
}
|
||
|
||
if(GetOutput3Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x04;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x04;
|
||
}
|
||
|
||
if(GetOutput4Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x08;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x08;
|
||
}
|
||
|
||
if(GetOutput5Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x10;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x10;
|
||
}
|
||
|
||
if(GetOutput6Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x20;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x20;
|
||
}
|
||
|
||
if(GetOutput7Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x40;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x40;
|
||
}
|
||
|
||
if(GetOutput8Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.outputSta |= 0x80;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.outputSta &= ~0x80;
|
||
}
|
||
}
|
||
|
||
// 刷新输入输出状态
|
||
void InOutStaRefreshTask(void)
|
||
{
|
||
if(GetInput1Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x01;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x01;
|
||
}
|
||
|
||
if(GetInput2Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x02;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x02;
|
||
}
|
||
|
||
if(GetInput3Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x04;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x04;
|
||
}
|
||
|
||
if(GetInput4Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x08;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x08;
|
||
}
|
||
|
||
if(GetInput5Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x10;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x10;
|
||
}
|
||
|
||
if(GetInput6Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x20;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x20;
|
||
}
|
||
|
||
if(GetInput7Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x40;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x40;
|
||
}
|
||
|
||
if(GetInput8Status() == SENSOR_ON)
|
||
{
|
||
g_headCtrl.inputSta |= 0x80;
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.inputSta &= ~0x80;
|
||
}
|
||
g_headCtrl.headCmdReg[0] = g_headCtrl.inputSta;
|
||
}
|
||
|
||
// 输出控制
|
||
void OutputCtrl(u8 num, u8 onof)
|
||
{
|
||
if(num == 0)
|
||
{
|
||
if((onof & 0x01) != 0)
|
||
{
|
||
SetOutput1On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput1Off();
|
||
}
|
||
|
||
if((onof & 0x02) != 0)
|
||
{
|
||
SetOutput2On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput2Off();
|
||
}
|
||
|
||
if((onof & 0x04) != 0)
|
||
{
|
||
SetOutput3On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput3Off();
|
||
}
|
||
|
||
if((onof & 0x08) != 0)
|
||
{
|
||
SetOutput4On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput4Off();
|
||
}
|
||
|
||
if((onof & 0x10) != 0)
|
||
{
|
||
SetOutput5On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput5Off();
|
||
}
|
||
|
||
//#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
|
||
//#else
|
||
if(g_headCtrl.pwmOutputFlag == 0)
|
||
{
|
||
if((onof & 0x20) != 0)
|
||
{
|
||
SetOutput6On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput6Off();
|
||
}
|
||
|
||
if((onof & 0x40) != 0)
|
||
{
|
||
SetOutput7On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput7Off();
|
||
}
|
||
}
|
||
//#endif
|
||
if((onof & 0x80) != 0)
|
||
{
|
||
SetOutput8On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput8Off();
|
||
}
|
||
}
|
||
else if(num == 1)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput1On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput1Off();
|
||
}
|
||
}
|
||
else if(num == 2)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput2On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput2Off();
|
||
}
|
||
}
|
||
else if(num == 3)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput3On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput3Off();
|
||
}
|
||
}
|
||
else if(num == 4)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput4On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput4Off();
|
||
}
|
||
}
|
||
else if(num == 5)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput5On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput5Off();
|
||
}
|
||
}
|
||
//#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
|
||
//#else
|
||
else if(num == 6)
|
||
{
|
||
if(g_headCtrl.pwmOutputFlag == 0)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput6On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput6Off();
|
||
}
|
||
}
|
||
}
|
||
else if(num == 7)
|
||
{
|
||
if(g_headCtrl.pwmOutputFlag == 0)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput7On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput7Off();
|
||
}
|
||
}
|
||
}
|
||
//#endif
|
||
else if(num == 8)
|
||
{
|
||
if(onof == 1)
|
||
{
|
||
SetOutput8On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput8Off();
|
||
}
|
||
}
|
||
//printf("N:%d\tO:%d\r\n",num,onof);
|
||
}
|
||
|
||
void TestOutputTask(void)
|
||
{
|
||
static u32 thistime = 0;
|
||
static int outsta = 0;
|
||
|
||
if(GetMsSoftTimer10() - thistime >= 500)
|
||
{
|
||
if(outsta != 1)
|
||
{
|
||
outsta = 1;
|
||
}
|
||
else
|
||
{
|
||
outsta = 0;
|
||
}
|
||
thistime = GetMsSoftTimer10();
|
||
}
|
||
|
||
if(outsta == 1)
|
||
{
|
||
SetOutput1On();
|
||
SetOutput2On();
|
||
SetOutput3On();
|
||
SetOutput4On();
|
||
SetOutput5On();
|
||
SetOutput6On();
|
||
SetOutput7On();
|
||
SetOutput8On();
|
||
}
|
||
else
|
||
{
|
||
SetOutput1Off();
|
||
SetOutput2Off();
|
||
SetOutput3Off();
|
||
SetOutput4Off();
|
||
SetOutput5Off();
|
||
SetOutput6Off();
|
||
SetOutput7Off();
|
||
SetOutput8Off();
|
||
|
||
}
|
||
}
|
||
|
||
//-------------------------------------------------------------------------------
|
||
// AP中断
|
||
void APIntProc(void)
|
||
{
|
||
if (g_headCtrl.enflag != EN_FALG)
|
||
{
|
||
return;
|
||
}
|
||
#if (TRIGGER_TYPE == TRG_ECD_AP || TRIGGER_TYPE == TRG_ECD_AB2)
|
||
if (GetInputAPStatus() == SENSOR_ON) // 下降沿触发
|
||
{
|
||
if (GetInputBPStatus() == SENSOR_OFF)
|
||
{
|
||
g_headCtrl.encoderValue++;
|
||
if(g_headCtrl.followEcdCheck == 1)
|
||
{
|
||
if(g_headCtrl.encoderValue == g_headCtrl.followStartPPos) // 上升
|
||
{
|
||
if(g_headCtrl.pfootSta == 2 || g_headCtrl.pfootSta == 3)
|
||
{
|
||
g_headCtrl.pfootSta = 3;
|
||
g_headCtrl.followEcdDir = 1;
|
||
g_headCtrl.followEcdEn = 1;
|
||
g_headCtrl.followTableIdx = 0;
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[0];
|
||
g_headCtrl.followNumHigh = 0;
|
||
g_headCtrl.followNumLow = 0;
|
||
}
|
||
}
|
||
else if(g_headCtrl.encoderValue == g_headCtrl.followStartNPos) // 下降
|
||
{
|
||
if(g_headCtrl.pfootSta == 1 || g_headCtrl.pfootSta == 3)
|
||
{
|
||
g_headCtrl.pfootSta = 3;
|
||
g_headCtrl.followEcdDir = -1;
|
||
g_headCtrl.followEcdEn = 1;
|
||
g_headCtrl.followTableIdx = 0;
|
||
g_headCtrl.followStepNum = FOLLOW_TABLE[0];
|
||
g_headCtrl.followNumHigh = 0;
|
||
g_headCtrl.followNumLow = 0;
|
||
if(g_headCtrl.followChangeFlag == 1)
|
||
{
|
||
g_headCtrl.followRunNum = g_headCtrl.followChangeNum;
|
||
g_headCtrl.followChangeFlag = 0;
|
||
GetFollowTableValue(g_headCtrl.followConPAng,g_headCtrl.followRunNum);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
g_headCtrl.FollowMotoSignOn();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.encoderValue--;
|
||
g_headCtrl.FollowMotoSignOff();
|
||
}
|
||
g_headCtrl.FollowMotoPulseOn();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.FollowMotoPulseOff();
|
||
}
|
||
#elif (TRIGGER_TYPE == TRG_ECD_AB4)
|
||
if (GetInputAPStatus() == GetInputBPStatus())
|
||
{
|
||
g_headCtrl.encoderValue++;
|
||
g_headCtrl.FollowMotoSignOn();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.encoderValue--;
|
||
g_headCtrl.FollowMotoSignOff();
|
||
}
|
||
g_headCtrl.FollowMotoPulseOn();
|
||
#endif
|
||
// printf("AP, evalue=%d\r\n", g_headCtrl.encoderValue);
|
||
}
|
||
|
||
void BPIntProc(void)
|
||
{
|
||
if (g_headCtrl.enflag != EN_FALG)
|
||
{
|
||
return;
|
||
}
|
||
#if (TRIGGER_TYPE == TRG_ECD_BP || TRIGGER_TYPE == TRG_ECD_AB2)
|
||
if (GetInputBPStatus() == SENSOR_ON) // 下降沿触发
|
||
{
|
||
if (GetInputAPStatus() == SENSOR_OFF)
|
||
{
|
||
g_headCtrl.encoderValue--;
|
||
g_headCtrl.FollowMotoSignOff();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.encoderValue++;
|
||
g_headCtrl.FollowMotoSignOn();
|
||
}
|
||
g_headCtrl.FollowMotoPulseOn();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.FollowMotoPulseOff();
|
||
}
|
||
#elif (TRIGGER_TYPE == TRG_ECD_AB4)
|
||
|
||
if (GetInputAPStatus() == GetInputBPStatus())
|
||
{
|
||
g_headCtrl.encoderValue--;
|
||
g_headCtrl.FollowMotoSignOff();
|
||
}
|
||
else
|
||
{
|
||
g_headCtrl.encoderValue++;
|
||
g_headCtrl.FollowMotoSignOn();
|
||
}
|
||
g_headCtrl.FollowMotoPulseOff();
|
||
|
||
#endif
|
||
|
||
// printf("BP\r\n");
|
||
}
|
||
|
||
// ZP中断
|
||
void ZPIntProc(void)
|
||
{
|
||
if (g_headCtrl.enflag != EN_FALG)
|
||
{
|
||
return;
|
||
}
|
||
// printf("zp, evalue=%d\r\n", g_headCtrl.encoderValue);
|
||
g_headCtrl.encoderValue = 0;
|
||
}
|
||
|
||
|
||
void TestAction(char * para1, char * para2)
|
||
{
|
||
int p1, p2;
|
||
|
||
if (para1 == NULL || para2 == NULL)
|
||
{
|
||
return;
|
||
}
|
||
|
||
printf("para1=%s, para2=%s\r\n", para1, para2);
|
||
|
||
if (strcmp(para1, "") == 0 || strcmp(para2, "") == 0)
|
||
{
|
||
return;
|
||
}
|
||
p1 = atoi(para1);
|
||
p2 = atoi(para2);
|
||
|
||
if (p1 == p2)
|
||
{
|
||
}
|
||
//--------------------------------
|
||
|
||
//--------------------------------
|
||
#if(PWM_TIMER_CTRL == 0)
|
||
if (p1 == 0)
|
||
{ // 打印历史数据
|
||
int i,j;
|
||
j = g_headCtrl.tabIdx;
|
||
|
||
for (i = 0; i<BUF_TAB_SIZE;i++)
|
||
{
|
||
if ((i % 5) == 0)
|
||
{
|
||
DelayRef(10);
|
||
}
|
||
|
||
j = j % BUF_TAB_SIZE;
|
||
printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTimeTab[j],g_headCtrl.curTimeTab[j],g_headCtrl.pwmTab[j]);
|
||
j++;
|
||
}
|
||
}
|
||
else if (p1 == 1) // 设置占空比
|
||
{
|
||
int cmpr;
|
||
if(p2 < 0 || p2 > 100)
|
||
{
|
||
return;
|
||
}
|
||
|
||
cmpr = g_headCtrl.pwmFreq * p2 / 100;
|
||
g_headCtrl.pwmCtrl = p2;
|
||
TIM_SetCompare1(TIM3,cmpr);
|
||
TIM_SetCompare2(TIM3,cmpr);
|
||
|
||
TIM_Cmd(TIM3, ENABLE);
|
||
printf("占空比：%d,频率：%d Hz\r\n",p2,1000000/g_headCtrl.pwmFreq);
|
||
}
|
||
else if (p1 == 2)
|
||
{
|
||
int freq,cmpr;
|
||
TIM_Cmd(TIM3, DISABLE);
|
||
DelayRef(100);
|
||
freq = (1000000/p2)-1;
|
||
g_headCtrl.pwmFreq = freq+1;
|
||
cmpr = g_headCtrl.pwmFreq * g_headCtrl.pwmCtrl / 100;
|
||
TIM_SetCompare1(TIM3,cmpr);
|
||
TIM_SetCompare2(TIM3,cmpr);
|
||
TIM_SetAutoreload(TIM3,freq);
|
||
TIM_Cmd(TIM3, ENABLE);
|
||
printf("占空比：%d,频率：%d Hz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
||
}
|
||
else if (p1 == 3) // 关闭
|
||
{
|
||
if(p2 == 1)
|
||
{
|
||
TIM_SetCompare1(TIM3,0);
|
||
}
|
||
else if(p2 == 2)
|
||
{
|
||
TIM_SetCompare2(TIM3,0);
|
||
}
|
||
}
|
||
else if (p1 == 4) // 开
|
||
{
|
||
u32 cmpr;
|
||
cmpr = g_headCtrl.pwmFreq * g_headCtrl.pwmCtrl / 100;
|
||
if(p2 == 1)
|
||
{
|
||
TIM_SetCompare1(TIM3,cmpr);
|
||
}
|
||
else if(p2 == 2)
|
||
{
|
||
TIM_SetCompare2(TIM3,cmpr);
|
||
}
|
||
}
|
||
else if (p1 == 5)
|
||
{
|
||
|
||
}
|
||
else if (p1 == 6)
|
||
{
|
||
|
||
}
|
||
else if (p1 == 10) // 开
|
||
{
|
||
if(p2 == 1)
|
||
{
|
||
TIM_Cmd(TIM3, ENABLE);
|
||
}
|
||
else
|
||
{
|
||
TIM_Cmd(TIM3, DISABLE);
|
||
}
|
||
}
|
||
#else
|
||
else if(p1 == 1)
|
||
{
|
||
g_headCtrl.pwmCtrl = p2;
|
||
LL_TIM_EnableCounter(TIM1);
|
||
printf("占空比：%d \r\n",g_headCtrl.pwmCtrl);
|
||
}
|
||
else if(p1 == 2)
|
||
{
|
||
LL_TIM_EnableIT_UPDATE(TIM1); /* 开启中断 */
|
||
LL_TIM_DisableCounter(TIM1); // 关闭定时器
|
||
printf("关闭定时器 \r\n");
|
||
SetOutput7Off();
|
||
SetOutput6Off();
|
||
}
|
||
else if(p1 == 3)
|
||
{
|
||
g_headCtrl.pwmFreq = p2;
|
||
}
|
||
else if(p1 == 4)
|
||
{
|
||
g_headCtrl.timeCount = 1;
|
||
}
|
||
#endif
|
||
else if(p1 == 11)
|
||
{
|
||
if(p2 == 1)
|
||
{
|
||
SetOutput1On();
|
||
}
|
||
else if(p2 == 2)
|
||
{
|
||
SetOutput2On();
|
||
}
|
||
else if(p2 == 6)
|
||
{
|
||
SetOutput6On();
|
||
}
|
||
else if(p2 == 7)
|
||
{
|
||
SetOutput7On();
|
||
}
|
||
else if(p2 == 8)
|
||
{
|
||
SetOutput8On();
|
||
}
|
||
}
|
||
else if(p1 == 12)
|
||
{
|
||
if(p2 == 1)
|
||
{
|
||
SetOutput1Off();
|
||
}
|
||
else if(p2 == 2)
|
||
{
|
||
SetOutput2Off();
|
||
}
|
||
else if(p2 == 6)
|
||
{
|
||
SetOutput6Off();
|
||
}
|
||
else if(p2 == 7)
|
||
{
|
||
SetOutput7Off();
|
||
}
|
||
else if(p2 == 8)
|
||
{
|
||
SetOutput8Off();
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
|