GripperFinger.h

//
// Created by Romain Pouvreau on 14/04/2026.
//
 
#ifndef GRIPPERFINGER_H
#define GRIPPERFINGER_H
 
#include <Wire.h>

namespace HazelnutGripper
{
    struct ColorData {
        uint16_t clear; 
        uint16_t red;   
        uint16_t green; 
        uint16_t blue;  
    };

    enum class OperationMode : uint8_t {
        Standby    = 0x01, 
        SingleRead = 0x02, 
        Continuous = 0x03  
    };

    class GripperFinger
    {
    public:
        GripperFinger(TwoWire* wire, uint8_t address = 0x24);

        void enableStatusLed(bool enable);

        void enableActionLed(bool enable);

        void setAngle(uint8_t servoIndex, uint8_t degrees);

        void open();

        void close();

        void stop();

        bool isMoving();

        bool isTargetReached();

        bool isFaulty();

        void setSensingMode(OperationMode mode);

        bool hasNewColorData();

        ColorData getColor();
 
    private:
        uint8_t _addr;
 
        TwoWire* _wire;
 
        void _write8(uint8_t cmd, uint8_t value);
        uint8_t _read8(uint8_t cmd);
        uint16_t _read16(uint8_t cmd);

        enum Register : uint8_t {
            REG_YELLOW_LED     = 0x01,
            REG_BLUE_LED       = 0x02,
            REG_SERVO_1_POS    = 0x03,
            REG_SERVO_2_POS    = 0x04,
            REG_MOTOR_TARGET   = 0x05,
            REG_MOTOR_STATUS   = 0x06,
            REG_COLOR_INT_TIME = 0x07,
            REG_COLOR_GAIN     = 0x08,
            REG_COLOR_STATUS   = 0x09,
            REG_COLOR_MODE     = 0x0A,
            REG_COLOR_C_HIGH   = 0x0B,
            REG_COLOR_R_HIGH   = 0x0D,
            REG_COLOR_G_HIGH   = 0x0F,
            REG_COLOR_B_HIGH   = 0x11
        };
    };
}
#endif //GRIPPERFINGER_H