Encoder for Rpm help!

[rbmj]

It's pretty easy actually. WPILib makes this easy for you.

We actually had two motors, so...

template<class T>
T coerce(T val, T min, T max) {
    if (val < min) {
        return min;
    }
    else if (val > max) {
        return max;
    }
    else {
        return val;
    }
}

class two_jags : public PIDOutput {
private:
    Jaguar& jag1;
    Jaguar& jag2;
    float speed;
public:
    two_jags(Jaguar& a, Jaguar& b) : jag1(a), jag2(b), speed(0.0) {}
    void PIDWrite(float output) {
        speed = coerce(output + speed, -1.0f, 1.0f);
        jag1.PIDWrite(speed);
        jag2.PIDWrite(speed);
    }
    void reset() {
        speed = 0.0;
    }
};

Some integration code (note that we were working with a counter hooked up to a hall effect sensor detecting a single magnet in the wheel, so one tick was one rotation):

two_jags jags(jag1, jag2);
freq_pid freq(cnt);
PIDController shooter_ctl(0.005, 0.0, 0.002, freq, jags);

shooter_ctl.SetTolerance(4.0); //accept a 4% steady state error
shooter_ctl.SetInputRange(0.0, 75.0); //our max freq was 75 hz
shooter_ctl.SetOutputRange(-0.25, 0.25); //only ramp a little at a time

//...
void enable_shooters(float freq) {
    jags.reset(); //zero out the accumulator
    shooter_ctl.SetSetpoint(freq);
    shooter_ctl.Enable();
}

You can do more stuff, like adding in a startup burst to get it close and then enable the PID controller to fine tune the speed. We ended up with a quarter second burst of full power before we started the PID controller. Make sure you set the starting speed (two_jags::speed) to your startup speed before, otherwise you'll put the motors back to zero power and defeat the whole purpose of the start speed.