7842F Flywheel Control System

emaFilter.cpp

#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; }

emaFilter.hpp

#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);
};

FlywheelImplementation.cpp

#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);
}

miscUtils.hpp

#pragma once

template <typename T> int sgn(T val) {
  return (T(0) < val) - (val < T(0));
}

pidVelSystem.cpp

#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; }

pidVelSystem.hpp

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