Накидал простенький ПИД-регулятор для того чтобы поддерживать заданную скорость робота с двумя ведущими колесами. Формулы брал тут. Он вроде бы и работает, но я не могу его никак подобрать ему коэффициенты, постоянно то с места сдвинуться не может, то едет рывками, то разгоняется за несколько секунд до максимальных оборотов. Гляньте пожалуйста, мб я где нибудь глупую ошибку допустил в коде.
// main.c
#include "asuro.h"
#include "def.h"
#include "pid.h"
extern uint16_t left_speed = 16;
extern uint16_t right_speed = 16;
extern uint8_t pid_enable = 1;
int main(void)
{
Init();
EncoderInit();
EncoderStart();
// main loop
while(1) {
process_pid();
}
return 0;
}
// def.h
#ifndef DEF_H
#define DEF_H
// PID regulator coefficients
#define kP 5
#define kI 0
#define kD 0
// PID regulator settings
#define PID_INTERVAL 100
#define MOTOR_MAX_SPEED 255
#define MOTOR_MIN_SPEED 0
#endif // DEF_H
// pid.h
#ifndef PID_H
#define PID_H
#include "asuro.h"
#include "def.h"
uint16_t left_speed = 0;
uint16_t right_speed = 0;
uint8_t pid_enable = 0;
static uint32_t prev_pid_time = 0;
static uint16_t left_prev_error = 1;
static uint16_t right_prev_error = 1;
static float left_prev_i = 1;
static float right_prev_i = 1;
void process_pid(void);
static uint8_t pid_compute(int current, int must_be, uint16_t *prev_error, float *prev_i);
#endif // PID_H
// pid.c
#include "pid.h"
void process_pid(void)
{
if(!pid_enable) return;
if(Gettime() > prev_pid_time){
if(Gettime() - prev_pid_time >= PID_INTERVAL){
uint8_t l_pwm = pid_compute(encoder[LEFT], left_speed, &left_prev_error, &left_prev_i);
uint8_t r_pwm = pid_compute(encoder[RIGHT], right_speed, &right_prev_error, &right_prev_i);
MotorSpeed(l_pwm, r_pwm);
encoder[LEFT] = 0;
encoder[RIGHT] = 0;
prev_pid_time = Gettime();
}
} else {
if(0xFF - prev_pid_time + Gettime() + 1 >= PID_INTERVAL){
uint8_t l_pwm = pid_compute(encoder[LEFT], left_speed, &left_prev_error, &left_prev_i);
uint8_t r_pwm = pid_compute(encoder[RIGHT], right_speed, &right_prev_error, &right_prev_i);
MotorSpeed(l_pwm, r_pwm);
encoder[LEFT] = 0;
encoder[RIGHT] = 0;
prev_pid_time = Gettime();
}
}
}
static uint8_t pid_compute(int current, int must_be, uint16_t *prev_error, float *prev_i)
{
float p, i, d;
uint8_t output;
int16_t error = must_be - current;
p = kP * error;
i = *prev_i + (kI * error);
d = kD * (current - *prev_error);
output = p + i + d;
if(output > MOTOR_MAX_SPEED) output = MOTOR_MAX_SPEED;
if(output < MOTOR_MIN_SPEED) output = MOTOR_MIN_SPEED;
*prev_i = i;
*prev_error = error;
return output;
}
