I copied the relevant parts from my teams code for the flywheel. Feel free to just copy and use this code but remember to adjust the values for your robot.
PID class I wrote
#pragma once
class pid
{
public:
double min = 0, max = 0;
double Kp = 0, Ki = 0, Kd = 0;
double slew = 0;
double negativeSlew = 0;
int updateMin = 100; //in milliseconds
pid(double Kp, double Ki, double Kd, double min, double max, double slew);
double calculate(double value = 0);
void setTarget(double target);
void setPowerPreSlew(double powerPreSlew);
void setPower(double power);
double getPower();
double getError();
double getChange();
void clear();
private:
double target = 0;
int step = 0;
double error = 0, integral = 0, derivative = 0;
double powerPreSlew = 0;
double power = 0;
double preError = 0;
long lastUpdate = -1;
};
…
#include "pid.hpp"
pid::pid(double Kp, double Ki, double Kd, double min, double max, double slew) :
Kp(Kp), Ki(Ki), Kd(Kd), min(min), max(max), slew(slew)
{
if(min > max) std::swap(min, max);
}
double pid::calculate(double value)
{
long currentTime = pros::millis(); //replace with way to get current time in milliseconds in vex coding studio
bool timeData = lastUpdate != -1;
bool timeUpdate = (currentTime - lastUpdate) >= updateMin;
double uM = (currentTime - lastUpdate) / 1000.0; //for update per second
if(timeUpdate || !timeData) lastUpdate = currentTime;
switch(step)
{
case 0:
error = target - value;
if(timeData && timeUpdate) integral += error * uM; //add error once per second
if(timeData && timeUpdate) derivative = (error - preError) / uM; //change per second
if(timeUpdate) preError = error;
//pid
powerPreSlew = Kp * error + Ki * integral + Kd * derivative;
//min/max
if(powerPreSlew > max) powerPreSlew = max;
if(powerPreSlew < min) powerPreSlew = min;
case 1:
//slew
if(timeData && timeUpdate)
{
double change = powerPreSlew - power;
if(change > slew * uM && slew != 0) change = slew * uM;
if(change < -negativeSlew * uM && negativeSlew != 0) change = -negativeSlew * uM;
else if(change < -slew * uM && slew != 0) change = -slew * uM;
power += change;
}
}
return power;
}
void pid::setTarget(double targetI)
{
target = targetI;
step = 0;
}
void pid::setPowerPreSlew(double powerPreSlewI)
{
powerPreSlew = powerPreSlewI;
step = 1;
}
void pid::setPower(double powerI)
{
power = powerI;
step = 2;
}
double pid::getPower()
{
return power;
}
double pid::getError()
{
return error;
}
double pid::getChange()
{
return derivative;
}
void pid::clear()
{
target = 0;
step = 0;
error = 0;
derivative = 0;
powerPreSlew = 0;
power = 0;
preError = 0;
lastUpdate = -1;
}
pid flywheelPID1(1.8, 0, 0, -127, 127, 127);
pid flywheelPID2(0.15, 0, 0, -127, 127, 127);
double flywheelTargetSpeed;
double flywheelActualSpeed;
double flywheelPower;
then put this in your loop…
//set flywheelActualSpeed to flywheel speed
flywheelPID1.setTarget(flywheelTargetSpeed);
flywheelPID2.setTarget(flywheelTargetSpeed);
flywheelPower = flywheelPID1.calculate(flywheelActualSpeed);
if(fabs(flywheelPID1.getError()) < 0.15 * 600.0) //threshold (0.15 * 600 = 15% of max speed which is 600)
{
flywheelPower = flywheelPID2.calculate(flywheelActualSpeed) + flywheelSpeed * 1.2; //You must adjust the 1.2 value
//The flywheelSpeed * 1.2 is the holding power
flywheelPID1.setPower(flywheelPower);
}
else
{
flywheelPID2.clear();
}
//set the motor power to flywheelPower
The 1.2 values is the holding power. To find the value for your robot run the flywheel at one power and recorded what the speed settles at (For example a power of 50 might result in 350 rpm); after doing that for some different values find an equation to approximate it. For us it was flywheelSpeed * 1.2 .
Hope this helps.