60 lines
1.6 KiB
C++
60 lines
1.6 KiB
C++
#include "headers.h"
|
|
#include <PID_v1.h>
|
|
|
|
double angKp=0.0444, angKi=0.6666, angKd=0.001;
|
|
double posKp=0.8, posKi=0.2, posKd=1;
|
|
double turnKp=0.014, turnKi=0.0333, turnKd=0.0;
|
|
|
|
|
|
double angleInput, angleOutput, angleSetpoint;
|
|
PID anglePID(&angleInput, &angleOutput, &angleSetpoint, angKp, angKi, angKd, P_ON_M, REVERSE);
|
|
|
|
double posInput, posOutput, posSetpoint;
|
|
PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, P_ON_E, DIRECT);
|
|
|
|
double turnInput, turnOutput, turnSetpoint;
|
|
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT);
|
|
|
|
PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
|
|
|
|
const float MAX_TURN_SPEED = 0.15;
|
|
|
|
void initPID(){
|
|
angleSetpoint = 0;
|
|
anglePID.SetOutputLimits(-1, 1); // speed forward/backward
|
|
anglePID.SetMode(AUTOMATIC);
|
|
anglePID.SetSampleTime(5);
|
|
|
|
posSetpoint = 0;
|
|
posPID.SetOutputLimits(-2, 2); // degrees forward/backward
|
|
posPID.SetMode(AUTOMATIC);
|
|
posPID.SetSampleTime(5);
|
|
posPID.SetControllerDirection(DIRECT);
|
|
|
|
turnSetpoint = 0;
|
|
turnPID.SetOutputLimits(-MAX_TURN_SPEED, MAX_TURN_SPEED); // speed forward/backward
|
|
turnPID.SetMode(AUTOMATIC);
|
|
turnPID.SetSampleTime(5);
|
|
}
|
|
|
|
|
|
Speeds updatePID(){
|
|
posPID.Compute();
|
|
angleSetpoint = posOutput;
|
|
anglePID.Compute();
|
|
|
|
turnSetpoint = 0;
|
|
float maxTurn = max(0.0f, MAX_TURN_SPEED-abs((float)angleOutput)/90);
|
|
turnPID.SetOutputLimits(-maxTurn, maxTurn);
|
|
turnPID.Compute();
|
|
|
|
Speeds speeds;
|
|
speeds.left = (float)(angleOutput + turnOutput);
|
|
speeds.right = (float)(-angleOutput + turnOutput);
|
|
if(angleInput>20 || angleInput<-20){
|
|
speeds.left = 0;
|
|
speeds.right = 0;
|
|
}
|
|
return speeds;
|
|
}
|