Merge pull request #14 from SoonerRobotics/feat/vroomvroom

firmware/motor_control/CANBusDriver.h

@@ -0,0 +1,146 @@
+#ifndef CANBUS_DRIVER_H
+#define CANBUS_DRIVER_H
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "CONBus.h"
+
+namespace CONBus{
+
+typedef struct{
+    uint8_t registerAddress;
+} CAN_readRegisterMessage;
+
+typedef struct{
+    uint8_t registerAddress;
+    uint8_t length;
+    uint8_t reserved_;
+    uint8_t value[4];
+} CAN_readRegisterResponseMessage;
+
+typedef struct{
+    uint8_t registerAddress;
+    uint8_t length;
+    uint8_t reserved_;
+    uint8_t value[4];
+} CAN_writeRegisterMessage;
+
+typedef struct{
+    uint8_t registerAddress;
+    uint8_t length;
+    uint8_t reserved_;
+    uint8_t value[4];
+} CAN_writeRegisterResponseMessage;
+
+class CANBusDriver {
+    public:
+        CANBusDriver(CONBus& conbus, const uint32_t device_id);
+
+        uint8_t readCanMessage(const uint32_t can_id, const void* buffer);
+        bool isReplyReady();
+        uint8_t peekReply(uint32_t& can_id, uint8_t& can_len, void* buffer);
+        uint8_t popReply();
+
+    private:
+        CONBus& conbus_;
+        const uint8_t device_id_;
+
+        CAN_readRegisterMessage readRegisterMessage_;
+        CAN_readRegisterResponseMessage readRegisterResponseMessage_;
+        CAN_writeRegisterMessage writeRegisterMessage_;
+        CAN_writeRegisterResponseMessage writeRegisterResponseMessage_;
+
+        bool awaiting_write_response_ = false;
+
+        void putRegisterAddressInQueue(const uint32_t register_address);
+        uint8_t register_fetch_queue_[256];
+        uint8_t register_fetch_queue_head_ = 0; // located at next entry to send
+        uint8_t register_fetch_queue_tail_ = 0; // located *after* the last entry in the queue
+};
+
+inline CANBusDriver::CANBusDriver(CONBus& conbus, const uint32_t device_id) : conbus_(conbus), device_id_(device_id) {}
+
+inline uint8_t CANBusDriver::readCanMessage(const uint32_t can_id, const void* buffer) {
+    // CONBus read register
+    if (can_id == ((uint32_t)1000 + device_id_)) {
+        readRegisterMessage_ = *(CAN_readRegisterMessage*)buffer;
+        if (readRegisterMessage_.registerAddress != 0xFF) {
+            putRegisterAddressInQueue(readRegisterMessage_.registerAddress);
+        } else {
+            // Put the whole memory map into the queue
+            for (int i=0; i<255; i++) {
+                if (conbus_.hasRegister(i)) {
+                    putRegisterAddressInQueue(i);
+                }
+            }
+        }
+    }
+
+    // CONBus write register
+    if (can_id == ((uint32_t)1200 + device_id_)) {
+        awaiting_write_response_ = true;
+
+        writeRegisterMessage_ = *(CAN_writeRegisterMessage*)buffer;
+        conbus_.writeRegisterBytes(writeRegisterMessage_.registerAddress, writeRegisterMessage_.value, writeRegisterMessage_.length);
+
+        memcpy(&writeRegisterResponseMessage_, &writeRegisterMessage_, sizeof(writeRegisterResponseMessage_));
+    }
+
+    return SUCCESS;
+}
+
+inline bool CANBusDriver::isReplyReady() {
+    return (register_fetch_queue_head_ != register_fetch_queue_tail_) || awaiting_write_response_;
+}
+
+inline uint8_t CANBusDriver::peekReply(uint32_t& can_id, uint8_t& can_len, void* buffer) {
+    if (register_fetch_queue_head_ != register_fetch_queue_tail_) {
+        readRegisterResponseMessage_.registerAddress = register_fetch_queue_[register_fetch_queue_head_];
+        conbus_.readRegisterBytes(readRegisterResponseMessage_.registerAddress, readRegisterResponseMessage_.value, readRegisterResponseMessage_.length);
+
+        can_id = 1100 + device_id_;
+        // The readRegisterResponseMessage_ is the full 7 bytes for a 4 byte buffer
+        // but if we have a smaller message, we should reduce the size
+        can_len = sizeof(readRegisterResponseMessage_) - (4 - readRegisterResponseMessage_.length);
+
+        memcpy(buffer, &readRegisterResponseMessage_, sizeof(readRegisterResponseMessage_));
+
+        return SUCCESS; // end early so we dont overwrite a read response with a write response
+    }
+
+    if (awaiting_write_response_) {
+        can_id = 1300 + device_id_;
+        // Same as above, we have to reduce the size appropriately.
+        can_len = sizeof(writeRegisterResponseMessage_) - (4 - writeRegisterResponseMessage_.length);
+        memcpy(buffer, &writeRegisterResponseMessage_, sizeof(writeRegisterResponseMessage_));
+    }
+
+    return SUCCESS;
+}
+
+inline uint8_t CANBusDriver::popReply() {
+    if (register_fetch_queue_head_ != register_fetch_queue_tail_) {
+        // Move head of the queue
+        register_fetch_queue_head_++;
+
+        return SUCCESS; // end early so we dont overwrite a read response with a write response
+    }
+
+    if (awaiting_write_response_) {
+        awaiting_write_response_ = false;
+    }
+
+    return SUCCESS;
+}
+
+inline void CANBusDriver::putRegisterAddressInQueue(const uint32_t register_address) {
+    register_fetch_queue_[register_fetch_queue_tail_] = register_address;
+    register_fetch_queue_tail_++;
+}
+
+
+} // end CONBus namespace
+
+#endif

firmware/motor_control/CONBus.h

@@ -0,0 +1,165 @@
+#ifndef CONBUS_H
+#define CONBUS_H
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+namespace CONBus{
+
+// Define error codes for all CONBus methods
+static const uint8_t SUCCESS = 0;
+static const uint8_t ERROR_UNKNOWN = 1;
+static const uint8_t ERROR_ADDRESS_ALREADY_USED = 2;
+static const uint8_t ERROR_INVALID_LENGTH = 3;
+static const uint8_t ERROR_ADDRESS_NOT_REGISTERED = 4;
+static const uint8_t ERROR_DIFFERENT_TYPE_THAN_REGISTERED = 5;
+
+// Define CONBus specification constants
+static const uint8_t MAX_REGISTER_LENGTH = 4;
+
+class CONBus {
+    public:
+        CONBus();
+
+        template <typename T>
+        uint8_t addRegister(const uint8_t conbus_address, T* register_address);
+        template <typename T>
+        uint8_t addReadOnlyRegister(const uint8_t conbus_address, T* register_address);
+
+        bool hasRegister(const uint8_t conbus_address);
+
+        template <typename T>
+        uint8_t writeRegister(const uint8_t conbus_address, const T value);
+        uint8_t writeRegisterBytes(const uint8_t conbus_address, const void* buffer, const uint8_t length);
+
+        template <typename T>
+        uint8_t readRegister(const uint8_t conbus_address, T& value);
+        uint8_t readRegisterBytes(const uint8_t conbus_address, void* buffer, uint8_t& length);
+    private:
+        uint8_t length_map_[256];
+        void* pointer_map_[256];
+        bool read_only_map_[256];
+};
+
+inline CONBus::CONBus() {
+    // Clear Length map
+    for (int i=0; i<256; i++) {
+        length_map_[i] = 0;
+    }
+
+    // Clear Pointer map
+    for (int i=0; i<256; i++) {
+        pointer_map_[i] = nullptr;
+    }
+
+    // Clear Read Only map
+    for (int i=0; i<256; i++) {
+        read_only_map_[i] = false;
+    }
+}
+
+template <typename T>
+inline uint8_t CONBus::addRegister(const uint8_t conbus_address, T* register_address) {
+    if (length_map_[conbus_address] > 0) {
+        return ERROR_ADDRESS_ALREADY_USED;
+    }
+
+    if (sizeof(T) > MAX_REGISTER_LENGTH) {
+        return ERROR_INVALID_LENGTH;
+    }
+
+    length_map_[conbus_address] = sizeof(T);
+    pointer_map_[conbus_address] = register_address;
+    read_only_map_[conbus_address] = false;
+
+    return SUCCESS;
+}
+
+template <typename T>
+inline uint8_t CONBus::addReadOnlyRegister(const uint8_t conbus_address, T* register_address) {
+    if (length_map_[conbus_address] > 0) {
+        return ERROR_ADDRESS_ALREADY_USED;
+    }
+
+    if (sizeof(T) > MAX_REGISTER_LENGTH) {
+        return ERROR_INVALID_LENGTH;
+    }
+
+    length_map_[conbus_address] = sizeof(T);
+    pointer_map_[conbus_address] = register_address;
+    read_only_map_[conbus_address] = true;
+
+    return SUCCESS;
+}
+
+inline bool CONBus::hasRegister(const uint8_t conbus_address) {
+    return length_map_[conbus_address] > 0;
+}
+
+template <typename T>
+inline uint8_t CONBus::writeRegister(const uint8_t conbus_address, const T value) {
+    if (length_map_[conbus_address] == 0) {
+        return ERROR_ADDRESS_NOT_REGISTERED;
+    }
+
+    if (sizeof(T) != length_map_[conbus_address]) {
+        return ERROR_DIFFERENT_TYPE_THAN_REGISTERED;
+    }
+
+    if (read_only_map_[conbus_address]) {
+        // If this register is read only, simply don't write.
+        // TODO: Allow this to be configured to be an ERROR.
+        return SUCCESS;
+    }
+
+    *(T*)(pointer_map_[conbus_address]) = value;
+
+    return SUCCESS;
+}
+
+inline uint8_t CONBus::writeRegisterBytes(const uint8_t conbus_address, const void* buffer, const uint8_t length) {
+    if (length_map_[conbus_address] == 0) {
+        return ERROR_ADDRESS_NOT_REGISTERED;
+    }
+
+    if (length != length_map_[conbus_address]) {
+        return ERROR_DIFFERENT_TYPE_THAN_REGISTERED;
+    }
+
+    if (read_only_map_[conbus_address]) {
+        // If this register is read only, simply don't write.
+        // TODO: Allow this to be configured to be an ERROR.
+        return SUCCESS;
+    }
+
+    memcpy(pointer_map_[conbus_address], buffer, length);
+
+    return SUCCESS;
+}
+
+template <typename T>
+inline uint8_t CONBus::readRegister(const uint8_t conbus_address, T& value) {
+    if (length_map_[conbus_address] == 0) {
+        return ERROR_ADDRESS_NOT_REGISTERED;
+    }
+
+    if (sizeof(T) != length_map_[conbus_address]) {
+        return ERROR_DIFFERENT_TYPE_THAN_REGISTERED;
+    }
+
+    value = *(T*)(pointer_map_[conbus_address]);
+
+    return SUCCESS;
+}
+
+inline uint8_t CONBus::readRegisterBytes(const uint8_t conbus_address, void* buffer, uint8_t& length) {
+    memcpy(buffer, pointer_map_[conbus_address], length_map_[conbus_address]);
+    length = length_map_[conbus_address];
+
+    return SUCCESS;
+}
+
+} // end CONBus namespace
+
+#endif

firmware/motor_control/differential_drive.h

@@ -44,9 +44,9 @@ class DifferentialDrive {
     MotorWithEncoder right_motor_;
 
     float update_period_ = 0.025f;
-    float pulses_per_radian_ = 600.0f * 20.0f / 16.8f;
-    float wheel_radius_ = 0.135f;
-    float wheelbase_length_ = 0.45f;
+    float pulses_per_radian_ = 600.0f;
+    float wheel_radius_ = 0.19f;
+    float wheelbase_length_ = 0.575f;
     float left_encoder_factor_ = 1.00f;
     float right_encoder_factor_ = 1.01f;