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 // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><>λ 100us
|
|||
|
|
#define MODRSV_TIMEOUT 820 // <20><>ȡ<EFBFBD><C8A1>ʱʱ<CAB1><CAB1>, <20><>λΪ CHK_TIME_GAP * 100us
|
|||
|
|
#define MODSEND_GAP 50 // <20><><EFBFBD>ͺͶ<CDBA>ȡ<EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>, <20><>λΪ CHK_TIME_GAP * 100us
|
|||
|
|
//-------------------------------------------------------------------------------
|
|||
|
|
|
|||
|
|
#define BUF_TAB_SIZE 1000 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
u8 boardId;
|
|||
|
|
u8 outputSta;
|
|||
|
|
u8 inputSta;
|
|||
|
|
u8 outputCtrl;
|
|||
|
|
u8 pwmOutputFlag;
|
|||
|
|
u8 softLockSta;
|
|||
|
|
// u16 d29cmdReg[MODBUS_REG_MAX-MODBUS_REG_MIN];
|
|||
|
|
|
|||
|
|
u16 pwmCtrl; // <20><><EFBFBD><EFBFBD>ռ<EFBFBD>ձ<EFBFBD> 0~100
|
|||
|
|
u16 pwmFreq; // <20><>ʱ<EFBFBD><CAB1>PWMƵ<4D><C6B5>
|
|||
|
|
u16 pwmCount;
|
|||
|
|
u32 timeCount;
|
|||
|
|
u32 pwmPulsTime;
|
|||
|
|
u32 startTime; // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
|
|||
|
|
u32 chgTime; // <20>仯<EFBFBD><E4BBAF><EFBFBD><EFBFBD>
|
|||
|
|
u32 cmprbuf; // <20><>Ƶϵ<C6B5><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int workingSta;
|
|||
|
|
|
|||
|
|
u32 tabIdx; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
u32 chgTimeTab[BUF_TAB_SIZE]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
u32 curTimeTab[BUF_TAB_SIZE]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
u8 pwmTab[BUF_TAB_SIZE]; // PWM<57><4D><EFBFBD>滺<EFBFBD><E6BBBA><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
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; // ѹ<>Ÿ<EFBFBD><C5B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
|
|||
|
|
u32 pfootFlowNAng; // ѹ<>Ÿ<EFBFBD><C5B8><EFBFBD><EFBFBD>½<EFBFBD>λ
|
|||
|
|
u32 pfootFlowPuls; // ѹ<>Ÿ<EFBFBD><C5B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
u32 pfootRunLowPos; // ѹ<><D1B9><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>λ
|
|||
|
|
u32 pfootRunBackPos; // ѹ<><D1B9><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD>λ
|
|||
|
|
s32 ecOfstValue; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
u32 pfootRunConAng; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
//------------
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><>ʼ<EFBFBD><CABC>
|
|||
|
|
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'); // <20><>ʼ<EFBFBD><CABC>485
|
|||
|
|
#else
|
|||
|
|
if((g_headCtrl.boardId & 0x08) != 0)
|
|||
|
|
{
|
|||
|
|
InitModbusS(COMM_USART2, B115200, '8', 'N', '1','R'); // <20><>ʼ<EFBFBD><CABC>485
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
InitModbusS(COMM_USART2, B115200, '8', 'N', '1','A'); // <20><>ʼ<EFBFBD><CABC>485
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
//InitModbusS(COMM_USART1,B115200, '9', 'E', '1', 'A'); // <20><><EFBFBD><EFBFBD>3,115200<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
InitModbusReg(MODBUS_REG_MIN, &g_headCtrl.d29cmdReg[0], (MODBUS_REG_MAX-MODBUS_REG_MIN));
|
|||
|
|
InitModbusSCtrl(D29_BOARD_ID,30); // <20><><EFBFBD><EFBFBD>modbus(<28>հ<EFBFBD><D5B0><EFBFBD>ʱʱ<CAB1><CAB1>:2ms)
|
|||
|
|
|
|||
|
|
RegModbusSCmdProc(ModBusCmdProcTask); // <20>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
InitModbus_m(0,COMM_USART3, B115200, '8', 'N', '1','R'); // <20><>ʼ<EFBFBD><CABC>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;
|
|||
|
|
/* <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; /* ͨ<><CDA8> */
|
|||
|
|
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; /* <20><>ռʽ<D5BC>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ȼ<EFBFBD> */
|
|||
|
|
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; /* <20><>Ӧʽ<D3A6>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ȼ<EFBFBD> */
|
|||
|
|
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; /* ʹ<><CAB9><EFBFBD>ж<EFBFBD> */
|
|||
|
|
NVIC_Init(&NVIC_InitStructure);
|
|||
|
|
/*IO<49><4F><EFBFBD><EFBFBD>*/
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
///* <20><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ʱ<EFBFBD><CAB1> */
|
|||
|
|
TIM_DeInit(TIM3); /* <20><><EFBFBD>½<EFBFBD>Timer<65><72><EFBFBD><EFBFBD>Ϊȱʡֵ */
|
|||
|
|
///* ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
|||
|
|
TIM_TimeBaseStructure.TIM_Prescaler = (72-1); /* Ԥ<><D4A4>Ƶ */
|
|||
|
|
TIM_TimeBaseStructure.TIM_Period = (3334-1); /* <20>Զ<EFBFBD>װ<EFBFBD><D7B0>ֵ 500us<75>ж<EFBFBD>*/
|
|||
|
|
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; /* <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> */
|
|||
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /* <20><><EFBFBD>ϼ<EFBFBD><CFBC><EFBFBD>ģʽ */
|
|||
|
|
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); /* <20><>ʼ<EFBFBD><CABC>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); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<D0B6>־ */
|
|||
|
|
//TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); /* <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> */
|
|||
|
|
TIM_Cmd(TIM3, ENABLE); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> */
|
|||
|
|
|
|||
|
|
#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; // <20><>ʼռ<CABC>ձ<EFBFBD>Ϊ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Ƶ<6D><C6B5> = %dHz,pwmռ<6D>ձ<EFBFBD> %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++;
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD>
|
|||
|
|
if (g_headCtrl.followTableIdx < FOLLOW_TAB_NUM && g_headCtrl.followNumHigh < FOLLOW_TAB_NUM)
|
|||
|
|
{
|
|||
|
|
g_headCtrl.followTableIdx++; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
g_headCtrl.followStepNum = FOLLOW_TABLE[g_headCtrl.followTableIdx]; // װ<><D7B0><EFBFBD>µ<EFBFBD>ֵ
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD>
|
|||
|
|
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]; // װ<><D7B0><EFBFBD>µ<EFBFBD>ֵ
|
|||
|
|
g_headCtrl.followTableIdx--; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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: // ѹ<><D1B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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: // <20>趨ѹ<E8B6A8>Ź<EFBFBD><C5B9><EFBFBD><EFBFBD><EFBFBD>λ
|
|||
|
|
{
|
|||
|
|
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: // У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
|
{
|
|||
|
|
int lpos;
|
|||
|
|
lpos = rCmd.wrdat[0]*1000/36000;
|
|||
|
|
g_headCtrl.ecOfstValue = g_headCtrl.encoderValue - lpos;
|
|||
|
|
CheckParasValue();
|
|||
|
|
SaveD32McPara();
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case 200: // У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case 1000: // <20><><EFBFBD>ò<EFBFBD><C3B2><EFBFBD>
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
case 10:
|
|||
|
|
{// <20><><EFBFBD><EFBFBD>
|
|||
|
|
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 <20><><EFBFBD><EFBFBD><EEB4A6>
|
|||
|
|
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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
if (dev != BN_PROTOCOL_ALL || tid != BN_ID_BROADCAST)
|
|||
|
|
{
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
g_workCtrl.hostId = sid;
|
|||
|
|
|
|||
|
|
// ......<2E><><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
else if (cmd == BN_CMD_EXCTRL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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 : // <20><>ͷ<EFBFBD><CDB7><EFBFBD>ƹ<EFBFBD><C6B9>ܹر<DCB9>
|
|||
|
|
{
|
|||
|
|
printf("ctrl func close\r\n");
|
|||
|
|
g_headCtrl.softLockSta = 1;
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case CTRL_SUBCMD_SEW_ENABLE : // <20><>ͷ<EFBFBD><CDB7><EFBFBD>ƹ<EFBFBD><C6B9>ܴ<EFBFBD><DCB4><EFBFBD>
|
|||
|
|
{
|
|||
|
|
printf("ctrl func en\r\n");
|
|||
|
|
g_headCtrl.softLockSta = 0;
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case CTRL_SUBCMD_SEW_WORK_STA: // <20><><EFBFBD><EFBFBD>״̬ˢ<CCAC><CBA2>
|
|||
|
|
{
|
|||
|
|
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)
|
|||
|
|
// {// <20><>ӡ<EFBFBD>Ƿ<C7B7>
|
|||
|
|
// g_headCtrl.chgTime++;
|
|||
|
|
// g_headCtrl.cmprbuf = pwmval;
|
|||
|
|
// printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval);
|
|||
|
|
//
|
|||
|
|
// // <20><><EFBFBD>仯<EFBFBD><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵽<EFBFBD><EBB5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// 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<49><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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: // <20><><EFBFBD>Ӽ<EFBFBD><D3BC>߿<EFBFBD><DFBF><EFBFBD>
|
|||
|
|
{
|
|||
|
|
#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: // <20><><EFBFBD>빤<EFBFBD><EBB9A4>״̬
|
|||
|
|
{
|
|||
|
|
if(pCmd->headCtrl.para[0] == 0)
|
|||
|
|
{
|
|||
|
|
g_headCtrl.workingSta = 0;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
g_headCtrl.workingSta = 1;
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case CTRL_SUBCMD_SEW_PFOOT_CTRL: // ѹ<>ŵ<EFBFBD><C5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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: // ѹ<>ŵ<EFBFBD><C5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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: // <20>趨ѹ<E8B6A8>Ź<EFBFBD><C5B9><EFBFBD><EFBFBD><EFBFBD>λ
|
|||
|
|
{
|
|||
|
|
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: // У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
|
{
|
|||
|
|
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: // <20><><EFBFBD>һ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
// 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();
|
|||
|
|
}
|
|||
|
|
// <20>ٶ<EFBFBD>
|
|||
|
|
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;// λ<><CEBB><EFBFBD><EFBFBD>
|
|||
|
|
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
|
|||
|
|
// <20>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȣ<C7B6><C8A3><EFBFBD>λ0.01<EFBFBD><EFBFBD>
|
|||
|
|
int GetAngleFromEncoder(void)
|
|||
|
|
{
|
|||
|
|
int ang = g_headCtrl.encoderValue * 36000 / ENCODER_PNUM + g_headCtrl.stopAngle;
|
|||
|
|
|
|||
|
|
// ת<><D7AA>Ϊ 0 -- 35999
|
|||
|
|
while (ang >= 36000)
|
|||
|
|
{
|
|||
|
|
ang -= 36000;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
while (ang <= -36000)
|
|||
|
|
{
|
|||
|
|
ang += 36000;
|
|||
|
|
}
|
|||
|
|
return ang;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
#if(CONTROL_BUS == COMM_485)
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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) // <20><>IO<49><4F><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
u8 ionum,sta;
|
|||
|
|
ionum = 0;
|
|||
|
|
|
|||
|
|
#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
|
|||
|
|
if (D29_BOARD_ID == 1)
|
|||
|
|
{// ͬ<><CDAC>ˢ<EFBFBD><CBA2>PWMռ<4D>ձ<EFBFBD>
|
|||
|
|
u8 pwmval;
|
|||
|
|
pwmval = (rCmd.wrdat[0] & 0xFF00) >> 8;
|
|||
|
|
if ((pwmval & 0x80) == 0x80) // <20>յ<EFBFBD>PWMռ<4D>ձȱ<D5B1>־
|
|||
|
|
{
|
|||
|
|
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); /* <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> */
|
|||
|
|
// TIM_Cmd(TIM1, ENABLE);
|
|||
|
|
g_headCtrl.pwmCtrl = pwmval;
|
|||
|
|
|
|||
|
|
if (g_headCtrl.cmprbuf != pwmval)
|
|||
|
|
{// <20><>ӡ<EFBFBD>Ƿ<C7B7>
|
|||
|
|
g_headCtrl.chgTime++;
|
|||
|
|
g_headCtrl.cmprbuf = pwmval;
|
|||
|
|
printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD>仯<EFBFBD><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵽<EFBFBD><EBB5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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)
|
|||
|
|
{// <20><>ӡ<EFBFBD>Ƿ<C7B7>
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD>仯<EFBFBD><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵽<EFBFBD><EBB5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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<49><4F><EFBFBD><EFBFBD>ON
|
|||
|
|
{
|
|||
|
|
u8 ionum,sta;
|
|||
|
|
ionum = rCmd.wrdat[0];
|
|||
|
|
//sta = rCmd.wrdat[1];
|
|||
|
|
sta = 1;
|
|||
|
|
OutputCtrl(ionum,sta);
|
|||
|
|
}
|
|||
|
|
else if(rCmd.addr == 2) // IO<49><4F><EFBFBD><EFBFBD>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<57><4D><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
#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); /* <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> */
|
|||
|
|
//TIM_Cmd(TIM1, ENABLE);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
g_headCtrl.pwmCtrl = 0;
|
|||
|
|
//TIM_ITConfig(TIM1, TIM_IT_Update, DISABLE); /* <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> */
|
|||
|
|
//TIM_Cmd(TIM1, DISABLE); // <20>رն<D8B1>ʱ<EFBFBD><CAB1>
|
|||
|
|
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)
|
|||
|
|
{// <20><>ӡ<EFBFBD>Ƿ<C7B7>
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD>仯<EFBFBD><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵽<EFBFBD><EBB5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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("ռ<>ձȣ<D5B1>%d,Ƶ<>ʣ<EFBFBD>%d Hz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{// ͨѶ<CDA8>쳣,<2C><><EFBFBD><EFBFBD>Ӧ
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
else if(rCmd.addr == 4) // <20><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>λ
|
|||
|
|
{
|
|||
|
|
printf("startTime reset\r\n");
|
|||
|
|
|
|||
|
|
g_headCtrl.chgTime = 0;
|
|||
|
|
g_headCtrl.startTime = GetMsSoftTimer10();
|
|||
|
|
}
|
|||
|
|
else if(rCmd.addr == 5) // <20><><EFBFBD>빤<EFBFBD><EBB9A4>״̬
|
|||
|
|
{
|
|||
|
|
if(rCmd.wrdat[0] == 0)
|
|||
|
|
{
|
|||
|
|
g_headCtrl.workingSta = 0;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
g_headCtrl.workingSta = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if(rCmd.addr == 100) // <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
if(rCmd.wrdat[0] == 1)
|
|||
|
|
{
|
|||
|
|
TIM_Cmd(TIM3, DISABLE); // <20>رն<D8B1>ʱ<EFBFBD><CAB1>
|
|||
|
|
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
|
|||
|
|
// ˢ<><CBA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ˢ<><CBA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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<41>ж<EFBFBD>
|
|||
|
|
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) // <20>½<EFBFBD><C2BD>ش<EFBFBD><D8B4><EFBFBD>
|
|||
|
|
{
|
|||
|
|
if (GetInputBPStatus() == SENSOR_OFF)
|
|||
|
|
{
|
|||
|
|
g_headCtrl.encoderValue++;
|
|||
|
|
if(g_headCtrl.followEcdCheck == 1)
|
|||
|
|
{
|
|||
|
|
if(g_headCtrl.encoderValue == g_headCtrl.followStartPPos) // <20><><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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) // <20>½<EFBFBD>
|
|||
|
|
{
|
|||
|
|
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) // <20>½<EFBFBD><C2BD>ش<EFBFBD><D8B4><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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<5A>ж<EFBFBD>
|
|||
|
|
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)
|
|||
|
|
{ // <20><>ӡ<EFBFBD><D3A1>ʷ<EFBFBD><CAB7><EFBFBD><EFBFBD>
|
|||
|
|
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) // <20><><EFBFBD><EFBFBD>ռ<EFBFBD>ձ<EFBFBD>
|
|||
|
|
{
|
|||
|
|
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("ռ<EFBFBD>ձȣ<EFBFBD>%d,Ƶ<>ʣ<EFBFBD>%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("ռ<EFBFBD>ձȣ<EFBFBD>%d,Ƶ<>ʣ<EFBFBD>%d Hz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
|
|||
|
|
}
|
|||
|
|
else if (p1 == 3) // <20>ر<EFBFBD>
|
|||
|
|
{
|
|||
|
|
if(p2 == 1)
|
|||
|
|
{
|
|||
|
|
TIM_SetCompare1(TIM3,0);
|
|||
|
|
}
|
|||
|
|
else if(p2 == 2)
|
|||
|
|
{
|
|||
|
|
TIM_SetCompare2(TIM3,0);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (p1 == 4) // <20><>
|
|||
|
|
{
|
|||
|
|
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) // <20><>
|
|||
|
|
{
|
|||
|
|
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("ռ<EFBFBD>ձȣ<EFBFBD>%d \r\n",g_headCtrl.pwmCtrl);
|
|||
|
|
}
|
|||
|
|
else if(p1 == 2)
|
|||
|
|
{
|
|||
|
|
LL_TIM_EnableIT_UPDATE(TIM1); /* <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> */
|
|||
|
|
LL_TIM_DisableCounter(TIM1); // <20>رն<D8B1>ʱ<EFBFBD><CAB1>
|
|||
|
|
printf("<EFBFBD>رն<EFBFBD>ʱ<EFBFBD><EFBFBD> \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
|
|||
|
|
|
|||
|
|
|
|||
|
|
|