Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
7842F Flywheel Control System
#include "emaFilter.hpp"
emaFilter::emaFilter(double alpha) : m_alpha(alpha) {}
double emaFilter::filter(double readIn) {
m_output = m_alpha * readIn + (1.0 - m_alpha) * m_outputOld;
m_outputOld = m_output;
return m_output;
}
double emaFilter::filter(double readIn, double alpha) {
m_alpha = alpha;
return filter(readIn);
}
void emaFilter::setGains(double alpha) { m_alpha = alpha; }
#pragma once
class emaFilter {
private:
double m_alpha = 1.0;
double m_output = 0.0;
double m_outputOld = 0.0;
public:
emaFilter(double);
double filter(double);
double filter(double, double);
void setGains(double);
};
#include "main.h"
#include "pidVelSystem.hpp"
Motor flywheel(1);
double flywheelRatio = 15;
velPID pid(0.35, 0.05, 0.045, 0.9);
emaFilter rpmFilter(0.15);
double motorSlew = 0.7;
double targetRPM = 0;
double currentRPM = 0;
double lastPower = 0;
double motorPower = 0;
while(true) {
currentRPM = rpmFilter.filter(flywheel.getActualVelocity() * flywheelRatio);
motorPower = pid.calculate(targetRPM, currentRPM);
if(motorPower <= 0) motorPower = 0; //Prevent motor from spinning backward
//Give the motor a bit of a starting boost
if(motorPower > lastPower && lastPower < 10 && motorPower > 10) lastPower = 10;
//This slews the motor by limiting the rate of change of the motor speed
double increment = motorPower - lastPower;
if(std::abs(increment) > motorSlew) motorPower = lastPower + (motorSlew * sgn(increment));
lastPower = motorPower;
flywheel.move(motorPower);
//std::cout << "RPM: " << currentRPM << " Power: "<< motorPower << " Error: "<< flywheelPID.getError() << "\n";
pros::delay(20);
}
#pragma once
template <typename T> int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
#include "pidVelSystem.hpp"
velPID::velPID(double Kp, double Kd, double Kf, double emaAlpha)
: m_dFilter(emaAlpha) {
m_Kp = Kp;
m_Kd = Kd;
m_Kf = Kf;
}
double velPID::calculate(double wantedRPM, double currentRPM) {
m_Error = wantedRPM - currentRPM;
m_derivative = (m_Error - m_lastError);
m_lastError = m_Error;
if(m_derivative < 0) m_derivative;
m_derivative = m_dFilter.filter(m_derivative);
double finalPower = (m_Error * m_Kp) + (m_derivative * m_Kd) + (wantedRPM * m_Kf);
if(std::abs(finalPower) > 127) { finalPower = sgn(finalPower) * 127; }
return finalPower;
}
void velPID::setGains(double Kp, double Kd, double Kf, double emaAlpha) {
m_Kp = Kp;
m_Kd = Kd;
m_Kf = Kf;
m_dFilter.setGains(emaAlpha);
}
double velPID::getError() { return m_Error; }
#pragma once
#include "main.h"
#include "emaFilter.hpp"
#include "miscUtils.hpp"
class velPID {
private:
double m_Kp = 0;
double m_Kd = 0;
double m_Kf = 0;
double m_Error = 0;
double m_lastError = 0;
double m_derivative = 0;
emaFilter m_dFilter;
public:
velPID(double, double, double, double);
double calculate(double, double);
void setGains(double, double, double, double);
double getError();
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.