Flywheel problems (coding) PLEASE HELP (im desperate!)

Flywheel rpm keeps oscillating. It will not stay consistent. On the motor details section, the flywheel dot (on that one graph with the power on y axis and rpm on x axis) keeps going up and down.
Flywheel code:
if(Controller1.ButtonR1.pressing()) {

    //-90, 90 is max, good for top flag shooting/full court
if(Controller1.ButtonR2.pressing())  {
else {

Well firstly, this is on VCS I suppose and the integrated PID on VCS does not fully stabilize a flywheel. You would have to either tune your own PID or TBH control loop. Also, you are running your motors at 45% their capabilities which leads me to believe you have a lot of friction in the flywheel system if it can’t go higher. Also, why do you have the flywheel motor spinning in 2 opposite directions? You need to further explain your code if you want any real help.


They are spinning in opposite directions because if they spun in the same direction, they would be pushing against each other. They are running at 45% because I want to double shot, and because higher than that will go to high. (I can do 100%, but it hits my ceiling)

Does it shoot consistently? It could just be some technobabble error with the rpm output fluctuating, despite consistent targeting.

This kind of seems like a mechanical problem to me. There might be a bent axle or something similar causing spikes in speed (due to inertia and potential->kinetic energy)

Pictures might help us to diagnose-or perhaps rule out-some problems.

Im pretty sure it is not structual, but I may be wrong. If I had the guess, the cortex is attempting to find the correct rpm, but overshoots it, then overcompensates for this, causing it to fall under the targeted rpm. Eventually, the flywheel does maintain a constant rpm (after about 20 seconds without firing balls), but once a ball is fired (causing the rpm of the flywheel to temorarily change) the rpm starts oscillating again. Is there any way to fix this? I think a PID loop might work, but I have no idea how to make it.
Sooo, if anyone knows how to make a PID loop please share

Sorry, not a coder. Wish I could do more. But that’s just how rpm is set. I’m sure there’s some way to avoid such overcompensation, but I don’t know what it is.

It would’ve been helpful to mention that you were using a double flywheel setup. Also, sorry to break it to you, but you won’t be able to consistently have a double shot with your code. It’s simply not possible without a moving hood, at least not with a double flywheel set up.

We don’t have a double flywheel setup.

So you have a 2 motor flywheel, no double flywheel setup?

yes, sorry for the confusion

The built in v5 pid isn’t very good especially for reaching a target speed. If you want it to be fast and not overshoot the target speed you will want to use you own pid.

For my robot the flywheel control was really simple. When the error was larger than some threshold it used a p controller with slew control. Then when the error was below the threshold it applied a holding power and a p controller with a small Kp value.
To find the holding power we ran the flywheel at one power and recorded what the speed settled at; after doing that for some different values we found an equation to approximate it. For us it was flywheelSpeed * 1.2 .

This is my team’s code. It’s very big but there is a pid class with slew control which you can use.

Also having a doing double shot with just a flywheel requires the flywheel to spin up or down after the first ball which could take too long. Many teams have a macro that launches a ball drive forward and launches another ball.


Thanks so much! We were able to double shot at certain speeds, but since the variability of the flywheel was so high, we could not implement it. Could you explain what you mean when you say “Many teams have a macro that launches a ball drive forward and launches another ball.”

1 Like

They click a button and robot runs the intake untill the program somehow detects when the ball launches. Then the intake stop, the robot drives forward a little and finally launches another ball. This isn’t easy to program though.

Also if your robot can simply launch two balls and the first ball launches slows the flywheel down enough to launch the second ball at the right speed to hit the middle flag, then you could do that. This does requires a consistent flywheel pid and a correctely weighted flywheel.

I could not figure out how to integrate the PID onto our flywheel. Please help (if possible).

In what way do you mean?

to try to make the velocity of the flywheel consistent. Our flywheel’s speed oscillates so much that it makes it extremely hard to accurately make shots (as stated earlier).

Ok, so that is a tuning issue…
Otherwise the integration, like updating your constants, works?

For tuning, there’s no great fast way to do it. If it’s oscillating try lowering P a little and I a lot. Maybe cut D also, then work them back up. Wiki has a good page on tuning methods if you want a method other than trial and error.

P is like the initial push (to get to velocity), I is the boost at the end, and D is the breaks. Thinking about it this way, they shouldn’t “overlap”; they fight each other and that causes the oscillations.

Im an kinda confused. How exactly can I tune these things?

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
        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();

        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;

        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;

            powerPreSlew = Kp * error + Ki * integral + Kd * derivative;

            if(powerPreSlew > max) powerPreSlew = max;
            if(powerPreSlew < min) powerPreSlew = min;
        case 1:

            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


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

//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.


For tuning the loop to your bot, I’d recommend you start with this page;
the Manual Tuning section has great graph gif of how PIand D affect each other. There’s a lot of math in this page that you don’t need to study but tuning methods and the overview are worth a read.

Like I said, there is no quick way to do this, so if you’re really crunched on time, consider taking out I and or D to simplify the process. If you just use P it’s a lot easier to avoid endless oscillation loops, but takes longer to level out at desired speed. (Keeping flywheel on constantly can help this, tho can cause battery issues)