BaseController.h

 
#pragma once
 
#include <memory>
#include <string>
 
#include <ctre/phoenix6/CANBus.hpp>
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/TalonFX.hpp>
#include <frc/smartdashboard/SendableChooser.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/system/plant/DCMotor.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/current.h>
#include <units/voltage.h>
#include <frc/simulation/DCMotorSim.h>
 
#include "valkyrie/Loggable.h"
#include "valkyrie/controllers/PIDF.h"
 
namespace valor {
 
enum NeutralMode { Brake, Coast };
 
class BaseController : public valor::Loggable {
   public:
    explicit BaseController(frc::DCMotor _motorSpec)
          : motorSpec{_motorSpec}, rotorToSensor(1), sensorToMech(1), voltageCompensation(units::volt_t{12.0}) {
        tuningModeChooser.AddOption("Current", TuningMode::Current);
        tuningModeChooser.AddOption("Duty Cycle", TuningMode::DutyCycle);
        tuningModeChooser.AddOption("Speed", TuningMode::Speed);
        tuningModeChooser.AddOption("Position", TuningMode::Position);
        tuningModeChooser.SetDefaultOption("Voltage", TuningMode::Voltage);
        opModeChooser.AddOption("Tuning", OperationalMode::Tuning);
        opModeChooser.AddOption("Non Operational", OperationalMode::NonOperational);
        opModeChooser.SetDefaultOption("Operational", OperationalMode::Operational);
 
        LogChild("Tuning Mode", &tuningModeChooser);
        LogChild("Operational Mode", &opModeChooser);
    }
 
    void OnLoggingStart() override {
        WriteLog("Tuning Setpoint", 0.0);
        WriteLog("Tune", false);
    }
 
    virtual void applyConfig() = 0;
 
    enum class OperationalMode { NonOperational, Tuning, Operational };
    enum class TuningMode { Voltage, Current, DutyCycle, Speed, Position };
 
    OperationalMode getOpMode() { return opModeChooser.GetSelected(); }
 
    TuningMode getTuneMode() { return tuningModeChooser.GetSelected(); }
 
    units::turns_per_second_t getMaxMechSpeed() { return motorSpec.freeSpeed / (rotorToSensor * sensorToMech); }
 
    void setVoltageCompensation(units::volt_t _voltageCompensation) { voltageCompensation = _voltageCompensation; }
 
    units::volt_t getVoltageCompensation() { return voltageCompensation; }
 
    virtual void reset() = 0;
 
    virtual units::turn_t getPosition() = 0;
 
    virtual units::ampere_t getCurrent() = 0;
 
    virtual units::turns_per_second_t getSpeed() = 0;
 
    virtual void setEncoderPosition(units::turn_t position) = 0;
 
    virtual void positionSetter(units::turn_t position, int slot = 0) = 0;
 
    void setPosition(units::turn_t position, int slot = 0) {
        if (getOpMode() == OperationalMode::Operational)
            positionSetter(position, slot);
        else if (getOpMode() == OperationalMode::Tuning && ReadLog<bool>("Tune"))
            handleTuningMode();
        else
            powerSetter(0_V);
    }
 
    virtual void setPositionWithFeedForward(units::turn_t position, units::turns_per_second_t feedforwardVelocity, int slot = 0) {
        // Default: ignore feedforward, just call normal position setter
        setPosition(position, slot);
    }
 
    virtual void speedSetter(units::turns_per_second_t speed, int slot = 0) = 0;
 
    void setSpeed(units::turns_per_second_t speed, int slot = 0) {
        if (getOpMode() == OperationalMode::Operational)
            speedSetter(speed, slot);
        else if (getOpMode() == OperationalMode::Tuning && ReadLog<bool>("Tune"))
            handleTuningMode();
        else
            powerSetter(0_V);
    }
 
    virtual void powerSetter(units::volt_t voltage) { assert(false); }
    virtual void powerSetter(units::ampere_t current) { assert(false); }
    virtual void powerSetter(double dutyCycle) { assert(false); }
 
    template <typename T>
    void setPower(T power) {
        if (getOpMode() == OperationalMode::Operational)
            powerSetter(static_cast<T>(power));
        else if (getOpMode() == OperationalMode::Tuning && ReadLog<bool>("Tune"))
            handleTuningMode();
        else
            powerSetter(0_V);
    }
 
    virtual void setupFollower(int canID, bool followerInverted = false) = 0;
 
    virtual void setPIDF(valor::PIDF pidf, int slot = 0, bool saveImmediately = false) = 0;
 
    void setLimits(units::turn_t reverse, units::turn_t forward, bool saveImmediately = false) {
        setForwardLimit(forward, saveImmediately);
        setReverseLimit(reverse, saveImmediately);
    }
 
    virtual void setForwardLimit(units::turn_t forward, bool saveImmediately = false) = 0;
 
    virtual void setReverseLimit(units::turn_t reverse, bool saveImmediately = false) = 0;
 
    virtual void setGearRatios(double rotorToSensor, double sensorToMech, bool saveImmediately = false) = 0;
 
    virtual void setOpenLoopRamp(units::second_t time, bool saveImmediately = false) = 0;
 
    virtual units::turn_t getAbsEncoderPosition() = 0;
 
    virtual units::volt_t calculateAppliedVoltage() = 0;
    virtual void setSimState(frc::sim::DCMotorSim&) = 0;
 
    frc::DCMotor motorSpec;
    double rotorToSensor;
    double sensorToMech;
 
   protected:
    units::volt_t voltageCompensation;
 
   private:
    void handleTuningMode() {
        double setpoint = ReadLog<double>("Tuning Setpoint");
        switch (tuningModeChooser.GetSelected()) {
            case TuningMode::Voltage:
                return powerSetter(units::volt_t{setpoint});
            case TuningMode::Current:
                return powerSetter(units::ampere_t{setpoint});
            case TuningMode::DutyCycle:
                return powerSetter(setpoint);
            case TuningMode::Speed:
                return speedSetter(units::turns_per_second_t{setpoint});
            case TuningMode::Position:
                return positionSetter(units::turn_t{setpoint});
        }
    }
 
    frc::SendableChooser<TuningMode> tuningModeChooser;
    frc::SendableChooser<OperationalMode> opModeChooser;
};
}  // namespace valor