/*
** ===================================================================
** Method : SetRatio (bean PPG)
**
** Description :
** This method is internal. It is used by Processor Expert
** only.
** ===================================================================
*/
void PWM_SetRatio(byte salida)
{
if (salida<3)
PWMDTY_ARRAY[salida] = (word)((PWMPER_ARRAY[salida] * (dword)RatioStore[salida] + 500) / 1000); // Calculate new value of duty
else
PWM_SetDuty(RatioStore[salida]);
}
/*
* 函数名: Mecanum_test
* 描 述: 轮子调试
* 输 入: 轮子状态信息
* 输 出: 无
* 调 用: 外部调用
*/
void Mecanum_test(struct Mecanum_State *mec)
{
rt_uint32_t key_value;
double speed_pwm=50;
/*->*/mec_test:
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("======Mecanum=======");
LCD_SetXY(0,1);
LCD_WriteString("1.Speed");
LCD_SetXY(0,2);
LCD_WriteString("2.ChangeArg");
LCD_SetXY(0,3);
LCD_WriteString("3.ChangePort now:");
LCD_WriteInt((*mec).port);
//
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
goto mec_speed;
case key2:
Input_IntValue(&(*mec).arg,"Argument");
break;
case key3:
// Input_IntValue(int(&(*mec).ID_NUM),"Can");
break;
case keyback:
return;
}
}
Delay_ms(10);
}
/*->*/mec_speed:
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("======Mecanum=======");
LCD_SetXY(0,1);
LCD_WriteString("1.SpeedUp");
LCD_SetXY(0,2);
LCD_WriteString("2.SpeedDown");
LCD_SetXY(0,3);
LCD_WriteString("3.Stop");
LCD_SetXY(0,4);
LCD_WriteString("4.Set Speed");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
speed_pwm+=1;
break;
case key2:
speed_pwm-=1;
break;
case key3:
speed_pwm=50;
break;
case key4:
Input_DoubleValue(&speed_pwm,"Goal Speed");
break;
case keyback:
// motor_speed=0;
goto mec_test;
}
PWM_SetDuty((*mec).port,speed_pwm);
}
Delay_ms(10);
}
}
//void SetSpeed(double fl_speed,double fr_speed,double bl_speed,double br_speed)
void SetSpeed(double speed_x,double speed_y,double speed_rotation)
{
double fl_speed;
double fr_speed;
double bl_speed;
double br_speed;
double fl_delta;
double fr_delta;
double bl_delta;
double br_delta;
double real_fl_speed;
double real_fr_speed;
double real_bl_speed;
double real_br_speed;
static double fl_last=0; //储存四轮的速度
static double fr_last=0;
static double bl_last=0;
static double br_last=0;
double delta_max; //速度变化量最大值
double PWM_Max; //生成的PWM后对应的速度最大值
double PWM_FL,PWM_FR,PWM_BL,PWM_BR;
/*******************anti-slip************************/
// coeff_x = coeff_y = 1/wheel_r/(2*pi)*60;
// coeff_r = -(robo_L+robo_W)/2/wheel_r/(2*pi)*60;
// 这两个参数随意设置 不让轮子速度过快即可 主要调的是雅克比矩阵的各个正负号
coeff_x = coeff_y = 0.7071*0.05;
coeff_r = 0.9806;
/****************************通过雅克比矩阵把速度分解到各个轮子******************************/
fr_speed = + coeff_r*speed_rotation + coeff_x*speed_x - coeff_y*speed_y;
fl_speed = + coeff_r*speed_rotation + coeff_x*speed_x + coeff_y*speed_y;
bl_speed = + coeff_r*speed_rotation - coeff_x*speed_x + coeff_y*speed_y;
br_speed = + coeff_r*speed_rotation - coeff_x*speed_x - coeff_y*speed_y;
/****************************对加速度进行限制(以理论速度限制赋速度)******************************/
fl_delta = fl_speed - fl_last;
fr_delta = fr_speed - fr_last;
bl_delta = bl_speed - bl_last;
br_delta = br_speed - br_last;
delta_max=MaxFour(Abs(fl_delta),Abs(fr_delta),Abs(bl_delta),Abs(br_delta));
if((delta_max >= Max_Acc) && (Acc_Limit_Enable==1))
{
fl_last += Max_Acc*(fl_delta/delta_max);
fr_last += Max_Acc*(fr_delta/delta_max);
bl_last += Max_Acc*(bl_delta/delta_max);
br_last += Max_Acc*(br_delta/delta_max);
}
else if((delta_max <= -Max_Acc) && (Acc_Limit_Enable==1))
{
fl_last -= Max_Acc*(fl_delta/delta_max);
fr_last -= Max_Acc*(fr_delta/delta_max);
bl_last -= Max_Acc*(bl_delta/delta_max);
br_last -= Max_Acc*(br_delta/delta_max);
}
else
{
fl_last = fl_speed;
fr_last = fr_speed;
bl_last = bl_speed;
br_last = br_speed;
}
fl_speed = fl_last * Mec_FL.arg;
fr_speed = fr_last * Mec_FR.arg;
bl_speed = bl_last * Mec_BL.arg;
br_speed = br_last * Mec_BR.arg;
/**************************速度转换为转速*****************************
r = v / c
*************************************************************************/
real_fl_speed = fl_speed * RATE *14;
real_fr_speed = fr_speed * RATE *14;
real_bl_speed = bl_speed * RATE *14;
real_br_speed = br_speed * RATE *14;
// W_MOTOR_OLD_FUNC(W_MOTOR2_SPEED_ID, 0, real_fr_speed, CMD_SET_SG);
// W_MOTOR_OLD_FUNC(W_MOTOR4_SPEED_ID, 0, real_br_speed, CMD_SET_SG);
// W_MOTOR_OLD_FUNC(W_MOTOR1_SPEED_ID, 0, real_fl_speed, CMD_SET_SG);
// W_MOTOR_OLD_FUNC(W_MOTOR3_SPEED_ID, 0, real_bl_speed, CMD_SET_SG);
/*******************************************************
*************************************************************************/
PWM_FL = (50.0/6000)*14*fl_speed+50;
PWM_FR = (50.0/6000)*14*fr_speed+50;
PWM_BL = (50.0/6000)*14*bl_speed+50;
PWM_BR = (50.0/6000)*14*br_speed+50;
//
// /************************对最终速度再次进行限制****************************/
//
PWM_Max=MaxFour(Abs(PWM_FL-50),Abs(PWM_FR-50),Abs(PWM_BL-50),Abs(PWM_BR-50));
if(PWM_Max >= 48)
{
PWM_FL = 48*(PWM_FL-50)/PWM_Max+50;
PWM_FR = 48*(PWM_FR-50)/PWM_Max+50;
PWM_BL = 48*(PWM_BL-50)/PWM_Max+50;
PWM_BR = 48*(PWM_BR-50)/PWM_Max+50;
}
/************************给相应的端口进行赋值******************************/
PWM_SetDuty(Mec_FL.port,PWM_FL);
PWM_SetDuty(Mec_FR.port,PWM_FR);
PWM_SetDuty(Mec_BL.port,PWM_BL);
PWM_SetDuty(Mec_BR.port,PWM_BR);
}
void PWM_Init(PWM_Settings_t PWM_Settings)
{
/* enable TIMER0 peripheral clock */
CMU_ClockEnable(cmuClock_TIMER0, true);
/* Setup Timer Channel Configuration for PWM */
TIMER_InitCC_TypeDef TimerCCInit = {
.eventCtrl = timerEventEveryEdge, /* this value will be ignored since we aren't using input capture */
.edge = timerEdgeNone, /* this value will be ignored since we aren't using input capture */
.prsSel = timerPRSSELCh0, /* this value will be ignored since we aren't using PRS */
.cufoa = timerOutputActionNone, /* no action on underflow (up-count mode) */
.cofoa = timerOutputActionSet, /* on overflow, we want the output to go high, but in PWM mode this should happen automatically */
.cmoa = timerOutputActionClear, /* on compare match, we want output to clear, but in PWM mode this should happen automatically */
.mode = timerCCModePWM, /* set timer channel to run in PWM mode */
.filter = false, /* not using input, so don't need a filter */
.prsInput = false, /* not using PRS */
.coist = false, /* initial state for PWM is high when timer is enabled */
.outInvert = false, /* non-inverted output */
};
/* Setup Timer Configuration for PWM */
TIMER_Init_TypeDef TimerPWMSetup =
{
.enable = true, /* start timer upon configuration */
.debugRun = true, /* run timer in debug mode */
.prescale = timerPrescale1, /* set prescaler to 1 */
.clkSel = timerClkSelHFPerClk, /* set clock source as HFPERCLK */
.fallAction = timerInputActionNone, /* no action from inputs */
.riseAction = timerInputActionNone, /* no action from inputs */
.mode = timerModeUp, /* use up-count mode */
.dmaClrAct = false, /* not using DMA */
.quadModeX4 = false, /* not using Quad Dec. mode */
.oneShot = false, /* not using one shot mode */
.sync = false, /* not synchronizing timer3 with other timers */
};
/* by default */
PWM_SetPeriod(1000ULL, 10000000ULL);
TIMER_CounterSet(TIMER0, 0); /* start counter at 0 (up-count mode) */
for (uint8_t CCx = 0; CCx < PWM_NUMBER_OF_CC_CHANNELS; CCx++)
{
PWM_SetDuty(CCx, 0);
TIMER_InitCC(TIMER0, CCx, &TimerCCInit); /* apply channel configuration to Timer0 channel 0 */
}
TIMER0->ROUTE = (PWM_Settings.location << _TIMER_ROUTE_LOCATION_SHIFT);
if (PWM_Settings.CC0_enable == true)
{
TIMER0->ROUTE |= TIMER_ROUTE_CC0PEN;
}
if (PWM_Settings.CC1_enable == true)
{
TIMER0->ROUTE |= TIMER_ROUTE_CC1PEN;
}
if (PWM_Settings.CC2_enable == true)
{
TIMER0->ROUTE |= TIMER_ROUTE_CC2PEN;
}
TIMER_Init(TIMER0, &TimerPWMSetup); /* apply PWM configuration to timer1 */
}
void PWM_TurnOff(PWM_CC_Channel CCx)
{
TIMER_CompareSet(TIMER0, CCx, 0);
TIMER_CompareBufSet(TIMER0, CCx, 0);
g_PWM_dutyCycle[CCx] = 0;
}
void PWM_SetDuty(PWM_CC_Channel CCx, uint32_t duty_miliPercents)
{
uint32_t duty = (uint32_t)(((uint64_t)g_PWM_Period * (uint64_t)duty_miliPercents) / 10000ULL);
TIMER_CompareSet(TIMER0, CCx, 0);
TIMER_CompareBufSet(TIMER0, CCx, duty);
g_PWM_dutyCycle[CCx] = duty;
}