robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

ADD: dummy foc

Browse source 
SIMPLE

CLEAN: cleanup code and add printf handler

FIX: counters

ADD: motor

ENH: temporary disable can

ADD: dummy foc, but creepy work
This commit is contained in:
vanyabeat 2024-02-06 11:32:46 +03:00 • committed by Igor Brylyov
parent 3af1d1a710
commit bd7f049b75
20 changed files with 629 additions and 567 deletions
Download patch file Download diff file Expand all files Collapse all files

82
firmware/Lib/pid/pid.c Normal file
View file

@ -0,0 +1,82 @@
#include "pid.h"
#include <stdint.h>
#include <stddef.h>
#define LimitMax(input, max) \
{ \
if (input > max) \
{ \
input = max; \
} \
else if (input < -max) \
{ \
input = -max; \
} \
}
void pid_init(pid_type_t *pid, uint8_t mode, float Kp, float Ki, float Kd, float max_out, float max_iout)
{
if (pid == NULL)
{
return;
}
pid->mode = mode;
pid->Kp = Kp;
pid->Ki = Ki;
pid->Kd = Kd;
pid->max_out = max_out;
pid->max_iout = max_iout;
pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;
}
float pid_calc(pid_type_t *pid, float ref, float set)
{
if (pid == NULL)
{
return 0.0f;
}
pid->error[2] = pid->error[1];
pid->error[1] = pid->error[0];
pid->set = set;
pid->fdb = ref;
pid->error[0] = set - ref;
if (pid->mode == PID_POSITION)
{
pid->Pout = pid->Kp * pid->error[0];
pid->Iout += pid->Ki * pid->error[0];
pid->Dbuf[2] = pid->Dbuf[1];
pid->Dbuf[1] = pid->Dbuf[0];
pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
pid->Dout = pid->Kd * pid->Dbuf[0];
LimitMax(pid->Iout, pid->max_iout);
pid->out = pid->Pout + pid->Iout + pid->Dout;
LimitMax(pid->out, pid->max_out);
}
else if (pid->mode == PID_DELTA)
{
pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
pid->Iout = pid->Ki * pid->error[0];
pid->Dbuf[2] = pid->Dbuf[1];
pid->Dbuf[1] = pid->Dbuf[0];
pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
pid->Dout = pid->Kd * pid->Dbuf[0];
pid->out += pid->Pout + pid->Iout + pid->Dout;
LimitMax(pid->out, pid->max_out);
}
return pid->out;
}
void pid_clear(pid_type_t *pid)
{
if (pid == NULL)
{
return;
}
pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;
pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;
pid->fdb = pid->set = 0.0f;
}