mrm-board.h

#pragma once
 
#include "Arduino.h"
#include <mrm-can-bus.h>
#include <mrm-common.h>
#include <mrm-pid.h>
#include <vector>
 
#define COMMAND_SENSORS_MEASURE_CONTINUOUS 0x10
#define COMMAND_SENSORS_MEASURE_ONCE 0x11
#define COMMAND_SENSORS_MEASURE_STOP 0x12
#define COMMAND_SENSORS_MEASURE_SENDING 0x13
#define COMMAND_SENSORS_MEASURE_CONTINUOUS_REQUEST_NOTIFICATION 0x14
#define COMMAND_SENSORS_MEASURE_CONTINUOUS_AND_RETURN_CALCULATED_DATA 0x15
#define COMMAND_SENSORS_MEASURE_CALCULATED_SENDING 0x16
#define COMMAND_SENSORS_MEASURE_CONTINUOUS_VERSION_2 0x17
#define COMMAND_SENSORS_MEASURE_CONTINUOUS_VERSION_3 0x18
#define COMMAND_FIRMWARE_REQUEST 0x19
#define COMMAND_FIRMWARE_SENDING 0x1A
#define COMMAND_RESET 0x1B
#define COMMAND_MESSAGE_SENDING_1 0x1C
#define COMMAND_MESSAGE_SENDING_2 0x1D
#define COMMAND_MESSAGE_SENDING_3 0x1E
#define COMMAND_MESSAGE_SENDING_4 0x1F
#define COMMAND_SPEED_SET 0x20
#define COMMAND_SPEED_SET_REQUEST_NOTIFICATION 0x21
#define COMMAND_DUPLICATE_ID_PING 0x22
#define COMMAND_DUPLICATE_ID_ECHO 0x23
#define COMMAND_INFO_REQUEST 0x24
#define COMMAND_INFO_SENDING_1 0x25
#define COMMAND_INFO_SENDING_2 0x26
#define COMMAND_INFO_SENDING_3 0x27
#define COMMAND_FPS_REQUEST 0x30
#define COMMAND_FPS_SENDING 0x31
#define COMMAND_ID_CHANGE_REQUEST 0x40
#define COMMAND_NOTIFICATION 0x41
#define COMMAND_OSCILLATOR_TEST 0x43
#define COMMAND_ERROR 0xEE
#define COMMAND_REPORT_ALIVE 0xFF
 
#define MAX_MOTORS_IN_GROUP 4
 
#ifndef toRad
#define toRad(x) ((x) / 180.0 * PI) // Degrees to radians
#endif
#ifndef toDeg
#define toDeg(x) ((x) / PI * 180.0) // Radians to degrees
#endif
 
enum BoardId{ID_MRM_8x8A, ID_ANY, ID_MRM_BLDC2X50, ID_MRM_BLDC4x2_5, ID_MRM_COL_B, ID_MRM_COL_CAN, ID_MRM_FET_CAN, ID_MRM_IR_FINDER_2, 
    ID_MRM_IR_FINDER3, ID_MRM_IR_FINDER_CAN, ID_MRM_LID_CAN_B, ID_MRM_LID_CAN_B2, ID_MRM_MOT2X50, ID_MRM_MOT4X3_6CAN, ID_MRM_MOT4X10, 
    ID_MRM_NODE, ID_MRM_REF_CAN, ID_MRM_SERVO, ID_MRM_SWITCH, ID_MRM_THERM_B_CAN, ID_MRM_US, ID_MRM_US_B, ID_MRM_US1};
 
enum BoardType{MOTOR_BOARD, SENSOR_BOARD};
 
class Robot;
 
class Board{
protected:
    uint32_t _alive; // Responded to ping, maximum 32 devices of the same class, stored bitwise.
    bool _aliveReport = false;
    char _boardsName[12];
    BoardType _boardType; // To differentiate derived boards
    uint8_t canData[8]; // Array used to store temporary CAN Bus data
    uint8_t devicesOnABoard; // Number of devices on a single board
    //std::vector<bool>(maxNumberOfBoards * devicesOn1Board) deviceStarted; //todo - not to allow reading if the device not started.
    uint8_t errorCode = 0;
    uint8_t errorInDeviceNumber = 0;
    std::vector<uint16_t>* fpsLast; // FPS local copy.
    BoardId _id;
    std::vector<uint32_t>* idIn;  // Inbound message id
    std::vector<uint32_t>* idOut; // Outbound message id
    std::vector<uint32_t>* lastMessageReceivedMs;
    std::vector<uint32_t>* _lastReadingMs;
    uint8_t maximumNumberOfBoards;
    uint8_t measuringMode = 0;
    uint8_t measuringModeLimit = 0;
    uint8_t _message[29]; // Message a device sent.
    std::vector<char[10]>* _name;// Device's name
    int nextFree;
    Robot* robotContainer;
 
    bool messageDecodeCommon(uint32_t canId, uint8_t data[8], uint8_t deviceNumber = 0);
 
public:
    
    Board(Robot* robot, uint8_t maxNumberOfBoards, uint8_t devicesOnABoard, const char * boardName, BoardType boardType, BoardId id);
 
    void add(char* deviceName, uint16_t canIn, uint16_t canOut);
 
    bool alive(uint8_t deviceNumber = 0, bool checkAgainIfDead = false, bool errorIfNotAfterCheckingAgain = false);
 
    uint8_t count();
 
    void aliveSet(bool yesOrNo, uint8_t deviceNumber = 0);
 
    BoardType boardType(){ return _boardType; }
 
    uint8_t deadOrAliveCount();
 
    uint8_t devicesOnASingleBoard() { return this->devicesOnABoard; }
 
    uint8_t devicesMaximumNumberInAllBoards() { return this->devicesOnABoard * this->maximumNumberOfBoards; }
 
    uint8_t devicesScan(bool verbose = true, uint16_t mask = 0xFFFF);
 
    uint8_t errorCodeLast() { return errorCode; }
 
    uint8_t errorWasInDeviceNumber() { return errorInDeviceNumber; }
 
    void firmwareRequest(uint8_t deviceNumber = 0xFF);
 
    uint16_t fps(uint8_t deviceNumber = 0);
 
    void fpsDisplay();
 
    void fpsRequest(uint8_t deviceNumber = 0xFF);
 
    BoardId id() { return _id; }
 
    void idChange(uint16_t newDeviceNumber, uint8_t deviceNumber = 0);
 
    void info(uint8_t deviceNumber = 0xFF);
 
    bool isForMe(uint32_t canIdOut, uint8_t deviceNumber);
 
    bool isFromMe(uint32_t canIdOut, uint8_t deviceNumber);
 
    uint32_t lastMessageMs(uint8_t deviceNumber = 0) { return (*lastMessageReceivedMs)[deviceNumber]; }
 
    virtual bool messageDecode(uint32_t canId, uint8_t data[8]) = 0;
 
    bool messagePrint(uint32_t msgId, uint8_t dlc, uint8_t * data, bool outbound);
 
    void messageSend(uint8_t* data, uint8_t dlc, uint8_t deviceNumber = 0);
 
    char *name(uint8_t deviceNumber);
 
    char* name() {return _boardsName;}
 
    void notificationRequest(uint8_t commandRequestingNotification, uint8_t deviceNumber);
 
    void oscillatorTest(uint8_t deviceNumber = 0xFF);
 
    void reset(uint8_t deviceNumber = 0xFF);
 
    void start(uint8_t deviceNumber = 0xFF, uint8_t measuringModeNow = 0, uint16_t refreshMs = 0);
 
    void swap(uint8_t deviceNumber1, uint8_t deviceNumber2);
 
    void stop(uint8_t deviceNumber = 0xFF);
 
    virtual void test(uint8_t deviceNumber = 0xFF, uint16_t betweenTestsMs = 0) {}
};
 
 
 
class MotorBoard : public Board {
protected:
    std::vector<uint32_t>* encoderCount; // Encoder count
    std::vector<bool>* reversed; // Change rotation
    std::vector<int8_t>* lastSpeed;
public:
 
    MotorBoard(Robot* robot, uint8_t devicesOnABoard, const char * boardName, uint8_t maxNumberOfBoards, BoardId id);
 
    void directionChange(uint8_t deviceNumber);
 
    bool messageDecode(uint32_t canId, uint8_t data[8]);
 
    uint16_t reading(uint8_t deviceNumber);
 
    void readingsPrint();
 
    void speedSet(uint8_t motorNumber, int8_t speed);
 
    void stop();
 
    void test(uint8_t deviceNumber = 0xFF, uint16_t betweenTestsMs = 0);
};
 
 
 
 
class SensorBoard : public Board {
public:
    SensorBoard(Robot* robot, uint8_t devicesOnABoard, const char* boardName, uint8_t maxNumberOfBoards, BoardId id);
 
    void continuousReadingCalculatedDataStart(uint8_t deviceNumber = 0xFF);
 
    virtual bool messageDecode(uint32_t canId, uint8_t data[8]){return false;}
};
 
//typedef void (*SpeedSetFunction)(uint8_t motorNumber, int8_t speed);
 
class MotorGroup {
protected:
    MotorBoard* motorBoard[MAX_MOTORS_IN_GROUP] = { NULL, NULL, NULL, NULL }; // Motor board for each wheel. It can the same, but need not be.
    uint8_t motorNumber[MAX_MOTORS_IN_GROUP];
    Robot* robotContainer;
 
    float angleNormalized(float angle);
public:
    MotorGroup(Robot* robot);
 
    void stop();
};
 
class MotorGroupDifferential : public MotorGroup {
private:
    int16_t checkBounds(int16_t speed);
 
public: 
    MotorGroupDifferential(Robot* robot, MotorBoard* motorBoardForLeft1, uint8_t motorNumberForLeft1, MotorBoard* motorBoardForRight1, uint8_t motorNumberForRight1,
        MotorBoard* motorBoardForLeft2 = NULL, uint8_t motorNumberForLeft2 = 0, MotorBoard* motorBoardForRight2 = NULL, uint8_t motorNumberForRight2 = 0);
 
    void go(int16_t leftSpeed = 0, int16_t rightSpeed = 0, int16_t lateralSpeedToRight = 0, uint8_t speedLimit = 127);
};
 
class MotorGroupStar : public MotorGroup {
public:
    MotorGroupStar(Robot* robot, MotorBoard* motorBoardFor45Degrees, uint8_t motorNumberFor45Degrees, MotorBoard* motorBoardFor135Degrees, uint8_t motorNumberFor135Degrees,
        MotorBoard* motorBoardForMinus135Degrees, uint8_t motorNumberForMinus135Degrees, MotorBoard* motorBoardForMinus45Degrees, uint8_t motorNumberForMinus45Degrees);
 
    void go(float speed, float angleDegrees = 0, float rotation = 0, uint8_t speedLimit = 127);
    
    void goToEliminateErrors(float errorX, float errorY, float headingToMaintain, Mrm_pid* pidXY, Mrm_pid* pidRotation, bool verbose = false);
};