xrp-style-wpilib-comms.h

Go to the documentation of this file.

 
#pragma once
 
// add includes when you add new message types
#include "message_types/xrp_accel.h"
#include "message_types/xrp_analog.h"
#include "message_types/xrp_dio.h"
#include "message_types/xrp_encoder.h"
#include "message_types/xrp_gyro.h"
#include "message_types/xrp_motor.h"
#include "message_types/xrp_servo.h"
 
#include <Arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <cstdint>
#include <vector>
 
#include "message_type.h"
 
#define UDP_PACKET_MAX_SIZE_XRP 1000 // I think the rpi pico xrp firmware uses 8192, but that's absurdly large
 
class XSWC {
protected:
    class MessageTypeFactory {
    public:
        static MessageType* createMessageType(uint8_t tag)
        {
            // add cases when you add new message types
            switch (tag) {
            case XRP_TAG_MOTOR:
                return new XrpMotor();
            case XRP_TAG_SERVO:
                return new XrpServo();
            case XRP_TAG_DIO:
                return new XrpDio();
            case XRP_TAG_ANALOG:
                return new XrpAnalog();
            case XRP_TAG_GYRO:
                return new XrpGyro();
            case XRP_TAG_ACCEL:
                return new XrpAccel();
            case XRP_TAG_ENCODER:
                return new XRPEncoder();
            default:
                return nullptr;
            }
        }
    };
 
public:
    // methods to receive data (add methods when you add new message types)
 
    bool getData_xrp_motor(xrp_motor_t& data, const uint8_t id)
    {
        return getData<xrp_motor_t>(data, id);
    }
    float getValue_xrp_motor(const uint8_t id)
    {
        xrp_motor_t data;
        if (getData_xrp_motor(data, id))
            return data.value;
        return 0.0f;
    }
    bool getData_xrp_servo(xrp_servo_t& data, const uint8_t id)
    {
        return getData<xrp_servo_t>(data, id);
    }
    float getValue_xrp_servo(const uint8_t id)
    {
        xrp_servo_t data;
        if (getData_xrp_servo(data, id))
            return data.value;
        return 0.0f;
    }
    bool getData_xrp_dio(xrp_dio_t& data, const uint8_t id)
    {
        return getData<xrp_dio_t>(data, id);
    }
    bool getValue_xrp_dio(const uint8_t id)
    {
        xrp_dio_t data;
        if (getData_xrp_dio(data, id))
            return data.value == 1;
        return false;
    }
    bool getData_xrp_analog(xrp_analog_t& data, const uint8_t id)
    {
        return getData<xrp_analog_t>(data, id);
    }
    float getValue_xrp_analog(const uint8_t id)
    {
        xrp_analog_t data;
        if (getData_xrp_analog(data, id))
            return data.value;
        return 0.0f;
    }
 
    // methods to send data (add methods when you add new message types)
    bool sendData_xrp_dio(const xrp_dio_t& data, bool checkUniqueness = false)
    {
        return sendData<xrp_dio_t>(data, checkUniqueness);
    }
    bool sendValue_xrp_dio(const uint8_t id, bool value, bool checkUniqueness = false)
    {
        xrp_dio_t data;
        data.id = id;
        data.value = value ? 1 : 0; // 1 for true, 0 for false
        return sendData_xrp_dio(data, checkUniqueness);
    }
 
    bool sendData_xrp_analog(const xrp_analog_t& data, bool checkUniqueness = false)
    {
        return sendData<xrp_analog_t>(data, checkUniqueness);
    }
 
    bool sendValue_xrp_analog(const uint8_t id, float value, bool checkUniqueness = false)
    {
        xrp_analog_t data;
        data.id = id;
        data.value = value;
        return sendData_xrp_analog(data, checkUniqueness);
    }
 
    bool sendData_xrp_encoder(const xrp_encoder_t& data, bool checkUniqueness = false)
    {
        return sendData<xrp_encoder_t>(data, checkUniqueness);
    }
 
    bool sendValue_xrp_encoder(const uint8_t id, int32_t count, int32_t period = 0, int32_t divisor = 1, bool checkUniqueness = false)
    {
        xrp_encoder_t data;
        data.id = id;
        data.count = count;
        data.period = period;
        data.divisor = divisor;
        return sendData_xrp_encoder(data, checkUniqueness);
    }
 
    bool sendData_xrp_gyro(const xrp_gyro_t& data, bool checkUniqueness = false)
    {
        return sendData<xrp_gyro_t>(data, checkUniqueness);
    }
 
    bool sendValue_xrp_gyro(float xRate, float yRate, float zRate, float roll, float pitch, float yaw, bool checkUniqueness = false)
    {
        xrp_gyro_t data;
        data.rates[0] = xRate;
        data.rates[1] = yRate;
        data.rates[2] = zRate;
        data.angles[0] = roll;
        data.angles[1] = pitch;
        data.angles[2] = yaw;
        return sendData_xrp_gyro(data, checkUniqueness);
    }
 
    bool sendData_xrp_accel(const xrp_accel_t& data, bool checkUniqueness = false)
    {
        return sendData<xrp_accel_t>(data, checkUniqueness);
    }
 
    bool sendValue_xrp_accel(const uint8_t id, float xAccel, float yAccel, float zAccel, bool checkUniqueness = false)
    {
        xrp_accel_t data;
        data.accels[0] = xAccel;
        data.accels[1] = yAccel;
        data.accels[2] = zAccel;
        return sendData_xrp_accel(data, checkUniqueness);
    }
 
    XSWC();
 
    bool begin(void (*_receiveCallback)(void), void (*_sendCallback)(void), uint16_t port = 3540);
 
    bool begin(const char* ssid, const char* password, void (*_receiveCallback)(void), void (*_sendCallback)(void), const char* hostname = "XRP-XSWC", uint16_t port = 3540);
 
    bool update();
 
    bool isConnected();
    bool isEnabled();
    bool isConnectedAndEnabled();
 
    unsigned long TIMEOUT_MS = 1000;
    unsigned long MIN_UPDATE_TIME_MS = 50; // 20Hz
 
    bool useAP = false;
 
protected:
    // recall data from list of received messages
    template <typename T>
    bool getData(T& data, const uint8_t id)
    {
        for (MessageType* msg : receivedMessages) {
            if (msg->getTag() == TYPE_TO_TAG_VAL(T) && (msg->hasId() == false || msg->getId() == id)) {
                // Check if we've found a message of the correct type and with the correct ID
                data = *static_cast<T*>(msg->getData());
                return true; // Data found
            }
        }
        return false; // No data found
    }
 
    // save data into list of messages to send
    template <typename T>
    bool sendData(T data, bool checkUniqueness)
    {
        MessageType* message = nullptr;
        if (checkUniqueness) {
            // search sent messages for a message with tag and id
            for (MessageType* msg : sentMessages) {
                if (msg->getTag() == TYPE_TO_TAG_VAL(T)) {
                    if constexpr (HAS_ID(T) == false) {
                        // Found a message of the correct type (and no ID is needed for this type)
                        message = msg; // overwrite it
                        break;
                    } else {
                        if (msg->getId() == data.id) {
                            // Found a message of the correct type and with the correct ID
                            message = msg; // overwrite it
                            break;
                        }
                    }
                }
            }
        }
        if (message == nullptr) {
            // No existing message found, create a new one
            message = MessageTypeFactory::createMessageType(TYPE_TO_TAG_VAL(T));
        }
        if (message != nullptr) {
            message->setData(&data);
            sentMessages.push_back(message);
            return true; // Data successfully added to the list
        }
        return false;
    }
 
    boolean
    processReceivedBufferIntoMessages(char* buffer, int length);
    int processMessagesIntoBufferToSend(char* buffer, int length);
 
    WiFiUDP udp; // UDP instance for communication
 
    std::vector<MessageType*> receivedMessages;
    std::vector<MessageType*> sentMessages;
 
    boolean cmdEnable = false;
 
    unsigned long millisWhenLastMessageReceived = -TIMEOUT_MS;
    unsigned long millisWhenLastSent = -MIN_UPDATE_TIME_MS;
 
    uint16_t txSeq = 0;
 
    char rxBuf[UDP_PACKET_MAX_SIZE_XRP + 1];
    char txBuf[UDP_PACKET_MAX_SIZE_XRP + 1];
 
    bool connectedToRemote = false;
    IPAddress udpRemoteAddr = IPAddress();
    int32_t udpRemotePort = -1;
 
    void (*sendCallback)(void);
    void (*receiveCallback)(void);
}; // end class XSWC
 
extern XSWC xswc; // a global instance is created in the .cpp file