程序
c
C
#include "zf_common_headfile.h"
#include "isr_config.h"
/* 使用类型A的电机控制器请注释掉下述代码 */
#define MOTOR_CONTROLLER_TYPE_B
/* 未实现 */
#define MOTOR_L_ROTATION 1
#define MOTOR_R_ROTATION -1
#define SERVO_MOTOR (ATOM1_CH1_P33_9) //舵机PWM通道
#define SERVO_MOTOR_FREQ (100) //舵机PWM频率
#define SERVO_MOTOR_MAX (110) //舵机最大绝对偏转角
#define SERVO_MOTOR_MIN (80) //舵机最小绝对偏转角
#define SERVO_MOTOR_MID (95) //舵机中心绝对偏转角
#define SERVO_MOTOR_DUTY(x) ((double)PWM_DUTY_MAX/(1000.0/(double)SERVO_MOTOR_FREQ)*(0.5+(double)(x)/90.0))
#define PIT0 (CCU60_CH0) //中断源
#define GEAR_RATIO (2320) //轮速编码器比
#define PID_MS (1) //两轮运算时间差
#define MAX_RPM (100)
#define WHEEL_CIRCUMFERENCE (6.7 * 3.14 * 0.01) // 轮子周长
#define FRONT_WHEEL_DISTANCE (0.152) // 前轮轴距
#define BACK_WHEEL_DISTANCE (0.152) // 后轮轴距
#define WHEELBASE (0.20) // 前后轮距离
#define MOTOR_MAX (60) // 最大PWM
/* 接口与通道定义 */
#define MOTORL_1 (ATOM0_CH4_P02_4)
#define MOTORL_2 (ATOM0_CH5_P02_5)
#define MOTORR_2 (ATOM1_CH6_P02_6)
#define MOTORR_1 (ATOM1_CH7_P02_7)
#define MOTOR_L (ATOM1_CH7_P02_7)
#define MOTOR_R (ATOM0_CH5_P02_5)
#define MOTOR_L_DIRECTION (P02_6)
#define MOTOR_R_DIRECTION (P02_4)
#define ENCODER1_DIR (TIM5_ENCODER)
#define ENCODER1_DIR_PULSE (TIM5_ENCODER_CH1_P10_3)
#define ENCODER1_DIR_DIR (TIM5_ENCODER_CH2_P10_1)
#define ENCODER2_DIR (TIM6_ENCODER)
#define ENCODER2_DIR_PULSE (TIM6_ENCODER_CH1_P20_3)
#define ENCODER2_DIR_DIR (TIM6_ENCODER_CH2_P20_0)
int32 Encoder1Count = 0;
int32 Encoder2Count = 0;
double Encoder1RPM = 0;
double Encoder2RPM = 0;
typedef struct {
double SetSpeed;
double ActualSpeed;
double CurrSpeed;
double Kp;
double Ki;
double Kd;
double Err;
double ErrLast;
double ErrPrev;
}pid_config_t;
pid_config_t MotorPid1 = {
.Kp = 0.8,
.Ki = 0,
.Kd = 2.5,
};
pid_config_t MotorPid2 = {
.Kp = 0.8,
.Ki = 0.0,
.Kd = 2.5,
};
double speed_test = 0.3;
double distance_test= 0.45;
double offset_test = 0;
double L = 0;
double R = 0;
void movControl(double speed, double distance, double offset);
void motorPwmExpect (double LPwm,double RPwm);
void motorPwmSet(double LPwm,double RPwm);
void servoAngleSet(double angle);
void motorPwmInit() {
pwm_init(SERVO_MOTOR, SERVO_MOTOR_FREQ, 0);
#ifdef MOTOR_CONTROLLER_TYPE_B
pwm_init(MOTOR_L, 17000, 0);
pwm_init(MOTOR_R, 17000, 0);
gpio_init(MOTOR_R_DIRECTION, GPO, GPIO_LOW, GPO_PUSH_PULL);
gpio_init(MOTOR_L_DIRECTION, GPO, GPIO_LOW, GPO_PUSH_PULL);
#else
pwm_init(MOTORL_1, 17000, 0);
pwm_init(MOTORL_2, 17000, 0);
pwm_init(MOTORR_1, 17000, 0);
pwm_init(MOTORR_2, 17000, 0);
#endif
encoder_dir_init(ENCODER1_DIR, ENCODER1_DIR_PULSE, ENCODER1_DIR_DIR);
encoder_dir_init(ENCODER2_DIR, ENCODER2_DIR_PULSE, ENCODER2_DIR_DIR);
motorPwmExpect(0,8);
pit_ms_init(PIT0, PID_MS);
while (TRUE) {
printf("1:%f\n",Encoder1RPM);
printf("2:%f\n",Encoder2RPM);
// motorPwmSet(L,R);
movControl(speed_test, distance_test, offset_test);
system_delay_ms(100);
}
}
/* 算法存在潜在问题，已废弃 */
//void movControl(double speed, double distance, double offset) {
// if (offset == 0) {
// servoAngleSet (0);
// motorPwmExpect (speed / WHEEL_CIRCUMFERENCE,1.0 / WHEEL_CIRCUMFERENCE * speed);
// return;
// }
// double Radius = sqrt(distance * distance + offset * offset) / 2 * 1.0 / cos (atan (distance / offset));
// double Delta = (atan (WHEELBASE / Radius)) * ((offset > 0) ? -1.0 : 1.0);
// Delta *= 180;
// Delta /= 3.1415926;
// servoAngleSet (Delta);
// double DeltaVPrime = speed * BACK_WHEEL_DISTANCE / Radius;
// DeltaVPrime /= 2;
// double LeftWheelRPM = (speed + ((offset > 0) ? DeltaVPrime : -DeltaVPrime)) / WHEEL_CIRCUMFERENCE;
// double RightWheelRPM = 1.0 / WHEEL_CIRCUMFERENCE * (speed + ((offset < 0) ? DeltaVPrime : -DeltaVPrime));
// motorPwmExpect (LeftWheelRPM,RightWheelRPM);
//}
void movControl(double speed, double distance, double offset)
{
if (offset == 0) {
servoAngleSet (0);
motorPwmExpect (speed / WHEEL_CIRCUMFERENCE,1.0 / WHEEL_CIRCUMFERENCE * speed);
return;
}
double Radius = (distance * distance + offset * offset) / (2 * offset);
double Delta = atan(WHEELBASE / Radius);
servoAngleSet (Delta * -180.0 / 3.1415926);
double DeltaSpeed = speed * BACK_WHEEL_DISTANCE / Radius / 2.0;
double LeftWheelRPM = (speed + DeltaSpeed) / WHEEL_CIRCUMFERENCE;
double RightWheelRPM = 1.0 / WHEEL_CIRCUMFERENCE * (speed - DeltaSpeed);
motorPwmExpect (LeftWheelRPM,RightWheelRPM);
}
void servoAngleSet(double angle) {
angle /= 3;
angle += SERVO_MOTOR_MID;
double angle1 = angle;
if (angle1 > SERVO_MOTOR_MAX) angle1 = SERVO_MOTOR_MAX;
if (angle1 < SERVO_MOTOR_MIN) angle1 = SERVO_MOTOR_MIN;
pwm_set_duty(SERVO_MOTOR, SERVO_MOTOR_DUTY(angle1));
}
void motorPwmExpect (double LPwm,double RPwm) {
MotorPid1.SetSpeed = LPwm;
MotorPid2.SetSpeed = RPwm;
}
void motorPwmSet(double LPwm,double RPwm) {
LPwm = (LPwm > MOTOR_MAX) ? MOTOR_MAX : LPwm ;
LPwm = (LPwm < -MOTOR_MAX) ? -MOTOR_MAX : LPwm ;
RPwm = (RPwm > MOTOR_MAX) ? MOTOR_MAX : RPwm ;
RPwm = (RPwm < -MOTOR_MAX) ? -MOTOR_MAX : RPwm ;
#ifdef MOTOR_CONTROLLER_TYPE_B
pwm_set_duty(MOTOR_L, ((LPwm >= 0) ? LPwm : -LPwm) * 100);
pwm_set_duty(MOTOR_R, ((RPwm >= 0) ? RPwm : -RPwm) * 100);
gpio_set_level(MOTOR_L_DIRECTION, (LPwm >= 0) ? 0 : 1);
gpio_set_level(MOTOR_R_DIRECTION, (RPwm >= 0) ? 1 : 0);
#else
if (LPwm >= 0) {
pwm_set_duty(MOTORL_1, LPwm * 100);
pwm_set_duty(MOTORL_2, 0);
} else {
pwm_set_duty(MOTORL_1, 0);
pwm_set_duty(MOTORL_2, -LPwm * 100);
}
if (RPwm >= 0) {
pwm_set_duty(MOTORR_1, RPwm * 100);
pwm_set_duty(MOTORR_2, 0);
} else {
pwm_set_duty(MOTORR_1, 0);
pwm_set_duty(MOTORR_2, -RPwm * 100);
}
#endif
}
void pidCalc(pid_config_t* Pid) {
Pid->ErrPrev = Pid->ErrLast;
Pid->ErrLast = Pid->Err;
Pid->Err = Pid->SetSpeed - Pid->CurrSpeed;
double Delta = Pid->Kd * (Pid->Err - Pid->ErrLast)
+ Pid->Kp * Pid->Err
+ Pid->Ki * (Pid->Err - 2 * Pid->ErrLast + Pid->ErrPrev);
Pid->ActualSpeed += Delta;
if (Pid->ActualSpeed > MAX_RPM)
Pid->ActualSpeed = MAX_RPM;
if (Pid->ActualSpeed < -MAX_RPM)
Pid->ActualSpeed = -MAX_RPM;
}
IFX_INTERRUPT(cc60_pit_ch0_isr, 0, CCU6_0_CH0_ISR_PRIORITY) {
interrupt_global_enable(0);
pit_clear_flag(CCU60_CH0);
Encoder1Count = encoder_get_count(ENCODER1_DIR);
Encoder2Count = encoder_get_count(ENCODER2_DIR);
encoder_clear_count(ENCODER1_DIR);
encoder_clear_count(ENCODER2_DIR);
Encoder1RPM = 1.0 * Encoder1Count / GEAR_RATIO * (1000/PID_MS);
Encoder2RPM = 1.0 * Encoder2Count / GEAR_RATIO * (1000/PID_MS);
MotorPid1.CurrSpeed = Encoder1RPM;
MotorPid2.CurrSpeed = -Encoder2RPM;
pidCalc(&MotorPid1);
pidCalc(&MotorPid2);
motorPwmSet(MotorPid1.ActualSpeed,MotorPid2.ActualSpeed);
}
h
C
#ifndef MOTOR_H
#define MOTOR_H
void motorPwmInit();
void movControl(double speed, double distance, double offset);
#endif
Comments NOTHING