From 6e86ce61eca33758ba877696332b2dbd5c389d3c Mon Sep 17 00:00:00 2001 From: Yannick Richter Date: Sun, 9 Jun 2024 15:29:45 +0200 Subject: [PATCH] Odrive added option to store and reload encoder offset --- .../FFBoard/UserExtensions/Inc/ODriveCAN.h | 3 +- .../UserExtensions/Inc/eeprom_addresses.h | 6 ++-- .../FFBoard/UserExtensions/Src/ODriveCAN.cpp | 30 +++++++++++++++++-- .../UserExtensions/Src/eeprom_addresses.c | 4 +++ Firmware/scripts/memory_map.csv | 2 ++ 5 files changed, 40 insertions(+), 5 deletions(-) diff --git a/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h b/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h index 384b095e..c36006fd 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h +++ b/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h @@ -29,7 +29,7 @@ enum class ODriveEncoderFlags : uint32_t {ERROR_NONE = 0,ERROR_UNSTABLE_GAIN = 0 enum class ODriveAxisError : uint32_t {AXIS_ERROR_NONE = 0x00000000,AXIS_ERROR_INVALID_STATE = 0x00000001, AXIS_ERROR_WATCHDOG_TIMER_EXPIRED = 0x00000800,AXIS_ERROR_MIN_ENDSTOP_PRESSED = 0x00001000, AXIS_ERROR_MAX_ENDSTOP_PRESSED = 0x00002000,AXIS_ERROR_ESTOP_REQUESTED = 0x00004000,AXIS_ERROR_HOMING_WITHOUT_ENDSTOP = 0x00020000,AXIS_ERROR_OVER_TEMP = 0x00040000,AXIS_ERROR_UNKNOWN_POSITION = 0x00080000}; enum class ODriveCAN_commands : uint32_t{ - canid,canspd,errors,state,maxtorque,vbus,anticogging,connected + canid,canspd,errors,state,maxtorque,vbus,anticogging,connected,storepos }; class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, public CanHandler, public CommandHandler, cpp_freertos::Thread{ @@ -106,6 +106,7 @@ class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, p uint32_t lastPosTime = 0; bool posWaiting = false; + bool reloadPosAfterStartup = false; int8_t nodeId = 0; // 6 bits can ID int8_t motorId = 0; diff --git a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h index 0350f7c1..c4d53a38 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h +++ b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h @@ -13,11 +13,11 @@ #include "main.h" // Change this to the amount of currently registered variables -#define NB_OF_VAR 148 +#define NB_OF_VAR 150 extern const uint16_t VirtAddVarTab[NB_OF_VAR]; // Amount of variables in exportable list -#define NB_EXPORTABLE_ADR 133 +#define NB_EXPORTABLE_ADR 135 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR]; @@ -165,6 +165,8 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_ODRIVE_CANID 0x3D0 //0-6 ID M0, 7-12 ID M1, 13-15 can speed #define ADR_ODRIVE_SETTING1_M0 0x3D1 #define ADR_ODRIVE_SETTING1_M1 0x3D2 +#define ADR_ODRIVE_OFS_M0 0x3D3 // Encoder offset position reload +#define ADR_ODRIVE_OFS_M1 0x3D4 // VESC Section #define ADR_VESC1_CANID 0x3E0 //0-7 AxisCanID, 8-16 VescCanId #define ADR_VESC1_DATA 0x3E1 //0-2 can speed, 3 useVescEncoder diff --git a/Firmware/FFBoard/UserExtensions/Src/ODriveCAN.cpp b/Firmware/FFBoard/UserExtensions/Src/ODriveCAN.cpp index 8242e1d2..b2509822 100644 --- a/Firmware/FFBoard/UserExtensions/Src/ODriveCAN.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/ODriveCAN.cpp @@ -98,6 +98,7 @@ void ODriveCAN::registerCommands(){ registerCommand("vbus", ODriveCAN_commands::vbus, "ODrive Vbus",CMDFLAG_GET); registerCommand("anticogging", ODriveCAN_commands::anticogging, "Set 1 to start anticogging calibration",CMDFLAG_SET); registerCommand("connected", ODriveCAN_commands::connected, "ODrive connection state",CMDFLAG_GET); + registerCommand("storepos", ODriveCAN_commands::storepos, "Store encoder offset",CMDFLAG_GET | CMDFLAG_SET); } void ODriveCAN::restoreFlash(){ @@ -118,6 +119,15 @@ void ODriveCAN::restoreFlash(){ uint16_t settings1 = 0; if(Flash_Read(setting1addr, &settings1)){ maxTorque = (float)clip(settings1 & 0xfff, 0, 0xfff) / 100.0; + uint8_t settings1_2 = (settings1 >> 12) & 0xf; + reloadPosAfterStartup = (settings1_2 & 0x1) != 0; + } + + if(reloadPosAfterStartup){ + int16_t posOfs = 0; + if(Flash_Read(motorId == 0 ? ADR_ODRIVE_OFS_M0 : ADR_ODRIVE_OFS_M1, (uint16_t*)&posOfs)){ + posOffset = (float)posOfs / getCpr(); + } } } @@ -139,7 +149,15 @@ void ODriveCAN::saveFlash(){ Flash_Write(ADR_ODRIVE_CANID,canIds); uint16_t settings1 = ((int32_t)(maxTorque*100) & 0xfff); + uint8_t settings1_2 = reloadPosAfterStartup ? 1 : 0; // 4 bits + settings1 |= (settings1_2 & 0xf) << 12; + Flash_Write(setting1addr, settings1); + + if(reloadPosAfterStartup){ + int32_t posOfs = posOffset * getCpr(); + Flash_Write(motorId == 0 ? ADR_ODRIVE_OFS_M0 : ADR_ODRIVE_OFS_M1, posOfs); + } } void ODriveCAN::Run(){ @@ -171,7 +189,9 @@ void ODriveCAN::Run(){ // Odrive is active, // enable torque mode if(odriveCurrentState == ODriveState::AXIS_STATE_CLOSED_LOOP_CONTROL){ - //this->setPos(0); + if(!reloadPosAfterStartup){ + this->setPos(0); // Assume this as the zero position and let the user correct it + } setMode(ODriveControlMode::CONTROL_MODE_TORQUE_CONTROL, ODriveInputMode::INPUT_MODE_PASSTHROUGH); } @@ -365,7 +385,13 @@ CommandStatus ODriveCAN::command(const ParsedCommand& cmd,std::vector