Hello everyone! My team and I have been trying to program a position-based PID for our drivetrain for the past two days, and we have been struggling with it. It is giving us inconsistent results every time, which is confusing us.
In the code, we have two motor groups defined: LeftMotorGroup and RightMotorGroup. There are also two rotation sensors defined: LeftRotationSensor and RightRotationSensor.
What we tried to do is give the position in inches as a target for the PID, and then we would take the derivative with deltaTime and then convert that into motor rpm. Someone, please tell me if this approach is wrong.
pid.h:
#ifndef PIDCALC_H
#define PIDCALC_H
#include <cmath>
#include "vex.hpp"
class PIDCalc
{
public:
PIDCalc(double kp, double ki, double kd, double integralBound);
inline void SetTarget(double target) { m_Target = target; }
inline float GetComputed() const { return m_ComputeResult; }
void Calculate(double actual);
private:
const double c_KP;
const double c_KI;
const double c_KD;
const double c_IntegralBound; // Avoid oscillation of integral
double m_Target;
double m_Integral;
double m_LastError;
double m_ComputeResult;
};
class PIDController
{
public:
static void Start();
static void Stop();
static void SetLeftTarget(double target) { s_LeftPID.SetTarget(target); }
static void SetRightTarget(double target) { s_RightPID.SetTarget(target); }
static void DriveForward(double dist);
private:
static constexpr double s_WheelC = 3.25 * M_PI;
static double _PositionToDistance(rotation& rot);
static double _ComputeSpeed(double dt, double curr, double last);
static PIDCalc s_LeftPID;
static PIDCalc s_RightPID;
static bool s_Running;
};
#endif
pid.cpp:
#include "pid.hpp"
#include <iostream>
PIDCalc PIDController::s_LeftPID(1, 1, 1, 127);
PIDCalc PIDController::s_RightPID(1, 1, 1, 127);
bool PIDController::s_Running = false;
PIDCalc::PIDCalc(double kp, double ki, double kd, double integralBound)
: c_KP(kp), c_KI(ki), c_KD(kd), c_IntegralBound(integralBound) {}
void PIDCalc::Calculate(double actual)
{
double error = m_Target - actual;
/* Proportional*/ double proportional = error * c_KP;
/* Integral */ m_Integral += error;
if (m_Integral > c_IntegralBound)
{
m_Integral = c_IntegralBound;
}
else if (m_Integral < -c_IntegralBound)
{
m_Integral = -c_IntegralBound;
}
double integral = m_Integral * c_KI;
/* Derivative */ double derivative = error - m_LastError;
m_LastError = error;
m_ComputeResult = proportional + integral + derivative;
}
double PIDController::_PositionToDistance(rotation &rot)
{
return rot.position(turns) * s_WheelC;
}
double PIDController::_ComputeSpeed(double dt, double curr, double last)
{
float dv = (curr - last) / dt;
float minuteDist = dv * 60;
float rpm = minuteDist / s_WheelC;
return rpm;
}
void PIDController::Start()
{
s_Running = true;
thread runningThread = thread([]() -> int
{
LeftRotationSensor.resetPosition();
RightRotationSensor.resetPosition();
double lastLeft = 0;
double lastRight = 0;
timer dtTimer;
while (s_Running)
{
float dt = dtTimer.time(sec);
s_LeftPID.Calculate(_PositionToDistance(LeftRotationSensor));
s_RightPID.Calculate(_PositionToDistance(RightRotationSensor));
LeftMotorGroup.spin(forward, _ComputeSpeed(dt, s_LeftPID.GetComputed(), lastLeft), rpm);
RightMotorGroup.spin(forward, _ComputeSpeed(dt, s_RightPID.GetComputed(), lastRight), rpm);
lastLeft = s_LeftPID.GetComputed();
lastRight = s_RightPID.GetComputed();
dtTimer.reset();
wait(20, msec);
}
LeftMotorGroup.stop();
RightMotorGroup.stop();
return 0; });
}
void PIDController::Stop() { s_Running = false; }
void PIDController::DriveForward(double dist)
{
SetLeftTarget(dist);
SetRightTarget(dist);
}
Thank you for all your help 