504 lines
17 KiB
C
504 lines
17 KiB
C
#include <motorControlTask.h>
|
||
#include <pwm_aux_device.h>
|
||
#include <linStackTask.h>
|
||
#include <pdsTask.h>
|
||
#include <gtimer_device.h>
|
||
#include <adc_device.h>
|
||
#include <safetyMonitorTask.h>
|
||
#include <lin_device.h>
|
||
#include <flash_device.h>
|
||
#include <flash_sfrs.h>
|
||
#include <linSlaveTask.h>
|
||
|
||
static TaskState_t motorState = TASK_STATE_INIT;
|
||
static ADCMeasureParam_t motoradcItem;
|
||
|
||
/*Data sent from LIN host, including setting parameters and control parameters*/
|
||
static M_Re_pra_t M_Re_pra = {
|
||
.M_ON = 0,
|
||
.Constantspeed = 500U,
|
||
.Startspeed = 1000U,
|
||
.Slowspeed = 1000U,
|
||
.LockACoilValtage = 500U,
|
||
.LockBCoilValtage = 500U,
|
||
.ClimbTime = 120U,
|
||
.SlowTime = 120U,
|
||
.Position = 0,
|
||
};
|
||
|
||
static uint8_t SysPowerup = 0;
|
||
static uint16_t MotorPosition = 0;
|
||
static uint8_t Motor_runnstatuschanged = 0;
|
||
static uint8_t Onestep_csa_adcnt = 0U;
|
||
static uint16_t Count_20ms = 0;
|
||
static uint16_t MotorOCstop10s = 0;
|
||
|
||
static Motorpra_t motorpra;
|
||
static Motor_and_ChipPar_t motorandchippar = {
|
||
.state.byte[0] = 0,
|
||
.state.byte[1] = 0,
|
||
};
|
||
|
||
static uint16_t bufftemp[TransferCNT];
|
||
|
||
void delay(uint32_t delay)//1000000 1s
|
||
{
|
||
for (uint32_t i = 0; i < delay; i++){
|
||
for (uint32_t j = 0; j < 3; j++){
|
||
__asm("NOP");
|
||
}
|
||
}
|
||
}
|
||
|
||
void Get_MotorprafromLINM(LIN_Device_Frame_t const *frame)
|
||
{
|
||
MotorCtrlFrame_t const *motor = (MotorCtrlFrame_t const*)((void const*)frame->data);
|
||
// if ( (motor->nodeAddress == ls_read_nad()) || (motor->nodeAddress == LIN_NAD_WILDCARD))
|
||
// {
|
||
M_Re_pra.M_ON = (motor->data[1])&0x01U ; //0X01 ON
|
||
M_Re_pra.Position = motor->data[3];
|
||
M_Re_pra.Position = (M_Re_pra.Position << 8U) + motor->data[2];
|
||
|
||
if((M_Re_pra.M_ON != Motor_runnstatuschanged) || (MotorPosition != M_Re_pra.Position)){
|
||
/*The position has changed*/
|
||
motorpra.ClimbTime = 0U;
|
||
motorpra.SlowTime = 0U;
|
||
}
|
||
MotorPosition = M_Re_pra.Position;
|
||
Motor_runnstatuschanged = M_Re_pra.M_ON;
|
||
motorandchippar.MotorErrorState10s = 10000000U/M_Re_pra.Constantspeed;
|
||
// }
|
||
}
|
||
|
||
void StopMotor(void)
|
||
{
|
||
MotorPower_OFF;
|
||
VDD3V3EXT_OFF;
|
||
Twobridge_OFF;
|
||
|
||
motorpra.ClimbTime = 0U;
|
||
motorpra.SlowTime = 0U;
|
||
|
||
motorandchippar.state.Motor_runsta = (uint8_t)MotorStop;
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Climbstage;
|
||
|
||
if(SysPowerup == 0U){
|
||
|
||
PDS_PowerONRead_MotorParamfromFlash(bufftemp,TransferCNT);
|
||
motorpra.stepn = PDS_GetMotorstepnfromflash();/*From flash*/
|
||
|
||
M_Re_pra.Constantspeed = bufftemp[0];
|
||
M_Re_pra.Startspeed = bufftemp[1];
|
||
M_Re_pra.Slowspeed = bufftemp[2];
|
||
M_Re_pra.LockACoilValtage = bufftemp[3];
|
||
M_Re_pra.LockBCoilValtage = bufftemp[4];
|
||
M_Re_pra.ClimbTime = (uint8_t)bufftemp[5];
|
||
M_Re_pra.SlowTime = (uint8_t)(bufftemp[5]>>8U);
|
||
|
||
motorpra.ClimbSteper = (uint16_t)((uint32_t)(M_Re_pra.ClimbTime)*1000U*2U/((uint32_t)(M_Re_pra.Startspeed )+ (uint32_t)(M_Re_pra.Constantspeed)));
|
||
motorpra.SlowSteper = (uint16_t)((uint32_t)(M_Re_pra.SlowTime)*1000U*2U/((uint32_t)(M_Re_pra.Slowspeed) + (uint32_t)(M_Re_pra.Constantspeed)));
|
||
Onestep_csa_adcnt = M_Re_pra.Constantspeed*16 / PWMAUX_Period; //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ڵIJ<DAB5><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
SysPowerup = 1u;
|
||
}
|
||
if(LINS_TransferMotorPhyParamtobuffBy3C(bufftemp,TransferCNT) == 1U){//3C
|
||
|
||
M_Re_pra.Constantspeed = bufftemp[0];
|
||
M_Re_pra.Startspeed = bufftemp[1];
|
||
M_Re_pra.Slowspeed = bufftemp[2];
|
||
M_Re_pra.LockACoilValtage = bufftemp[3];
|
||
M_Re_pra.LockBCoilValtage = bufftemp[4];
|
||
M_Re_pra.ClimbTime = (uint8_t)bufftemp[5];
|
||
M_Re_pra.SlowTime = (uint8_t)(bufftemp[5]>>8U);
|
||
|
||
motorpra.ClimbSteper = (uint16_t)((uint32_t)(M_Re_pra.ClimbTime)*1000U*2U/((uint32_t)(M_Re_pra.Startspeed )+ (uint32_t)(M_Re_pra.Constantspeed)));
|
||
motorpra.SlowSteper = (uint16_t)((uint32_t)(M_Re_pra.SlowTime)*1000U*2U/((uint32_t)(M_Re_pra.Slowspeed) + (uint32_t)(M_Re_pra.Constantspeed)));
|
||
Onestep_csa_adcnt = M_Re_pra.Constantspeed*16 / PWMAUX_Period;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
void Calculation_position(void){
|
||
|
||
if(M_Re_pra.M_ON == 1U){
|
||
|
||
VDD3V3EXT_ON;
|
||
if(M_Re_pra.Position > (uint16_t)(motorpra.stepn)){
|
||
|
||
if((SAFM_TransferState() & 0xFFU) == 0U){
|
||
|
||
motorpra.s = M_Re_pra.Position - motorpra.stepn;
|
||
|
||
if(motorpra.s>0u){
|
||
|
||
MotorPower_ON;
|
||
|
||
if(motorpra.ClimbTime < motorpra.ClimbSteper){
|
||
motorpra.ClimbTime++;
|
||
}else{
|
||
if(motorpra.s <= motorpra.SlowSteper){
|
||
motorpra.SlowTime++;
|
||
}
|
||
}
|
||
if(motorpra.ClimbTime < motorpra.ClimbSteper){
|
||
TIMER_Disable(TIMER1);
|
||
motorpra.T1timercnt = (uint32_t)(M_Re_pra.Startspeed)*16U-(uint32_t)(16U*((uint32_t)(M_Re_pra.Startspeed)-(uint32_t)(M_Re_pra.Constantspeed))*(uint32_t)(motorpra.ClimbTime))/(uint32_t)(motorpra.ClimbSteper);
|
||
|
||
TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Climbstage;
|
||
}else if((motorpra.SlowTime<motorpra.SlowSteper)&&(motorpra.SlowSteper > motorpra.s)){
|
||
TIMER_Disable(TIMER1);
|
||
motorpra.T1timercnt = (uint32_t)((uint32_t)(M_Re_pra.Constantspeed)*16U+(16U*(((uint32_t)(M_Re_pra.Slowspeed)-(uint32_t)(M_Re_pra.Constantspeed)))*(uint32_t)(motorpra.SlowTime))/(uint32_t)(motorpra.SlowSteper));
|
||
TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Slowstage;
|
||
}else{
|
||
if(motorandchippar.state.Motor_runstage != (uint8_t)Constantstage){
|
||
TIMER_Disable(TIMER1);
|
||
TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, (uint32_t)M_Re_pra.Constantspeed*16U, Timer1_INTTest);
|
||
}
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Constantstage;
|
||
}
|
||
#if control_type == Divide_2
|
||
motorpra.MM_Step=(uint8_t)(motorpra.stepn % 8U);
|
||
#elif control_type == Divide_6
|
||
motorpra.MM_Step=(uint8_t)(motorpra.stepn % 24U);
|
||
#endif
|
||
motorpra.stepn++;
|
||
motorandchippar.state.Motor_runsta = (uint8_t)MotorRun;
|
||
}
|
||
}
|
||
}else if(M_Re_pra.Position < (uint16_t)(motorpra.stepn)){
|
||
if((SAFM_TransferState() & 0xffu) == 0U){
|
||
motorpra.s = motorpra.stepn - M_Re_pra.Position;
|
||
|
||
if(motorpra.s>0U)
|
||
{
|
||
MotorPower_ON;
|
||
|
||
if(motorpra.ClimbTime < motorpra.ClimbSteper){
|
||
motorpra.ClimbTime++;
|
||
}else{
|
||
if(motorpra.s <= motorpra.SlowSteper){
|
||
motorpra.SlowTime++;
|
||
}
|
||
}
|
||
if(motorpra.ClimbTime < motorpra.ClimbSteper){
|
||
TIMER_Disable(TIMER1);
|
||
motorpra.T1timercnt = (uint32_t)((uint32_t)M_Re_pra.Startspeed*16U-(16U*((uint32_t)M_Re_pra.Startspeed-(uint32_t)M_Re_pra.Constantspeed)*motorpra.ClimbTime)/motorpra.ClimbSteper);
|
||
TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Climbstage;
|
||
}else if((motorpra.SlowTime < motorpra.SlowSteper)&&(motorpra.SlowSteper > motorpra.s)){
|
||
TIMER_Disable(TIMER1);
|
||
motorpra.T1timercnt = (uint32_t)((uint32_t)M_Re_pra.Constantspeed*16U+(16U*((uint32_t)M_Re_pra.Slowspeed-(uint32_t)M_Re_pra.Constantspeed)*motorpra.SlowTime)/motorpra.SlowSteper);
|
||
TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Slowstage;
|
||
}else{
|
||
if(motorandchippar.state.Motor_runstage != (uint8_t)Constantstage){
|
||
TIMER_Disable(TIMER1);
|
||
TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, (uint32_t)M_Re_pra.Constantspeed*16U, Timer1_INTTest);
|
||
}
|
||
motorandchippar.state.Motor_runstage = (uint8_t)Constantstage;
|
||
}
|
||
|
||
#if control_type == Divide_2
|
||
motorpra.MM_Step=(uint8_t)(motorpra.stepn % 8U);
|
||
#elif control_type == Divide_6
|
||
motorpra.MM_Step=(uint8_t)(motorpra.stepn % 24U);
|
||
#endif
|
||
motorpra.stepn--;
|
||
motorandchippar.state.Motor_runsta = (uint8_t)MotorRun;
|
||
}
|
||
}
|
||
}else{
|
||
StopMotor();
|
||
}
|
||
}
|
||
else{
|
||
StopMotor();
|
||
}
|
||
motorandchippar.Motor_position = motorpra.stepn;
|
||
}
|
||
|
||
#if control_type == Divide_2
|
||
|
||
void MOTORunP_8(uint8_t motorstep){
|
||
switch(motorstep){
|
||
case 0://1000 Phase B zero crossing, detect reverse electromotive force of B phase
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
|
||
Downpwm_APhaseNeg_Low_ON;//PA5 OFF(Lower bridge arm)
|
||
break;
|
||
case 1://1010
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_APhasePos_High_ON;
|
||
Downpwm_APhaseNeg_Low_ON;
|
||
|
||
Downpwm_BPhasePos_High_ON;
|
||
Downpwm_BPhaseNeg_Low_ON;
|
||
break;
|
||
case 2://0010 Phase A zero crossing, detect reverse electromotive force of A phase
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_BPhasePos_High_ON;
|
||
Downpwm_BPhaseNeg_Low_ON;
|
||
break;
|
||
case 3://0110
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_APhaseNeg_High_ON;
|
||
Downpwm_APhasePos_Low_ON;
|
||
|
||
Downpwm_BPhasePos_High_ON;
|
||
Downpwm_BPhaseNeg_Low_ON;
|
||
break;
|
||
case 4://0100 Phase B zero crossing, detect reverse electromotive force of B phase
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_APhaseNeg_High_ON;
|
||
Downpwm_APhasePos_Low_ON;
|
||
break;
|
||
case 5://0101
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_APhaseNeg_High_ON;
|
||
Downpwm_APhasePos_Low_ON;
|
||
|
||
Downpwm_BPhaseNeg_High_ON;
|
||
Downpwm_BPhasePos_Low_ON;
|
||
break;
|
||
case 6://0001 Phase A zero crossing, detect reverse electromotive force of A phase
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_BPhaseNeg_High_ON;
|
||
Downpwm_BPhasePos_Low_ON;
|
||
break;
|
||
case 7://1001
|
||
Twobridge_OFF;
|
||
|
||
Downpwm_APhasePos_High_ON;
|
||
Downpwm_APhaseNeg_Low_ON;
|
||
|
||
Downpwm_BPhaseNeg_High_ON;
|
||
Downpwm_BPhasePos_Low_ON;
|
||
break;
|
||
default:
|
||
Twobridge_OFF;
|
||
break;
|
||
}
|
||
}
|
||
#elif control_type == Divide_6
|
||
|
||
const uint16_t Downpwm_ACoil[24] = {247U,221U,181U,128U,66U,0U,66U,128U,181U,221U,247U,256U,247U,221U,181U,128U,66U,0U,66U,128U,181U,221U,247U,256U};
|
||
const uint16_t Downpwm_BCoil[24] = {66U,128U,181U,221U,247U,256U,247U,221U,181U,128U,66U,0U,66U,128U,181U,221U,247U,256U,247U,221U,181U,128U,66U,0U};
|
||
|
||
void MOTORunP_24(uint8_t motorstep){
|
||
|
||
uint16_t temp1;
|
||
uint16_t temp2;
|
||
|
||
temp1 = (uint16_t)(((uint32_t)PWM_AUX_SFRS->PWM_AUX[PWMAUX_BASE_0].BASE.PERIOD * Downpwm_ACoil[motorstep])>>8U);
|
||
temp2 = (uint16_t)(((uint32_t)PWM_AUX_SFRS->PWM_AUX[PWMAUX_BASE_0].BASE.PERIOD * Downpwm_BCoil[motorstep])>>8U);
|
||
PWM_AUX_SFRS->INVERT = 0X3FU;
|
||
|
||
switch(motorstep){
|
||
case 0://
|
||
case 1:
|
||
case 2:
|
||
case 3:
|
||
case 4:
|
||
case 5://A<><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>
|
||
Downpwm_BPhasePos_Low_OFF;//PB1
|
||
Downpwm_BPhaseNeg_High_OFF;//PA4 OFF(Upper bridge arm)
|
||
Downpwm_APhasePos_Low_OFF;//PA2 PWMAUX0
|
||
Downpwm_APhaseNeg_High_OFF;//PA6
|
||
delay(25);
|
||
|
||
Downpwm_BPhasePos_High_ON; //PB2 ON(Upper bridge arm)
|
||
PWMAUX_SetMatchValue(PWMAUX_CH1, 0, temp2);//PB0 PWMAUX1
|
||
Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
|
||
PWMAUX_SetMatchValue(PWMAUX_CH0, 0, temp1);//PA5 PWMAUX0
|
||
break;
|
||
case 6:
|
||
case 7:
|
||
case 8:
|
||
case 9:
|
||
case 10:
|
||
case 11://B<><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>
|
||
Downpwm_BPhasePos_Low_OFF;//PB2
|
||
Downpwm_BPhaseNeg_High_OFF;//PA4 OFF(Upper bridge arm)
|
||
Downpwm_APhasePos_High_OFF; //PA3 ON(Upper bridge arm)
|
||
Downpwm_APhaseNeg_Low_OFF;//PA5 PWMAUX0
|
||
delay(25);
|
||
|
||
Downpwm_BPhasePos_High_ON; //PB2 ON(B phase Upper bridge arm)
|
||
//Downpwm_BPhasePos_High_OFF; //PB2 ON(B phase Upper bridge arm)
|
||
PWMAUX_SetMatchValue(PWMAUX_CH1, 0, temp2);//PB0 PWMAUX1
|
||
PWMAUX_SetMatchValue(PWMAUX_CH5, 0, temp1);//PA2 PWMAUX5
|
||
Downpwm_APhaseNeg_High_ON;//PA6
|
||
break;
|
||
|
||
case 12:
|
||
case 13:
|
||
case 14:
|
||
case 15:
|
||
case 16:
|
||
case 17://A<><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>
|
||
Downpwm_BPhasePos_High_OFF; //PB2(Upper bridge arm)
|
||
Downpwm_BPhaseNeg_Low_OFF;//PB0 PWMAUX1
|
||
Downpwm_APhasePos_High_OFF; //PA3 ON(Upper bridge arm)
|
||
Downpwm_APhaseNeg_Low_OFF;//PA5 PWMAUX0
|
||
delay(25);
|
||
|
||
PWMAUX_SetMatchValue(PWMAUX_CH2, 0, temp2);//PB1 PWMAUX2
|
||
Downpwm_BPhaseNeg_High_ON;//PA4(Upper bridge arm)
|
||
PWMAUX_SetMatchValue(PWMAUX_CH5, 0, temp1);//PA2 PWMAUX5
|
||
Downpwm_APhaseNeg_High_ON;//PA6
|
||
break;
|
||
case 18:
|
||
case 19:
|
||
case 20:
|
||
case 21:
|
||
case 22:
|
||
// case 23://B<><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>
|
||
Downpwm_BPhasePos_High_OFF; //PB2(Upper bridge arm)
|
||
Downpwm_BPhaseNeg_Low_OFF;//PB0 PWMAUX1
|
||
Downpwm_APhasePos_Low_OFF;//PA2 PWMAUX0
|
||
Downpwm_APhaseNeg_High_OFF;//PA6
|
||
delay(25);
|
||
|
||
PWMAUX_SetMatchValue(PWMAUX_CH2, 0, temp2);//PB1 PWMAUX2
|
||
Downpwm_BPhaseNeg_High_ON;//PA4(Upper bridge arm)
|
||
Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
|
||
PWMAUX_SetMatchValue(PWMAUX_CH0, 0, temp1);//PA5 PWMAUX0
|
||
break;
|
||
case 23://B<><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>
|
||
Downpwm_BPhasePos_High_OFF; //PB2(Upper bridge arm)
|
||
Downpwm_BPhaseNeg_Low_OFF;//PB0 PWMAUX1
|
||
Downpwm_APhasePos_Low_OFF;//PA2 PWMAUX0
|
||
Downpwm_APhaseNeg_High_OFF;//PA6
|
||
delay(25);
|
||
|
||
PWMAUX_SetMatchValue(PWMAUX_CH2, 0, temp2);//PB1 PWMAUX2
|
||
Downpwm_BPhaseNeg_High_OFF;//PA4(Upper bridge arm)
|
||
Downpwm_BPhasePos_High_ON;
|
||
|
||
Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
|
||
PWMAUX_SetMatchValue(PWMAUX_CH0, 0, temp1);//PA5 PWMAUX0
|
||
break;
|
||
|
||
default:
|
||
|
||
break;
|
||
}
|
||
}
|
||
|
||
uint8_t Motor_GetMotorState(void){
|
||
return motorandchippar.state.Motor_runsta ;
|
||
}
|
||
|
||
uint8_t Motor_GetMotorRunStage(void){
|
||
return motorandchippar.state.Motor_runstage ;;
|
||
}
|
||
|
||
uint32_t Motor_GetMotorErrorState10s(void){
|
||
return motorandchippar.MotorErrorState10s;
|
||
}
|
||
|
||
uint8_t Motor_GetMotorStep(void){
|
||
return motorpra.MM_Step;
|
||
}
|
||
|
||
/*Invoke all set parameters of the motor*/
|
||
M_Re_pra_t Motor_GetALLMotorSetparameters (void){
|
||
return M_Re_pra;
|
||
}
|
||
|
||
uint16_t Motor_GetMotorConstantspeed(void){
|
||
return M_Re_pra.Constantspeed*16U;
|
||
}
|
||
|
||
void Motor_GetSETLockCurrent(uint16_t *data1,uint16_t *data2){
|
||
*data1 = M_Re_pra.LockACoilValtage;
|
||
*data2 = M_Re_pra.LockBCoilValtage;
|
||
}
|
||
|
||
uint16_t Motor_GetCurrentPosition(void){
|
||
return motorandchippar.Motor_position;
|
||
}
|
||
|
||
uint8_t Motor_GetOnestep_csa_adcnt(void){
|
||
static uint8_t adcnt = 0;
|
||
adcnt = Onestep_csa_adcnt;
|
||
if(adcnt == 0){
|
||
adcnt = 10u;
|
||
}
|
||
return adcnt;
|
||
}
|
||
|
||
#endif
|
||
|
||
#if control_type == Divide_2
|
||
|
||
void Timer1_INTTest(void)
|
||
{
|
||
|
||
}
|
||
#elif control_type == Divide_6
|
||
//uint32_t *flashdata = (uint32_t *)0xAC00U;
|
||
//uint32_t dataflash;
|
||
//uint32_t dataflash_b;
|
||
void Timer1_INTTest(void)
|
||
{
|
||
uint8_t Motosteper = 0;
|
||
Count_20ms++;
|
||
|
||
Calculation_position();
|
||
|
||
if(Motor_GetMotorState() == (uint8_t)MotorRun){
|
||
motoradcItem = ADC_GetadcMeasParamm();
|
||
if(motoradcItem.item != ADC_MEASURE_ITEM_VAMP){
|
||
ADC_Init(ADC_MEASURE_ITEM_VAMP, 0);
|
||
SAR_CTRL_SFRS->SARINT.CLEAR.INT_CONV_DONE_CLR = 1U;
|
||
SAR_CTRL_SFRS->SARCTRL.SARENAREQ = 1U;
|
||
SAR_CTRL_SFRS->SARCTRL.CONVERT = 1U;
|
||
}
|
||
Motosteper = Motor_GetMotorStep();
|
||
MOTORunP_24(Motosteper);
|
||
|
||
}
|
||
|
||
if((SAFM_TransferState() & 0xffu) != 0U) {
|
||
MotorOCstop10s++;
|
||
if(MotorOCstop10s > Motor_GetMotorErrorState10s()){
|
||
MotorOCstop10s = 0U;
|
||
ClearMotorandchipparstate();
|
||
}
|
||
}
|
||
// if(Count_20ms > 2000U){
|
||
// Count_20ms = 0;
|
||
// f_FLASH_EraseSector(0xAC00U);
|
||
// FLASH_Write2WordsWithECC(0xAC00U,0XAAAAAAAA,0X55555555);
|
||
// dataflash = *flashdata;
|
||
// flashdata+=1;
|
||
// dataflash_b = *flashdata;
|
||
// }
|
||
}
|
||
#endif
|
||
|
||
void MotorCtrl_TaskHandler(void)
|
||
{
|
||
switch(motorState){
|
||
case TASK_STATE_INIT:
|
||
/* Motor control initialization */
|
||
StopMotor();
|
||
/* End of Motor control initialization */
|
||
motorState = TASK_STATE_ACTIVE;
|
||
break;
|
||
case TASK_STATE_ACTIVE:
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
} |