Did stuff

common-libaries/gobot_rpc/CMakeLists.txt

@@ -0,0 +1,10 @@
+idf_component_register(SRCS
+                        "gobot_rpc_sender.cpp"
+                        "gobot_rpc_receiver.cpp"
+                        "gobot_rpc_statemaschine.cpp"
+                        "gobot_rpc_numberMap.cpp"
+                        "gobot_rpc_transiver.cpp"
+                    REQUIRES mcp2521
+                    REQUIRES mcp2521_hardware_interface
+                    REQUIRES driver
+                    INCLUDE_DIRS "include")

common-libaries/gobot_rpc/gobot_rpc_numberMap.cpp

@@ -0,0 +1,19 @@
+#include "gobot_rpc.hpp"
+#include "protocol_spec.hpp"
+#include <string.h>
+
+GobotRPCNumberMap::GobotRPCNumberMap() {
+    memset(rpc_number_map, 0xffffffff, 16);
+}
+
+GobotRPCNumberMap::~GobotRPCNumberMap() {
+
+}
+
+void GobotRPCNumberMap::set(RpcNum rpc_num, uint32_t id) {
+    rpc_number_map[(int)rpc_num] = id;
+}
+
+uint32_t GobotRPCNumberMap::get(RpcNum rpc_num) {
+    return rpc_number_map[(int)rpc_num];
+}

common-libaries/gobot_rpc/gobot_rpc_receiver.cpp

@@ -0,0 +1,43 @@
+#include <string.h>
+#include "gobot_rpc.hpp"
+#include "sm.hpp"
+#include "protocol_spec.hpp"
+
+#include "freertos/FreeRTOS.h"
+
+GobotRPCReciver::GobotRPCReciver(MCP2521 * can_interface) {
+    this->can_interface = can_interface;
+    this->state_mashine = GobotRPCStateMashine();
+
+    can_interface->register_rx0_handler(onRX0_GoboRPC, can_interface);
+}
+
+
+void onRX0_GoboRPC(void *arg) {
+    GobotRPCReciver *rpc_socket = (GobotRPCReciver *)arg;
+    rpc_socket->onRX();
+}
+
+void GobotRPCReciver::onRX() {
+    rx_info info = can_interface->get_rx_id(MCP2521_RX_BUFFER::RXB0);
+    uint8_t data[8];
+
+    can_interface->read_rx_buf(MCP2521_RX_BUFFER::RXB0, MCP2521_BUFFER_TYPE::DATA, data, info.length);
+
+    RPCFrame frame;
+    memcpy(((char *)data) + 1, (char *)frame.data, 7);
+    frame.header = (RPCHeader)(data[0]);
+
+    SMResult smResult;
+    state_mashine.submitFrame(&frame, &smResult);
+
+    if(smResult.done) {
+        
+    }
+}
+
+void GobotRPCReciver::registerOnDoneFrameHandler(GobotRPCHandler_t handler, void * arg) {
+    onDoneFrameHandler = handler;
+    argOnDoneFrameHandler = arg;
+}
+

common-libaries/gobot_rpc/gobot_rpc_statemaschine.cpp

@@ -0,0 +1,74 @@
+#include "sm.hpp"
+#include "protocol_spec.hpp"
+#include <string.h>
+
+GobotRPCStateMashine::GobotRPCStateMashine() {
+    memset(requestDataBuffer, 0, REQUEST_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
+    memset(responseDataBuffer, 0, RESPONSE_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
+    memset(errorDataBuffer, 0, RESPONSE_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
+
+    memset(requestSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
+    memset(responseSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
+    memset(errorSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
+
+    slotCounter = 0;
+    rpc_number_map = GobotRPCNumberMap();
+}
+
+void GobotRPCStateMashine::registerRPC(RpcNum rpc_num) {
+    rpc_number_map.set(rpc_num, slotCounter);
+    slotCounter = (slotCounter + 1) % SLOTS;
+}
+
+void GobotRPCStateMashine::submitFrame(RPCFrame * frame, SMResult * result) {
+    RpcNum rpcNum = (RpcNum)(frame->header.rpc_num);
+
+    result->done = false;
+    result->rpc_num = rpcNum;
+
+    if(rpcNum == RpcNum::INVALID)
+        return;
+
+    size_t slot = rpc_number_map.get(rpcNum);
+    if (slot == 0xffffffff)
+        return;
+
+    uint8_t segNum = frame->header.rpc_segement;
+
+    if(frame->header.error) {
+        memcpy(result->data, frame->data, 7);
+        errorSegmentArrivedFlags[slot] |= (1 << segNum);
+
+        result->done = true;
+        result->type = RPCPackageType::ERROR;
+
+    } else if(frame->header.response) {
+        memcpy(responseDataBuffer[slot] + segNum, frame->data, 7);
+        responseSegmentArrivedFlags[slot] |= (1 << segNum);
+        
+        uint8_t mask = SEGMENT_MASK_RESPONSE[(int)rpcNum];
+
+        if(((responseSegmentArrivedFlags[slot] & mask) ^ mask) == 0) {
+            responseSegmentArrivedFlags[slot] = 0;
+
+            result->done = true;
+            memcpy(result->data, responseDataBuffer[slot], 7*4);
+            result->type = RPCPackageType::RESPONSE;
+        }
+
+    } else {
+        memcpy(requestDataBuffer[slot] + segNum, frame->data, 7);
+        requestSegmentArrivedFlags[slot] |= (1 << segNum);
+        
+        uint8_t mask = SEGMENT_MASK_RESPONSE[(int)rpcNum];
+        
+        if(((requestSegmentArrivedFlags[slot] & mask) ^ mask) == 0) {
+            requestSegmentArrivedFlags[slot] = 0;
+
+            result->done = true;
+            memcpy(result->data, requestDataBuffer[slot], 7*4);
+            result->type = RPCPackageType::REQUEST;
+        }
+    }
+
+}

common-libaries/gobot_rpc/include/gobot_rpc.hpp

@@ -0,0 +1,56 @@
+#pragma once
+#include "protocol_spec.hpp"
+#include "mcp2521.hpp"
+#include "sm.hpp"
+
+#include "freertos/FreeRTOS.h"
+
+
+
+typedef void (*GobotRPCHandler_t)(void *);
+typedef void (*GobotRPCFrameHandler)(void *, RPCFrame *);
+
+class GobotRPCReciver {
+protected:
+    MCP2521 * can_interface;
+    GobotRPCStateMashine state_mashine;
+
+    GobotRPCHandler_t onDoneFrameHandler;
+    void * argOnDoneFrameHandler;
+
+public:
+    GobotRPCReciver(MCP2521 * can_interface);
+    ~GobotRPCReciver();
+
+    void onRX();
+
+    void registerOnDoneFrameHandler(GobotRPCHandler_t handler, void * arg);
+};
+
+void onRX0_GoboRPC(void *arg);
+
+class GoboRPCTransiver {
+private:
+    MCP2521 * can_interface;
+    GobotRPCReciver rx;
+
+    GobotRPCHandler_t onRequestHandler;
+    GobotRPCHandler_t onResponseHandler;
+    GobotRPCHandler_t onResponseErrorHandler;
+
+    void * argOnRequestHandler;
+    void * argOnResponseHandler;
+    void * argOnResponseErrorHandler;
+
+
+
+public:
+    GoboRPCTransiver(MCP2521 * can_interface);
+    ~GoboRPCTransiver();
+
+    void send(RPCFrame * frame);
+
+    void registerOnRequestHandler(GobotRPCHandler_t handler, void * arg);
+    void registerOnResponseHandler(GobotRPCHandler_t handler, void * arg);
+    void registerOnResponseErrorHandler(GobotRPCHandler_t handler, void * arg);
+};

common-libaries/gobot_rpc/include/protocol_spec.hpp

@@ -0,0 +1,123 @@
+#pragma once
+#include <stdint.h>
+
+enum RpcNum {
+    Get_Info            = 0x0,
+    Home                = 0x1,
+    Move_Step           = 0x2,
+    Move_XY             = 0x3,
+    Set_Padding         = 0x4,
+    Release_Motors      = 0x5,
+    Drop_Stones         = 0x6,
+    Get_Stone_Status    = 0x7,
+    Mov_Z_Axis          = 0x8,
+    Set_Vacum           = 0x9,
+    INVALID             = 0xF,
+};
+
+struct RPCHeader {
+    unsigned char rpc_num : 4;
+    unsigned char response : 1;
+    unsigned char error : 1;
+    unsigned char rpc_segement : 1;
+};
+
+struct RPCFrame {
+    RPCHeader header;
+    uint8_t data[7];
+};
+
+enum RPC_Node_Type {
+    NODE_ALL        = 0x0,
+    NODE_CORE_XY    = 0x1,
+    NODE_HEAD       = 0x2,
+    NODE_VACUM      = 0x3,
+};
+
+// RPC Request and Response structures
+struct RPC_RES_Get_Info {
+    uint32_t can_address;
+    RPC_Node_Type node_type;
+    uint32_t status;
+    uint32_t error;
+};
+
+enum RPC_Home_Corner {
+    HOME_CORNER_0 = 0b00,
+    HOME_CORNER_1 = 0b01,
+    HOME_CORNER_2 = 0b10,
+    HOME_CORNER_3 = 0b11,
+};
+
+struct RPC_REQ_Home {
+    RPC_Home_Corner corner;
+};
+
+struct RPC_RES_Home {
+    uint32_t x;
+    uint32_t y;
+};
+
+struct RPC_REQ_Move_Step {
+    int32_t x;
+    int32_t y;
+};
+
+struct RPC_RES_Move_Step {
+    uint32_t x;
+    uint32_t y;
+};
+
+struct RPC_REQ_Set_Padding {
+    uint32_t c_1x;
+    uint32_t c_1y;
+    uint32_t c_2x;
+    uint32_t c_2y;
+    uint8_t n_x;
+    uint8_t n_y;
+};
+
+struct RPC_REQ_Move_Z_Axis {
+    bool up;
+};
+
+struct RPC_REQ_Set_Vacum {
+    bool on;
+};
+
+// Segment3, Segment2, Segment1, Segment0
+const uint8_t SEGMENT_MASK_REQUEST[16] = {
+    0b0001, // Get_Info
+    0b0001, // Home
+    0b0001, // Move_Step
+    0b0001, // Move_XY
+    0b0111, // Set_Padding
+    0b0001, // Release_Motors
+    0b0001, // Drop_Stones
+    0b0001, // Get_Stone_Status
+    0b0001, // Mov_Z_Axis
+    0b0001, // Set_Vacum
+    0b0001, //
+    0b0001, //
+    0b0001, //
+    0b0001, //
+    0b0001, //
+};
+
+const uint8_t SEGMENT_MASK_RESPONSE[16] = {
+    0b0001, // Get_Info
+    0b0001, // Home
+    0b0001, // Move_Step
+    0b0001, // Move_XY
+    0b0001, // Set_Padding
+    0b0001, // Release_Motors
+    0b0001, // Drop_Stones
+    0b0001, // Get_Stone_Status
+    0b0001, // Mov_Z_Axis
+    0b0001, // Set_Vacum
+    0b0001, //
+    0b0001, //
+    0b0001, //
+    0b0001, //
+    0b0001, //
+};

common-libaries/gobot_rpc/include/sm.hpp

@@ -0,0 +1,58 @@
+#pragma once
+#include <stdint.h>
+#include "protocol_spec.hpp"
+#include <strings.h>
+
+class GobotRPCNumberMap {
+private:
+    int32_t rpc_number_map[16];
+public:
+    GobotRPCNumberMap();
+    ~GobotRPCNumberMap();
+
+    void set(RpcNum rpc_num, uint32_t id);
+    uint32_t get(RpcNum rpc_num);
+};
+
+
+enum RPCPackageType {
+    REQUEST,
+    RESPONSE,
+    ERROR
+};
+
+struct SMResult {
+    bool done;
+    uint8_t data[7*4];
+    RpcNum rpc_num;
+    RPCPackageType type;
+};
+
+class GobotRPCStateMashine {
+private:
+    static const size_t SLOTS = 6;
+    static const size_t REQUEST_DATA_BUFFER_SIZE = 7*4;
+    static const size_t RESPONSE_DATA_BUFFER_SIZE = 7*4;
+
+
+    GobotRPCNumberMap rpc_number_map;
+    size_t slotCounter;
+
+    uint8_t requestDataBuffer[REQUEST_DATA_BUFFER_SIZE][SLOTS];
+    bool requestSegmentArrivedFlags [SLOTS];
+    
+    uint8_t responseDataBuffer[RESPONSE_DATA_BUFFER_SIZE][SLOTS];
+    uint8_t responseSegmentArrivedFlags [SLOTS];
+
+    uint8_t errorDataBuffer[RESPONSE_DATA_BUFFER_SIZE][SLOTS];
+    uint8_t errorSegmentArrivedFlags [SLOTS];
+
+public:
+    GobotRPCStateMashine();
+    ~GobotRPCStateMashine();
+
+    void registerRPC(RpcNum rpc_num);
+
+    void submitFrame(RPCFrame * frame, SMResult * result);
+
+};

common-libaries/mcp2521_hardware_interface/esp_implementation_cmd.cpp

@@ -76,7 +76,7 @@ void MCP2521_HardwareHandle_ESP::read(uint8_t cmd, uint8_t *data, size_t length)
             .addr = 0,
             .length = 8*length,
             .rxlength = 8*length,
-            .tx_buffer = NULL,
+            .tx_buffer = null_buffer,
             .rx_buffer = data
         },
         .command_bits = 8,

common-libaries/mcp2521_hardware_interface/esp_implementation_init.cpp

@@ -73,7 +73,7 @@ void MCP2521_HardwareHandle_ESP::initSPIDevice(
         // Learned this the hard way
         .cs_ena_pretrans = 0,
         .cs_ena_posttrans = 0,
-        .clock_speed_hz = 10000,
+        .clock_speed_hz = 2000,
 
         .spics_io_num = cs,
         .flags = SPI_DEVICE_NO_DUMMY,