|
@@ -12,6 +12,19 @@ |
|
|
#include "../../system/inc/system.h" |
|
|
#include "../../system/inc/system.h" |
|
|
#include "../../bsp/inc/time.h" |
|
|
#include "../../bsp/inc/time.h" |
|
|
#include "../inc/plsr.h" |
|
|
#include "../inc/plsr.h" |
|
|
|
|
|
#include "../../bsp/inc/gpio.h" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
typedef struct { |
|
|
|
|
|
int32_t inc; |
|
|
|
|
|
int32_t dec; |
|
|
|
|
|
int32_t currentSpeed; |
|
|
|
|
|
uint8_t currentState; ///< 0:无操作 1:匀速 2:加速 3:减速 |
|
|
|
|
|
uint8_t currentPart; |
|
|
|
|
|
int32_t firstPartNum; |
|
|
|
|
|
int32_t secondPartNum; |
|
|
|
|
|
int32_t thirdPartNum; |
|
|
|
|
|
} SEGMENT_PART; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#define PLSR_MODULE_PRIO 5 |
|
|
#define PLSR_MODULE_PRIO 5 |
|
@@ -19,6 +32,14 @@ |
|
|
static OS_STK PlsrModuleStack[PLSR_MODULE_STACK_SIZE]; |
|
|
static OS_STK PlsrModuleStack[PLSR_MODULE_STACK_SIZE]; |
|
|
static void PlsrModuleTask(void * pArg); |
|
|
static void PlsrModuleTask(void * pArg); |
|
|
|
|
|
|
|
|
|
|
|
static void TimCallBack(void *pArg); |
|
|
|
|
|
static void ReadRegisterParam(PLSR *plsr); |
|
|
|
|
|
static void configPulsePort(PLSR *plsr); |
|
|
|
|
|
static void ComputePartNum(PLSR *plsr, SEGMENT_PART *segmentPart); |
|
|
|
|
|
static void FirstSegmentProc(SEGMENT_PART *segmentPart); |
|
|
|
|
|
static void SecondSegmentProc(SEGMENT_PART *segmentPart); |
|
|
|
|
|
static void ThirdSegmentProc(SEGMENT_PART *segmentPart); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
/** |
|
|
* @brief PLSR功能初始化 |
|
|
* @brief PLSR功能初始化 |
|
@@ -56,34 +77,6 @@ RESUIL PLSRInit(void) |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#include "../../bsp/inc/gpio.h" |
|
|
|
|
|
void Callack(void *pArg) |
|
|
|
|
|
{ |
|
|
|
|
|
gpio_obj_t *gpio_f_6; |
|
|
|
|
|
gpio_f_6 = get_gpio_obj("gpio_f"); |
|
|
|
|
|
gpio_f_6->gpio_init(gpio_f_6, gpio_pin_6, "OUT_PP", "UP"); |
|
|
|
|
|
|
|
|
|
|
|
if (*((uint8_t *)pArg) < 13) |
|
|
|
|
|
printf("Callback tim2 count %d\r\n", (*((uint8_t *)pArg))++); |
|
|
|
|
|
else |
|
|
|
|
|
printf("Callback tim13 count %d\r\n", (*((uint8_t *)pArg))++); |
|
|
|
|
|
uint16_t read_gpio_pin = 0; |
|
|
|
|
|
read_gpio_pin = gpio_f_6->gpio_read_output(gpio_f_6); |
|
|
|
|
|
if(gpio_f_6->gpio_pin & read_gpio_pin) |
|
|
|
|
|
{ |
|
|
|
|
|
gpio_f_6->gpio_set_output(gpio_f_6, gpio_f_6->gpio_pin, 0); |
|
|
|
|
|
#ifdef USE_LED_PIN_TRACE |
|
|
|
|
|
pDev->led_state = "on"; |
|
|
|
|
|
#endif |
|
|
|
|
|
} |
|
|
|
|
|
else |
|
|
|
|
|
{ |
|
|
|
|
|
gpio_f_6->gpio_set_output(gpio_f_6, gpio_f_6->gpio_pin, 1); |
|
|
|
|
|
#ifdef USE_LED_PIN_TRACE |
|
|
|
|
|
pDev->led_state = "off"; |
|
|
|
|
|
#endif |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
/** |
|
|
/** |
|
|
* @brief PLSR处理任务 |
|
|
* @brief PLSR处理任务 |
|
|
* @details |
|
|
* @details |
|
@@ -94,63 +87,374 @@ void Callack(void *pArg) |
|
|
* @retval MODBUS_TURE 成功 |
|
|
* @retval MODBUS_TURE 成功 |
|
|
* @retval MODBUS_FALSE 失败 |
|
|
* @retval MODBUS_FALSE 失败 |
|
|
*/ |
|
|
*/ |
|
|
|
|
|
TIM_OBJ tim2; |
|
|
|
|
|
TIM_OBJ tim10; |
|
|
|
|
|
int32_t accSpeedSlope = 0; |
|
|
|
|
|
int32_t decSpeedSlope = 0; |
|
|
|
|
|
int32_t accTime = 0; |
|
|
|
|
|
int32_t decTime = 0; |
|
|
|
|
|
int32_t accPulse = 0; |
|
|
|
|
|
int32_t decPulse = 0; |
|
|
|
|
|
int32_t uniformTime = 0; |
|
|
|
|
|
int32_t uniformPulse = 0; |
|
|
|
|
|
uint8_t currentSegment = 0; |
|
|
|
|
|
uint32_t currentSpeed = 0; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int32_t pulseFreq = 168 - 1; |
|
|
|
|
|
uint32_t pulseReload = 20; |
|
|
|
|
|
|
|
|
|
|
|
SEGMENT_PART currentSeg = { |
|
|
|
|
|
.currentState = 2, |
|
|
|
|
|
.currentPart = 1 |
|
|
|
|
|
}; |
|
|
|
|
|
PLSR plsr = { |
|
|
|
|
|
.segmentAllNum = 1, |
|
|
|
|
|
.segment[0].jumpNumber = 0 |
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
void PlsrModuleTask(void *pArg) |
|
|
void PlsrModuleTask(void *pArg) |
|
|
{ |
|
|
{ |
|
|
TIM_OBJ tim2; |
|
|
|
|
|
TIM_OBJ tim10; |
|
|
|
|
|
int8_t res; |
|
|
int8_t res; |
|
|
uint8_t pArgment = 0; |
|
|
uint8_t pArgment = 0; |
|
|
|
|
|
uint8_t RunFlag = 0; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int32_t pulseFreq = 0; |
|
|
|
|
|
int32_t pulseCount = 0; |
|
|
|
|
|
uint32_t pulseReload = 0; |
|
|
|
|
|
|
|
|
uint8_t nextSegment = plsr.startRunSegment; |
|
|
|
|
|
|
|
|
|
|
|
// ReadRegisterParam(&plsr); |
|
|
|
|
|
|
|
|
res = GetTimObj("tim2", &tim2); |
|
|
res = GetTimObj("tim2", &tim2); |
|
|
res = GetTimObj("tim10", &tim10); |
|
|
res = GetTimObj("tim10", &tim10); |
|
|
if (res) |
|
|
|
|
|
while (1); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
tim10.init(&tim10, 168 - 1, 65535); |
|
|
|
|
|
// tim10.open(&tim10); |
|
|
|
|
|
tim10.registerIrq(&tim10, TimCallBack, ¤tSeg); |
|
|
|
|
|
// tim10.openIrq(&tim10); |
|
|
|
|
|
tim10.pwmInit(&tim10); |
|
|
|
|
|
tim10.pwmSetPulse(&tim10, TIM10->ARR); |
|
|
|
|
|
|
|
|
tim2.init(&tim2, 8399, 9999); |
|
|
|
|
|
tim2.open(&tim2); |
|
|
|
|
|
tim2.registerIrq(&tim2, &Callack, &pArgment); |
|
|
|
|
|
tim2.openIrq(&tim2); |
|
|
|
|
|
|
|
|
plsr.sendEnableState = 0; |
|
|
|
|
|
plsr.accDecSpeedMode = STRAIGHT_LINE_MODE; |
|
|
|
|
|
plsr.segment[0].pulseFreq = 2000; |
|
|
|
|
|
plsr.segment[0].pulseNumber = 3000; |
|
|
|
|
|
plsr.defaultSpeed = 1000; |
|
|
|
|
|
plsr.defaultAccSpeedTime = 10; |
|
|
|
|
|
plsr.defaultDecSpeedTime = 10; |
|
|
|
|
|
while (1) |
|
|
|
|
|
{ |
|
|
|
|
|
// ReadRegisterParam(&plsr); |
|
|
|
|
|
currentSegment = plsr.currentSegment; |
|
|
|
|
|
|
|
|
tim10.init(&tim10, pulseFreq, pulseCount * 2); |
|
|
|
|
|
tim10.open(&tim10); |
|
|
|
|
|
|
|
|
if (plsr.sendEnableState != 1) |
|
|
|
|
|
{ |
|
|
|
|
|
continue; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
if (pulseFreq >= 0 || pulseFreq <= 5000) |
|
|
|
|
|
{ |
|
|
|
|
|
pulseReload = (uint32_t)(((1.f / (float)pulseFreq) * 1000 * 1000) / 20); |
|
|
|
|
|
tim10.init(&tim10, 1679, pulseReload); |
|
|
|
|
|
} |
|
|
|
|
|
else if (pulseFreq >= 5000 || pulseFreq <= 100000) |
|
|
|
|
|
{ |
|
|
|
|
|
pulseReload = (uint32_t)(((1.f / (float)pulseFreq) * 1000 * 1000) / 1); |
|
|
|
|
|
tim10.init(&tim10, 83, pulseReload); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
if (plsr.segment[currentSegment].pulseFreq <= 5000) |
|
|
|
|
|
pulseFreq = 1680 - 1; |
|
|
|
|
|
else if ((plsr.segment[currentSegment].pulseFreq > 5000) |
|
|
|
|
|
&& (plsr.segment[currentSegment].pulseFreq <= 100000)) |
|
|
|
|
|
pulseFreq = 84 - 1; |
|
|
|
|
|
else |
|
|
|
|
|
continue; |
|
|
|
|
|
|
|
|
tim10.pwmInit(&tim10); |
|
|
|
|
|
tim10.pwmSetPulse(&tim10, pulseReload / 2); |
|
|
|
|
|
|
|
|
/* 配置输入输出相关端子 */ |
|
|
|
|
|
configPulsePort(&plsr); |
|
|
|
|
|
|
|
|
while (1) |
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
if (plsr.outMode == ABSOLUTE_MODE) |
|
|
|
|
|
{ |
|
|
|
|
|
if (plsr.monitorPulseNum |
|
|
|
|
|
>= plsr.segment[currentSegment].pulseNumber) |
|
|
|
|
|
{ |
|
|
|
|
|
continue; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if ((plsr.accDecSpeedMode == STRAIGHT_LINE_MODE) && (RunFlag == 0)) |
|
|
|
|
|
{ |
|
|
|
|
|
RunFlag = 1; |
|
|
|
|
|
|
|
|
|
|
|
ComputePartNum(&plsr, ¤tSeg); |
|
|
|
|
|
|
|
|
|
|
|
switch (currentSeg.currentPart) |
|
|
|
|
|
{ |
|
|
|
|
|
case 1: |
|
|
|
|
|
{ |
|
|
|
|
|
FirstSegmentProc(¤tSeg); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
case 2: |
|
|
|
|
|
{ |
|
|
|
|
|
SecondSegmentProc(¤tSeg); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
case 3: |
|
|
|
|
|
{ |
|
|
|
|
|
ThirdSegmentProc(¤tSeg); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
//1HZ |
|
|
//1HZ |
|
|
//psc 1680 |
|
|
|
|
|
|
|
|
//psc 1680 一次10us |
|
|
//per 50000 |
|
|
//per 50000 |
|
|
|
|
|
|
|
|
//5KHZ |
|
|
//5KHZ |
|
|
//psc 1680 |
|
|
//psc 1680 |
|
|
//per 10 |
|
|
//per 10 |
|
|
|
|
|
|
|
|
//100KHZ |
|
|
//100KHZ |
|
|
//psc 84 |
|
|
|
|
|
//per 840 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//psc 168 一次0.5us |
|
|
|
|
|
//per 10 |
|
|
//定时器3 外部输入时钟源ETR引脚PB4,定时器11输出 |
|
|
//定时器3 外部输入时钟源ETR引脚PB4,定时器11输出 |
|
|
//定时器4 外部输入时钟源ETR引脚PB6,定时器14输出 |
|
|
//定时器4 外部输入时钟源ETR引脚PB6,定时器14输出 |
|
|
//定时器9 内部输入时钟源ITx主从模式定时器10 |
|
|
//定时器9 内部输入时钟源ITx主从模式定时器10 |
|
|
//定时器12 内部输入时钟源ITx主从模式定时器13 |
|
|
|
|
|
|
|
|
//定时器12 内部输入时钟源ITx主从模式定时器13 |
|
|
|
|
|
|
|
|
|
|
|
void TimCallBack(void *pArg) |
|
|
|
|
|
{ |
|
|
|
|
|
uint16_t reloadValue = 0; |
|
|
|
|
|
uint32_t diffValue; |
|
|
|
|
|
static int32_t pulseCount = 0; |
|
|
|
|
|
static uint8_t endFlag = 0; |
|
|
|
|
|
|
|
|
|
|
|
pulseCount++; |
|
|
|
|
|
if (currentSeg.currentPart == 1) |
|
|
|
|
|
{ |
|
|
|
|
|
switch (currentSeg.currentState) |
|
|
|
|
|
{ |
|
|
|
|
|
case 2: ///< 加速 |
|
|
|
|
|
{ |
|
|
|
|
|
if ((currentSpeed + currentSeg.inc) >= plsr.segment[currentSegment].pulseFreq) |
|
|
|
|
|
{ |
|
|
|
|
|
diffValue = currentSpeed + currentSeg.inc - plsr.segment[currentSegment].pulseFreq; |
|
|
|
|
|
currentSeg.inc = currentSeg.inc - diffValue; |
|
|
|
|
|
} |
|
|
|
|
|
currentSpeed += currentSeg.inc; |
|
|
|
|
|
|
|
|
|
|
|
reloadValue = 168000000 / (TIM10->PSC + 1) / (currentSpeed); |
|
|
|
|
|
tim10.setPer(&tim10, reloadValue - 1); |
|
|
|
|
|
tim10.pwmSetPulse(&tim10, TIM10->ARR / 2 + 1); |
|
|
|
|
|
|
|
|
|
|
|
if (pulseCount > currentSeg.firstPartNum - 1) |
|
|
|
|
|
{ |
|
|
|
|
|
currentSeg.currentPart = 2; |
|
|
|
|
|
pulseCount = 0; |
|
|
|
|
|
tim10.registerIrq(&tim10, TimCallBack, ¤tSeg.currentPart); |
|
|
|
|
|
} |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
default: |
|
|
|
|
|
{ |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
else if (currentSeg.currentPart == 2) |
|
|
|
|
|
{ |
|
|
|
|
|
reloadValue = 168000000 / (TIM10->PSC + 1) / (currentSpeed); |
|
|
|
|
|
tim10.setPer(&tim10, reloadValue - 1); |
|
|
|
|
|
tim10.pwmSetPulse(&tim10, TIM10->ARR / 2 + 1); |
|
|
|
|
|
|
|
|
|
|
|
if (pulseCount > currentSeg.secondPartNum - 1) |
|
|
|
|
|
{ |
|
|
|
|
|
currentSeg.currentPart = 3; |
|
|
|
|
|
pulseCount = 0; |
|
|
|
|
|
tim10.registerIrq(&tim10, TimCallBack, ¤tSeg.currentPart); |
|
|
|
|
|
|
|
|
|
|
|
currentSeg.dec = currentSpeed / currentSeg.thirdPartNum; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
else if (currentSeg.currentPart == 3) |
|
|
|
|
|
{ |
|
|
|
|
|
if (((currentSpeed - currentSeg.dec) <= 0) |
|
|
|
|
|
|| (pulseCount > currentSeg.thirdPartNum - 1)) |
|
|
|
|
|
{ |
|
|
|
|
|
pulseCount = 0; |
|
|
|
|
|
currentSeg.currentPart = 1; |
|
|
|
|
|
|
|
|
|
|
|
tim10.setPer(&tim10, 100); |
|
|
|
|
|
tim10.pwmSetPulse(&tim10, 65535); |
|
|
|
|
|
tim10.closeIrq(&tim10); |
|
|
|
|
|
|
|
|
|
|
|
tim10.registerIrq(&tim10, TimCallBack, ¤tSeg.currentPart); |
|
|
|
|
|
return; |
|
|
|
|
|
} |
|
|
|
|
|
currentSpeed -= currentSeg.dec; |
|
|
|
|
|
reloadValue = 168000000 / (TIM10->PSC + 1) / (currentSpeed); |
|
|
|
|
|
tim10.setPer(&tim10, reloadValue - 1); |
|
|
|
|
|
tim10.pwmSetPulse(&tim10, TIM10->ARR / 2 + 1); |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void ReadRegisterParam(PLSR *plsr) |
|
|
|
|
|
{ |
|
|
|
|
|
int16_t *pDataRegister = (uint16_t *)(DATA_REGISTER_ADRR + 0x1000); |
|
|
|
|
|
|
|
|
|
|
|
plsr->outputPort = pDataRegister[0]; |
|
|
|
|
|
plsr->dirPort = pDataRegister[1]; |
|
|
|
|
|
plsr->extPort = pDataRegister[2]; |
|
|
|
|
|
plsr->dirDelayTime = pDataRegister[3]; |
|
|
|
|
|
plsr->dirLogic = pDataRegister[4]; |
|
|
|
|
|
plsr->accDecSpeedMode = pDataRegister[5]; |
|
|
|
|
|
plsr->outMode = pDataRegister[6]; |
|
|
|
|
|
plsr->segmentAllNum = pDataRegister[7]; |
|
|
|
|
|
plsr->startRunSegment = pDataRegister[8]; |
|
|
|
|
|
plsr->defaultSpeed = *((uint32_t *)pDataRegister[9]); |
|
|
|
|
|
plsr->defaultAccSpeedTime = pDataRegister[11]; |
|
|
|
|
|
plsr->defaultDecSpeedTime = pDataRegister[12]; |
|
|
|
|
|
plsr->monitorPulseNum = *((int32_t *)(pDataRegister + 0x1000)); |
|
|
|
|
|
plsr->sendEnableState = *(pDataRegister + 0x2000); |
|
|
|
|
|
|
|
|
|
|
|
pDataRegister += 0x100; |
|
|
|
|
|
for (uint8_t i = 0; i < PLSR_PULSE_MAX_SEGMENT_NUM; i++) |
|
|
|
|
|
{ |
|
|
|
|
|
plsr->segment[i].pulseFreq = *((uint32_t *)pDataRegister[i]); |
|
|
|
|
|
plsr->segment[i].pulseNumber = *((int32_t *)pDataRegister[i + 2]); |
|
|
|
|
|
plsr->segment[i].waitType = pDataRegister[i + 4]; |
|
|
|
|
|
plsr->segment[i].jumpNumber = pDataRegister[i + 5]; |
|
|
|
|
|
|
|
|
|
|
|
pDataRegister += 0x10; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void configPulsePort(PLSR *plsr) |
|
|
|
|
|
{ |
|
|
|
|
|
gpio_obj_t *outDirPort = NULL; |
|
|
|
|
|
|
|
|
|
|
|
outDirPort = get_gpio_obj("gpio_h"); |
|
|
|
|
|
|
|
|
|
|
|
if ((outDirPort == NULL) || (plsr == NULL)) |
|
|
|
|
|
return; |
|
|
|
|
|
|
|
|
|
|
|
switch (plsr->dirPort) |
|
|
|
|
|
{ |
|
|
|
|
|
case OUTPUT_DIR_PORT_Y12: |
|
|
|
|
|
{ |
|
|
|
|
|
outDirPort->gpio_init(outDirPort, gpio_pin_9, "OUT_PP", "UP"); |
|
|
|
|
|
outDirPort->gpio_set_output(outDirPort, gpio_pin_9, 1); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
case OUTPUT_DIR_PORT_Y13: |
|
|
|
|
|
{ |
|
|
|
|
|
outDirPort->gpio_init(outDirPort, gpio_pin_8, "OUT_PP", "UP"); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
case OUTPUT_DIR_PORT_Y14: |
|
|
|
|
|
{ |
|
|
|
|
|
outDirPort->gpio_init(outDirPort, gpio_pin_7, "OUT_PP", "UP"); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
case OUTPUT_DIR_PORT_Y15: |
|
|
|
|
|
{ |
|
|
|
|
|
outDirPort->gpio_init(outDirPort, gpio_pin_6, "OUT_PP", "UP"); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
// switch (plsr->outputPort) |
|
|
|
|
|
// { |
|
|
|
|
|
// case OUTPUT_PORT_Y0: |
|
|
|
|
|
// { |
|
|
|
|
|
// outDirPort->gpio_init(outDirPort, gpio_pin) |
|
|
|
|
|
// } |
|
|
|
|
|
// } |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void ComputePartNum(PLSR *plsr, SEGMENT_PART *segmentPart) |
|
|
|
|
|
{ |
|
|
|
|
|
uint32_t target = plsr->segment[currentSegment].pulseFreq; |
|
|
|
|
|
accSpeedSlope = plsr->defaultSpeed / plsr->defaultAccSpeedTime; |
|
|
|
|
|
decSpeedSlope = plsr->defaultSpeed / plsr->defaultDecSpeedTime; |
|
|
|
|
|
|
|
|
|
|
|
/* 最后一段 */ |
|
|
|
|
|
if ((currentSegment == (plsr->segmentAllNum -1)) |
|
|
|
|
|
&& (plsr->segment[currentSegment].jumpNumber == 0)) |
|
|
|
|
|
{ |
|
|
|
|
|
decTime = target / decSpeedSlope; |
|
|
|
|
|
segmentPart->thirdPartNum = target / 2 * decTime / 1000; |
|
|
|
|
|
} |
|
|
|
|
|
else |
|
|
|
|
|
{ |
|
|
|
|
|
segmentPart->thirdPartNum = 0; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (currentSpeed < target) |
|
|
|
|
|
{ |
|
|
|
|
|
accTime = (target - currentSpeed) / accSpeedSlope; |
|
|
|
|
|
segmentPart->firstPartNum = (target + currentSpeed) / 2 * accTime / 1000; |
|
|
|
|
|
} |
|
|
|
|
|
else if (currentSpeed > target) |
|
|
|
|
|
{ |
|
|
|
|
|
decTime = (currentSpeed - target) / decSpeedSlope; |
|
|
|
|
|
segmentPart->firstPartNum = (target + currentSpeed) / 2 * decTime / 1000; |
|
|
|
|
|
} |
|
|
|
|
|
segmentPart->secondPartNum = plsr->segment[currentSegment].pulseNumber |
|
|
|
|
|
- segmentPart->firstPartNum |
|
|
|
|
|
- segmentPart->thirdPartNum; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void FirstSegmentProc(SEGMENT_PART *segmentPart) |
|
|
|
|
|
{ |
|
|
|
|
|
int32_t targetFreq = 0; |
|
|
|
|
|
int32_t slope = 0; |
|
|
|
|
|
int32_t diffValue = 0; |
|
|
|
|
|
static int32_t incAll = 0; |
|
|
|
|
|
|
|
|
|
|
|
switch (segmentPart->currentState) |
|
|
|
|
|
{ |
|
|
|
|
|
case 2 : |
|
|
|
|
|
{ |
|
|
|
|
|
targetFreq = plsr.segment[currentSegment].pulseFreq; |
|
|
|
|
|
slope = plsr.defaultSpeed / plsr.defaultAccSpeedTime; |
|
|
|
|
|
segmentPart->inc = ((targetFreq - currentSpeed) / slope) |
|
|
|
|
|
* ((targetFreq + currentSpeed) / 2) / 1000; |
|
|
|
|
|
segmentPart->inc = (targetFreq - currentSpeed) / segmentPart->inc; |
|
|
|
|
|
if ((currentSpeed + segmentPart->inc) >= plsr.segment[currentSegment].pulseFreq) |
|
|
|
|
|
{ |
|
|
|
|
|
diffValue = currentSpeed + segmentPart->inc - plsr.segment[currentSegment].pulseFreq; |
|
|
|
|
|
segmentPart->inc = segmentPart->inc - diffValue; |
|
|
|
|
|
} |
|
|
|
|
|
incAll += segmentPart->inc; |
|
|
|
|
|
if (plsr.segment[currentSegment].pulseFreq <= 5000) |
|
|
|
|
|
{ |
|
|
|
|
|
tim10.setPsc(&tim10, 1680 - 1); |
|
|
|
|
|
if (currentSpeed != 0) |
|
|
|
|
|
tim10.setPer(&tim10, 100000 / currentSpeed); |
|
|
|
|
|
else |
|
|
|
|
|
tim10.setPer(&tim10, 1000); |
|
|
|
|
|
tim10.registerIrq(&tim10, TimCallBack, ¤tSeg.currentPart); |
|
|
|
|
|
tim10.open(&tim10); |
|
|
|
|
|
tim10.openIrq(&tim10); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
else |
|
|
|
|
|
{ |
|
|
|
|
|
tim10.setPsc(&tim10, 168 - 1); |
|
|
|
|
|
tim10.setPer(&tim10, 1000000 / currentSpeed); |
|
|
|
|
|
tim10.registerIrq(&tim10, TimCallBack, ¤tSeg.currentPart); |
|
|
|
|
|
tim10.open(&tim10); |
|
|
|
|
|
tim10.openIrq(&tim10); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void SecondSegmentProc(SEGMENT_PART *segmentPart) |
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void ThirdSegmentProc(SEGMENT_PART *segmentPart) |
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
|
|
} |