131 lines
2.5 KiB
C
131 lines
2.5 KiB
C
#include "work.h"
|
|
#include "board.h"
|
|
|
|
work_t work;
|
|
|
|
static void pwm_map(void)
|
|
{
|
|
set_motor_pwm(work.pwm_percent);
|
|
}
|
|
|
|
static void motor_pos(void)
|
|
{
|
|
work.pid.sub_type = PID_SUB_TYPE_POSITION;
|
|
}
|
|
|
|
static void motor_speed(void)
|
|
{
|
|
work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
|
|
}
|
|
|
|
static void pwm_map_key_handle(button_id_e id)
|
|
{
|
|
int8_t min = -100, max = 100;
|
|
int8_t step = 10;
|
|
switch (id)
|
|
{
|
|
case KEY_ADD:
|
|
if (work.pwm_percent < max)
|
|
{
|
|
work.pwm_percent += step;
|
|
}
|
|
break;
|
|
case KEY_SUB:
|
|
if (work.pwm_percent > min)
|
|
{
|
|
work.pwm_percent -= step;
|
|
}
|
|
break;
|
|
case KEY_S:
|
|
work.pwm_percent = 0;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void motot_pos_key_handle(button_id_e id)
|
|
{
|
|
}
|
|
|
|
static void motor_speed_key_handle(button_id_e id)
|
|
{
|
|
}
|
|
|
|
void work_key_handle_cb(button_id_e id)
|
|
{
|
|
|
|
switch (work.type)
|
|
{
|
|
case WORK_PWM_MAP:
|
|
pwm_map_key_handle(id);
|
|
break;
|
|
case WORK_MOTOR_POS:
|
|
motot_pos_key_handle(id);
|
|
break;
|
|
case WORK_MOTOR_SPEED:
|
|
motor_speed_key_handle(id);
|
|
break;
|
|
default:
|
|
DBG_ASSERT(FALSE __DBG_LINE);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void work_process(void)
|
|
{
|
|
switch (work.type)
|
|
{
|
|
case WORK_PWM_MAP:
|
|
pwm_map();
|
|
break;
|
|
case WORK_MOTOR_POS:
|
|
motor_pos();
|
|
break;
|
|
case WORK_MOTOR_SPEED:
|
|
motor_speed();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return;
|
|
}
|
|
|
|
void work_encoder_exti(void)
|
|
{
|
|
if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin))
|
|
{
|
|
work.encoder_cnt++;
|
|
}
|
|
else
|
|
{
|
|
work.encoder_cnt--;
|
|
}
|
|
}
|
|
|
|
void work_init(void)
|
|
{
|
|
osel_memset((uint8_t *)&work, 0, sizeof(work_t));
|
|
PWM_START(PWM_TIM, PWM_CHANNEL);
|
|
PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, 0);
|
|
work.pwm_feq = PWM_GET_FREQ(PWM_TIM);
|
|
|
|
work.type = WORK_PWM_MAP;
|
|
work.pid_params.kp = 100;
|
|
work.pid_params.ki = 0.01;
|
|
work.pid_params.kd = 50;
|
|
work.pid_params.dead_zone = 0;
|
|
// PID初始化
|
|
|
|
{
|
|
work.pid.type = PID_TYPE_FUZZY;
|
|
work.pid.sub_type = PID_SUB_TYPE_POSITION;
|
|
work.pid.pid_u.fuzzy.deadzone_dir = DEAD_ZONE_BOTH;
|
|
pid_constructor(&work.pid);
|
|
work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
|
|
work.pid_params.kd, work.pid_params.dead_zone, 0, -100, 100);
|
|
work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, TRUE);
|
|
}
|
|
return;
|
|
}
|