diff --git a/firmware/motor_control/CANBusDriver.h b/firmware/motor_control/CANBusDriver.h new file mode 100644 index 00000000..35789d87 --- /dev/null +++ b/firmware/motor_control/CANBusDriver.h @@ -0,0 +1,146 @@ +#ifndef CANBUS_DRIVER_H +#define CANBUS_DRIVER_H + +#include +#include +#include + +#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 \ No newline at end of file diff --git a/firmware/motor_control/CONBus.h b/firmware/motor_control/CONBus.h new file mode 100644 index 00000000..941b385e --- /dev/null +++ b/firmware/motor_control/CONBus.h @@ -0,0 +1,165 @@ +#ifndef CONBUS_H +#define CONBUS_H + +#include +#include +#include + +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 + uint8_t addRegister(const uint8_t conbus_address, T* register_address); + template + uint8_t addReadOnlyRegister(const uint8_t conbus_address, T* register_address); + + bool hasRegister(const uint8_t conbus_address); + + template + 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 + 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 +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 +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 +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 +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 \ No newline at end of file diff --git a/firmware/motor_control/differential_drive.h b/firmware/motor_control/differential_drive.h index d5a912ab..c4a0e937 100644 --- a/firmware/motor_control/differential_drive.h +++ b/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; diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 0851951d..fab68d40 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -4,37 +4,44 @@ #include "differential_drive.h" #include "motor_with_encoder.h" #include "common.h" -#include -#include - -//LEDs -const int ledPin = LED_BUILTIN; -const int CANLED = 0; -const int LED0 = 1; - -//Encoder Pins -const int encoderLeftA = 3; -const int encoderLeftB = 2; -const int encoderRightA = 5; -const int encoderRightB = 4; -//Motor PWM pins -const int leftMotorPwmPin = 16; -const int rightMotorPwmPin = 14; - -const int estopPin = 27; - -//SPI pins -static const byte MCP2515_SCK = 10; // SCK input of MCP2515 -static const byte MCP2515_MOSI = 11; // SDI input of MCP2515 -static const byte MCP2515_MISO = 8; // SDO output of MCP2515 -static const byte MCP2515_CS = 9; // CS input of MCP2515 -static const byte MCP2515_INT = 7; // INT output of MCP2515 - -ACAN2515 can(MCP2515_CS, SPI1, MCP2515_INT); +#include "CONBus.h" +#include "CANBusDriver.h" -TickTwo motor_update_timer(setMotorUpdateFlag, 25); +//LED +static int LED1 = 22; +static int LED2 = 21; + +//ESTOP +static int ESTOP = 27; + +//PWM +static int PWM0 = 6; //right motor controller +static int PWM1 = 7; //left motor controller + +//ENCODER +static int ENC0A = 9; //right encoder +static int ENC0B = 8; //right encoder +static int ENC1A = 11; //left encoder +static int ENC1B = 10; //left encoder + +//EEPROM +static int EEPROMSDA = 0; +static int EEPROMSCL = 1; +static int EEPROMWP = 2; + +//SPI +static int MCP2515_MISO = 16; +static int MCP2515_CS = 17; +static int MCP2515_SCK = 18; +static int MCP2515_MOSI = 19; +static int MCP2515_INT = 20; + +//Quartz oscillator - 8MHz +static uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL; -static const uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL; // 8 MHz +ACAN2515 can(MCP2515_CS, SPI, MCP2515_INT); + +TickTwo motor_update_timer(setMotorUpdateFlag, 25); CANMessage frame; CANMessage outFrame; @@ -47,11 +54,11 @@ robotStatus_t roboStatus; distance motorDistances; MotorCommand motorCommand; -// motor leftMotor(leftMotorPwmPin, true); -// motor rightMotor(rightMotorPwmPin, false); +// motor leftMotor(PWM1, true); +// motor rightMotor(PWM0, false); -MotorWithEncoder leftMotor(leftMotorPwmPin, encoderLeftA, encoderLeftB, true); -MotorWithEncoder rightMotor(rightMotorPwmPin, encoderRightA, encoderRightB, false); +MotorWithEncoder leftMotor(PWM1, ENC1A, ENC1B, true); +MotorWithEncoder rightMotor(PWM0, ENC0A, ENC0B, false); DifferentialDrive drivetrain(leftMotor, rightMotor, 0.025); void configureCan(); @@ -80,28 +87,37 @@ float desired_forward_velocity; float desired_angular_velocity; void setup() { + Serial.begin(9600); + //while(!Serial){ + //delay(50); + //} delay(50); + Serial.println("Serial Connected"); drivetrain.setup(); - pinMode(encoderRightA, INPUT); - pinMode(encoderRightB, INPUT); - pinMode(encoderLeftA, INPUT); - pinMode(encoderLeftB, INPUT); + pinMode(ENC0A, INPUT); + pinMode(ENC0B, INPUT); + pinMode(ENC1A, INPUT); + pinMode(ENC1B, INPUT); - attachInterrupt(encoderLeftA, updateLeft, CHANGE); - attachInterrupt(encoderRightA, updateRight, CHANGE); + attachInterrupt(ENC1A, updateLeft, CHANGE); + attachInterrupt(ENC0A, updateRight, CHANGE); motor_update_timer.start(); -} -void setup1(){ - Serial.begin(9600); + + //setup1 delay(50); configureCan(); - pinMode(LED0, OUTPUT); - pinMode(CANLED, OUTPUT); - pinMode(estopPin, INPUT); + pinMode(LED1, OUTPUT); + pinMode(LED2, OUTPUT); + digitalWrite(LED1, HIGH); + digitalWrite(LED2, HIGH); + delay(1000); + digitalWrite(LED1, LOW); + digitalWrite(LED2, LOW); + pinMode(ESTOP, INPUT); conbus.addReadOnlyRegister(0x00, drivetrain.getUpdatePeriod()); conbus.addRegister(0x01, drivetrain.getPulsesPerRadian()); @@ -128,7 +144,9 @@ void setup1(){ conbus.addRegister(0x50, &motor_updates_between_deltaodom); } + void loop() { + updateTimers(); if (MOTOR_UPDATE_FLAG) { @@ -137,9 +155,8 @@ void loop() { MOTOR_UPDATE_FLAG = false; } -} -void loop1(){ + //loop1 if (motor_updates_in_deltaodom >= motor_updates_between_deltaodom) { motor_updates_in_deltaodom = 0; sendCanOdomMsgOut(); @@ -155,24 +172,26 @@ void loop1(){ conbus_can.popReply(); } } + } -void configureCan() { - SPI1.setSCK(MCP2515_SCK); - SPI1.setTX(MCP2515_MOSI); - SPI1.setRX(MCP2515_MISO); - SPI1.setCS(MCP2515_CS); - SPI1.begin(); +void configureCan() { + SPI.setSCK(MCP2515_SCK); + SPI.setTX(MCP2515_MOSI); + SPI.setRX(MCP2515_MISO); + SPI.setCS(MCP2515_CS); + SPI.begin(); Serial.println("Configure ACAN2515"); ACAN2515Settings settings(QUARTZ_FREQUENCY, 100UL * 1000UL); // CAN bit rate 100 kb/s settings.mRequestedMode = ACAN2515Settings::NormalMode ; // Select Normal mode const uint16_t errorCode = can.begin(settings, onCanRecieve ); if (errorCode == 0) { + Serial.println("CAN Configured"); } else{ Serial.print("Error: "); - Serial.print(errorCode); + Serial.println(errorCode); } } void onCanRecieve() { @@ -186,13 +205,16 @@ void onCanRecieve() { conbus_can.readCanMessage(frame.id, frame.data); - switch (frame.id) { + printCanMsg(frame); + + switch (frame.id) { case 10: - motorCommand = *(MotorCommand*)(frame.data); //Noah made me cry. I dont know what they did but I dont like it one bit - Jorge + motorCommand = *(MotorCommand*)(frame.data); desired_forward_velocity = (float)motorCommand.setpoint_forward_velocity / SPEED_SCALE_FACTOR; desired_angular_velocity = (float)motorCommand.setpoint_angular_velocity / SPEED_SCALE_FACTOR; + if (useObstacleAvoidance && isDetectingObstacle && desired_forward_velocity > 0) { desired_forward_velocity = 0; } @@ -209,6 +231,7 @@ void onCanRecieve() { drivetrain.setOutput(desired_forward_velocity, desired_angular_velocity); break; } + } PIDSetpoints pid_setpoints; @@ -251,12 +274,15 @@ void printCanMsg(CANMessage frame) { } Serial.println(""); } + void updateLeft(){ drivetrain.pulseLeftEncoder(); } + void updateRight(){ drivetrain.pulseRightEncoder(); } + void resetDelta(){ motorDistances.xn = 0; motorDistances.yn = 0; @@ -265,7 +291,7 @@ void resetDelta(){ delta_y = 0; delta_theta = 0; } + void updateTimers() { motor_update_timer.update(); } -