#include "main.h" #include "headfile.h" unsigned int adc_value[5]; int error[2]; float kp = 1.1; float kd = 0.5; unsigned int left_motor_duty; unsigned int right_motor_duty; unsigned int motor_duty_center = 3000; int main(void) { car_init(); while(1) { adc_value[0] = adc_get(ADC0, ADC_CH_10); adc_value[1] = adc_get(ADC0, ADC_CH_11); adc_value[2] = adc_get(ADC0, ADC_CH_12); adc_value[3] = adc_get(ADC0, ADC_CH_13); adc_value[4] = adc_get(ADC0, ADC_CH_15); car_race(); } } void car_race() { error[0] = adc_value[0] - adc_value[4]; int speed_c = (int)(error[0] * kp + (error[0] + error[1]) * kd); left_motor_duty = motor_duty_center - speed_c; right_motor_duty = motor_duty_center + speed_c; error[1] = error[0]; if(left_motor_duty > 10000) { left_motor_duty = 10000; } else if(left_motor_duty < 0) { left_motor_duty = 0; } if(right_motor_duty > 10000) { right_motor_duty = 10000; } else if(right_motor_duty < 0) { right_motor_duty = 0; } motor_forward(left, left_motor_duty); motor_forward(right, right_motor_duty); } 修改为使用滑动平均值滤波算法的代码

#include "main.h"
#include "headfile.h"

unsigned int adc_value[5];
unsigned int adc_value_filtered[5];

int error[2];

float kp = 1.1;
float kd = 0.5;

unsigned int left_motor_duty;
unsigned int right_motor_duty;

unsigned int motor_duty_center = 3000;

void sliding_average_filter(unsigned int* adc_value, unsigned int* adc_value_filtered, int size, int index)
{
static unsigned int sum[5] = {0};
static unsigned int count = 0;

count++;
sum[index] += adc_value[index];
adc_value_filtered[index] = sum[index] / count;

if (count >= size)
{
count = 0;
sum[index] = 0;
}
}

int main(void)
{
car_init();

while(1)
{
adc_value[0] = adc_get(ADC0, ADC_CH_10);
adc_value[1] = adc_get(ADC0, ADC_CH_11);
adc_value[2] = adc_get(ADC0, ADC_CH_12);
adc_value[3] = adc_get(ADC0, ADC_CH_13);
adc_value[4] = adc_get(ADC0, ADC_CH_15);

sliding_average_filter(adc_value, adc_value_filtered, 5, 0);
sliding_average_filter(adc_value, adc_value_filtered, 5, 1);
sliding_average_filter(adc_value, adc_value_filtered, 5, 2);
sliding_average_filter(adc_value, adc_value_filtered, 5, 3);
sliding_average_filter(adc_value, adc_value_filtered, 5, 4);

car_race();
}
}

void car_race()
{
error[0] = adc_value_filtered[0] - adc_value_filtered[4];
int speed_c = (int)(error[0] * kp + (error[0] + error[1]) * kd);
left_motor_duty = motor_duty_center - speed_c;
right_motor_duty = motor_duty_center + speed_c;
error[1] = error[0];

if (left_motor_duty > 10000)
{
left_motor_duty = 10000;
}
else if (left_motor_duty < 0)
{
left_motor_duty = 0;
}
if (right_motor_duty > 10000)
{
right_motor_duty = 10000;
}
else if (right_motor_duty < 0)
{
right_motor_duty = 0;
}

motor_forward(left, left_motor_duty);
motor_forward(right, right_motor_duty);
}