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.