huahua/K74B
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
K74B/app_Indie/usr/common/motorControlTask.c

504 lines
17 KiB
C
Raw Normal View History

增加indie代码
2024-01-16 10:46:17 +08:00
#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;
}
}
Reference in New Issue Copy Permalink