CANdleSensor.h

 
#pragma once
 
#include <bitset>
#include <iostream>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/core/CoreCANdle.hpp>
#include <frc/TimedRobot.h>
 
#include "valkyrie/sensors/BaseSensor.h"
 
namespace valor {
 
class CANdleSensor : public valor::BaseSensor<int>, public ctre::phoenix6::hardware::core::CoreCANdle {
   public:
    static constexpr frc::Color8Bit VALOR_GOLD{0xEE, 0xA8, 0x00};
    static constexpr frc::Color8Bit VALOR_PURPLE{0xFF, 0x00, 0xFF};
    static constexpr frc::Color8Bit RED{0xFF, 0x00, 0x00};
    static constexpr frc::Color8Bit ORANGE{0xFF, 0x30, 0x00};
    static constexpr frc::Color8Bit GREEN{0x00, 0xFF, 0x00};
    static constexpr frc::Color8Bit BLUE{0x00, 0x00, 0xFF};
    static constexpr frc::Color8Bit LIGHT_BLUE{0xFF, 0x00, 0xF9};
    static constexpr frc::Color8Bit WHITE{0xFF, 0xFF, 0xFF};
    static constexpr frc::Color8Bit OFF{0x00, 0x00, 0x00};
 
    enum Priority { LOW, MEDIUM, HIGH };
 
    static frc::Color8Bit cancoderMagnetHealthGetter(ctre::phoenix6::hardware::CANcoder* cancoder);
 
    CANdleSensor(frc::TimedRobot& robot, int ledCount, int segments, int canID, std::string canbus = "baseCAN");
 
    void setBrightness(double brightness, bool saveImmediately = true);
    void setStripType(ctre::phoenix6::signals::StripTypeValue stripType, bool saveImmediately = true);
 
    void setLED(int led, frc::Color8Bit color);
 
    void setColor(int segment, frc::Color8Bit color, Priority priority = LOW);
 
    void setColor(frc::Color8Bit color, Priority priority = LOW);
 
    template <typename T>
    void setAnimation(int segment, frc::Color8Bit color, Priority priority = LOW) {
        if (priority < currentPriority)
            return;
        currentPriority = priority;
        if (!segments[segment].animationSlot) {
            for (size_t i = 0; i < slots.size(); i++) {
                if (!slots.test(i)) {
                    segments[segment].animationSlot = i;
                    slots.set(i);
                    break;
                }
            }
        }
        auto controlRequest = std::make_unique<T>(segments[segment].startLed, segments[segment].endLed);
        if constexpr (requires(T& t) { t.Color; })
            controlRequest->Color = color;
        controlRequest->Slot = segments[segment].animationSlot.value();
        segments[segment].controlRequest = std::move(controlRequest);
    }
 
    template <typename T>
    void setAnimation(frc::Color8Bit color, Priority priority = LOW) {
        if (priority < currentPriority)
            return;
        currentPriority = priority;
        slots.reset();
        slots.set(0);
        segments[0].animationSlot = 0;
        for (size_t i = 1; i < segments.size(); i++) {
            segments[i].animationSlot.reset();
            segments[i].controlRequest.reset();
        }
        auto controlRequest = std::make_unique<T>(segments[1].startLed, segments.back().endLed);
        controlRequest->Slot = 0;
        if constexpr (requires(T& t) { t.Color; })
            controlRequest->Color = color;
        segments[0].controlRequest = std::move(controlRequest);
    }
 
    void clearAnimation(int segment);
 
    void reset() override;
 
   private:
    struct Segment {
        std::unique_ptr<ctre::phoenix6::controls::ControlRequest> controlRequest; 
        std::optional<int> animationSlot;                                         
        int startLed;                                                             
        int endLed;                                                               
    };
 
    Priority currentPriority;      
    std::vector<Segment> segments; 
    void calculate() override;
 
    std::bitset<8> slots; 
    ctre::phoenix6::configs::CANdleConfiguration config;
};
}  // namespace valor