From 1f8844c7377d4df0dbf799b5378ccb959376e55a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 3 Feb 2022 19:57:20 +0100 Subject: [PATCH 01/41] interim commit, nothing working here yet... --- ...manderRegisters.h => CommanderRegisters.h} | 0 src/comms/i2c/I2CCommander.h | 2 +- src/comms/i2c/I2CCommanderMaster.h | 2 +- src/comms/serial/BinarySerialCommander.cpp | 0 src/comms/serial/BinarySerialCommander.h | 0 src/comms/serial/BinarySerialMonitoring.cpp | 0 src/comms/serial/BinarySerialMonitoring.h | 28 +++++++++++++++++++ 7 files changed, 30 insertions(+), 2 deletions(-) rename src/comms/{i2c/I2CCommanderRegisters.h => CommanderRegisters.h} (100%) create mode 100644 src/comms/serial/BinarySerialCommander.cpp create mode 100644 src/comms/serial/BinarySerialCommander.h create mode 100644 src/comms/serial/BinarySerialMonitoring.cpp create mode 100644 src/comms/serial/BinarySerialMonitoring.h diff --git a/src/comms/i2c/I2CCommanderRegisters.h b/src/comms/CommanderRegisters.h similarity index 100% rename from src/comms/i2c/I2CCommanderRegisters.h rename to src/comms/CommanderRegisters.h diff --git a/src/comms/i2c/I2CCommander.h b/src/comms/i2c/I2CCommander.h index 2cb467a..122113b 100644 --- a/src/comms/i2c/I2CCommander.h +++ b/src/comms/i2c/I2CCommander.h @@ -5,7 +5,7 @@ #include "Arduino.h" #include "Wire.h" #include "common/base_classes/FOCMotor.h" -#include "I2CCommanderRegisters.h" +#include "../CommanderRegisters.h" #ifndef I2CCOMMANDER_MAX_MOTORS_COMMANDABLE #define I2CCOMMANDER_MAX_MOTORS_COMMANDABLE 4 diff --git a/src/comms/i2c/I2CCommanderMaster.h b/src/comms/i2c/I2CCommanderMaster.h index ffba9e1..60d9339 100644 --- a/src/comms/i2c/I2CCommanderMaster.h +++ b/src/comms/i2c/I2CCommanderMaster.h @@ -4,7 +4,7 @@ #include #include -#include "I2CCommanderRegisters.h" +#include "../CommanderRegisters.h" #define I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS 4 diff --git a/src/comms/serial/BinarySerialCommander.cpp b/src/comms/serial/BinarySerialCommander.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/comms/serial/BinarySerialCommander.h b/src/comms/serial/BinarySerialCommander.h new file mode 100644 index 0000000..e69de29 diff --git a/src/comms/serial/BinarySerialMonitoring.cpp b/src/comms/serial/BinarySerialMonitoring.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/comms/serial/BinarySerialMonitoring.h b/src/comms/serial/BinarySerialMonitoring.h new file mode 100644 index 0000000..1d09161 --- /dev/null +++ b/src/comms/serial/BinarySerialMonitoring.h @@ -0,0 +1,28 @@ + +#ifndef __BINARY_SERIAL_MONITORING_H__ +#define __BINARY_SERIAL_MONITORING_H__ + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "../CommanderRegisters.h" + +class BinarySerialMonitoring : public Monitoring { +public: + BinarySerialMonitoring(); + ~BinarySerialMonitoring(); + + void init(Stream& serial); + void addMotor(FOCMotor& motor); + void setMonitoringRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); + + void run() override; + void startFrame() override; + void endFrame() override; + void writeFrame(uint8_t* data, uint8_t size) override; + void writeFrameFloat(float data) override; + void writeFrameInt(int data) override; + +}; + + +#endif From b226e1b36bb7e495d95d0d77b28ef8133fe669a4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 6 Mar 2022 21:30:27 +0100 Subject: [PATCH 02/41] binary comms refactoring --- src/comms/RegisterBasedCommander.cpp | 9 + src/comms/RegisterBasedCommander.h | 42 ++++ ...manderRegisters.h => SimpleFOCRegisters.h} | 3 +- src/comms/binary/BinaryCommander.cpp | 41 ++++ src/comms/binary/BinaryCommander.h | 37 ++++ src/comms/binary/Binary_receiveRegister.cpp | 9 + src/comms/binary/Binary_sendRegister.cpp | 188 ++++++++++++++++++ src/comms/binary/Binary_setRegister.cpp | 10 + src/comms/i2c/I2CCommander.cpp | 2 +- src/comms/i2c/I2CCommander.h | 4 +- src/comms/i2c/I2CCommanderMaster.cpp | 4 +- src/comms/i2c/I2CCommanderMaster.h | 6 +- src/comms/serial/BinarySerialCommander.cpp | 0 src/comms/serial/BinarySerialCommander.h | 0 src/comms/serial/BinarySerialMonitoring.cpp | 0 src/comms/serial/BinarySerialMonitoring.h | 28 --- src/comms/serial/SerialBinaryCommander.cpp | 88 ++++++++ src/comms/serial/SerialBinaryCommander.h | 36 ++++ src/comms/telemetry/Telemetry.cpp | 62 ++++++ src/comms/telemetry/Telemetry.h | 63 ++++++ src/voltage/GenericVoltageSense.cpp | 5 +- 21 files changed, 598 insertions(+), 39 deletions(-) create mode 100644 src/comms/RegisterBasedCommander.cpp create mode 100644 src/comms/RegisterBasedCommander.h rename src/comms/{CommanderRegisters.h => SimpleFOCRegisters.h} (98%) create mode 100644 src/comms/binary/BinaryCommander.cpp create mode 100644 src/comms/binary/BinaryCommander.h create mode 100644 src/comms/binary/Binary_receiveRegister.cpp create mode 100644 src/comms/binary/Binary_sendRegister.cpp create mode 100644 src/comms/binary/Binary_setRegister.cpp delete mode 100644 src/comms/serial/BinarySerialCommander.cpp delete mode 100644 src/comms/serial/BinarySerialCommander.h delete mode 100644 src/comms/serial/BinarySerialMonitoring.cpp delete mode 100644 src/comms/serial/BinarySerialMonitoring.h create mode 100644 src/comms/serial/SerialBinaryCommander.cpp create mode 100644 src/comms/serial/SerialBinaryCommander.h create mode 100644 src/comms/telemetry/Telemetry.cpp create mode 100644 src/comms/telemetry/Telemetry.h diff --git a/src/comms/RegisterBasedCommander.cpp b/src/comms/RegisterBasedCommander.cpp new file mode 100644 index 0000000..add2e49 --- /dev/null +++ b/src/comms/RegisterBasedCommander.cpp @@ -0,0 +1,9 @@ + +#include "./RegisterBasedCommander.h" + +void RegisterBasedCommander::addMotor(FOCMotor* motor){ + if (numMotors < COMMS_MAX_MOTORS_COMMANDABLE){ + motors[numMotors] = motor; + numMotors++; + } +}; diff --git a/src/comms/RegisterBasedCommander.h b/src/comms/RegisterBasedCommander.h new file mode 100644 index 0000000..3113c32 --- /dev/null +++ b/src/comms/RegisterBasedCommander.h @@ -0,0 +1,42 @@ + +#ifndef __REGISTER_BASED_COMMS_DRIVER_H__ +#define __REGISTER_BASED_COMMS_DRIVER_H__ + + + +#include "./SimpleFOCRegisters.h" +#include "common/base_classes/FOCMotor.h" + + +#ifndef COMMS_MAX_MOTORS_COMMANDABLE +#define COMMS_MAX_MOTORS_COMMANDABLE 4 +#endif +#ifndef COMMS_MAX_REPORT_REGISTERS +#define COMMS_MAX_REPORT_REGISTERS 7 +#endif + + + +class RegisterBasedCommander { +public: + virtual void addMotor(FOCMotor* motor) = 0; + virtual void runCommander() = 0; +protected: + virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) = 0; + virtual void receiveRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) = 0; + virtual void setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum = 0) = 0; + uint8_t numMotors = 0; + uint8_t curMotor = 0; + SimpleFOCRegister curRegister = REG_STATUS; + bool commanderror = false; + bool lastcommanderror = false; + uint8_t lastcommandregister = REG_STATUS; + FOCMotor* motors[COMMS_MAX_MOTORS_COMMANDABLE]; + uint8_t numReportRegisters = 0; + uint8_t reportMotors[COMMS_MAX_REPORT_REGISTERS]; + SimpleFOCRegister reportRegisters[COMMS_MAX_REPORT_REGISTERS]; +}; + + + +#endif diff --git a/src/comms/CommanderRegisters.h b/src/comms/SimpleFOCRegisters.h similarity index 98% rename from src/comms/CommanderRegisters.h rename to src/comms/SimpleFOCRegisters.h index cee8b9b..faf6ec8 100644 --- a/src/comms/CommanderRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -1,6 +1,7 @@ #ifndef __I2CCOMMANDERREGISTERS_H__ #define __I2CCOMMANDERREGISTERS_H__ +#include typedef enum : uint8_t { REG_STATUS = 0x00, // RO - 3 bytes (currently set motor, last command register, last command error status) + 1 byte / motor (motor stati) @@ -57,7 +58,7 @@ typedef enum : uint8_t { REG_NUM_MOTORS = 0x70, // RO - 1 byte REG_SYS_TIME = 0x71, // RO - uint32_t -} I2CCommander_Register; +} SimpleFOCRegister; diff --git a/src/comms/binary/BinaryCommander.cpp b/src/comms/binary/BinaryCommander.cpp new file mode 100644 index 0000000..1390024 --- /dev/null +++ b/src/comms/binary/BinaryCommander.cpp @@ -0,0 +1,41 @@ + +#include "./BinaryCommander.h" + + + + +uint8_t BinaryCommander::writeByte(uint8_t value){ + return writeBytes(&value, 1); +}; + + + +uint8_t BinaryCommander::writeFloat(float value){ + return writeBytes(&value, 4); +}; + + + +uint8_t BinaryCommander::writeInt(uint32_t value){ + return writeBytes(&value, 4); +}; + + + +uint8_t BinaryCommander::readByte(void* valueToSet){ + return 0; +}; + + + +uint8_t BinaryCommander::readFloat(void* valueToSet){ + return 0; +}; + + + +uint8_t BinaryCommander::readInt(void* valueToSet){ + return 0; +}; + + diff --git a/src/comms/binary/BinaryCommander.h b/src/comms/binary/BinaryCommander.h new file mode 100644 index 0000000..70a8aaa --- /dev/null +++ b/src/comms/binary/BinaryCommander.h @@ -0,0 +1,37 @@ + +#ifndef __BINARY_COMMS_DRIVER_H__ +#define __BINARY_COMMS_DRIVER_H__ + +/* + + Abstracts the binary communication protocol, seperating the part that + interacts with SimpleFOC from the comms SPI, so individual binary comms drivers + don't have to all implement the register setting/getting functions. + +*/ + +#include "../RegisterBasedCommander.h" + + + +class BinaryCommander : public RegisterBasedCommander { +protected: + virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) override; + virtual void receiveRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) override; + virtual void setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum = 0) override; + + virtual uint8_t writeBytes(void* valueToWrite, uint8_t numBytes) = 0; + virtual uint8_t writeByte(uint8_t value); + virtual uint8_t writeFloat(float value); + virtual uint8_t writeInt(uint32_t value); + virtual uint8_t readBytes(void* valueToSet, uint8_t numBytes) = 0; + virtual uint8_t readByte(void* valueToSet); + virtual uint8_t readFloat(void* valueToSet); + virtual uint8_t readInt(void* valueToSet); + +}; + + + +#endif + diff --git a/src/comms/binary/Binary_receiveRegister.cpp b/src/comms/binary/Binary_receiveRegister.cpp new file mode 100644 index 0000000..53fe04f --- /dev/null +++ b/src/comms/binary/Binary_receiveRegister.cpp @@ -0,0 +1,9 @@ + +#include "./BinaryCommander.h" + + + +void BinaryCommander::receiveRegister(SimpleFOCRegister reg, uint8_t motor){ + +}; + diff --git a/src/comms/binary/Binary_sendRegister.cpp b/src/comms/binary/Binary_sendRegister.cpp new file mode 100644 index 0000000..00e9c62 --- /dev/null +++ b/src/comms/binary/Binary_sendRegister.cpp @@ -0,0 +1,188 @@ + +#include "./BinaryCommander.h" + + + + +bool BinaryCommander::sendRegister(SimpleFOCRegister reg, uint8_t motorNum){ + // read the current register + switch(reg) { + case REG_STATUS: + writeByte(curMotor); + writeByte((uint8_t)lastcommandregister); + writeByte((uint8_t)lastcommanderror+1); + for (int i=0;(imotor_status); + } + break; + case REG_MOTOR_ADDRESS: + writeByte(curMotor); + break; + case REG_REPORT: + for (int i=0;ienabled); + break; + case REG_MODULATION_MODE: + writeByte(motors[motorNum]->foc_modulation); + break; + case REG_TORQUE_MODE: + writeByte(motors[motorNum]->torque_controller); + break; + case REG_CONTROL_MODE: + writeByte(motors[motorNum]->controller); + break; + + case REG_TARGET: + writeFloat(motors[motorNum]->target); + break; + case REG_ANGLE: + writeFloat(motors[motorNum]->shaft_angle); + break; + case REG_VELOCITY: + writeFloat(motors[motorNum]->shaft_velocity); + break; + case REG_SENSOR_ANGLE: + if (motors[motorNum]->sensor) + writeFloat(motors[motorNum]->sensor->getAngle()); // stored angle + else + writeFloat(motors[motorNum]->shaft_angle); + break; + + case REG_VOLTAGE_Q: + writeFloat(motors[motorNum]->voltage.q); + break; + case REG_VOLTAGE_D: + writeFloat(motors[motorNum]->voltage.d); + break; + case REG_CURRENT_Q: + writeFloat(motors[motorNum]->current.q); + break; + case REG_CURRENT_D: + writeFloat(motors[motorNum]->current.d); + break; + case REG_CURRENT_A: + if (motors[motorNum]->current_sense) // TODO check if current sense can be called from inside this callback function + writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().a); + else + writeFloat(0.0f); + break; + case REG_CURRENT_B: + if (motors[motorNum]->current_sense) + writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().b); + else + writeFloat(0.0f); + break; + case REG_CURRENT_C: + if (motors[motorNum]->current_sense) + writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().c); + else + writeFloat(0.0f); + break; + case REG_CURRENT_ABC: + if (motors[motorNum]->current_sense) { + PhaseCurrent_s currents = motors[motorNum]->current_sense->getPhaseCurrents(); + writeFloat(currents.a); + writeFloat(currents.b); + writeFloat(currents.c); + } + else { + writeFloat(0.0f); + writeFloat(0.0f); + writeFloat(0.0f); + } + break; + case REG_VEL_PID_P: + writeFloat(motors[motorNum]->PID_velocity.P); + break; + case REG_VEL_PID_I: + writeFloat(motors[motorNum]->PID_velocity.I); + break; + case REG_VEL_PID_D: + writeFloat(motors[motorNum]->PID_velocity.D); + break; + case REG_VEL_LPF_T: + writeFloat(motors[motorNum]->LPF_velocity.Tf); + break; + case REG_ANG_PID_P: + writeFloat(motors[motorNum]->P_angle.P); + break; + case REG_VEL_LIMIT: + writeFloat(motors[motorNum]->velocity_limit); + break; + case REG_VEL_MAX_RAMP: + writeFloat(motors[motorNum]->PID_velocity.output_ramp); + break; + + case REG_CURQ_PID_P: + writeFloat(motors[motorNum]->PID_current_q.P); + break; + case REG_CURQ_PID_I: + writeFloat(motors[motorNum]->PID_current_q.I); + break; + case REG_CURQ_PID_D: + writeFloat(motors[motorNum]->PID_current_q.D); + break; + case REG_CURQ_LPF_T: + writeFloat(motors[motorNum]->LPF_current_q.Tf); + break; + case REG_CURD_PID_P: + writeFloat(motors[motorNum]->PID_current_d.P); + break; + case REG_CURD_PID_I: + writeFloat(motors[motorNum]->PID_current_d.I); + break; + case REG_CURD_PID_D: + writeFloat(motors[motorNum]->PID_current_d.D); + break; + case REG_CURD_LPF_T: + writeFloat(motors[motorNum]->LPF_current_d.Tf); + break; + + case REG_MAX_VOLTAGE: + writeFloat(motors[motorNum]->voltage_limit); + break; + case REG_MAX_CURRENT: + writeFloat(motors[motorNum]->current_limit); + break; + case REG_MOTION_DOWNSAMPLE: + writeByte((uint8_t)motors[motorNum]->motion_downsample); // TODO int will have different sizes on different platforms + // but using uint32 doesn't compile clean on all, e.g. RP2040 + break; + + case REG_ZERO_ELECTRIC_ANGLE: + writeFloat(motors[motorNum]->zero_electric_angle); + break; + case REG_SENSOR_DIRECTION: + writeByte((int8_t)motors[motorNum]->sensor_direction); + break; + case REG_ZERO_OFFSET: + writeFloat(motors[motorNum]->sensor_offset); + break; + case REG_PHASE_RESISTANCE: + writeFloat(motors[motorNum]->phase_resistance); + break; + case REG_POLE_PAIRS: + writeByte((uint8_t)motors[motorNum]->pole_pairs); + break; + + case REG_SYS_TIME: + // TODO how big is millis()? Same on all platforms? + writeInt((int)millis()); + break; + case REG_NUM_MOTORS: + writeByte(numMotors); + break; + + // unknown register or write only register (no read) + case REG_ENABLE_ALL: + default: + writeByte(0); // TODO what to send in this case? + return false; + } + return true; +}; + diff --git a/src/comms/binary/Binary_setRegister.cpp b/src/comms/binary/Binary_setRegister.cpp new file mode 100644 index 0000000..754a69c --- /dev/null +++ b/src/comms/binary/Binary_setRegister.cpp @@ -0,0 +1,10 @@ + +#include "./BinaryCommander.h" + + + + +void BinaryCommander::setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum){ + +}; + diff --git a/src/comms/i2c/I2CCommander.cpp b/src/comms/i2c/I2CCommander.cpp index 6ef5068..a7731ba 100644 --- a/src/comms/i2c/I2CCommander.cpp +++ b/src/comms/i2c/I2CCommander.cpp @@ -49,7 +49,7 @@ void I2CCommander::onReceive(int numBytes){ lastcommandregister = curRegister; commanderror = false; if (numBytes>=1) { // set the current register - curRegister = static_cast(_wire->read()); + curRegister = static_cast(_wire->read()); } if (numBytes>1) { // read from i2c and update value represented by register as well... if (!receiveRegister(curMotor, curRegister, numBytes)) diff --git a/src/comms/i2c/I2CCommander.h b/src/comms/i2c/I2CCommander.h index 122113b..65cefde 100644 --- a/src/comms/i2c/I2CCommander.h +++ b/src/comms/i2c/I2CCommander.h @@ -5,7 +5,7 @@ #include "Arduino.h" #include "Wire.h" #include "common/base_classes/FOCMotor.h" -#include "../CommanderRegisters.h" +#include "../SimpleFOCRegisters.h" #ifndef I2CCOMMANDER_MAX_MOTORS_COMMANDABLE #define I2CCOMMANDER_MAX_MOTORS_COMMANDABLE 4 @@ -36,7 +36,7 @@ class I2CCommander { TwoWire* _wire; uint8_t numMotors = 0; uint8_t curMotor = 0; - I2CCommander_Register curRegister = REG_STATUS; + SimpleFOCRegister curRegister = REG_STATUS; bool commanderror = false; bool lastcommanderror = false; uint8_t lastcommandregister = REG_STATUS; diff --git a/src/comms/i2c/I2CCommanderMaster.cpp b/src/comms/i2c/I2CCommanderMaster.cpp index a63c9df..666e009 100644 --- a/src/comms/i2c/I2CCommanderMaster.cpp +++ b/src/comms/i2c/I2CCommanderMaster.cpp @@ -29,7 +29,7 @@ void I2CCommanderMaster::addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, Tw -int I2CCommanderMaster::writeRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size){ +int I2CCommanderMaster::writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size){ motors[motor].wire->beginTransmission(motors[motor].address); motors[motor].wire->write((uint8_t)registerNum); motors[motor].wire->write((uint8_t*)data, size); @@ -38,7 +38,7 @@ int I2CCommanderMaster::writeRegister(int motor, I2CCommander_Register registerN }; -int I2CCommanderMaster::readRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size){ +int I2CCommanderMaster::readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size){ motors[motor].wire->beginTransmission(motors[motor].address); int numWrite = motors[motor].wire->write((uint8_t)registerNum); // TODO check return value motors[motor].wire->endTransmission(); diff --git a/src/comms/i2c/I2CCommanderMaster.h b/src/comms/i2c/I2CCommanderMaster.h index 60d9339..9b52ae7 100644 --- a/src/comms/i2c/I2CCommanderMaster.h +++ b/src/comms/i2c/I2CCommanderMaster.h @@ -4,7 +4,7 @@ #include #include -#include "../CommanderRegisters.h" +#include "../SimpleFOCRegisters.h" #define I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS 4 @@ -23,8 +23,8 @@ class I2CCommanderMaster { void init(); void addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, TwoWire *wire = &Wire); - int writeRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size); - int readRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size); + int writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size); + int readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size); int readLastUsedRegister(int motor, void* data, uint8_t size); // Motor intialization interface for convenience - think about how this will best work diff --git a/src/comms/serial/BinarySerialCommander.cpp b/src/comms/serial/BinarySerialCommander.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/comms/serial/BinarySerialCommander.h b/src/comms/serial/BinarySerialCommander.h deleted file mode 100644 index e69de29..0000000 diff --git a/src/comms/serial/BinarySerialMonitoring.cpp b/src/comms/serial/BinarySerialMonitoring.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/comms/serial/BinarySerialMonitoring.h b/src/comms/serial/BinarySerialMonitoring.h deleted file mode 100644 index 1d09161..0000000 --- a/src/comms/serial/BinarySerialMonitoring.h +++ /dev/null @@ -1,28 +0,0 @@ - -#ifndef __BINARY_SERIAL_MONITORING_H__ -#define __BINARY_SERIAL_MONITORING_H__ - -#include "Arduino.h" -#include "common/base_classes/FOCMotor.h" -#include "../CommanderRegisters.h" - -class BinarySerialMonitoring : public Monitoring { -public: - BinarySerialMonitoring(); - ~BinarySerialMonitoring(); - - void init(Stream& serial); - void addMotor(FOCMotor& motor); - void setMonitoringRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); - - void run() override; - void startFrame() override; - void endFrame() override; - void writeFrame(uint8_t* data, uint8_t size) override; - void writeFrameFloat(float data) override; - void writeFrameInt(int data) override; - -}; - - -#endif diff --git a/src/comms/serial/SerialBinaryCommander.cpp b/src/comms/serial/SerialBinaryCommander.cpp new file mode 100644 index 0000000..c61c37d --- /dev/null +++ b/src/comms/serial/SerialBinaryCommander.cpp @@ -0,0 +1,88 @@ + +#include "SerialBinaryCommander.h" + + + +SerialBinaryCommander::SerialBinaryCommander(bool echo) : echo(echo) { + +}; + + + +SerialBinaryCommander::~SerialBinaryCommander(){ + +}; + + + +void SerialBinaryCommander::init(Stream &serial) { + _serial = &serial; + this->Telemetry::init(); +}; + + + +void SerialBinaryCommander::runCommander() { + if (_serial->available()>2) { // TODO make this work with partial packets... + uint8_t command = _serial->read(); + uint8_t registerNum = _serial->read(); + uint8_t motorNum = _serial->read(); + if (command==SERIALBINARY_COMMAND_READ){ + startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); + sendRegister(static_cast(registerNum), motorNum); + endFrame(); + } + else if (command==SERIALBINARY_COMMAND_WRITE) { + receiveRegister(static_cast(registerNum), motorNum); + if (echo) { + startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); + sendRegister(static_cast(registerNum), motorNum); + endFrame(); + } + } + } +}; + + + +uint8_t SerialBinaryCommander::readBytes(void* valueToSet, uint8_t numBytes){ + if (_serial->available()read(); + } + return numBytes; +}; + + + + +uint8_t SerialBinaryCommander::writeBytes(void* valueToSend, uint8_t numBytes){ + uint8_t* value = (uint8_t*)valueToSend; + for (uint8_t i=0; iwrite(value[i]); + } + return numBytes; +}; + + + + + + +void SerialBinaryCommander::startFrame(uint8_t frameSize, uint8_t type){ + _serial->write(0xA5); + _serial->write((uint8_t)((type<<6)|frameSize)); +}; + + + + + +void SerialBinaryCommander::endFrame(){ + _serial->write(0x5A); +}; + + + diff --git a/src/comms/serial/SerialBinaryCommander.h b/src/comms/serial/SerialBinaryCommander.h new file mode 100644 index 0000000..7ab7805 --- /dev/null +++ b/src/comms/serial/SerialBinaryCommander.h @@ -0,0 +1,36 @@ + +#ifndef __SERIAL_BINARY_COMMANDER_H__ +#define __SERIAL_BINARY_COMMANDER_H__ + +#include +#include "../telemetry/Telemetry.h" +#include "../binary/BinaryCommander.h" + +#define SERIALBINARY_FRAMETYPE_REGISTER 0x03 +#define SERIALBINARY_COMMAND_READ 0x00 +#define SERIALBINARY_COMMAND_WRITE 0x80 + + +class SerialBinaryCommander : public BinaryTelemetry, public BinaryCommander { +public: + SerialBinaryCommander(bool echo = false); + virtual ~SerialBinaryCommander(); + + void init(Stream &serial = Serial); + void runCommander() override; + + bool echo; +protected: + using BinaryCommander::sendRegister; + virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) override; + virtual void endFrame() override; + uint8_t readBytes(void* valueToSet, uint8_t numBytes) override; + uint8_t writeBytes(void* valueToSend, uint8_t numBytes) override; + + Stream* _serial; +}; + + + + +#endif \ No newline at end of file diff --git a/src/comms/telemetry/Telemetry.cpp b/src/comms/telemetry/Telemetry.cpp new file mode 100644 index 0000000..a7ad967 --- /dev/null +++ b/src/comms/telemetry/Telemetry.cpp @@ -0,0 +1,62 @@ + + +#include "./Telemetry.h" + + + +Telemetry::Telemetry(){ + +}; + + + +Telemetry::~Telemetry(){ + +}; + + + + + + + +void Telemetry::addTelemetryMotor(FOCMotor& motor){ + if (numMotors < TELEMETRY_MAX_MOTORS) + motors[numMotors++] = &motor; +}; + + + +void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){ + this->numRegisters = numRegisters; + // TODO +}; + + + +void Telemetry::init() { + sendHeader(); +}; + + + +void BinaryTelemetry::sendHeader() { + if (numRegisters > 0) { + startFrame(numRegisters*2, TELEMETRY_FRAMETYPE_HEADER); + for (uint8_t i = 0; i < numRegisters; i++) { + writeByte(registers[i]); + writeByte(registers_motor[i]); + }; + endFrame(); + }; +}; + + + +void BinaryTelemetry::sendTelemetry(){ + startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA); + for (uint8_t i = 0; i < numRegisters; i++) { + sendRegister(static_cast(registers[i]), registers_motor[i]); + }; + endFrame(); +}; diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h new file mode 100644 index 0000000..62cf8c0 --- /dev/null +++ b/src/comms/telemetry/Telemetry.h @@ -0,0 +1,63 @@ +#ifndef __TELEMETRY_H__ +#define __TELEMETRY_H__ + + +#include "../SimpleFOCRegisters.h" +#include "common/base_classes/FOCMotor.h" + +#ifndef TELEMETRY_MAX_MOTORS +#define TELEMETRY_MAX_MOTORS 4 +#endif + +#ifndef TELEMETRY_MAX_REGISTERS +#define TELEMETRY_MAX_REGISTERS 8 +#endif + + +enum : uint8_t { + TELEMETRY_FRAMETYPE_DATA = 0x01, + TELEMETRY_FRAMETYPE_HEADER = 0x02 +} TelemetryFrameType; + + + + +class Telemetry { +public: + Telemetry(); + virtual ~Telemetry(); + virtual void init(); + void addTelemetryMotor(FOCMotor& motor); + void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); + virtual void sendTelemetry() = 0; +protected: + virtual void sendHeader() = 0; + virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) = 0; + virtual void endFrame() = 0; + + FOCMotor* motors[TELEMETRY_MAX_MOTORS]; + uint8_t numMotors = 0; + + uint8_t numRegisters; + uint8_t registers[TELEMETRY_MAX_REGISTERS]; + uint8_t registers_motor[TELEMETRY_MAX_REGISTERS]; + uint8_t frameSize; +}; + + + +class BinaryTelemetry : public Telemetry { +public: + BinaryTelemetry(); + virtual ~BinaryTelemetry(); + void sendTelemetry(); + void sendHeader(); + + virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) = 0; + virtual uint8_t writeByte(uint8_t value); + +}; + + + +#endif \ No newline at end of file diff --git a/src/voltage/GenericVoltageSense.cpp b/src/voltage/GenericVoltageSense.cpp index 3c2de3a..047993e 100644 --- a/src/voltage/GenericVoltageSense.cpp +++ b/src/voltage/GenericVoltageSense.cpp @@ -4,20 +4,21 @@ -GenericVoltageSense::GenericVoltageSense(int pin, float fullScaleVoltage, float gain, float offset, float Tf) : VoltageSense(gain, offset, Tf), pin(pin), fullScaleVoltage(fullScaleVoltage) { +GenericVoltageSense::GenericVoltageSense(int pin, float fullScaleVoltage, float gain, float offset, float Tf) : VoltageSense(gain, offset, Tf), fullScaleVoltage(fullScaleVoltage), pin(pin) { }; bool GenericVoltageSense::init(int resolution){ - pinMode(pin, INPUT_ANALOG); + pinMode(pin, INPUT); if (resolution>0) { analogReadResolution(resolution); maxValue = powl(2, resolution); } else maxValue = powl(2, ADC_RESOLUTION); + return true; }; From 37bf93dad5c3e888afe3aee8f5f5c29f984e5f3d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 24 Apr 2022 11:51:35 +0200 Subject: [PATCH 03/41] work in progress - not final state - not tested --- src/comms/ConfiguredMotors.cpp | 10 +++ src/comms/ConfiguredMotors.h | 22 +++++ src/comms/RegisterBasedCommander.cpp | 16 ++-- src/comms/RegisterBasedCommander.h | 19 ++--- ...ry_sendRegister.cpp => RegisterSender.cpp} | 43 ++++++---- src/comms/RegisterSender.h | 33 ++++++++ src/comms/ascii/ASCIIRegisterSender.cpp | 32 +++++++ src/comms/ascii/ASCIIRegisterSender.h | 25 ++++++ src/comms/binary/BinaryCommander.cpp | 23 ++--- src/comms/binary/BinaryCommander.h | 21 ++--- src/comms/binary/BinaryRegisterSender.cpp | 22 +++++ src/comms/binary/BinaryRegisterSender.h | 18 ++++ src/comms/binary/Binary_receiveRegister.cpp | 9 -- src/comms/binary/Binary_setRegister.cpp | 10 --- src/comms/serial/SerialASCIITelemetry.cpp | 15 ++++ src/comms/serial/SerialASCIITelemetry.h | 21 +++++ src/comms/serial/SerialBinaryCommander.cpp | 2 +- src/comms/serial/SerialBinaryCommander.h | 3 +- src/comms/telemetry/Telemetry.cpp | 83 +++++++++++++++---- src/comms/telemetry/Telemetry.h | 41 +++++---- 20 files changed, 348 insertions(+), 120 deletions(-) create mode 100644 src/comms/ConfiguredMotors.cpp create mode 100644 src/comms/ConfiguredMotors.h rename src/comms/{binary/Binary_sendRegister.cpp => RegisterSender.cpp} (85%) create mode 100644 src/comms/RegisterSender.h create mode 100644 src/comms/ascii/ASCIIRegisterSender.cpp create mode 100644 src/comms/ascii/ASCIIRegisterSender.h create mode 100644 src/comms/binary/BinaryRegisterSender.cpp create mode 100644 src/comms/binary/BinaryRegisterSender.h delete mode 100644 src/comms/binary/Binary_receiveRegister.cpp delete mode 100644 src/comms/binary/Binary_setRegister.cpp create mode 100644 src/comms/serial/SerialASCIITelemetry.cpp create mode 100644 src/comms/serial/SerialASCIITelemetry.h diff --git a/src/comms/ConfiguredMotors.cpp b/src/comms/ConfiguredMotors.cpp new file mode 100644 index 0000000..504c836 --- /dev/null +++ b/src/comms/ConfiguredMotors.cpp @@ -0,0 +1,10 @@ + +#include "./ConfiguredMotors.h" + + +void ConfiguredMotors::addMotor(FOCMotor& motor) { + if (numMotors < TELEMETRY_MAX_MOTORS) { + motors[numMotors] = &motor; + numMotors++; + } +}; diff --git a/src/comms/ConfiguredMotors.h b/src/comms/ConfiguredMotors.h new file mode 100644 index 0000000..8b74484 --- /dev/null +++ b/src/comms/ConfiguredMotors.h @@ -0,0 +1,22 @@ + +#ifndef __CONFIGUREDMOTORS_H__ +#define __CONFIGUREDMOTORS_H__ + +#include "common/base_classes/FOCMotor.h" + +#ifndef TELEMETRY_MAX_MOTORS +#define TELEMETRY_MAX_MOTORS 4 +#endif + + + +class ConfiguredMotors { +public: + void addMotor(FOCMotor& motor); +protected: + FOCMotor* motors[TELEMETRY_MAX_MOTORS]; + uint8_t numMotors = 0; +}; + + +#endif diff --git a/src/comms/RegisterBasedCommander.cpp b/src/comms/RegisterBasedCommander.cpp index add2e49..6d84920 100644 --- a/src/comms/RegisterBasedCommander.cpp +++ b/src/comms/RegisterBasedCommander.cpp @@ -1,9 +1,15 @@ #include "./RegisterBasedCommander.h" -void RegisterBasedCommander::addMotor(FOCMotor* motor){ - if (numMotors < COMMS_MAX_MOTORS_COMMANDABLE){ - motors[numMotors] = motor; - numMotors++; - } + + +void RegisterBasedCommander::receiveRegister(SimpleFOCRegister reg, uint8_t motor){ + }; + + + +void RegisterBasedCommander::setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum){ + +}; + diff --git a/src/comms/RegisterBasedCommander.h b/src/comms/RegisterBasedCommander.h index 3113c32..2c431d3 100644 --- a/src/comms/RegisterBasedCommander.h +++ b/src/comms/RegisterBasedCommander.h @@ -5,12 +5,10 @@ #include "./SimpleFOCRegisters.h" -#include "common/base_classes/FOCMotor.h" +#include "./RegisterSender.h" + -#ifndef COMMS_MAX_MOTORS_COMMANDABLE -#define COMMS_MAX_MOTORS_COMMANDABLE 4 -#endif #ifndef COMMS_MAX_REPORT_REGISTERS #define COMMS_MAX_REPORT_REGISTERS 7 #endif @@ -19,19 +17,20 @@ class RegisterBasedCommander { public: - virtual void addMotor(FOCMotor* motor) = 0; virtual void runCommander() = 0; protected: - virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) = 0; - virtual void receiveRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) = 0; - virtual void setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum = 0) = 0; - uint8_t numMotors = 0; + virtual void receiveRegister(SimpleFOCRegister reg, uint8_t motorNum = 0); + virtual void setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum = 0); + virtual uint8_t readByte(void* valueToSet) = 0; + virtual uint8_t readFloat(void* valueToSet) = 0; + virtual uint8_t readInt(void* valueToSet) = 0; + uint8_t curMotor = 0; SimpleFOCRegister curRegister = REG_STATUS; bool commanderror = false; bool lastcommanderror = false; uint8_t lastcommandregister = REG_STATUS; - FOCMotor* motors[COMMS_MAX_MOTORS_COMMANDABLE]; + uint8_t numReportRegisters = 0; uint8_t reportMotors[COMMS_MAX_REPORT_REGISTERS]; SimpleFOCRegister reportRegisters[COMMS_MAX_REPORT_REGISTERS]; diff --git a/src/comms/binary/Binary_sendRegister.cpp b/src/comms/RegisterSender.cpp similarity index 85% rename from src/comms/binary/Binary_sendRegister.cpp rename to src/comms/RegisterSender.cpp index 00e9c62..44ab5af 100644 --- a/src/comms/binary/Binary_sendRegister.cpp +++ b/src/comms/RegisterSender.cpp @@ -1,28 +1,35 @@ -#include "./BinaryCommander.h" +#include "./RegisterSender.h" +void RegisterSender::addMotor(FOCMotor& motor) { + if (numMotors < TELEMETRY_MAX_MOTORS) { + motors[numMotors] = &motor; + numMotors++; + } +}; + -bool BinaryCommander::sendRegister(SimpleFOCRegister reg, uint8_t motorNum){ +bool RegisterSender::sendRegister(SimpleFOCRegister reg, uint8_t motorNum){ // read the current register switch(reg) { - case REG_STATUS: - writeByte(curMotor); - writeByte((uint8_t)lastcommandregister); - writeByte((uint8_t)lastcommanderror+1); - for (int i=0;(imotor_status); - } - break; - case REG_MOTOR_ADDRESS: - writeByte(curMotor); - break; - case REG_REPORT: - for (int i=0;imotor_status); + // } + // break; + // case REG_MOTOR_ADDRESS: + // writeByte(curMotor); + // break; + // case REG_REPORT: + // for (int i=0;ienabled); break; diff --git a/src/comms/RegisterSender.h b/src/comms/RegisterSender.h new file mode 100644 index 0000000..0fd54a4 --- /dev/null +++ b/src/comms/RegisterSender.h @@ -0,0 +1,33 @@ + + +#ifndef __REGISTERSENDER_H__ +#define __REGISTERSENDER_H__ + +#include "./SimpleFOCRegisters.h" +#include "common/base_classes/FOCMotor.h" + +#ifndef TELEMETRY_MAX_MOTORS +#define TELEMETRY_MAX_MOTORS 4 +#endif + + +/** + * Register sending functionality is shared by Commander and Telemetry implementations. + * Since the code to access all the registers is quite large, it makes sense to abstract it out, + * and also make sure registers are addressed in the same way for all implementations. + */ +class RegisterSender { +public: + virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0); + void addMotor(FOCMotor& motor); + + virtual uint8_t writeByte(uint8_t value) = 0; + virtual uint8_t writeFloat(float value) = 0; + virtual uint8_t writeInt(uint32_t value) = 0; +protected: + FOCMotor* motors[TELEMETRY_MAX_MOTORS]; + uint8_t numMotors = 0; +}; + + +#endif \ No newline at end of file diff --git a/src/comms/ascii/ASCIIRegisterSender.cpp b/src/comms/ascii/ASCIIRegisterSender.cpp new file mode 100644 index 0000000..3cc6030 --- /dev/null +++ b/src/comms/ascii/ASCIIRegisterSender.cpp @@ -0,0 +1,32 @@ + +#include "./ASCIIRegisterSender.h" + +ASCIIRegisterSender::ASCIIRegisterSender(int floatPrecision) : floatPrecision(floatPrecision) { + // nix +}; + +ASCIIRegisterSender::~ASCIIRegisterSender(){ + +}; + +void ASCIIRegisterSender::init(Print *print){ + this->_print = print; +}; + +uint8_t ASCIIRegisterSender::writeChar(char value){ + return _print->print(value); +}; + +uint8_t ASCIIRegisterSender::writeByte(uint8_t value){ + return _print->print(value); +}; + + +uint8_t ASCIIRegisterSender::writeFloat(float value){ + return _print->print(value, floatPrecision); +}; + + +uint8_t ASCIIRegisterSender::writeInt(uint32_t value){ + return _print->print(value); +}; \ No newline at end of file diff --git a/src/comms/ascii/ASCIIRegisterSender.h b/src/comms/ascii/ASCIIRegisterSender.h new file mode 100644 index 0000000..ab8ffc1 --- /dev/null +++ b/src/comms/ascii/ASCIIRegisterSender.h @@ -0,0 +1,25 @@ + +#ifndef __ASCII_REGISTER_SENDER_H__ +#define __ASCII_REGISTER_SENDER_H__ + + +#include "Print.h" +#include "../RegisterSender.h" + +class ASCIIRegisterSender : public RegisterSender { +public: + ASCIIRegisterSender(int floatPrecision = 2); + virtual ~ASCIIRegisterSender(); + void init(Print *print); + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + virtual uint8_t writeChar(char value); + +protected: + Print* _print; + int floatPrecision = 2; +}; + + +#endif \ No newline at end of file diff --git a/src/comms/binary/BinaryCommander.cpp b/src/comms/binary/BinaryCommander.cpp index 1390024..5dfcff6 100644 --- a/src/comms/binary/BinaryCommander.cpp +++ b/src/comms/binary/BinaryCommander.cpp @@ -3,39 +3,26 @@ - -uint8_t BinaryCommander::writeByte(uint8_t value){ - return writeBytes(&value, 1); -}; - - - -uint8_t BinaryCommander::writeFloat(float value){ - return writeBytes(&value, 4); -}; - - - -uint8_t BinaryCommander::writeInt(uint32_t value){ - return writeBytes(&value, 4); +BinaryCommander::BinaryCommander() : BinaryRegisterSender(), RegisterBasedCommander() { + // nix }; uint8_t BinaryCommander::readByte(void* valueToSet){ - return 0; + return readBytes(valueToSet, 1); }; uint8_t BinaryCommander::readFloat(void* valueToSet){ - return 0; + return readBytes(valueToSet, 4); }; uint8_t BinaryCommander::readInt(void* valueToSet){ - return 0; + return readBytes(valueToSet, 4); }; diff --git a/src/comms/binary/BinaryCommander.h b/src/comms/binary/BinaryCommander.h index 70a8aaa..04a0a9e 100644 --- a/src/comms/binary/BinaryCommander.h +++ b/src/comms/binary/BinaryCommander.h @@ -11,24 +11,19 @@ */ #include "../RegisterBasedCommander.h" +#include "../binary/BinaryRegisterSender.h" -class BinaryCommander : public RegisterBasedCommander { +class BinaryCommander : public BinaryRegisterSender, public RegisterBasedCommander { +public: + BinaryCommander(); + virtual ~BinaryCommander(); protected: - virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) override; - virtual void receiveRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) override; - virtual void setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum = 0) override; - - virtual uint8_t writeBytes(void* valueToWrite, uint8_t numBytes) = 0; - virtual uint8_t writeByte(uint8_t value); - virtual uint8_t writeFloat(float value); - virtual uint8_t writeInt(uint32_t value); virtual uint8_t readBytes(void* valueToSet, uint8_t numBytes) = 0; - virtual uint8_t readByte(void* valueToSet); - virtual uint8_t readFloat(void* valueToSet); - virtual uint8_t readInt(void* valueToSet); - + uint8_t readByte(void* valueToSet) override; + uint8_t readFloat(void* valueToSet) override; + uint8_t readInt(void* valueToSet) override; }; diff --git a/src/comms/binary/BinaryRegisterSender.cpp b/src/comms/binary/BinaryRegisterSender.cpp new file mode 100644 index 0000000..4a45eca --- /dev/null +++ b/src/comms/binary/BinaryRegisterSender.cpp @@ -0,0 +1,22 @@ + + +#include "./BinaryRegisterSender.h" + + +uint8_t BinaryRegisterSender::writeByte(uint8_t value){ + return writeBytes(&value, 1); +}; + + + +uint8_t BinaryRegisterSender::writeFloat(float value){ + return writeBytes(&value, 4); +}; + + + +uint8_t BinaryRegisterSender::writeInt(uint32_t value){ + return writeBytes(&value, 4); +}; + + diff --git a/src/comms/binary/BinaryRegisterSender.h b/src/comms/binary/BinaryRegisterSender.h new file mode 100644 index 0000000..a2a24e5 --- /dev/null +++ b/src/comms/binary/BinaryRegisterSender.h @@ -0,0 +1,18 @@ + +#ifndef __BINARY_REGISTER_SENDER_H__ +#define __BINARY_REGISTER_SENDER_H__ + + +#include "../RegisterSender.h" + + + +class BinaryRegisterSender : public RegisterSender { + virtual uint8_t writeBytes(void* valueToWrite, uint8_t numBytes) = 0; + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; +}; + + +#endif diff --git a/src/comms/binary/Binary_receiveRegister.cpp b/src/comms/binary/Binary_receiveRegister.cpp deleted file mode 100644 index 53fe04f..0000000 --- a/src/comms/binary/Binary_receiveRegister.cpp +++ /dev/null @@ -1,9 +0,0 @@ - -#include "./BinaryCommander.h" - - - -void BinaryCommander::receiveRegister(SimpleFOCRegister reg, uint8_t motor){ - -}; - diff --git a/src/comms/binary/Binary_setRegister.cpp b/src/comms/binary/Binary_setRegister.cpp deleted file mode 100644 index 754a69c..0000000 --- a/src/comms/binary/Binary_setRegister.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include "./BinaryCommander.h" - - - - -void BinaryCommander::setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum){ - -}; - diff --git a/src/comms/serial/SerialASCIITelemetry.cpp b/src/comms/serial/SerialASCIITelemetry.cpp new file mode 100644 index 0000000..d291f33 --- /dev/null +++ b/src/comms/serial/SerialASCIITelemetry.cpp @@ -0,0 +1,15 @@ + +#include "./SerialASCIITelemetry.h" + +SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : ASCIIRegisterSender(floatPrecision), ASCIITelemetry(this) { + // nix +}; + +SerialASCIITelemetry::~SerialASCIITelemetry(){ + +}; + +void SerialASCIITelemetry::init(Print* print){ + this->ASCIIRegisterSender::init(print); + this->Telemetry::init(); +}; \ No newline at end of file diff --git a/src/comms/serial/SerialASCIITelemetry.h b/src/comms/serial/SerialASCIITelemetry.h new file mode 100644 index 0000000..8aca678 --- /dev/null +++ b/src/comms/serial/SerialASCIITelemetry.h @@ -0,0 +1,21 @@ + +#ifndef __SERIALASCIITELEMETRY_H__ +#define __SERIALASCIITELEMETRY_H__ + +#include "Arduino.h" +#include "../telemetry/Telemetry.h" +#include "../ascii/ASCIIRegisterSender.h" + +class SerialASCIITelemetry : public ASCIIRegisterSender, public ASCIITelemetry { +public: + SerialASCIITelemetry(int floatPrecision = 2); + virtual ~SerialASCIITelemetry(); + + void init(Print* print = &Serial); +}; + + + + + +#endif diff --git a/src/comms/serial/SerialBinaryCommander.cpp b/src/comms/serial/SerialBinaryCommander.cpp index c61c37d..085e817 100644 --- a/src/comms/serial/SerialBinaryCommander.cpp +++ b/src/comms/serial/SerialBinaryCommander.cpp @@ -3,7 +3,7 @@ -SerialBinaryCommander::SerialBinaryCommander(bool echo) : echo(echo) { +SerialBinaryCommander::SerialBinaryCommander(bool echo) : BinaryTelemetry(this), echo(echo) { }; diff --git a/src/comms/serial/SerialBinaryCommander.h b/src/comms/serial/SerialBinaryCommander.h index 7ab7805..9d8722c 100644 --- a/src/comms/serial/SerialBinaryCommander.h +++ b/src/comms/serial/SerialBinaryCommander.h @@ -11,7 +11,7 @@ #define SERIALBINARY_COMMAND_WRITE 0x80 -class SerialBinaryCommander : public BinaryTelemetry, public BinaryCommander { +class SerialBinaryCommander : public BinaryCommander, public BinaryTelemetry { public: SerialBinaryCommander(bool echo = false); virtual ~SerialBinaryCommander(); @@ -21,7 +21,6 @@ class SerialBinaryCommander : public BinaryTelemetry, public BinaryCommander { bool echo; protected: - using BinaryCommander::sendRegister; virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) override; virtual void endFrame() override; uint8_t readBytes(void* valueToSet, uint8_t numBytes) override; diff --git a/src/comms/telemetry/Telemetry.cpp b/src/comms/telemetry/Telemetry.cpp index a7ad967..7730837 100644 --- a/src/comms/telemetry/Telemetry.cpp +++ b/src/comms/telemetry/Telemetry.cpp @@ -4,8 +4,8 @@ -Telemetry::Telemetry(){ - +Telemetry::Telemetry() { + this->numRegisters = 0; }; @@ -18,34 +18,42 @@ Telemetry::~Telemetry(){ - - -void Telemetry::addTelemetryMotor(FOCMotor& motor){ - if (numMotors < TELEMETRY_MAX_MOTORS) - motors[numMotors++] = &motor; +void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){ + if (numRegisters<=TELEMETRY_MAX_REGISTERS) { + this->numRegisters = numRegisters; + for (uint8_t i=0; iregisters[i] = registers[i]; + if (motors!=NULL) + this->registers_motor[i] = motors[i]; + else + this->registers_motor[i] = 0; + } + } }; -void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){ - this->numRegisters = numRegisters; - // TODO +void Telemetry::init() { + sendHeader(); }; -void Telemetry::init() { - sendHeader(); +BinaryTelemetry::BinaryTelemetry(RegisterSender* sender) : sender(sender) { + // nix }; +BinaryTelemetry::~BinaryTelemetry(){ + +}; void BinaryTelemetry::sendHeader() { if (numRegisters > 0) { startFrame(numRegisters*2, TELEMETRY_FRAMETYPE_HEADER); for (uint8_t i = 0; i < numRegisters; i++) { - writeByte(registers[i]); - writeByte(registers_motor[i]); + sender->writeByte(registers[i]); + sender->writeByte(registers_motor[i]); }; endFrame(); }; @@ -54,9 +62,48 @@ void BinaryTelemetry::sendHeader() { void BinaryTelemetry::sendTelemetry(){ - startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA); - for (uint8_t i = 0; i < numRegisters; i++) { - sendRegister(static_cast(registers[i]), registers_motor[i]); + if (numRegisters > 0) { + startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA); + for (uint8_t i = 0; i < numRegisters; i++) { + sender->sendRegister(static_cast(registers[i]), registers_motor[i]); + }; + endFrame(); + } +}; + + +ASCIITelemetry::ASCIITelemetry(ASCIIRegisterSender* sender) : sender(sender) { + // nix +}; + +ASCIITelemetry::~ASCIITelemetry(){ + +}; + +void ASCIITelemetry::sendHeader() { + if (numRegisters > 0) { + sender->writeChar('H'); + sender->writeChar(' '); + for (uint8_t i = 0; i < numRegisters; i++) { + sender->writeByte(registers_motor[i]); + sender->writeChar(':'); + sender->writeByte(registers[i]); + if (i < numRegisters-1) + sender->writeChar(' '); + }; + sender->writeChar('\n'); }; - endFrame(); +}; + + + +void ASCIITelemetry::sendTelemetry(){ + if (numRegisters > 0) { + for (uint8_t i = 0; i < numRegisters; i++) { + sender->sendRegister(static_cast(registers[i]), registers_motor[i]); + if (i < numRegisters-1) + sender->writeChar(' '); + }; + sender->writeChar('\n'); + } }; diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h index 62cf8c0..f52fa82 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -3,11 +3,8 @@ #include "../SimpleFOCRegisters.h" -#include "common/base_classes/FOCMotor.h" - -#ifndef TELEMETRY_MAX_MOTORS -#define TELEMETRY_MAX_MOTORS 4 -#endif +#include "../ascii/ASCIIRegisterSender.h" +#include "../binary/BinaryRegisterSender.h" #ifndef TELEMETRY_MAX_REGISTERS #define TELEMETRY_MAX_REGISTERS 8 @@ -27,16 +24,10 @@ class Telemetry { Telemetry(); virtual ~Telemetry(); virtual void init(); - void addTelemetryMotor(FOCMotor& motor); void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); virtual void sendTelemetry() = 0; protected: virtual void sendHeader() = 0; - virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) = 0; - virtual void endFrame() = 0; - - FOCMotor* motors[TELEMETRY_MAX_MOTORS]; - uint8_t numMotors = 0; uint8_t numRegisters; uint8_t registers[TELEMETRY_MAX_REGISTERS]; @@ -48,16 +39,34 @@ class Telemetry { class BinaryTelemetry : public Telemetry { public: - BinaryTelemetry(); + BinaryTelemetry(RegisterSender* sender); virtual ~BinaryTelemetry(); - void sendTelemetry(); - void sendHeader(); + void sendTelemetry() override; + void sendHeader() override; + virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) = 0; + virtual void endFrame() = 0; +protected: + RegisterSender* sender; +}; - virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0) = 0; - virtual uint8_t writeByte(uint8_t value); + + + + +class ASCIITelemetry : public Telemetry { +public: + ASCIITelemetry(ASCIIRegisterSender* sender); + virtual ~ASCIITelemetry(); + void sendTelemetry() override; + void sendHeader() override; +protected: + ASCIIRegisterSender* sender; }; + + + #endif \ No newline at end of file From dbc2413e0410c6758c1c58a35b5c60e40e7e13ba Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 25 Mar 2023 22:53:13 +0100 Subject: [PATCH 04/41] refactoring comms code --- src/comms/ConfiguredMotors.cpp | 10 -- src/comms/ConfiguredMotors.h | 22 --- src/comms/README.md | 5 + src/comms/RegisterBasedCommander.cpp | 15 -- src/comms/RegisterBasedCommander.h | 41 ------ src/comms/RegisterReceiver.cpp | 9 ++ src/comms/RegisterReceiver.h | 15 ++ src/comms/RegisterSender.cpp | 159 ++++++++++----------- src/comms/RegisterSender.h | 21 +-- src/comms/SimpleFOCRegisters.h | 30 ++-- src/comms/ascii/ASCIIRegisterSender.cpp | 32 ----- src/comms/ascii/ASCIIRegisterSender.h | 25 ---- src/comms/binary/BinaryCommander.cpp | 28 ---- src/comms/binary/BinaryCommander.h | 32 ----- src/comms/binary/BinaryRegisterSender.cpp | 22 --- src/comms/binary/BinaryRegisterSender.h | 18 --- src/comms/i2c/I2CCommander.cpp | 10 +- src/comms/serial/README.md | 45 ++++++ src/comms/serial/SerialASCIITelemetry.cpp | 56 +++++++- src/comms/serial/SerialASCIITelemetry.h | 18 ++- src/comms/serial/SerialBinaryCommander.cpp | 79 +++++++++- src/comms/serial/SerialBinaryCommander.h | 46 ++++-- src/comms/telemetry/Telemetry.cpp | 84 +++-------- src/comms/telemetry/Telemetry.h | 63 +++----- 24 files changed, 389 insertions(+), 496 deletions(-) delete mode 100644 src/comms/ConfiguredMotors.cpp delete mode 100644 src/comms/ConfiguredMotors.h create mode 100644 src/comms/README.md delete mode 100644 src/comms/RegisterBasedCommander.cpp delete mode 100644 src/comms/RegisterBasedCommander.h create mode 100644 src/comms/RegisterReceiver.cpp create mode 100644 src/comms/RegisterReceiver.h delete mode 100644 src/comms/ascii/ASCIIRegisterSender.cpp delete mode 100644 src/comms/ascii/ASCIIRegisterSender.h delete mode 100644 src/comms/binary/BinaryCommander.cpp delete mode 100644 src/comms/binary/BinaryCommander.h delete mode 100644 src/comms/binary/BinaryRegisterSender.cpp delete mode 100644 src/comms/binary/BinaryRegisterSender.h create mode 100644 src/comms/serial/README.md diff --git a/src/comms/ConfiguredMotors.cpp b/src/comms/ConfiguredMotors.cpp deleted file mode 100644 index 504c836..0000000 --- a/src/comms/ConfiguredMotors.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include "./ConfiguredMotors.h" - - -void ConfiguredMotors::addMotor(FOCMotor& motor) { - if (numMotors < TELEMETRY_MAX_MOTORS) { - motors[numMotors] = &motor; - numMotors++; - } -}; diff --git a/src/comms/ConfiguredMotors.h b/src/comms/ConfiguredMotors.h deleted file mode 100644 index 8b74484..0000000 --- a/src/comms/ConfiguredMotors.h +++ /dev/null @@ -1,22 +0,0 @@ - -#ifndef __CONFIGUREDMOTORS_H__ -#define __CONFIGUREDMOTORS_H__ - -#include "common/base_classes/FOCMotor.h" - -#ifndef TELEMETRY_MAX_MOTORS -#define TELEMETRY_MAX_MOTORS 4 -#endif - - - -class ConfiguredMotors { -public: - void addMotor(FOCMotor& motor); -protected: - FOCMotor* motors[TELEMETRY_MAX_MOTORS]; - uint8_t numMotors = 0; -}; - - -#endif diff --git a/src/comms/README.md b/src/comms/README.md new file mode 100644 index 0000000..4469af6 --- /dev/null +++ b/src/comms/README.md @@ -0,0 +1,5 @@ + +# SimpleFOC communications support code + +This folder contains classes to support you communicating between MCUs running SimpleFOC, and other systems. + diff --git a/src/comms/RegisterBasedCommander.cpp b/src/comms/RegisterBasedCommander.cpp deleted file mode 100644 index 6d84920..0000000 --- a/src/comms/RegisterBasedCommander.cpp +++ /dev/null @@ -1,15 +0,0 @@ - -#include "./RegisterBasedCommander.h" - - - -void RegisterBasedCommander::receiveRegister(SimpleFOCRegister reg, uint8_t motor){ - -}; - - - -void RegisterBasedCommander::setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum){ - -}; - diff --git a/src/comms/RegisterBasedCommander.h b/src/comms/RegisterBasedCommander.h deleted file mode 100644 index 2c431d3..0000000 --- a/src/comms/RegisterBasedCommander.h +++ /dev/null @@ -1,41 +0,0 @@ - -#ifndef __REGISTER_BASED_COMMS_DRIVER_H__ -#define __REGISTER_BASED_COMMS_DRIVER_H__ - - - -#include "./SimpleFOCRegisters.h" -#include "./RegisterSender.h" - - - -#ifndef COMMS_MAX_REPORT_REGISTERS -#define COMMS_MAX_REPORT_REGISTERS 7 -#endif - - - -class RegisterBasedCommander { -public: - virtual void runCommander() = 0; -protected: - virtual void receiveRegister(SimpleFOCRegister reg, uint8_t motorNum = 0); - virtual void setRegister(SimpleFOCRegister reg, void* value, uint8_t motorNum = 0); - virtual uint8_t readByte(void* valueToSet) = 0; - virtual uint8_t readFloat(void* valueToSet) = 0; - virtual uint8_t readInt(void* valueToSet) = 0; - - uint8_t curMotor = 0; - SimpleFOCRegister curRegister = REG_STATUS; - bool commanderror = false; - bool lastcommanderror = false; - uint8_t lastcommandregister = REG_STATUS; - - uint8_t numReportRegisters = 0; - uint8_t reportMotors[COMMS_MAX_REPORT_REGISTERS]; - SimpleFOCRegister reportRegisters[COMMS_MAX_REPORT_REGISTERS]; -}; - - - -#endif diff --git a/src/comms/RegisterReceiver.cpp b/src/comms/RegisterReceiver.cpp new file mode 100644 index 0000000..ebcbccb --- /dev/null +++ b/src/comms/RegisterReceiver.cpp @@ -0,0 +1,9 @@ + +#include "./RegisterReceiver.h" + + + +void RegisterReceiver::setRegister(SimpleFOCRegister reg, void* value, FOCMotor* motor){ + +}; + diff --git a/src/comms/RegisterReceiver.h b/src/comms/RegisterReceiver.h new file mode 100644 index 0000000..5d46178 --- /dev/null +++ b/src/comms/RegisterReceiver.h @@ -0,0 +1,15 @@ + +#pragma once + +#include "./SimpleFOCRegisters.h" +#include "common/base_classes/FOCMotor.h" + + + +class RegisterReceiver { +protected: + virtual void setRegister(SimpleFOCRegister reg, void* value, FOCMotor* motor); + virtual uint8_t readByte(void* valueToSet) = 0; + virtual uint8_t readFloat(void* valueToSet) = 0; + virtual uint8_t readInt(void* valueToSet) = 0; +}; diff --git a/src/comms/RegisterSender.cpp b/src/comms/RegisterSender.cpp index 44ab5af..9e08f46 100644 --- a/src/comms/RegisterSender.cpp +++ b/src/comms/RegisterSender.cpp @@ -1,97 +1,86 @@ #include "./RegisterSender.h" +#include "common/foc_utils.h" +#include "BLDCMotor.h" - -void RegisterSender::addMotor(FOCMotor& motor) { - if (numMotors < TELEMETRY_MAX_MOTORS) { - motors[numMotors] = &motor; - numMotors++; - } -}; - - - -bool RegisterSender::sendRegister(SimpleFOCRegister reg, uint8_t motorNum){ +bool RegisterSender::sendRegister(SimpleFOCRegister reg, FOCMotor* motor){ // read the current register switch(reg) { - // case REG_STATUS: - // writeByte(curMotor); - // writeByte((uint8_t)lastcommandregister); - // writeByte((uint8_t)lastcommanderror+1); - // for (int i=0;(imotor_status); - // } - // break; - // case REG_MOTOR_ADDRESS: - // writeByte(curMotor); - // break; - // case REG_REPORT: - // for (int i=0;imotor_status); + break; case REG_ENABLE: - writeByte(motors[motorNum]->enabled); + writeByte(motor->enabled); break; case REG_MODULATION_MODE: - writeByte(motors[motorNum]->foc_modulation); + writeByte(motor->foc_modulation); break; case REG_TORQUE_MODE: - writeByte(motors[motorNum]->torque_controller); + writeByte(motor->torque_controller); break; case REG_CONTROL_MODE: - writeByte(motors[motorNum]->controller); + writeByte(motor->controller); break; case REG_TARGET: - writeFloat(motors[motorNum]->target); + writeFloat(motor->target); break; case REG_ANGLE: - writeFloat(motors[motorNum]->shaft_angle); + writeFloat(motor->shaft_angle); + break; + case REG_POSITION: + if (motor->sensor) { + writeInt(motor->sensor->getFullRotations()); + writeFloat(motor->sensor->getMechanicalAngle()); + } + else { + writeInt(motor->shaft_angle/_2PI); + writeFloat(fmod(motor->shaft_angle, _2PI)); + } break; case REG_VELOCITY: - writeFloat(motors[motorNum]->shaft_velocity); + writeFloat(motor->shaft_velocity); break; case REG_SENSOR_ANGLE: - if (motors[motorNum]->sensor) - writeFloat(motors[motorNum]->sensor->getAngle()); // stored angle + if (motor->sensor) + writeFloat(motor->sensor->getAngle()); // stored angle else - writeFloat(motors[motorNum]->shaft_angle); + writeFloat(motor->shaft_angle); break; case REG_VOLTAGE_Q: - writeFloat(motors[motorNum]->voltage.q); + writeFloat(motor->voltage.q); break; case REG_VOLTAGE_D: - writeFloat(motors[motorNum]->voltage.d); + writeFloat(motor->voltage.d); break; case REG_CURRENT_Q: - writeFloat(motors[motorNum]->current.q); + writeFloat(motor->current.q); break; case REG_CURRENT_D: - writeFloat(motors[motorNum]->current.d); + writeFloat(motor->current.d); break; case REG_CURRENT_A: - if (motors[motorNum]->current_sense) // TODO check if current sense can be called from inside this callback function - writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().a); + if (motor->current_sense) + writeFloat(motor->current_sense->getPhaseCurrents().a); else writeFloat(0.0f); break; case REG_CURRENT_B: - if (motors[motorNum]->current_sense) - writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().b); + if (motor->current_sense) + writeFloat(motor->current_sense->getPhaseCurrents().b); else writeFloat(0.0f); break; case REG_CURRENT_C: - if (motors[motorNum]->current_sense) - writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().c); + if (motor->current_sense) + writeFloat(motor->current_sense->getPhaseCurrents().c); else writeFloat(0.0f); break; case REG_CURRENT_ABC: - if (motors[motorNum]->current_sense) { - PhaseCurrent_s currents = motors[motorNum]->current_sense->getPhaseCurrents(); + if (motor->current_sense) { + PhaseCurrent_s currents = motor->current_sense->getPhaseCurrents(); writeFloat(currents.a); writeFloat(currents.b); writeFloat(currents.c); @@ -103,88 +92,98 @@ bool RegisterSender::sendRegister(SimpleFOCRegister reg, uint8_t motorNum){ } break; case REG_VEL_PID_P: - writeFloat(motors[motorNum]->PID_velocity.P); + writeFloat(motor->PID_velocity.P); break; case REG_VEL_PID_I: - writeFloat(motors[motorNum]->PID_velocity.I); + writeFloat(motor->PID_velocity.I); break; case REG_VEL_PID_D: - writeFloat(motors[motorNum]->PID_velocity.D); + writeFloat(motor->PID_velocity.D); break; case REG_VEL_LPF_T: - writeFloat(motors[motorNum]->LPF_velocity.Tf); + writeFloat(motor->LPF_velocity.Tf); break; case REG_ANG_PID_P: - writeFloat(motors[motorNum]->P_angle.P); + writeFloat(motor->P_angle.P); break; case REG_VEL_LIMIT: - writeFloat(motors[motorNum]->velocity_limit); + writeFloat(motor->velocity_limit); break; case REG_VEL_MAX_RAMP: - writeFloat(motors[motorNum]->PID_velocity.output_ramp); + writeFloat(motor->PID_velocity.output_ramp); break; case REG_CURQ_PID_P: - writeFloat(motors[motorNum]->PID_current_q.P); + writeFloat(motor->PID_current_q.P); break; case REG_CURQ_PID_I: - writeFloat(motors[motorNum]->PID_current_q.I); + writeFloat(motor->PID_current_q.I); break; case REG_CURQ_PID_D: - writeFloat(motors[motorNum]->PID_current_q.D); + writeFloat(motor->PID_current_q.D); break; case REG_CURQ_LPF_T: - writeFloat(motors[motorNum]->LPF_current_q.Tf); + writeFloat(motor->LPF_current_q.Tf); break; case REG_CURD_PID_P: - writeFloat(motors[motorNum]->PID_current_d.P); + writeFloat(motor->PID_current_d.P); break; case REG_CURD_PID_I: - writeFloat(motors[motorNum]->PID_current_d.I); + writeFloat(motor->PID_current_d.I); break; case REG_CURD_PID_D: - writeFloat(motors[motorNum]->PID_current_d.D); + writeFloat(motor->PID_current_d.D); break; case REG_CURD_LPF_T: - writeFloat(motors[motorNum]->LPF_current_d.Tf); + writeFloat(motor->LPF_current_d.Tf); break; - case REG_MAX_VOLTAGE: - writeFloat(motors[motorNum]->voltage_limit); + case REG_VOLTAGE_LIMIT: + writeFloat(motor->voltage_limit); break; - case REG_MAX_CURRENT: - writeFloat(motors[motorNum]->current_limit); + case REG_CURRENT_LIMIT: + writeFloat(motor->current_limit); break; case REG_MOTION_DOWNSAMPLE: - writeByte((uint8_t)motors[motorNum]->motion_downsample); // TODO int will have different sizes on different platforms - // but using uint32 doesn't compile clean on all, e.g. RP2040 - break; + writeByte((uint8_t)motor->motion_downsample); + break; + case REG_DRIVER_VOLTAGE_LIMIT: + writeFloat(((BLDCMotor*)motor)->driver->voltage_limit); + break; + case REG_PWM_FREQUENCY: + writeFloat(((BLDCMotor*)motor)->driver->pwm_frequency); + break; case REG_ZERO_ELECTRIC_ANGLE: - writeFloat(motors[motorNum]->zero_electric_angle); + writeFloat(motor->zero_electric_angle); break; case REG_SENSOR_DIRECTION: - writeByte((int8_t)motors[motorNum]->sensor_direction); + writeByte((int8_t)motor->sensor_direction); break; case REG_ZERO_OFFSET: - writeFloat(motors[motorNum]->sensor_offset); + writeFloat(motor->sensor_offset); break; case REG_PHASE_RESISTANCE: - writeFloat(motors[motorNum]->phase_resistance); + writeFloat(motor->phase_resistance); + break; + case REG_KV: + writeFloat(motor->KV_rating); + break; + case REG_INDUCTANCE: + writeFloat(motor->phase_inductance); break; case REG_POLE_PAIRS: - writeByte((uint8_t)motors[motorNum]->pole_pairs); + writeByte((uint8_t)motor->pole_pairs); break; case REG_SYS_TIME: // TODO how big is millis()? Same on all platforms? writeInt((int)millis()); break; + // unknown register or write only register (no read) or can't handle in superclass case REG_NUM_MOTORS: - writeByte(numMotors); - break; - - // unknown register or write only register (no read) + case REG_REPORT: + case REG_MOTOR_ADDRESS: case REG_ENABLE_ALL: default: writeByte(0); // TODO what to send in this case? diff --git a/src/comms/RegisterSender.h b/src/comms/RegisterSender.h index 0fd54a4..b2486a1 100644 --- a/src/comms/RegisterSender.h +++ b/src/comms/RegisterSender.h @@ -1,33 +1,18 @@ - -#ifndef __REGISTERSENDER_H__ -#define __REGISTERSENDER_H__ +#pragma once #include "./SimpleFOCRegisters.h" #include "common/base_classes/FOCMotor.h" -#ifndef TELEMETRY_MAX_MOTORS -#define TELEMETRY_MAX_MOTORS 4 -#endif - - /** * Register sending functionality is shared by Commander and Telemetry implementations. * Since the code to access all the registers is quite large, it makes sense to abstract it out, * and also make sure registers are addressed in the same way for all implementations. */ class RegisterSender { -public: - virtual bool sendRegister(SimpleFOCRegister reg, uint8_t motorNum = 0); - void addMotor(FOCMotor& motor); - +protected: + virtual bool sendRegister(SimpleFOCRegister reg, FOCMotor* motor); virtual uint8_t writeByte(uint8_t value) = 0; virtual uint8_t writeFloat(float value) = 0; virtual uint8_t writeInt(uint32_t value) = 0; -protected: - FOCMotor* motors[TELEMETRY_MAX_MOTORS]; - uint8_t numMotors = 0; }; - - -#endif \ No newline at end of file diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index faf6ec8..83eacbd 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -1,10 +1,10 @@ -#ifndef __I2CCOMMANDERREGISTERS_H__ -#define __I2CCOMMANDERREGISTERS_H__ + +#pragma once #include typedef enum : uint8_t { - REG_STATUS = 0x00, // RO - 3 bytes (currently set motor, last command register, last command error status) + 1 byte / motor (motor stati) + REG_STATUS = 0x00, // RO - 1 byte (motor status) REG_MOTOR_ADDRESS = 0x01, // R/W - 1 byte REG_REPORT = 0x02, // R/W - Write: variable, Read: variable, up to 32 bytes REG_ENABLE_ALL = 0x03, // WO - 1 byte @@ -15,7 +15,7 @@ typedef enum : uint8_t { REG_TARGET = 0x08, // R/W - float REG_ANGLE = 0x09, // RO - float - REG_POSITION = 0x10, // RO - double + REG_POSITION = 0x10, // RO - int32_t full rotations + float position (0-2PI, in radians) (4 bytes + 4 bytes) REG_VELOCITY = 0x11, // RO - float REG_SENSOR_ANGLE = 0x12, // RO - float @@ -27,6 +27,7 @@ typedef enum : uint8_t { REG_CURRENT_B = 0x25, // RO - float REG_CURRENT_C = 0x26, // RO - float REG_CURRENT_ABC = 0x27, // RO - 3xfloat = 12 bytes + REG_CURRENT_DC = 0x28, // RO - float REG_VEL_PID_P = 0x30, // R/W - float REG_VEL_PID_I = 0x31, // R/W - float @@ -45,23 +46,20 @@ typedef enum : uint8_t { REG_CURD_PID_D = 0x46, // R/W - float REG_CURD_LPF_T = 0x47, // R/W - float - REG_MAX_VOLTAGE = 0x50, // R/W - float - REG_MAX_CURRENT = 0x51, // R/W - float + REG_VOLTAGE_LIMIT = 0x50, // R/W - float + REG_CURRENT_LIMIT = 0x51, // R/W - float REG_MOTION_DOWNSAMPLE = 0x52, // R/W - uint32_t + REG_DRIVER_VOLTAGE_LIMIT = 0x53,// R/W - float + REG_PWM_FREQUENCY = 0x54, // R/W - uint32_t REG_ZERO_ELECTRIC_ANGLE = 0x60, // RO - float REG_SENSOR_DIRECTION = 0x61, // RO - 1 byte REG_ZERO_OFFSET = 0x62, // R/W - float REG_POLE_PAIRS = 0x63, // RO - uint32_t - REG_PHASE_RESISTANCE = 0x64, // RO - float - - REG_NUM_MOTORS = 0x70, // RO - 1 byte - REG_SYS_TIME = 0x71, // RO - uint32_t + REG_PHASE_RESISTANCE = 0x64, // R/W - float + REG_KV = 0x65, // R/W - float + REG_INDUCTANCE = 0x66, // R/W - float + REG_NUM_MOTORS = 0x70, // RO - 1 byte + REG_SYS_TIME = 0x71, // RO - uint32_t } SimpleFOCRegister; - - - - - -#endif \ No newline at end of file diff --git a/src/comms/ascii/ASCIIRegisterSender.cpp b/src/comms/ascii/ASCIIRegisterSender.cpp deleted file mode 100644 index 3cc6030..0000000 --- a/src/comms/ascii/ASCIIRegisterSender.cpp +++ /dev/null @@ -1,32 +0,0 @@ - -#include "./ASCIIRegisterSender.h" - -ASCIIRegisterSender::ASCIIRegisterSender(int floatPrecision) : floatPrecision(floatPrecision) { - // nix -}; - -ASCIIRegisterSender::~ASCIIRegisterSender(){ - -}; - -void ASCIIRegisterSender::init(Print *print){ - this->_print = print; -}; - -uint8_t ASCIIRegisterSender::writeChar(char value){ - return _print->print(value); -}; - -uint8_t ASCIIRegisterSender::writeByte(uint8_t value){ - return _print->print(value); -}; - - -uint8_t ASCIIRegisterSender::writeFloat(float value){ - return _print->print(value, floatPrecision); -}; - - -uint8_t ASCIIRegisterSender::writeInt(uint32_t value){ - return _print->print(value); -}; \ No newline at end of file diff --git a/src/comms/ascii/ASCIIRegisterSender.h b/src/comms/ascii/ASCIIRegisterSender.h deleted file mode 100644 index ab8ffc1..0000000 --- a/src/comms/ascii/ASCIIRegisterSender.h +++ /dev/null @@ -1,25 +0,0 @@ - -#ifndef __ASCII_REGISTER_SENDER_H__ -#define __ASCII_REGISTER_SENDER_H__ - - -#include "Print.h" -#include "../RegisterSender.h" - -class ASCIIRegisterSender : public RegisterSender { -public: - ASCIIRegisterSender(int floatPrecision = 2); - virtual ~ASCIIRegisterSender(); - void init(Print *print); - uint8_t writeByte(uint8_t value) override; - uint8_t writeFloat(float value) override; - uint8_t writeInt(uint32_t value) override; - virtual uint8_t writeChar(char value); - -protected: - Print* _print; - int floatPrecision = 2; -}; - - -#endif \ No newline at end of file diff --git a/src/comms/binary/BinaryCommander.cpp b/src/comms/binary/BinaryCommander.cpp deleted file mode 100644 index 5dfcff6..0000000 --- a/src/comms/binary/BinaryCommander.cpp +++ /dev/null @@ -1,28 +0,0 @@ - -#include "./BinaryCommander.h" - - - -BinaryCommander::BinaryCommander() : BinaryRegisterSender(), RegisterBasedCommander() { - // nix -}; - - - -uint8_t BinaryCommander::readByte(void* valueToSet){ - return readBytes(valueToSet, 1); -}; - - - -uint8_t BinaryCommander::readFloat(void* valueToSet){ - return readBytes(valueToSet, 4); -}; - - - -uint8_t BinaryCommander::readInt(void* valueToSet){ - return readBytes(valueToSet, 4); -}; - - diff --git a/src/comms/binary/BinaryCommander.h b/src/comms/binary/BinaryCommander.h deleted file mode 100644 index 04a0a9e..0000000 --- a/src/comms/binary/BinaryCommander.h +++ /dev/null @@ -1,32 +0,0 @@ - -#ifndef __BINARY_COMMS_DRIVER_H__ -#define __BINARY_COMMS_DRIVER_H__ - -/* - - Abstracts the binary communication protocol, seperating the part that - interacts with SimpleFOC from the comms SPI, so individual binary comms drivers - don't have to all implement the register setting/getting functions. - -*/ - -#include "../RegisterBasedCommander.h" -#include "../binary/BinaryRegisterSender.h" - - - -class BinaryCommander : public BinaryRegisterSender, public RegisterBasedCommander { -public: - BinaryCommander(); - virtual ~BinaryCommander(); -protected: - virtual uint8_t readBytes(void* valueToSet, uint8_t numBytes) = 0; - uint8_t readByte(void* valueToSet) override; - uint8_t readFloat(void* valueToSet) override; - uint8_t readInt(void* valueToSet) override; -}; - - - -#endif - diff --git a/src/comms/binary/BinaryRegisterSender.cpp b/src/comms/binary/BinaryRegisterSender.cpp deleted file mode 100644 index 4a45eca..0000000 --- a/src/comms/binary/BinaryRegisterSender.cpp +++ /dev/null @@ -1,22 +0,0 @@ - - -#include "./BinaryRegisterSender.h" - - -uint8_t BinaryRegisterSender::writeByte(uint8_t value){ - return writeBytes(&value, 1); -}; - - - -uint8_t BinaryRegisterSender::writeFloat(float value){ - return writeBytes(&value, 4); -}; - - - -uint8_t BinaryRegisterSender::writeInt(uint32_t value){ - return writeBytes(&value, 4); -}; - - diff --git a/src/comms/binary/BinaryRegisterSender.h b/src/comms/binary/BinaryRegisterSender.h deleted file mode 100644 index a2a24e5..0000000 --- a/src/comms/binary/BinaryRegisterSender.h +++ /dev/null @@ -1,18 +0,0 @@ - -#ifndef __BINARY_REGISTER_SENDER_H__ -#define __BINARY_REGISTER_SENDER_H__ - - -#include "../RegisterSender.h" - - - -class BinaryRegisterSender : public RegisterSender { - virtual uint8_t writeBytes(void* valueToWrite, uint8_t numBytes) = 0; - uint8_t writeByte(uint8_t value) override; - uint8_t writeFloat(float value) override; - uint8_t writeInt(uint32_t value) override; -}; - - -#endif diff --git a/src/comms/i2c/I2CCommander.cpp b/src/comms/i2c/I2CCommander.cpp index a7731ba..737b2ee 100644 --- a/src/comms/i2c/I2CCommander.cpp +++ b/src/comms/i2c/I2CCommander.cpp @@ -182,10 +182,10 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu case REG_CURD_LPF_T: readBytes(&(motors[motorNum]->LPF_current_d.Tf), 4); break; - case REG_MAX_VOLTAGE: + case REG_VOLTAGE_LIMIT: readBytes(&(motors[motorNum]->voltage_limit), 4); break; - case REG_MAX_CURRENT: + case REG_CURRENT_LIMIT: readBytes(&(motors[motorNum]->current_limit), 4); break; case REG_MOTION_DOWNSAMPLE: @@ -249,7 +249,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { _wire->write((uint8_t)lastcommandregister); _wire->write((uint8_t)lastcommanderror+1); for (int i=0;(iwrite(motors[motorNum]->motor_status); + _wire->write(motors[i]->motor_status); } break; case REG_MOTOR_ADDRESS: @@ -379,10 +379,10 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { writeFloat(motors[motorNum]->LPF_current_d.Tf); break; - case REG_MAX_VOLTAGE: + case REG_VOLTAGE_LIMIT: writeFloat(motors[motorNum]->voltage_limit); break; - case REG_MAX_CURRENT: + case REG_CURRENT_LIMIT: writeFloat(motors[motorNum]->current_limit); break; case REG_MOTION_DOWNSAMPLE: diff --git a/src/comms/serial/README.md b/src/comms/serial/README.md new file mode 100644 index 0000000..7096e6f --- /dev/null +++ b/src/comms/serial/README.md @@ -0,0 +1,45 @@ + +# Serial communications classes + +Serial communications classes for register-based control and telemetry from SimpleFOC. + +## SerialASCIITelemetry + +Telemetry class that sends telemetry as ASCII on a serial port. Similar to the classic "monitoring" functionality of SimpleFOC, but allows you to configure telemetry based on most of the defined registers. + +Usage: + +```c++ + +SerialASCIITelemetry telemetry = SerialASCIITelemetry(); // number of float digits to display + +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(2, [REG_VELOCITY, REG_VOLTAGE_Q]); + telemetry.init(); + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` + +Some options are supported: + +```c++ + telemetry.floatPrecision = 4; // send floats with 4 decimal places + telemetry.min_elapsed_time = 0; // microseconds between sending telemetry + telemetry.downsample = 100; // send every this many loop iterations +``` + + + +## SerialBinaryCommander + +Control SimpleFOC via a binary protocol over the serial port. The standard SimpleFOC registers are used. + +TODO document the protocol \ No newline at end of file diff --git a/src/comms/serial/SerialASCIITelemetry.cpp b/src/comms/serial/SerialASCIITelemetry.cpp index d291f33..084b940 100644 --- a/src/comms/serial/SerialASCIITelemetry.cpp +++ b/src/comms/serial/SerialASCIITelemetry.cpp @@ -1,8 +1,8 @@ #include "./SerialASCIITelemetry.h" -SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : ASCIIRegisterSender(floatPrecision), ASCIITelemetry(this) { - // nix +SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : Telemetry() { + this->floatPrecision = floatPrecision; }; SerialASCIITelemetry::~SerialASCIITelemetry(){ @@ -10,6 +10,56 @@ SerialASCIITelemetry::~SerialASCIITelemetry(){ }; void SerialASCIITelemetry::init(Print* print){ - this->ASCIIRegisterSender::init(print); + this->_print = print; this->Telemetry::init(); +}; + + + +void SerialASCIITelemetry::sendHeader() { + if (numRegisters > 0) { + writeChar('H'); + writeChar(' '); + for (uint8_t i = 0; i < numRegisters; i++) { + writeByte(registers_motor[i]); + writeChar(':'); + writeByte(registers[i]); + if (i < numRegisters-1) + writeChar(' '); + }; + writeChar('\n'); + }; +}; + + + +void SerialASCIITelemetry::sendTelemetry(){ + if (numRegisters > 0) { + for (uint8_t i = 0; i < numRegisters; i++) { + sendRegister(static_cast(registers[i]), motors[registers_motor[i]]); + if (i < numRegisters-1) + writeChar(' '); + }; + writeChar('\n'); + } +}; + + + +uint8_t SerialASCIITelemetry::writeChar(char value){ + return _print->print(value); +}; + +uint8_t SerialASCIITelemetry::writeByte(uint8_t value){ + return _print->print(value); +}; + + +uint8_t SerialASCIITelemetry::writeFloat(float value){ + return _print->print(value, floatPrecision); +}; + + +uint8_t SerialASCIITelemetry::writeInt(uint32_t value){ + return _print->print(value); }; \ No newline at end of file diff --git a/src/comms/serial/SerialASCIITelemetry.h b/src/comms/serial/SerialASCIITelemetry.h index 8aca678..e218f3b 100644 --- a/src/comms/serial/SerialASCIITelemetry.h +++ b/src/comms/serial/SerialASCIITelemetry.h @@ -1,21 +1,27 @@ -#ifndef __SERIALASCIITELEMETRY_H__ -#define __SERIALASCIITELEMETRY_H__ +#pragma once #include "Arduino.h" #include "../telemetry/Telemetry.h" -#include "../ascii/ASCIIRegisterSender.h" -class SerialASCIITelemetry : public ASCIIRegisterSender, public ASCIITelemetry { +class SerialASCIITelemetry : public Telemetry { public: SerialASCIITelemetry(int floatPrecision = 2); virtual ~SerialASCIITelemetry(); void init(Print* print = &Serial); -}; +protected: + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + uint8_t writeChar(char value); + void sendTelemetry() override; + void sendHeader() override; + Print* _print; + int floatPrecision = 2; +}; -#endif diff --git a/src/comms/serial/SerialBinaryCommander.cpp b/src/comms/serial/SerialBinaryCommander.cpp index 085e817..07bd87c 100644 --- a/src/comms/serial/SerialBinaryCommander.cpp +++ b/src/comms/serial/SerialBinaryCommander.cpp @@ -3,7 +3,7 @@ -SerialBinaryCommander::SerialBinaryCommander(bool echo) : BinaryTelemetry(this), echo(echo) { +SerialBinaryCommander::SerialBinaryCommander(bool echo) : Telemetry(), echo(echo) { }; @@ -22,25 +22,27 @@ void SerialBinaryCommander::init(Stream &serial) { -void SerialBinaryCommander::runCommander() { +void SerialBinaryCommander::run() { if (_serial->available()>2) { // TODO make this work with partial packets... uint8_t command = _serial->read(); uint8_t registerNum = _serial->read(); uint8_t motorNum = _serial->read(); if (command==SERIALBINARY_COMMAND_READ){ - startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); - sendRegister(static_cast(registerNum), motorNum); + startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect + sendRegister(static_cast(registerNum), motors[motorNum]); endFrame(); } else if (command==SERIALBINARY_COMMAND_WRITE) { - receiveRegister(static_cast(registerNum), motorNum); + setRegister(static_cast(registerNum), NULL, motors[motorNum]); if (echo) { - startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); - sendRegister(static_cast(registerNum), motorNum); + startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect + sendRegister(static_cast(registerNum), motors[motorNum]); endFrame(); } } } + // and handle the telemetry + this->Telemetry::run(); }; @@ -86,3 +88,66 @@ void SerialBinaryCommander::endFrame(){ + +uint8_t SerialBinaryCommander::writeByte(uint8_t value){ + return writeBytes(&value, 1); +}; + + + +uint8_t SerialBinaryCommander::writeFloat(float value){ + return writeBytes(&value, 4); +}; + + + +uint8_t SerialBinaryCommander::writeInt(uint32_t value){ + return writeBytes(&value, 4); +}; + + + + +uint8_t SerialBinaryCommander::readByte(void* valueToSet){ + return readBytes(valueToSet, 1); +}; + + + +uint8_t SerialBinaryCommander::readFloat(void* valueToSet){ + return readBytes(valueToSet, 4); +}; + + + +uint8_t SerialBinaryCommander::readInt(void* valueToSet){ + return readBytes(valueToSet, 4); +}; + + + + + + +void SerialBinaryCommander::sendHeader() { + if (numRegisters > 0) { + startFrame(numRegisters*2, TELEMETRY_FRAMETYPE_HEADER); + for (uint8_t i = 0; i < numRegisters; i++) { + writeByte(registers[i]); + writeByte(registers_motor[i]); + }; + endFrame(); + }; +}; + + + +void SerialBinaryCommander::sendTelemetry(){ + if (numRegisters > 0) { + startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA); + for (uint8_t i = 0; i < numRegisters; i++) { + sendRegister(static_cast(registers[i]), motors[registers_motor[i]]); + }; + endFrame(); + } +}; diff --git a/src/comms/serial/SerialBinaryCommander.h b/src/comms/serial/SerialBinaryCommander.h index 9d8722c..2f5bb65 100644 --- a/src/comms/serial/SerialBinaryCommander.h +++ b/src/comms/serial/SerialBinaryCommander.h @@ -1,35 +1,57 @@ -#ifndef __SERIAL_BINARY_COMMANDER_H__ -#define __SERIAL_BINARY_COMMANDER_H__ +#pragma once #include +#include "../RegisterReceiver.h" #include "../telemetry/Telemetry.h" -#include "../binary/BinaryCommander.h" + + +#ifndef COMMS_MAX_REPORT_REGISTERS +#define COMMS_MAX_REPORT_REGISTERS 7 +#endif + #define SERIALBINARY_FRAMETYPE_REGISTER 0x03 #define SERIALBINARY_COMMAND_READ 0x00 #define SERIALBINARY_COMMAND_WRITE 0x80 -class SerialBinaryCommander : public BinaryCommander, public BinaryTelemetry { +class SerialBinaryCommander : public Telemetry, public RegisterReceiver { public: SerialBinaryCommander(bool echo = false); virtual ~SerialBinaryCommander(); void init(Stream &serial = Serial); - void runCommander() override; + void run(); + bool echo; protected: - virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) override; - virtual void endFrame() override; - uint8_t readBytes(void* valueToSet, uint8_t numBytes) override; - uint8_t writeBytes(void* valueToSend, uint8_t numBytes) override; + virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA); + virtual void endFrame(); + uint8_t readBytes(void* valueToSet, uint8_t numBytes); + uint8_t writeBytes(void* valueToSend, uint8_t numBytes); - Stream* _serial; -}; + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + + uint8_t readByte(void* valueToSet) override; + uint8_t readFloat(void* valueToSet) override; + uint8_t readInt(void* valueToSet) override; + void sendTelemetry() override; + void sendHeader() override; + uint8_t curMotor = 0; + SimpleFOCRegister curRegister = REG_STATUS; + bool commanderror = false; + bool lastcommanderror = false; + uint8_t lastcommandregister = REG_STATUS; + uint8_t numReportRegisters = 0; + uint8_t reportMotors[COMMS_MAX_REPORT_REGISTERS]; + SimpleFOCRegister reportRegisters[COMMS_MAX_REPORT_REGISTERS]; -#endif \ No newline at end of file + Stream* _serial; +}; diff --git a/src/comms/telemetry/Telemetry.cpp b/src/comms/telemetry/Telemetry.cpp index 7730837..6a6c446 100644 --- a/src/comms/telemetry/Telemetry.cpp +++ b/src/comms/telemetry/Telemetry.cpp @@ -34,76 +34,34 @@ void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, void Telemetry::init() { - sendHeader(); + headerSent = false; }; -BinaryTelemetry::BinaryTelemetry(RegisterSender* sender) : sender(sender) { - // nix -}; - -BinaryTelemetry::~BinaryTelemetry(){ - -}; - - -void BinaryTelemetry::sendHeader() { - if (numRegisters > 0) { - startFrame(numRegisters*2, TELEMETRY_FRAMETYPE_HEADER); - for (uint8_t i = 0; i < numRegisters; i++) { - sender->writeByte(registers[i]); - sender->writeByte(registers_motor[i]); - }; - endFrame(); - }; -}; - - - -void BinaryTelemetry::sendTelemetry(){ - if (numRegisters > 0) { - startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA); - for (uint8_t i = 0; i < numRegisters; i++) { - sender->sendRegister(static_cast(registers[i]), registers_motor[i]); - }; - endFrame(); +void Telemetry::run() { + if (numRegisters<1) + return; + if (!headerSent) { + sendHeader(); + headerSent = true; } -}; - - -ASCIITelemetry::ASCIITelemetry(ASCIIRegisterSender* sender) : sender(sender) { - // nix -}; - -ASCIITelemetry::~ASCIITelemetry(){ - -}; - -void ASCIITelemetry::sendHeader() { - if (numRegisters > 0) { - sender->writeChar('H'); - sender->writeChar(' '); - for (uint8_t i = 0; i < numRegisters; i++) { - sender->writeByte(registers_motor[i]); - sender->writeChar(':'); - sender->writeByte(registers[i]); - if (i < numRegisters-1) - sender->writeChar(' '); - }; - sender->writeChar('\n'); - }; -}; + if (downsampleCnt++ < downsample) return; + downsampleCnt = 0; + if (min_elapsed_time > 0) { + long now = _micros(); + if (now - last_run_time < min_elapsed_time) return; + last_run_time = now; + } + sendTelemetry(); +} -void ASCIITelemetry::sendTelemetry(){ - if (numRegisters > 0) { - for (uint8_t i = 0; i < numRegisters; i++) { - sender->sendRegister(static_cast(registers[i]), registers_motor[i]); - if (i < numRegisters-1) - sender->writeChar(' '); - }; - sender->writeChar('\n'); +void Telemetry::addMotor(FOCMotor* motor) { + if (numMotors < TELEMETRY_MAX_MOTORS) { + motors[numMotors] = motor; + numMotors++; } }; + diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h index f52fa82..953e354 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -1,15 +1,20 @@ -#ifndef __TELEMETRY_H__ -#define __TELEMETRY_H__ +#pragma once #include "../SimpleFOCRegisters.h" -#include "../ascii/ASCIIRegisterSender.h" -#include "../binary/BinaryRegisterSender.h" +#include "../RegisterSender.h" #ifndef TELEMETRY_MAX_REGISTERS #define TELEMETRY_MAX_REGISTERS 8 #endif +#ifndef TELEMETRY_MAX_MOTORS +#define TELEMETRY_MAX_MOTORS 4 +#endif + + +#define DEF_TELEMETRY_DOWNSAMPLE 100 + enum : uint8_t { TELEMETRY_FRAMETYPE_DATA = 0x01, @@ -19,54 +24,30 @@ enum : uint8_t { -class Telemetry { +class Telemetry : public RegisterSender { public: Telemetry(); virtual ~Telemetry(); virtual void init(); + void addMotor(FOCMotor* motor); void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); - virtual void sendTelemetry() = 0; + void run(); + + uint16_t downsample = DEF_TELEMETRY_DOWNSAMPLE; + uint32_t min_elapsed_time = 0; protected: + virtual void sendTelemetry() = 0; virtual void sendHeader() = 0; + FOCMotor* motors[TELEMETRY_MAX_MOTORS]; + uint8_t numMotors = 0; + uint8_t numRegisters; uint8_t registers[TELEMETRY_MAX_REGISTERS]; uint8_t registers_motor[TELEMETRY_MAX_REGISTERS]; uint8_t frameSize; + bool headerSent; + long last_run_time = 0; + uint16_t downsampleCnt = 0; }; - - -class BinaryTelemetry : public Telemetry { -public: - BinaryTelemetry(RegisterSender* sender); - virtual ~BinaryTelemetry(); - void sendTelemetry() override; - void sendHeader() override; - virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA) = 0; - virtual void endFrame() = 0; -protected: - RegisterSender* sender; -}; - - - - - - -class ASCIITelemetry : public Telemetry { -public: - ASCIITelemetry(ASCIIRegisterSender* sender); - virtual ~ASCIITelemetry(); - void sendTelemetry() override; - void sendHeader() override; -protected: - ASCIIRegisterSender* sender; -}; - - - - - - -#endif \ No newline at end of file From edeccded7ca05d3c0d80e7fbde46541588307b14 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 04:53:27 -0500 Subject: [PATCH 05/41] Created SmoothingSensor The example sketch is untested due to lack of hardware, but minimal changes from the original hall sensor velocity example so it should work. --- examples/encoders/smoothing/smoothing.ino | 141 +++++++++++++++++++++ src/encoders/smoothing/README.md | 69 ++++++++++ src/encoders/smoothing/SmoothingSensor.cpp | 57 +++++++++ src/encoders/smoothing/SmoothingSensor.h | 46 +++++++ 4 files changed, 313 insertions(+) create mode 100644 examples/encoders/smoothing/smoothing.ino create mode 100644 src/encoders/smoothing/README.md create mode 100644 src/encoders/smoothing/SmoothingSensor.cpp create mode 100644 src/encoders/smoothing/SmoothingSensor.h diff --git a/examples/encoders/smoothing/smoothing.ino b/examples/encoders/smoothing/smoothing.ino new file mode 100644 index 0000000..1ff658d --- /dev/null +++ b/examples/encoders/smoothing/smoothing.ino @@ -0,0 +1,141 @@ +/** + * + * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor + * Steps: + * 1) Configure the motor and sensor + * 2) Run the code + * 3) Set the target velocity (in radians per second) from serial terminal + * 4) Try with and without smoothing to see the difference (send E1 and E0 commands from serial terminal) + * + * + * + * NOTE : + * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor + * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger + * + */ +#include +// software interrupt library +#include +#include +#include +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +HallSensor sensor = HallSensor(2, 3, 4, 11); +// wrapper instance +SmoothingSensor smooth(sensor, motor); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} +// If no available hadware interrupt pins use the software interrupt +PciListenerImp listenerIndex(sensor.pinC, doC); + +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } + +void enableSmoothing(char* cmd) { + float enable; + command.scalar(&enable, cmd); + motor.linkSensor(enable == 0 ? (Sensor*)&sensor : (Sensor*)&smooth); +} + +void setup() { + + // initialize sensor sensor hardware + sensor.init(); + sensor.enableInterrupts(doA, doB); //, doC); + // software interrupts + PciManager.registerListener(&listenerIndex); + // set SmoothingSensor phase correction for hall sensors + smooth.phase_correction = -_PI_6; + // link the SmoothingSensor to the motor + motor.linkSensor(&smooth); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + // add smoothing enable/disable command E (send E0 to use hall sensor alone, or E1 to use smoothing) + command.add('E', enableSmoothing, "enable smoothing"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + _delay(1000); +} + + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_velocity); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/src/encoders/smoothing/README.md b/src/encoders/smoothing/README.md new file mode 100644 index 0000000..733284c --- /dev/null +++ b/src/encoders/smoothing/README.md @@ -0,0 +1,69 @@ +# Smoothing Sensor + +by [@dekutree64](https://github.com/dekutree64) + +A SimpleFOC Sensor wrapper implementation which adds angle extrapolation + +Please also see our [forum thread](https://community.simplefoc.com/t/smoothingsensor-experimental-sensor-angle-extrapoltion/3105) on this topic. + + +SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better quality angle readings from sensors that are low resolution or are slow to communicate. It provides no improvement for sensors that are high resolution and quick to update. It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity from the motor to predict what the angle should be at the current time. + + +## Hardware setup + +Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without smoothing first. Once things are working and tuned without smoothing, you can add the SmoothingSensor to see if you get an improvement. + + +## Softwate setup + +The SmoothingSensor acts as a wrapper to the actual sensor class. When creating the SmoothingSensor object, provide the real sensor to the constructor of SmoothingSensor. Initialize the real sensor instance as normal. Then link the SmoothingSensor to the motor and call motor.initFOC(). + + +```c++ +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); +// real sensor instance +MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904); +// instantiate the smoothing sensor, providing the real sensor as a constructor argument +SmoothingSensor smooth = SmoothingSensor(sensor, motor); + +void doPWM(){sensor.handlePWM();} + +void setup() { + sensor.init(); + sensor.enableInterrupt(doPWM); + // Link motor to sensor + motor.linkSensor(&smooth); + // power supply voltage + driver.voltage_power_supply = 20; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage + motor.voltage_sensor_align = 8; + motor.voltage_limit = 20; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_VEL; + motor.monitor_downsample = 10; // default 10 + + // initialize motor + motor.init(); + motor.initFOC(); +} +``` + + +## Roadmap + +Possible future improvements we've thought about: + +- Add extrapolation of acceleration as well +- Switch to open loop control at very low speed with hall sensors, which otherwise move in steps even with smoothing. + diff --git a/src/encoders/smoothing/SmoothingSensor.cpp b/src/encoders/smoothing/SmoothingSensor.cpp new file mode 100644 index 0000000..86f8282 --- /dev/null +++ b/src/encoders/smoothing/SmoothingSensor.cpp @@ -0,0 +1,57 @@ +#include "SmoothingSensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + + +SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m) +{ +} + + +void SmoothingSensor::update() { + // Update sensor, with optional downsampling of update rate + if(sensor_cnt++ >= sensor_downsample) { + sensor_cnt = 0; + _wrapped.update(); + } + + // Copy state variables from the sensor + angle_prev = _wrapped.angle_prev; + angle_prev_ts = _wrapped.angle_prev_ts; + full_rotations = _wrapped.full_rotations; + + // Perform angle prediction, using low-pass filtered velocity. But don't advance more than + // pi/3 (equivalent to one step of block commutation) from the last true angle reading. + float dt = (_micros() - angle_prev_ts) * 1e-6f; + angle_prev += _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs); + + // Apply phase correction if needed + if (phase_correction != 0) { + if (_motor.shaft_velocity < -0) angle_prev -= phase_correction / _motor.pole_pairs; + else if (_motor.shaft_velocity > 0) angle_prev += phase_correction / _motor.pole_pairs; + } + + // Handle wraparound of the projected angle + if (angle_prev < 0) full_rotations -= 1, angle_prev += _2PI; + else if (angle_prev >= _2PI) full_rotations += 1, angle_prev -= _2PI; +} + + +float SmoothingSensor::getVelocity() { + return _wrapped.getVelocity(); +} + + +int SmoothingSensor::needsSearch() { + return _wrapped.needsSearch(); +} + + +float SmoothingSensor::getSensorAngle() { + return _wrapped.getSensorAngle(); +} + + +void SmoothingSensor::init() { + _wrapped.init(); +} diff --git a/src/encoders/smoothing/SmoothingSensor.h b/src/encoders/smoothing/SmoothingSensor.h new file mode 100644 index 0000000..c10c2a0 --- /dev/null +++ b/src/encoders/smoothing/SmoothingSensor.h @@ -0,0 +1,46 @@ +#ifndef SMOOTHING_SENSOR_H +#define SMOOTHING_SENSOR_H + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" + +/** + SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better + quality angle readings from sensors that are low resolution or are slow to communicate. It provides + no improvement for sensors that are high resolution and quick to update. + It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity + from the motor to predict what the angle should be at the current time. +*/ + +class SmoothingSensor : public Sensor +{ + public: + /** + SmoothingSensor class constructor + @param s Wrapped sensor + @param m Motor that the SmoothingSensor will be linked to + */ + SmoothingSensor(Sensor& s, const FOCMotor& m); + + void update() override; + float getVelocity() override; + int needsSearch() override; + + // For sensors with slow communication, use these to poll less often + unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update + unsigned int sensor_cnt = 0; // counting variable for downsampling + + // For hall sensors, the predicted angle is always 0 to 60 electrical degrees ahead of where it would be without + // smoothing, so offset it backward by 30 degrees (set this to -PI_6) to get the average in phase with the rotor + float phase_correction = 0; + + protected: + float getSensorAngle() override; + void init() override; + + Sensor& _wrapped; + const FOCMotor& _motor; +}; + +#endif From ce316bd70455dcfda96228dea9983620b522d7b9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 12 Jun 2023 20:03:04 +0200 Subject: [PATCH 06/41] fix SPI mode for SC60228 --- src/encoders/sc60228/README.md | 3 ++- src/encoders/sc60228/SC60228.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/encoders/sc60228/README.md b/src/encoders/sc60228/README.md index e0e832d..49f7901 100644 --- a/src/encoders/sc60228/README.md +++ b/src/encoders/sc60228/README.md @@ -31,9 +31,10 @@ void setup() { } ``` -Set some options: +Set some SPI options: ```c++ +SPISettings mySPISettings(1000000, SC60228_BITORDER, SPI_MODE0); // lower speed to 1Mhz MagneticSensorSC60228 sensor1(SENSOR1_CS, mySPISettings); ``` diff --git a/src/encoders/sc60228/SC60228.h b/src/encoders/sc60228/SC60228.h index f476102..03b4e50 100644 --- a/src/encoders/sc60228/SC60228.h +++ b/src/encoders/sc60228/SC60228.h @@ -21,7 +21,7 @@ typedef union { #define SC60228_CPR 4096 #define SC60228_BITORDER MSBFIRST -static SPISettings SC60228SPISettings(8000000, SC60228_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") +static SPISettings SC60228SPISettings(8000000, SC60228_BITORDER, SPI_MODE0); // @suppress("Invalid arguments") From 7617e7be7020d0c5fd3636ed1822eb92a51e6a3f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 22:52:32 +0200 Subject: [PATCH 07/41] add settings, based on registers --- README.md | 12 +- src/comms/RegisterReceiver.cpp | 124 +++++++++++++- src/comms/RegisterReceiver.h | 8 +- src/comms/RegisterSender.cpp | 5 +- src/comms/SimpleFOCRegisters.h | 7 + src/comms/serial/SerialBinaryCommander.cpp | 8 +- src/comms/serial/SerialBinaryCommander.h | 6 +- src/settings/README.md | 143 ++++++++++++++++ src/settings/SettingsStorage.cpp | 120 ++++++++++++++ src/settings/SettingsStorage.h | 70 ++++++++ .../i2c/CAT24I2CFlashSettingsStorage.cpp | 98 +++++++++++ .../i2c/CAT24I2CFlashSettingsStorage.h | 35 ++++ src/settings/samd/SAMDNVMSettingsStorage.cpp | 156 ++++++++++++++++++ src/settings/samd/SAMDNVMSettingsStorage.h | 80 +++++++++ 14 files changed, 856 insertions(+), 16 deletions(-) create mode 100644 src/settings/README.md create mode 100644 src/settings/SettingsStorage.cpp create mode 100644 src/settings/SettingsStorage.h create mode 100644 src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp create mode 100644 src/settings/i2c/CAT24I2CFlashSettingsStorage.h create mode 100644 src/settings/samd/SAMDNVMSettingsStorage.cpp create mode 100644 src/settings/samd/SAMDNVMSettingsStorage.h diff --git a/README.md b/README.md index 4c952d6..9452157 100644 --- a/README.md +++ b/README.md @@ -46,8 +46,18 @@ What is here? See the sections below. Each driver or function should come with i ### Communications - - [I2CCommander I2C driver](src/comms/i2c/) - I2C communications protocol and drivers for both controller and target devices. + - [I2CCommander I2C driver](src/comms/i2c/) - I2C communications protocol and drivers for both controller and target devices, based on register abstraction - [STM32 SpeedDir Input](src/comms/stm32speeddir/) - Control target velocity with PWM speed and direction inputs + - [SerialBinaryCommander](src/comms/serial/) - Serial communications with binary protocol, based on register abstraction + - [Telemetry](src/comms/telemetry/) - Telemetry abstraction, based on registers + - [SerialASCIITelemetry](src/comms/serial/) - Serial communications with ascii protocol, based on register abstraction + +### Settings + +Load and store SimpleFOC motor settings, based on register abstraction. + + - [SAMD NVM storage driver](src/settings/samd/) - Store settings to the NVM flash memory in your SAMD MCU + - [CAT24 I2C EEPROM storage driver](src/settings/i2c/) - Store settings to CAT24 I2C EEPROMs ### Utilities diff --git a/src/comms/RegisterReceiver.cpp b/src/comms/RegisterReceiver.cpp index ebcbccb..44f7c54 100644 --- a/src/comms/RegisterReceiver.cpp +++ b/src/comms/RegisterReceiver.cpp @@ -1,9 +1,131 @@ #include "./RegisterReceiver.h" +#include "BLDCMotor.h" +void RegisterReceiver::setRegister(SimpleFOCRegister reg, FOCMotor* motor){ + // write a register value for the given motor + switch(reg) { + case REG_ENABLE: + readByte((uint8_t*)&(motor->enabled)); + break; + case REG_MODULATION_MODE: + readByte((uint8_t*)&(motor->foc_modulation)); + break; + case REG_TORQUE_MODE: + readByte((uint8_t*)&(motor->torque_controller)); + break; + case REG_CONTROL_MODE: + readByte((uint8_t*)&(motor->controller)); + break; + case REG_TARGET: + readFloat(&(motor->target)); + break; + case REG_VEL_PID_P: + readFloat(&(motor->PID_velocity.P)); + break; + case REG_VEL_PID_I: + readFloat(&(motor->PID_velocity.I)); + break; + case REG_VEL_PID_D: + readFloat(&(motor->PID_velocity.D)); + break; + case REG_VEL_LPF_T: + readFloat(&(motor->LPF_velocity.Tf)); + break; + case REG_ANG_PID_P: + readFloat(&(motor->P_angle.P)); + break; + case REG_VEL_LIMIT: + readFloat(&(motor->velocity_limit)); + break; + case REG_VEL_MAX_RAMP: + readFloat(&(motor->PID_velocity.output_ramp)); + break; -void RegisterReceiver::setRegister(SimpleFOCRegister reg, void* value, FOCMotor* motor){ + case REG_CURQ_PID_P: + readFloat(&(motor->PID_current_q.P)); + break; + case REG_CURQ_PID_I: + readFloat(&(motor->PID_current_q.I)); + break; + case REG_CURQ_PID_D: + readFloat(&(motor->PID_current_q.D)); + break; + case REG_CURQ_LPF_T: + readFloat(&(motor->LPF_current_q.Tf)); + break; + case REG_CURD_PID_P: + readFloat(&(motor->PID_current_d.P)); + break; + case REG_CURD_PID_I: + readFloat(&(motor->PID_current_d.I)); + break; + case REG_CURD_PID_D: + readFloat(&(motor->PID_current_d.D)); + break; + case REG_CURD_LPF_T: + readFloat(&(motor->LPF_current_d.Tf)); + break; + case REG_VOLTAGE_LIMIT: + readFloat(&(motor->voltage_limit)); + break; + case REG_CURRENT_LIMIT: + readFloat(&(motor->current_limit)); + break; + case REG_MOTION_DOWNSAMPLE: + readByte((uint8_t*)&(motor->motion_downsample)); + break; + case REG_DRIVER_VOLTAGE_LIMIT: + readFloat(&(((BLDCMotor*)motor)->driver->voltage_limit)); + break; + case REG_PWM_FREQUENCY: + readInt((uint32_t*)&(((BLDCMotor*)motor)->driver->pwm_frequency)); + break; + + case REG_ZERO_ELECTRIC_ANGLE: + readFloat(&(motor->zero_electric_angle)); + break; + case REG_SENSOR_DIRECTION: + readByte((uint8_t*)&(motor->sensor_direction)); + break; + case REG_ZERO_OFFSET: + readFloat(&(motor->sensor_offset)); + break; + case REG_PHASE_RESISTANCE: + readFloat(&(motor->phase_resistance)); + break; + case REG_KV: + readFloat(&(motor->KV_rating)); + break; + case REG_INDUCTANCE: + readFloat(&(motor->phase_inductance)); + break; + case REG_POLE_PAIRS: + readByte((uint8_t*)&(motor->pole_pairs)); + break; + // unknown register or read-only register (no write) or can't handle in superclass + case REG_STATUS: + case REG_ANGLE: + case REG_POSITION: + case REG_VELOCITY: + case REG_SENSOR_ANGLE: + case REG_VOLTAGE_Q: + case REG_VOLTAGE_D: + case REG_CURRENT_Q: + case REG_CURRENT_D: + case REG_CURRENT_A: + case REG_CURRENT_B: + case REG_CURRENT_C: + case REG_CURRENT_ABC: + case REG_SYS_TIME: + case REG_NUM_MOTORS: + case REG_MOTOR_ADDRESS: + case REG_ENABLE_ALL: + case REG_REPORT: + default: + break; + } }; diff --git a/src/comms/RegisterReceiver.h b/src/comms/RegisterReceiver.h index 5d46178..fc2af1c 100644 --- a/src/comms/RegisterReceiver.h +++ b/src/comms/RegisterReceiver.h @@ -8,8 +8,8 @@ class RegisterReceiver { protected: - virtual void setRegister(SimpleFOCRegister reg, void* value, FOCMotor* motor); - virtual uint8_t readByte(void* valueToSet) = 0; - virtual uint8_t readFloat(void* valueToSet) = 0; - virtual uint8_t readInt(void* valueToSet) = 0; + virtual void setRegister(SimpleFOCRegister reg, FOCMotor* motor); + virtual uint8_t readByte(uint8_t* valueToSet) = 0; + virtual uint8_t readFloat(float* valueToSet) = 0; + virtual uint8_t readInt(uint32_t* valueToSet) = 0; }; diff --git a/src/comms/RegisterSender.cpp b/src/comms/RegisterSender.cpp index 9e08f46..2a163ce 100644 --- a/src/comms/RegisterSender.cpp +++ b/src/comms/RegisterSender.cpp @@ -4,7 +4,7 @@ #include "BLDCMotor.h" bool RegisterSender::sendRegister(SimpleFOCRegister reg, FOCMotor* motor){ - // read the current register + // write a register value for the given motor switch(reg) { case REG_STATUS: writeByte(motor->motor_status); @@ -21,7 +21,6 @@ bool RegisterSender::sendRegister(SimpleFOCRegister reg, FOCMotor* motor){ case REG_CONTROL_MODE: writeByte(motor->controller); break; - case REG_TARGET: writeFloat(motor->target); break; @@ -151,7 +150,7 @@ bool RegisterSender::sendRegister(SimpleFOCRegister reg, FOCMotor* motor){ writeFloat(((BLDCMotor*)motor)->driver->voltage_limit); break; case REG_PWM_FREQUENCY: - writeFloat(((BLDCMotor*)motor)->driver->pwm_frequency); + writeInt(((BLDCMotor*)motor)->driver->pwm_frequency); break; case REG_ZERO_ELECTRIC_ANGLE: diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index 83eacbd..6fea825 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -3,6 +3,13 @@ #include + +// this constant is changed each time the registers definition are changed *in an incompatible way*. This means that just adding new registers +// does not change the version, but removing or changing the meaning of existing registers does, or changing the number of an existing register. +#define SIMPLEFOC_REGISTERS_VERSION 0x01 + + + typedef enum : uint8_t { REG_STATUS = 0x00, // RO - 1 byte (motor status) REG_MOTOR_ADDRESS = 0x01, // R/W - 1 byte diff --git a/src/comms/serial/SerialBinaryCommander.cpp b/src/comms/serial/SerialBinaryCommander.cpp index 07bd87c..ffa2406 100644 --- a/src/comms/serial/SerialBinaryCommander.cpp +++ b/src/comms/serial/SerialBinaryCommander.cpp @@ -33,7 +33,7 @@ void SerialBinaryCommander::run() { endFrame(); } else if (command==SERIALBINARY_COMMAND_WRITE) { - setRegister(static_cast(registerNum), NULL, motors[motorNum]); + setRegister(static_cast(registerNum), motors[motorNum]); if (echo) { startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect sendRegister(static_cast(registerNum), motors[motorNum]); @@ -108,19 +108,19 @@ uint8_t SerialBinaryCommander::writeInt(uint32_t value){ -uint8_t SerialBinaryCommander::readByte(void* valueToSet){ +uint8_t SerialBinaryCommander::readByte(uint8_t* valueToSet){ return readBytes(valueToSet, 1); }; -uint8_t SerialBinaryCommander::readFloat(void* valueToSet){ +uint8_t SerialBinaryCommander::readFloat(float* valueToSet){ return readBytes(valueToSet, 4); }; -uint8_t SerialBinaryCommander::readInt(void* valueToSet){ +uint8_t SerialBinaryCommander::readInt(uint32_t* valueToSet){ return readBytes(valueToSet, 4); }; diff --git a/src/comms/serial/SerialBinaryCommander.h b/src/comms/serial/SerialBinaryCommander.h index 2f5bb65..f80ccc4 100644 --- a/src/comms/serial/SerialBinaryCommander.h +++ b/src/comms/serial/SerialBinaryCommander.h @@ -36,9 +36,9 @@ class SerialBinaryCommander : public Telemetry, public RegisterReceiver { uint8_t writeFloat(float value) override; uint8_t writeInt(uint32_t value) override; - uint8_t readByte(void* valueToSet) override; - uint8_t readFloat(void* valueToSet) override; - uint8_t readInt(void* valueToSet) override; + uint8_t readByte(uint8_t* valueToSet) override; + uint8_t readFloat(float* valueToSet) override; + uint8_t readInt(uint32_t* valueToSet) override; void sendTelemetry() override; void sendHeader() override; diff --git a/src/settings/README.md b/src/settings/README.md new file mode 100644 index 0000000..d8a44de --- /dev/null +++ b/src/settings/README.md @@ -0,0 +1,143 @@ +# SimpleFOC Settings Storage + +Code to persist calibration data and motor settings to and from storage. The SettingsStorage is abstract, and different implementations exist for different concete storage media. + +The SettingsStorage is based on the SimpleFOCRegisters abstraction, so this means you can load or store any of the registers (representing SimpleFOC settings) that is both readable and writeable (e.g. while the current shaft velocity is available as a register, you can't store it as a setting because its read-only). + +The SettingsStorage can save the settings for one or more motors, and you can customize which settings are saved. If you don't customize, only the motor +calibration will be stored (REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION). + +## Hardware requirements + +Obviously, to save settings you need a place to store them where they won't be lost when you power down. There can be many solutions to this question, and we attempt to provide some simple drivers for the common storage solutions people might want. + +But its also quite easy to extend the system to support your specific needs. + +Supported hardware: + +- [SAMD NVM](samd/) \ +Some SAMD chips have a built-in NVRAM flash, which can be written from user code. +- [CAT24 I2C Flash EEPROMs](i2c/) \ :warning: UNFINISHED +Simple programmable memories with I2C interface. Memory space is quite limited, but enough for storing settings. + +See Roadmap, below, for systems we may support in the future. + + +## Basic Usage + +Using SettingsStorage is simple. Just add a `settings.loadSettings()` after setting up your defaults, but before calling init() on the objects. And call `saveSettings()` after initialization of the motor has (successfully) completed. + +This example saves just the motor calibration, but you might want to add additional code to save other registers, or make your solution more user-friendly. See the other examples below. + +```c++ + +#include "settings/samd/SAMDNVMSettingsStorage.h" + +SAMDNVMSettingsStorage settings = SAMDNVMSettingsStorage(); +BLDCMotor motor = BLDCMotor(...); + +void setup() { + SimpleFOCDebug::enable(); // show debug messages from settings + + // initialize the settings early in the setup + settings.addMotor(&motor); + settings.init(); + + // first set your defaults + driver.voltage_power_supply = 12.0f; + driver.voltage_limit = 12.0f; // 12V driver limit, like PSU + motor.voltage_limit = 6.0f; // 6V motor limit + + // then try to load the settings + SettingsStatus loadStatus = settings.loadSettings(); + + // driver init code, etc... + ... + + // then init the motor + motor.init(); + motor.initFOC(); + + // and if the settings were not loaded earlier, then save them + if (motor.motor_status == FOCMotorStatus::motor_ready && loadStatus != SETTINGS_SUCCESS) { + settings.saveSettings(); + } + + // any other initialization + ... +} + +``` + +### Choosing registers + +By default, only the calibration data (motor native direction, electrical zero position) are saved. To save additional registers, you can choose them before calling `SettingsStorage.init()`, as in the example below: + +```c++ + +SimpleFOCRegister settingsRegisters[] = { REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION, REG_VEL_PID_P, REG_VEL_PID_I, REG_VEL_PID_D, REG_VEL_LPF_T, REG_VEL_LIMIT, REG_VEL_MAX_RAMP, REG_VOLTAGE_LIMIT, REG_MOTION_DOWNSAMPLE, REG_CONTROL_MODE, REG_TORQUE_MODE, REG_PHASE_RESISTANCE, REG_KV, REG_INDUCTANCE }; +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); +BLDCMotor motor = BLDCMotor(...); + +void setup() { + SimpleFOCDebug::enable(); // show debug messages from settings + + // initialize the settings early in the setup + settings.setRegisters(mySettingsRegisters, sizeof(mySettingsRegisters)); + settings.addMotor(&motor); + settings.init(); + ... +} +``` + + + +### Using settings with commander + +You can easily interact with the settings via the commander, for example to save your PID tuning values: + +```c++ + +SimpleFOCRegister settingsRegisters[] = { REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION, REG_VEL_PID_P, REG_VEL_PID_I, REG_VEL_PID_D, REG_VEL_LPF_T }; + +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); + +Commander commander; + +void doSaveSettings(char *cmd) { settings.saveSettings(); } +void doLoadSettings(char *cmd) { settings.loadSettings(); } + + +void setup() { + ... + + settings.setRegisters(settingsRegisters, sizeof(settingsRegisters)); + settings.addMotor(&motor); + settings.init(); + + ... + + commander.addCommand('s', doSaveSettings, "Save settings"); + commander.addCommand('l', doLoadSettings, "Load settings"); + + ... + +} + + +void run() { + motor.move(); + motor.loopFOC(); + commander.run(); +} +``` + +## Roadmap + +The following ideas for storage backends are on the roadmap, with no definate schedule for implementation: + +- ESP32 SPIFFs Filesystem +- ESP32 Preferences +- Arduino EEPROM library +- In-Memory Buffer (for use with other libraries or abstractions) +- JSON String (for printing to console) diff --git a/src/settings/SettingsStorage.cpp b/src/settings/SettingsStorage.cpp new file mode 100644 index 0000000..6c3c4cd --- /dev/null +++ b/src/settings/SettingsStorage.cpp @@ -0,0 +1,120 @@ + +#include "./SettingsStorage.h" +#include "communication/SimpleFOCDebug.h" + + +SettingsStorage::SettingsStorage() { + +}; + + +SettingsStorage::~SettingsStorage() { + +}; + + + +void SettingsStorage::addMotor(BLDCMotor* motor) { + if (numMotors < SIMPLEFOC_SETTINGS_MAX_MOTORS) { + motors[numMotors] = motor; + numMotors++; + } + else + SimpleFOCDebug::println("SS: too many motors"); +}; + + +void SettingsStorage::setRegisters(SimpleFOCRegister* registers, int numRegisters){ + this->registers = registers; + this->numRegisters = numRegisters; +}; + + +void SettingsStorage::init() { + // make sure we have motors and registers + if (numMotors < 1) { + SimpleFOCDebug::println("SS: no motors"); + return; + } + if (registers==NULL || numRegisters < 1) { + SimpleFOCDebug::println("SS: no registers"); + return; + } +}; + + + +SettingsStatus SettingsStorage::loadSettings() { + SimpleFOCDebug::println("Loading settings..."); + beforeLoad(); + uint8_t magic; readByte(&magic); + if (magic != SIMPLEFOC_SETTINGS_MAGIC_BYTE) { + SimpleFOCDebug::println("No settings found "); + return NO_SETTINGS; + } + uint8_t rversion; readByte(&rversion); + if (rversion != SIMPLEFOC_REGISTERS_VERSION) { + SimpleFOCDebug::println("Registers version mismatch"); + return OLD_SETTINGS; + } + uint8_t version; readByte(&version); + if (version != settings_version) { + SimpleFOCDebug::println("Settings version mismatch"); + return OLD_SETTINGS; + } + for (int m = 0; m < numMotors; m++) { + if (numMotors>1) + SimpleFOCDebug::println("Loading settings for motor ", m); + startLoadMotor(m); + for (int i = 0; i < numRegisters; i++) { + SimpleFOCRegister reg = registers[i]; + startLoadRegister(reg); + setRegister(reg, motors[m]); + endLoadRegister(); + } + endLoadMotor(); + } + afterLoad(); + SimpleFOCDebug::println("Settings loaded"); + return SETTINGS_SUCCESS; +}; + + +SettingsStatus SettingsStorage::saveSettings() { + SimpleFOCDebug::println("Saving settings..."); + beforeSave(); + writeByte(SIMPLEFOC_SETTINGS_MAGIC_BYTE); + writeByte(SIMPLEFOC_REGISTERS_VERSION); + writeByte(settings_version); + for (int m = 0; m < numMotors; m++) { + if (numMotors>1) + SimpleFOCDebug::println("Saving settings for motor ", m); + startSaveMotor(m); + for (int i = 0; i < numRegisters; i++) { + SimpleFOCRegister reg = registers[i]; + startSaveRegister(reg); + sendRegister(reg, motors[m]); + endSaveRegister(); + } + endSaveMotor(); + } + afterSave(); + SimpleFOCDebug::println("Settings saved"); + return SETTINGS_SUCCESS; +}; + +// empty implementation for these + +void SettingsStorage::startSaveMotor(uint8_t num) {}; +void SettingsStorage::endSaveMotor() {}; +void SettingsStorage::startSaveRegister(SimpleFOCRegister reg) {}; +void SettingsStorage::endSaveRegister() {}; +void SettingsStorage::startLoadMotor(uint8_t num) {}; +void SettingsStorage::endLoadMotor() {}; +void SettingsStorage::startLoadRegister(SimpleFOCRegister reg) {}; +void SettingsStorage::endLoadRegister() {}; + +void SettingsStorage::beforeLoad() {}; +void SettingsStorage::afterLoad() {}; +void SettingsStorage::beforeSave() {}; +void SettingsStorage::afterSave() {}; diff --git a/src/settings/SettingsStorage.h b/src/settings/SettingsStorage.h new file mode 100644 index 0000000..4c9e172 --- /dev/null +++ b/src/settings/SettingsStorage.h @@ -0,0 +1,70 @@ + +#pragma once + + +#include "../comms/SimpleFOCRegisters.h" +#include "../comms/RegisterReceiver.h" +#include "../comms/RegisterSender.h" +#include "BLDCMotor.h" + +#define SIMPLEFOC_SETTINGS_MAGIC_BYTE 0x42 +#define SIMPLEFOC_SETTINGS_VERSION 0x01 + +#define SIMPLEFOC_SETTINGS_REGISTERS_MINIMAL { REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION } + + +#if !defined(SIMPLEFOC_SETTINGS_MAX_MOTORS) +#define SIMPLEFOC_SETTINGS_MAX_MOTORS 4 +#endif + + +typedef enum : uint8_t { + SETTINGS_SUCCESS = 0, // settings loaded/saved successfully + NO_SETTINGS = 1, // no settings found to load + OLD_SETTINGS = 2, // settings found, but version is old + ERROR = 3 // other error +} SettingsStatus; + + + + +class SettingsStorage : public RegisterReceiver, public RegisterSender { +public: + SettingsStorage(); + ~SettingsStorage(); + + void addMotor(BLDCMotor* motor); + void setRegisters(SimpleFOCRegister* registers, int numRegisters); + + virtual void init(); + + SettingsStatus loadSettings(); + SettingsStatus saveSettings(); + + /* + Change this value if you make changes to the registers saved in code, and have saved settings on existing devices. + This will cause the device to reset to defaults on the next boot. + */ + uint8_t settings_version = SIMPLEFOC_SETTINGS_VERSION; + +protected: + virtual void startSaveMotor(uint8_t num); + virtual void endSaveMotor(); + virtual void startSaveRegister(SimpleFOCRegister reg); + virtual void endSaveRegister(); + virtual void startLoadMotor(uint8_t num); + virtual void endLoadMotor(); + virtual void startLoadRegister(SimpleFOCRegister reg); + virtual void endLoadRegister(); + + + virtual void beforeLoad(); + virtual void afterLoad(); + virtual void beforeSave(); + virtual void afterSave(); + + FOCMotor* motors[SIMPLEFOC_SETTINGS_MAX_MOTORS]; + int numMotors = 0; + SimpleFOCRegister* registers = NULL; + int numRegisters = 0; +}; diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp new file mode 100644 index 0000000..1ed50f7 --- /dev/null +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp @@ -0,0 +1,98 @@ + +#include "./CAT24I2CFlashSettingsStorage.h" + + +CAT24I2CFlashSettingsStorage::CAT24I2CFlashSettingsStorage(uint8_t address, uint16_t offset) { + _address = address; + _offset = offset; +}; + + + +CAT24I2CFlashSettingsStorage::~CAT24I2CFlashSettingsStorage() {}; + + + +void CAT24I2CFlashSettingsStorage::init(TwoWire* wire) { + SettingsStorage::init(); + _wire = wire; + reset(); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::readByte(uint8_t* valueToSet) { + return readBytes(valueToSet, 1); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::readFloat(float* valueToSet) { + return readBytes(valueToSet, 4); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::readInt(uint32_t* valueToSet) { + return readBytes(valueToSet, 4); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::writeByte(uint8_t value) { + return writeBytes(&value, 1); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::writeFloat(float value) { + return writeBytes(&value, 4); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::writeInt(uint32_t value) { + return writeBytes(&value, 4); +}; + + + +void CAT24I2CFlashSettingsStorage::beforeSave() { + reset(); +}; + + +void CAT24I2CFlashSettingsStorage::beforeLoad() { + reset(); + _wire->beginTransmission(_address); + _wire->write(_currptr >> 8); + _wire->write(_currptr & 0xFF); + _wire->endTransmission(false); +}; + + + +void CAT24I2CFlashSettingsStorage::reset() { + _currptr = _offset; +}; + + +int CAT24I2CFlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { + int read = _wire->requestFrom(_address, numBytes, true); + _currptr += read; + if (read==numBytes) + _wire->readBytes((uint8_t*)valueToSet, numBytes); + return read; +}; + + +int CAT24I2CFlashSettingsStorage::writeBytes(void* value, int numBytes) { + _wire->beginTransmission(_address); + _wire->write(_currptr >> 8); + _wire->write(_currptr & 0xFF); + size_t written = _wire->write((uint8_t*)value, numBytes); + _wire->endTransmission(true); + _currptr += written; + delay(5); // write-cycle time + return written; +}; diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.h b/src/settings/i2c/CAT24I2CFlashSettingsStorage.h new file mode 100644 index 0000000..4f6c511 --- /dev/null +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.h @@ -0,0 +1,35 @@ + +#pragma once + +#include "../SettingsStorage.h" +#include "Wire.h" + +class CAT24I2CFlashSettingsStorage : public SettingsStorage { +public: + CAT24I2CFlashSettingsStorage(uint8_t address = 0xA0, uint16_t offset = 0x0); + ~CAT24I2CFlashSettingsStorage(); + + void init(TwoWire* wire = &Wire); + +protected: + uint8_t readByte(uint8_t* valueToSet) override; + uint8_t readFloat(float* valueToSet) override; + uint8_t readInt(uint32_t* valueToSet) override; + + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + + void beforeSave() override; + void beforeLoad() override; + + void reset(); + int readBytes(void* valueToSet, int numBytes); + int writeBytes(void* value, int numBytes); + + TwoWire* _wire; + uint8_t _address; // i2c address + uint16_t _offset; // offset into NVRAM to store settings + uint16_t _currptr = 0; // pointer to current location in NVRAM + +}; diff --git a/src/settings/samd/SAMDNVMSettingsStorage.cpp b/src/settings/samd/SAMDNVMSettingsStorage.cpp new file mode 100644 index 0000000..109a577 --- /dev/null +++ b/src/settings/samd/SAMDNVMSettingsStorage.cpp @@ -0,0 +1,156 @@ + +#include "./SAMDNVMSettingsStorage.h" +#include "communication/SimpleFOCDebug.h" + + +SAMDNVMSettingsStorage::SAMDNVMSettingsStorage(uint32_t offset) { + _offset = offset; +}; + +SAMDNVMSettingsStorage::~SAMDNVMSettingsStorage(){}; + +void SAMDNVMSettingsStorage::init(){ + SettingsStorage::init(); + reset(); +}; + + + +void SAMDNVMSettingsStorage::reset(){ + _currptr = (uint8_t*) SIMPLEFOC_SAMDNVMSETTINGS_BASE + _offset; // add offset to NVM base address +}; + + + +void SAMDNVMSettingsStorage::beforeLoad(){ + reset(); +}; + + +void SAMDNVMSettingsStorage::beforeSave(){ + reset(); + _writeIndex = 0; + + // set manual write mode + _ctlB = NVMCTRL->CTRLB.reg; + NVMCTRL->CTRLB.reg |= NVMCTRL_CTRLB_MANW; + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + // unlock region - user can do it with fuses + // erase rows + // TODO handle case that it is more than one row + NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + NVMCTRL->STATUS.reg |= NVMCTRL_STATUS_MASK; + NVMCTRL->INTFLAG.bit.ERROR = 0; + NVMCTRL->CTRLA.reg = 0x1A | NVMCTRL_CTRLA_CMDEX_KEY; + delay(1); + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + if (NVMCTRL->INTFLAG.bit.ERROR == 1) + SimpleFOCDebug::println("NVM erase error ", NVMCTRL->STATUS.reg); + else + SimpleFOCDebug::println("NVM erased row @", (int)_currptr); +}; + + +void SAMDNVMSettingsStorage::afterSave() { + if (_writeIndex>0) + flushPage(); + // restore ctlb + delay(1); + NVMCTRL->CTRLB.reg =_ctlB; + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; +} + + +void SAMDNVMSettingsStorage::flushPage(){ + // if we have an odd number of bytes, pad with 0xFF + if (_writeIndex%2==1) { + _writeBuffer[_writeIndex++] = 0xFF; + } + // erase page buffer + // NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + // NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMD_PBC | NVMCTRL_CTRLA_CMDEX_KEY; + // while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + // copy writeBuffer to NVM, using 16-bit writes + uint16_t* src = (uint16_t*)_writeBuffer; + uint16_t* dst = (uint16_t*)_currptr; + for (int i=0;i<_writeIndex/2;i++) { + *dst++ = *src++; + } + // write page + NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + NVMCTRL->STATUS.reg |= NVMCTRL_STATUS_MASK; + NVMCTRL->INTFLAG.bit.ERROR = 0; + NVMCTRL->CTRLA.reg = 0x1C | NVMCTRL_CTRLA_CMDEX_KEY; + delay(1); + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + if (NVMCTRL->INTFLAG.bit.ERROR == 1) + SimpleFOCDebug::println("NVM write error ", NVMCTRL->STATUS.reg); + else + SimpleFOCDebug::println("NVM wrote page @", (int)_currptr); + // reset writeBuffer pointer and advance currptr to next page + _writeIndex = 0; + _currptr+=NVMCTRL_PAGE_SIZE; +} + + +int SAMDNVMSettingsStorage::readBytes(void* valueToSet, int numBytes) { + uint8_t* bytes = (uint8_t*)valueToSet; + for (int i=0;i +#include "../SettingsStorage.h" + +#define RWWEE_BASE 0x00400000 + +#if !defined(SIMPLEFOC_SAMDNVMSETTINGS_BASE) +#define SIMPLEFOC_SAMDNVMSETTINGS_BASE RWWEE_BASE +#endif + + +class SAMDNVMSettingsStorage : public SettingsStorage { +public: + SAMDNVMSettingsStorage(uint32_t offset = 0x0); + ~SAMDNVMSettingsStorage(); + + void init() override; + +protected: + uint8_t readByte(uint8_t* valueToSet) override; + uint8_t readFloat(float* valueToSet) override; + uint8_t readInt(uint32_t* valueToSet) override; + + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + + void beforeSave() override; + void afterSave() override; + void beforeLoad() override; + + + void reset(); + int readBytes(void* valueToSet, int numBytes); + int writeBytes(void* value, int numBytes); + void flushPage(); + + uint32_t _offset; // offset into NVRAM to store settings + + uint32_t _ctlB; + uint8_t* _currptr; // pointer to current location in NVM + int _writeIndex; // index into current page being written + uint8_t _writeBuffer[NVMCTRL_PAGE_SIZE]; // buffer for writing to NVM +}; \ No newline at end of file From 141a528ca2a287d67ce3bc08675363d70d2263c3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 23:53:02 +0200 Subject: [PATCH 08/41] add ifdef to limit to SAMD arch --- src/settings/samd/SAMDNVMSettingsStorage.cpp | 10 +++++++++- src/settings/samd/SAMDNVMSettingsStorage.h | 7 ++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/settings/samd/SAMDNVMSettingsStorage.cpp b/src/settings/samd/SAMDNVMSettingsStorage.cpp index 109a577..5569600 100644 --- a/src/settings/samd/SAMDNVMSettingsStorage.cpp +++ b/src/settings/samd/SAMDNVMSettingsStorage.cpp @@ -1,5 +1,11 @@ + + #include "./SAMDNVMSettingsStorage.h" + + +#if defined(_SAMD21_) + #include "communication/SimpleFOCDebug.h" @@ -153,4 +159,6 @@ uint8_t SAMDNVMSettingsStorage::writeFloat(float value){ uint8_t SAMDNVMSettingsStorage::writeInt(uint32_t value){ return writeBytes(&value, 4); -}; \ No newline at end of file +}; + +#endif diff --git a/src/settings/samd/SAMDNVMSettingsStorage.h b/src/settings/samd/SAMDNVMSettingsStorage.h index 38a3fe6..6e8a855 100644 --- a/src/settings/samd/SAMDNVMSettingsStorage.h +++ b/src/settings/samd/SAMDNVMSettingsStorage.h @@ -36,6 +36,9 @@ #include + +#if defined(_SAMD21_) + #include "../SettingsStorage.h" #define RWWEE_BASE 0x00400000 @@ -77,4 +80,6 @@ class SAMDNVMSettingsStorage : public SettingsStorage { uint8_t* _currptr; // pointer to current location in NVM int _writeIndex; // index into current page being written uint8_t _writeBuffer[NVMCTRL_PAGE_SIZE]; // buffer for writing to NVM -}; \ No newline at end of file +}; + +#endif From f3f4377e42db19b941fe384c282b8aa1e4af1a23 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Jun 2023 23:53:39 +0200 Subject: [PATCH 09/41] add some READMEs for settings --- src/settings/i2c/README.md | 26 ++++++++++ src/settings/samd/README.md | 49 ++++++++++++++++++ .../samd/eeprom_emulation_screenshot.jpg | Bin 0 -> 73543 bytes 3 files changed, 75 insertions(+) create mode 100644 src/settings/i2c/README.md create mode 100644 src/settings/samd/README.md create mode 100644 src/settings/samd/eeprom_emulation_screenshot.jpg diff --git a/src/settings/i2c/README.md b/src/settings/i2c/README.md new file mode 100644 index 0000000..cf2740f --- /dev/null +++ b/src/settings/i2c/README.md @@ -0,0 +1,26 @@ + +# I2C memories settings drivers + +Store settings to I2C EEPROMs. + +## CAT24C64 I2C EEPROMs + +:warning: not yet finished or tested! + +Datasheet describes the CAT24C64 as: _The CAT24C64 is a 64 Kb CMOS Serial EEPROM device, internally organized as 8192 words of 8 bits each._ + +Provide I2C address in constructor: + +`CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0);` + +If you want to use a different I2C bus than the default, you can pass it to the constructor: + +`settings.init(&Wire2);` + +You can use multiple settings objects in the same EEPROM, by providing an offset to the constructor: + +``` +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0, 80); +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0, 160); +``` diff --git a/src/settings/samd/README.md b/src/settings/samd/README.md new file mode 100644 index 0000000..7316011 --- /dev/null +++ b/src/settings/samd/README.md @@ -0,0 +1,49 @@ +# SAMD21 settings driver + +Driver for storing settings on SAMD21 chips. + +There are two options: + +1. **RWW Flash** \ +Some SAMD21 chips have a seperate RWW flash area, of 2kB-8kB size. This area is seperate from the main flash array. \ +Note: this method is implemented and tested. + +2. **EEPROM emulation** \ +All SAMD21s support EEPROM emulation, where an area of the main flash is reserved for the application storage. \ +:warning: This method is not yet implemented fully, and untested. + +## Using RWW flash + +To use the RWW flash to store settings, you have to make sure the flash region is not locked (you can set the lock fuses using MCP Studio or other SAM development tools). + +Otherwise there is nothing special to do. When reprogramming the chip, be careful not to erase the RWW area. + +You can use multiple settings objects to store to different areas of the RWW. +To do so, specify an offset to the constructor: + +``` +SAMDNVMSettingsStorage settings0 = SAMDNVMSettingsStorage(); +SAMDNVMSettingsStorage settings1 = SAMDNVMSettingsStorage(NVMCTRL_ROW_SIZE); +SAMDNVMSettingsStorage settings2 = SAMDNVMSettingsStorage(NVMCTRL_ROW_SIZE*2); +``` + +Note: the offset must be a multiple of the flash memory row size, typically 256 bytes. + + +## Using EEPROM emulation + +TODO implement and test this method + +To use the EEPROM emulation, use a tool like MCP Studio to set the chip fuses to reserve an area of flash for EEPROM emulation: + +![](eeprom_emulation_screenshot.jpg) + +Normally 256 bytes (one flash page) should be plenty for SimpleFOC settings... Obviously, the area you reserve for EEPROM can't be used for your main program. + +Add a build-flag to your build to set the base address of your EEPROM memory: + +`-DSIMPLEFOC_SAMDNVMSETTINGS_BASE=0x003FF800` + +Then use the settings as normal. + +You'll have to adjust your board files to exclude the EEPROM area in the ldscript to prevent it being erased when you re-program, if you care to keep your settings when reflashing. diff --git a/src/settings/samd/eeprom_emulation_screenshot.jpg b/src/settings/samd/eeprom_emulation_screenshot.jpg new file mode 100644 index 0000000000000000000000000000000000000000..c743439c8dba800f86f86fe7d46d77738cf713f8 GIT binary patch literal 73543 zcmc$_2UJu`(UIBubVvl0iU3B!`ilvj`|K0!MO20YO0|gMj2HNEQ)La*mQD z4S9fJcpJ{q^WFd6|E~M)dTYIgP1o+-UENh(RbAEHTuxjr0Hk+SHBe7?4ub(8j##4@}oE=QT(=!W1(Z4^(1qJjYAhLMR4ZE)j8PC@DejKhROt&{DZ3 zlZevN!@~ug6aZY^9(z4dQ)D$VHetnC1ILO9kO9&FxuvzYhr<2)T30xK=hyiE{5c-G zrVJPsxWe_P`2P$bv$6HI23d0-tZrrFZS4xe)!?&Z?cwzp05EdFa(3Ux9#`-e5T^73 z2MEG5SFruxF#HO(x`wa$K(*c*7hJCLS+yZcC&GG z17YwTkau-;w*ld55at2vc!2N~ekrT}z+?X(u%)HdADot!_J82NX#rb;9ViWN7Yi}ix zmw&glu~gRl4ZFGAyPEH7oOX_?R|0qqJ3iKU_#3wMR?+(nd$~XO)0dr>lEUwDOD_=m zr>&2({-1IOTa7>XpE^7={9WgXqrsKFT=(VetoLU;PaTy0lzaI+_%nW6H#NoI_3gY= z|IqI04VM0c$H7tKPus^153ls_8o#yWzvH)cQ}`FBx6z-m+1M(p{H|~7rvHatkH?CC z`toqO){SeNwk~Ra%DtcH{b~Cc6v*FrJRaZw!$T*_yH|35-4@US)Bsk{J}3hZz}yhf zy@r9uwtiPK3MjdI_Xv`Y z+;c%uJ_dCr@!vA5mjF-~1^~2E|CVuN0szTh08ra*?c??2Z+w_n85V#KkN}hb9l#8* z13Z8LAOc7LGQb@`1-J|701p9Uzyh!a8~|6q3-AR3fM6gDhz1gXS3o+D1>^vQKq*iO z)B(*v8_*5(0pEd9U>aBeR)9?a4jcpLXlQ6SXhdifXmn^SXq;$#Xd-A*X!2+(Xj*6w z(2UU@q1mIkq4}T%poOAEqa~rGqh+HNp;e$YptYg(qWwUdL|a7LK-)(76#XysIP^61_vjzcYth@#`_M{s9320UN<>0$l<}g69Nj1Rn{$5G)d$ z5|R_%Bvc|aBlIDRBP<~NOgKq+Ktx2uMRbSAgy=C*98nQb2hki6f|!C>kXVD*h8RYi zL0n5bM7%?SN5V;>Kw?4ilq7|uie!*vixiKPi&Tl!k~D}kgS3Hkob-r{g6tNV4w)-i z3|TQ*FWEXd4mlUO3b_sWU*tLD9puXt7!>RjN)*- z1C($oaw;(@11dkN460953)C3YT+|xWF4XbVmDFR@XEaPS3N*Gfku+sALo_F}477J> zZE2%u%V|ex&*)g`ROp=P66xybX6Z5MZ_?}1`_gC9chPS#P%_9cSTjU1R5DC4qBGuP ze8BjOF^BOR;}H`xlNyr;(`%*=SKF8fg9&++-&-6A#CMrv+VfnQtWo@FWKAK;T$X++8hBKB^*)f>58r)C0OSorvhe)MtHG#rFflr-|~LH ziGEZ3ro+vQn}dAld=h+)d~f-F@MH1c=6B_v&;2 z9Sxmio!R@`_dV}7>0;~N*UivfeQ@hRz=Iw=3O%S^vEK1R#fR|^r}cUCee~N6NDRyj ziVRK+l?{^&7mS3A0*(5N>5UzY>rHS>3{3J(j!acdUzx6$iJOI)jhS-k^HFVtR?gb{~%g^h>Hgl9!yMmRJ`D4#w*I{%`;tze<>L1AZ+WKmHuV{v>5dWlcT#s{+x-%FKC z>&pbnvdgKayztry2Gerq*J$Zpi8ss zOSfuwM~^~J>le8%&ArmS4PT|c*7ZsB)qWHIR?{!uUo#*vP&+6&SpWU@_r@QxKU#($ zLv6##!`&kqBYmUyMt_Xyk4=o5jW0~tOsr43OzurRnL3|-K7%zAHA_01GRHXgex7%} zd_iL2)1va?w1FHX&7a;skt=_#60N4Jv8)xXi>xX`*H>T`w;^EWIqReSxc}2i*>gh4u>ofXEmCD82&#zUkjS^>vNwnCKq} zbVskDzw`fq|Mt7DzexZ<&Nu)(d*xP8^sg6T;SrHhNy#r?rKG04PJf@1 zo0nfuSX5kDRb5kCSKrXs-qG3B-SegQ>(KDX=-Bwgo!^FCA3!7X)7st|*f=whCmr^k? zyP^$`UGxE*%F62pJ~fBfBIn)}Y1fqfpAi=Fe?{3J!v3Xe8c+hG+pB~Q{$ik`gHwe8 zCeVAkO4vA{0|qA0(Yj8!*9q?`fqv0H=@LYOcGVIC0}K2mz{SS>{ptTaznlfbXx7Uq zfDi)>6ebKZ00N*8HE&Jt!cnih%bionCMS)R4`y`uFynaMW4)b?N7GjT{cx(ciV7OHIm@ej-=R8@ITJH}9>_c$LZPO^m-q!VrN=tx>JS+=#$M z)AK;l^Si5~LiY91gDj6UUhP;L4SwAgbF%8N^!S;XRvymZY6*!*XuvDWY8+uz?s28^w1}*>14L|p zcas3Jh(M&@rQdhuWzv%B^LzXG9Vx0RE=hXa;Wn2*xv;^f*78kcy!W^~B!C(|I#ys` zj!17GGN&z`$KwL-Z2fqrD*JpTFUDnHt8wp3S?%(YPze9yei_5oQGtcJ(a;RF*Uha` zY7_(KN{`-FsP4>o6l*LP_GEHiyhRCHAlb))@Ej>N`10S{pIhG>Azxkny07&svWRwN z-^!iK!bZu5F>YWQ+V?=6DYXk0Hc!#4I7ip`{qebJMt!xmz4BW(A)8DF4If%7vKiT( zXt6IX3bAQwtG5zt0OOF23z!8Ij<4Zu{E5VRq}4$vgkE`^Iz2q9x+Zh@UghZOUuHR) z%vnWZ6;m^ny!0zXVuX@Ez53O)mk;jf%HiQnpQ(fI4(GuUocOiu*>L;H}J@ z5&crN<@k1in+$y!yQIeaCIKG3-fX>}*iBy;Mtk$H3&xaTd)I_oJ7Tv&0*KDB1Jn>U z0fe%*OO2c`7GoM;3?v>Gq_b?(iF>s=n3h(wCM7JK%0n}nR%U3ZoIkTvxc5Lq&RjC- z5spjF{$i)DCPRCxElss4r-DHP9FIa|B3xE#(oe^JscMAdU&&z<$wpfzM9Zj^RmB~Rq+8I2v^~X8D`DA+$Mu8t$0~DCWY1}M z%58?bU_JD(Qkgsdj1#9onRx+9|2|*wwtLUn$oa8xc9=xE9exC!)l=ZM_!^X`MI?Y9 z(UUJx1+RlvjBo4Sc-d<$@=dnsFLFgB471Qz^%C$If5l-iyBjW!w_C%oTx_mEv|XOa z^fE-KN)f^OCC{3C#|1f^YrFh{>hudZ_N*S$LA3QJm57jGt4g)bHk3SSg*^PNa?U9 zuW3_a;S5deB$<3aA#aVjCet7>YJi2>CFq79Usq~+#K)m(0*<i zq>UGgteg21WO!=I;!`grzWy4cUy7$0MbyQ?ctgR)i)1*XSA>n3_cDp|Lot}(D>__Hn) zW}`FZio)J72^^2i9Pg~#$nJl}i7E;d7)?D6N_BP9wH5M`ES=KiZ@YCId+HcUZM|Wl zwq>~|syqf;5ND=-P!zB=^m_| zS~@EhwkY(eIdg+cI<|4nsZMd9aU+E9aTEoALXoM=Ur%bIzd@^I3i@xGy`|uy3grXfeH73C?Ss*Sx`FgVK;b!^CPHCBtWfO&?ebYu zqsf6WUlRM{j9z8xJ&u>7V7{X4r>@l1u4lFY<+vw(>h0y>oHvmdwU3%L-cNZ}auQMBdETE${n$w^|J|ma^mY)4}UZ%jD6K(BjVzmsd7Ki@+-9)b8uBSyC zP-;JnW4k>_aEp+}xnfVT>5N}aXOk;Z$IU>YK>Qs?cP-h^i%g??-w&*P7NCbQFGG9J z??3yFvmbd0V5MHvK%4_22p3lBRV(Ma>yR&e;%CO_1j$K9}lN zI4tkoMYzKz*-=rcFD?PNqx&V$HU5hPNnO4W9*p7$nt(gcanfM>A?2)r{k(}Ls#VMG z;Q^!ZpY}+R@OF32`EwKm5w#l?s*XR<#*K1C9o)=a0v+Z4DVh7M}_hd zF_%DJ#U;?vdN#T`6^o=dk2#BN|GM`%Z_+YyHQ@ylwN!^p-TOlRXZSx`7?F43;a!1f z&3X%t_6;A6bL$T5s_Uz<)s1pq(2#Za*R&b1lG0KQC=}$k4Xrg^0`0D}Yg?t4Kp^a> z@DliIQS=gE?vFnK`}$ee!UIQTEoHD^&Y49VnctmS-I))h)g# z=)3{1+|;|ZY3Empse`#m){sfz$%wwt^OG)<%8?o(>VIFI>X#lQk>cL3E?rHg;&1A@ z$dT=@`j^r}ta}^8MHBD+&9YgC3s{FtF}72RS`< zKi|9rN;NM5yk_h8s=rAtqdY8?v*c{0IKYY99)hW)m9kp1!_(SV! z#6=z5Kh`qz{_f;!v)YmXMLRvwe3Y!3hky&ewaB0-x<2V6@;jI(TSTU#KCXO<&NgE^ z)>V?ec#b%4re!C84tyDg_)p{Di`?T4KRR)HYKY|ead>{~Zz96G3#I;nM^~8n_vC{Y z+zFg4#P@uL9M23AT{&l0Mj9pwNsH{CeC7tw{pd;sB#xO(bwc&3jWG>xeJJFJT?#D2 zAo{XsPE2z1E3Gg3^SUQbh4&gsPM`eEgnOW1ErBe7T#H7%T&_<)7nXnw&bM9IqEtL8 zF#cK|a!?eYJgiMz0#^dD@R8?pLBXTbx4TGgT-eG#rBHb$x{|vLkl!<~gJ1KnF9G}oyzasBL-AGm)b7?WX|+yFgk{k$J0jbkwSTD|A4CLf zj{4R`%=ic0Nz#vvQnewR_)N#DJNAVnr*%;#8Wm`I09jN+a(luCOM>i?e1CcL(j}BU zLifYWlmmWdS@s)NHpJhrjJ_XiW3+nb7ia8}2MI((%0TZIExI%*5vv>tilV-CZM0Ps zMG0&7r9qz6t-{)y&-QI9s#A__6O*U}+r=DN@I_y!V5Obi4AHrRSCsv2kAJUZ+lNq+ zS1&~L3oB`$0v#pVm1_NNACI`VbR~euL4@IJ=cfbli*MOBJDJ%UquSfMCDK=y7CSwa z=#T8}KI=2}F4ZWzJw<=Em;MB!9-M<6q0=MxZmO$F{3k}g`CwV+#ra5i%F4ag(7g0z%@Jz~Q*D9fv2OJ=TQiO?@ioC; zYiERSV@7Qfy4}7BV}CgMHZ564zhdTJzWyg)BK!9U+Ju#yJueC{6X%?=CMsmLZ1f7< z9by}JaE&pe?uUIgWY8GEx-(CX#BagLTGALXJjb8SH5QVP|7?}^d4j>0loLw<2pzHU z*37#E%HsE}Yim(Wu-7}Ve>n%r>p#;=wVEzpmFpMP<^v^}RS9&y;bZ*HlZ}Fbb_Uxl zM1Q2{kTvdo^*0yk5Qwy^cn(eWe=}@?U~LZx$?roL-yw^V(9Sjuie zGlfx|^dlzUTS4L)1iB| zHc#I9Jyn=2j{-;D{jVFeGC%KVaGp`oiB%uT4lbl_Rr$kndZb~oJL1mdwY90oYxjmr zZR#px%h^6g`!s6shp~2l=@aIx>*IBaA)mL|h3Z-`EW((Of_r7LED=cFW!0976w?TD zza#yrp(*t}t(+&s)_$|5aT3YH>B-&lx8Oq0k(|$#<4OJEkd9}8FVkbf2!>^2U1UDvaY0yQeOr^iQhKLx7zvBbBqA|VKC{`-h=i_ucTvE(_mT4URJrsf1? zeMMdYzFtpq75b2UQA-8ZF}tiip^mBW=&fe#qNQ~o(`iQ`dc5VNTkh_RA?zGm_eLIU zNi0;z?zOdRo}oU`PE|Ic8d=mpI_~1i5&5?iPcgqBG?boe3Up*V=y7G}570Gnj2pS5 zj^WKr=_{+REu>q~{ONm5JkchNbZlJi{^HcyHmN3Uh^m2J3|WH$p`08q?mi0@YC!rV zIv}v@*LdTh#1wTy(MEbl8&*>^VyC*qlJJI|h`*(Bak359ZmPRd;xdE;W+}6au_4VI zBdbjpf{Xm*Pvur%{%Be?e?aLtI+A`erwLkpw<4)>=TVTIJ;Wbzq7UcpC=a7Q>4ru$ zh7~yH`2~$F9d8-YttR)@KIpQn3pUZPeI$|VnIjvgSjc~;oNJGy2O8YM*iupVsbNpH z_AD=k{?)53Hh%&8+QuYYE87G{^aHaFQiBdRH`VV(VndD4@gqA!QCCRT$|V4tR>CIY z%^Kj;*$dAG$tKM;0tKV_emyWX+ z9b7F$42C`3;CGn8!#|qo=tld2ht$q^ay4NgaD(a1k(IXhKH?Hs=+BBj)Oqo=wxE2% zFBCc?Bwpey6JK+~)S0~G=GSvA&#%4|j9QC%Mn4@o_}$cAYMvXSdT#}Y!)dx+?8!F? z#_!$yx_SS*mgijcew($eVD{~FGM3kF)?qY+To2jIR8SSKG+SaHyYHdOR9+=-!SfCb=0>WO-#WD_mPlhhRAwdJQBeFGqF;k* zEh{V$9hWRG%z1hiwO@~751`A^HD?~azlSm3(jXb0Q=Kp~J#bRH_n~rIqtiIj+SXHO z#zVDzI#miqh3JKIcTLSOGYub+kSX~1K4W@SITM}sS3iHMCr6a}CGc>__+Iva{JI>S z26@XjesJ?(^UHDn<}aSPmiIYu!Q=(^2o!yER}1w>jj@Ye$`X~MAZKj!^j&!Gek}=c z4BF0ZM*5+Qb`)maNO!bnxmn)g&G*k9W4>LONN(&+0yyn1~nfbouhRy_odezugfD(3gzPXv{W=AJQ^f{fN!O~Bs@uwk8 zG_TV%86{oaDdO09a)nYbt#{_>RpevyG%Rg>JTlx%w~@P|qW)fHsvjgXy=x){%?!g4 zRZD9fyI7m)als`{7B^C~=^2cV=v)IP7ppYe4Jbr;WF)t(i1A-M&|vD^=C}$TWzK7# zIkjAnyktS))#Uwx5$`mkEtNi*HgY_CYt^<>Lt7>kbMo}%eZ|zFF4=ARfoMDte+A{3 zm0&%K#Bx-3Ci|FdU^${2Lgtu1_%NxXZz#Vac}vvP$jrZD+Cqu(OOk3uGA+-$B&w{Z zp>#Vw?o-waPmMq^Hk;HJi9jIef}B@Gu;A~Hg)I|%C^o=qPYG#q1Urx18#XEH#rJ%>-J{d%I#TxPVv!0>wgeA*ASs`=J-$GYa0>Bhe5pi}Wa zpY$}90a`cZo4PxEqbNE=e3!4he5&H;QLlK0Xwsw@bjy_>g=^ncSK?g(`1< zCQHZz_YU_H!(XDKw>B?`{4as1mK*EL_%Y^)D*dXVh4hV5OVJ+L4^M8=BqHA_dvfiy zs7=$O6A1S+z8QqxS#xv<(1#x!6OSVC>=!>tL#X&aPTdRb%p2QTJlb^j8(kR8@@gM* zukE7h>}NiF6)?ir_}~`;Vi`|X&;k1J=up4+WBaRT^UAR`jsgXyOO1D@BKg0Cc+F3q zKZyA=xPsF_QG7Wpt2r`2r7zOt( zEY)C;zsk4WvWLr7E7J8GeJE9*`8xbGn!8iNqh_G$(8u#g)r+qd^y~Dxpa%zFh#Leh zs7r;K%EM6jB{2R=oa^n-PC(ANJZhj|;YsV#C$B*5BgX)ZK)LZOjyf)#o0_k8*kYHG z41-OmvU&|)+^=?sy7N#4THvgPBGaLQ4fe-hoTq{UTP}>KDM6Gh4u;gYdSO?ktj=pS zH`U2-@Nm}Z^L^3``KsxK!Mj|3o2X>kytVwhDfHWJVR;0@amct@=WTf^5*kNqa}C+@ zs<-4G`B1)X^jp03`&#)dbElRkTc;Jzoqxa{x1{3RdW} z2399^ym#F*9o8>sA`%HoKK+3*zMS8M6>O{e>ocX$k~vimMuJ1nqRg77d@odUmqt?7 zk;_A^kIY>*smc8cx(ph_-r6$|fD)?GW zG67o>ijlQklJYsd1S+e+MYA%6*EVXY>u6HIb|lkZh?<}$p|^IQT=0IkiBTx#R58u5 zl%GzQ8enF0a0zH#%~nZYbt}XAS$AT|rnDUxWUcGr3R#_F{hCo@$Kt`nQXX8z)}vFl zWM$1?G^}f$PfP9intJ3M1RWiG&EINsyP9Njw;PIuq$u2IWX>sbs%zYdFK#QS4(WK1 z;_2(`=}B=ne`C9h$|&m={X?@`gqnsF+*`~)G@m&Z_=VtZbJdtS*gi96KIo$!1D86x zbS1$_{?T{-L#)Nhl{Rm}gpSQ1WU{>SuPr~6M;zAFj2v$Eo6M=jJtlH2*gn^MJ2Cxp z_x`U5+uO^b@pp$G%fa-Aw~tI2ns`|oBe`Omi(Xc$WAL^;QdM9iOWaTstZPl3Em#+q zY}}cjFJBF^f^}davQ%q-l*-C1ISVk{gKwpGj{R^*f1HvE-4m5uiK@Jpz2fOh8+fV+ z$5v51y=T2hvZcT5s%9SDjk>kpEa#Yy2wOTHRIH;h@u^b&xbdUMhtc39NxV5` z%!)d-0whV#$a!Sks=zOee8`M@c6o|8lqo>}`(a6`x50RI-!n}fn}$q-Ar>bU@>E}` zCT{$9$>!tjB#eH^-P}gnPj|*pe2**BhnB`BOGCT-2{gV|SFXKd{psPMa>wZS`yC81 z(5HmTAnnG)HH9eGQ-qyqo@yr>IT?yxq%69IHVZiwnom5|Ww%ZcnK>^1CdZD#r&+vE zG9GplO72NvrW-GFsqPmtZAg4yo$;Xl^->DW>*vAj5mv9M+EYDUS|9R>|?gGDQ{71rE)E&DPNkl(9=W{DvfU)ONscHB4l*@OcX%ol93A2dz z)tOU!Le0nb)iNy{9?irlZb!S{+#y-dI;O5BHkb__cpBwaY&>0D@-loT=`pR30%k(<4{pDnF{Fo<1>-AG5Qv!mRm`pi#Jh;e`z}sQVJYaVz&B zNNd)|HNB7s6oH7$8gE%Anf8)SZog)WV4D+DVZMF8xC}kX`IzCH0eT5!id=0(K7~}& zTmq-d+*jLi|O7J!6lOR!7%3A^&!XOox>Y?Q=EO%eN4R#e&w9?)s1nB z+I^bwT&Bu=#IpGYwl}eEHIwE4f>Kwr#GrWh4C0G6=>=Y%`3dE9M|~U=`Kn!Ut|Zr# z&}3YvSXdPJ1Z1_Xb~Nh<+@qG$No&?ALEhS{3ok=*FXId! zX$zu6iTd-#CM{D$M~zKcqsE46c1fI=Ile#c$96Cmm~*XAh~aQ{XN#kGf$?1CNqP4F z*juhNSM!V*v0B3=usJdz=*cXN@W}@~mnUx*9ThkYTGh>A4JYsvxdRJ~SUOKd?_5M7 zZ|4vQ+}m6dEcgB3=qnwNVo2ig9=G%ZTPB%{YaL~hDAVi|`pRnq0I*0AV>gi~Pcurx7z|h z(aXQHGJiEILmC1!sf!vN>gHiuAFszZH5%3waWHC{+KvHtl|A8*F^tuLk_o_ zQR>Pn597r9NFzkPOee=jy)31S@fqjWEKQ3FppRwI^#`hF38A+YuVqpsZA>een`P{A(` zlHIo2Whp@pDM2NX)(CB!&I2))sgk@tfwJ=V=KRNcX1VE4s#8Lq#6KcfEuZ|XIGvo_ zpdI*Nj(O}O{&cOvpbJdeB$ca7p`Lwg-i@EJa6h&nDRcT%|7RMyuY5w{atLA zR-;%lQn1wR(MV08P*6_ZOb=s`_(k>D7~JkOCO5bAc^dH3hNTtq5eXjC%931}&75u=QxTuNR{Uw5=wWK!z-IQ7v4IB)pYW7P_qG>+a~hbX(w?iCqS` zf{~ZN*`b9@P%9kc8)(U21TVUu?sB6r`Y(a*&9&Vrh{bTtZkWkox^9v#c6iZgcyA&w zleoOg(<`Hc!5wwn`X6j7P}&1nBZ$@RIW*oG?2M0U#R=eC&SF5+b)%RNv5q+wh_~F; zV0y zF$x;;F*av;p5Vpj`UFzQ+8h{*R&Oq6=nw=v`0moI3khwuwk@#m%RDseYt?COBn2WF z#6-TX>L;mS2CS+tqG39qznwP#C&zgh zlbuS~e+XKx^wsRoTnSo9k2$2g-rzv~9%8rLp%%qCgX`qd77>c1_l@ z8F#II_T>ndfBP^)r&{H!Oq)E1*=x$WQMd%06NLK|}Js06Ag>5m}qXIJyC}EE_A>i$bw0DG2 ze9&_oW6)QzLw13#+aP#>VU-7iEovO}>0oPPW@5VE$TSmJZhZ~sOH7V?7q#KMG#EKw0S#^8K^*8Ig3F*cnwz68TnTG+wZ=r| z%hKK(na3HZPiv_fFx-O{PKi*~9Lbk}*Ku@k5aa6c7Vx+_=fX)%@vcT~toF+4+PEM0 zp0B0E{&>{`Pw?)tLco@3f&=uHV%^*6i9b}$BAz%ub6#R`;q~;UO_wrwpxB&g%18A; zolvy5E5K?Vt}{qNq&T(h^UPgvOwNur2Q}c3LN)4E&_v$(aaK8GN#bqd&qNN5M7>)@ z7B=IMg{(~4jJDJ{47fo#cBp0HoROI}RZA*)PnFKF>lT|nX)hjO7@Dk2g}(2N#BnMU zci68p6iZe@R%#ZKWDtG;x$|>uE61NL{Y=ck_<@S!OG^9m>|mE7x6k{}^zuiyUGpSP zBqJ$<312YZ|3LP%0yDJQ*Da)lbkBZEG!IeL@eyHh2^iI;4-7&-jayL|sWb1ME4UdL z)!+&2EF`TLOc{0X_`St1Q`*-t-zzy*t3z}w$!VI!o1?np9}bm1-b{N`b3ima@hy3} zrZZO9o6^>t@O~O$Bp)6VJkP%1Xc~FQ0^Mx7fQZfsw#Wu>RqwI;N$5}-)qWJhep8F* zRqAyT>*Q{LrL~glth;tIuBiVhRc))iH%Yo+2pR3`Ar16i`ouSlYkYC13x;5<{E;QB zy5E3EQ>$fY8@vFL6?_qWG(21nH=#1RLd>t}RXjM-DKk0gZ4}<8*cvM8d2Q=y=Z2~dwSxX6k@umM zfZdQIIjxYj?<>vsAKG5O^tk)n>&}kqosyOS(7pW8x|;(<3;GJJY>jN8m~qS^4NwzE zi7nD5&KDQp)~SlrMa>3!`nofyS86aiI|G{wS73ro?*cixTiGT@Y%NgV|5P-Mn} zhwk*U=kcfO^Cv$F3yUzS;WJ4RZwsld)bEKr^>B`-n0XYm2X9?Rj>ghzRpTGJhDG-GahB78-h&;;wdTajFQcAHUK~0b5Mk&w#Au@D6s_xzgr3r; zqR8G_qx|E~*I}`M)#(Y#H5F=Eo}h2QfI1TgRU3BzyecKzb8*PSOgpB7tYSFOu!c?Z>o6?8dgRBBUAqghZhb-jS9;&fI=6cS zwZrbR>E_EQqa9n(LE2b5+{AufArZFXubrtx$>!x@Er6eMJGDtl^5Uo4rTjjYY=d@n z-s0~l+zw<5>6IvD`fj=N>Una!$#fVkm_J&VbQQROsh>(N?rxVBIAxLC*sJS`z11x0 zZJBNAM1S#;qUK;@e7~_|I(w2VUEo7ym#~VXwe6!&DW)mZWGg0%sWhSF)Q9+Hp0{Fs z-8HrXE~RY!E2plMuD)&@3FL(~z?%}+9iRD$x`l#>qtrdYoJG^eBR?xX4r^~W?@yZ( znDcAZRzzmM;$VI-qZOru&CAZqd5`a1?Zqn;o$*wMt@C2v<0#zR5OMMr>f080=4ylA zKYbv&1YCMtRx_aEB^LtVwt!Bc1bBpS0sAZ}PlJ4l@Z3vUdsu_;F8*}xuz%BxDK0u( zu=q=VM-J@_y4-CHcKL?28Q36r9NZFXg%Dck&VcWa1wz^-DyODq)NRjY%=^X2s5v9T zZreP5A=2|R68z_w&t>DFbp4i8!M4$~%*WR%C;DGyb-3?uq0?RbBaDER|NmC7A>E)F z1lpIUKz6fHVrY?nM#mi3gFJLUUmsh5*H-)QgE|1|jz_4}l%K9%S2Yg~|5I0uX=EQ= zIkto9yrGLihPnE&(m>p;b90hiz_#k?NBtMfmq$105c$9Cilyg5p26*bL* zM>|6gzQN{|kPhJHjVTy2WLl%nSeEb5TDZ*5yMG({t@!`DszLuS&Ls^_dm%7EE@NLB;F}vXi6T|59Fg^;SU(nUe3W{?SJN|b z{Lye@%w;bWmOk3mck0NgowZ$?+cz5h1b~uc`r-t|M=epME71}F)Ui>40FDeah6c$?+ioW6gb#9E*;N&U_i0jZ2ZlBa9U$GpampbrI_S^?11 ziv6DB|9%ecW3&7#H~&cVe_#6!?f;7z11x`wz`yzg?pWg1f(yXkBKDthkM^(F|51@O zB-_;ZZ(|Kvq#kQsO9M4L9Pu3-HRM{2#tBN1s>6pYq;2oaI7hOZtGqqQQ^JM^Wh@Wk zyky4~9x;6~#YjK8Eh}Eq8Oz=+{4V)`IOL;ny)2<56jlnrxLDw(t?Pyq?dJK_L}hC~Uoh9InR&h}M31ok%uG8HV7=6o?P{4j zExs~b{F;gNUK#rx^YHJCV{N0Z7Ga-bKK0iS4EV8X{6gop>l2`gX&Mej^@!PXS_I#6YbUR) z_cGn~j+HR(a#dqR+^SfD<5^f{WytnP$S=a>IEjV8t#DiSO;^Z5h-T4Y`HL0~Kgn+2 zvw^pZ)2+9wiBzgH>TDdAX(EbuDc!(LXAEE(53TZ(0!F#>CY6-q5+2! zFNCfLiR6|xo4JKWByk2A403-wtikh!Or^yG*+?_w5HJ5^uOg>U$t4g@oVuBxHEz8F zr9h0gwcx|IPokU6i>DkLEnYiw9?gz+eoa(zCFS35;EotU*L+M>VEtj@7rqG>k8B4y z9d;Wpz}S;~P=v!;rmzitV<8QSh}Q&SsauE043SCseXP%9@T#hdw>41vP$P{{0tU+qGd>Zu(QvhA@N3Hj3{*7!%tiwpbI>Y{+7vdktHNp z79i=v&=Z(9VPw{YRXmkjN9}EXcU+4qZo`)52JM?Ss`A#~17);xYdLt{Mz=^&s;FG`V41f)qIBGQ|HbOoh~lt>Ly5_%C4=@4qD(i4ynAjEh7{^y+g-gE9b zsjlW&zy6wzOC+ia@Q99Da~8lkTU_AW%J$Tk4qOQcx$I< z`or3CoCXa(t8AbIkLr_pny4l29;hc|IuW2B<8YOeqKYm$x~rCFm{$A&ufQnvJ|c>xw|QMnyoTk%I>A-< z;ZJ?)b$hQ+2ckIMZ@25Y*9@<4?gCx`w*zio_NM2brYn-X%Y6UX_oB|eUNo7-eiLb; zMvAHG1kp)UG+{Ifo1tD$CiH(En}{szFA7!v>P*`A(Jr zfVX$Oj1&pw5&vy&u_e|79U!Fjv;;xtUxMFc+ln-O4lVaJ?OdXndci$h=QEvw&#`Tp z@U7hsx-+KN;hLb(P51O}dp0_lEy4dZwx#I0#tq_##x3|%lWGo@jCOcYV`+NR*YJFL zu5_8@_U%W30oxomRXNX4ichZN%Ddig)dXgS%62Pi@Tc|7a5=osuNJFUosOdKuD#Md zoU-yHz3aEhOY@2I6ZLDpjq~Tkwv*qH5PVX9r0Pt&OO4kOTO0N&R;Z9~g1visn|04t z4W&Cv*YAOosEbc)O3J&*`@)B+n<2doaruE@vr3JHc(3V9P2(I`Ols_RW^&R8uGMUQ z&R+Bt(W%$@c}$K{rV>((qN`mFia8Pas-}(Bqzi2wda=yrIgR>IubErt(4G15$rTE* zSk5j%rBOM_<(WU{XPE$KTakVyTnnJ)O^_lE$#&(61^ma~mbOi!QCCw@=HTb~dt+YD zy}kWYHq})IY5D!R_iS9#uQ}*0!JMWwWEKgE2o1JxSZi!M%CQz=0QSfA0kUdTH>sBe z$Euh@U$cUy+*`KbrJkV3+Ac6<^=)6DVx?75R+gua?{<=0BO(7OL(uIybU!lpUvo`6W4 zEBu?~D|o?I@lP5P_znLx3Dd46P6Q^kB#gJrb5)&uf6U{^&xSwyh~=BXYicl`u02we zEV&LvA93H}1;vc~N1z)WSu$3g=sbh-109;rMyi>*-JTXYIj0O{4vpIKHlH9D4)IH) z-xm|S7wh+CmWd7wv1&E|g+2{6lL9gMOi+*AXx9{}LZ?6T%RWNA(3UA1@1wm-{jN8X zkZu08L2aMu9b?%{c@g!p~Sfl#OiTod~1Z8h1=!c=xWQ8@~|Ti_0Va z-e2zL(6aWfzAF@9ctBy(iy33 zM|w3DJkH;=7lKI?_ojUoC{16w_mL^)ZGWXG&87bYfg3F$W#58T$SFhFd09P4I9QEh z4kP9Ja70Nr#}{<7?8TR8|E;A0Tll4~Fi=)@T2Dx941?CpoJHR3a=I>8l(@u$db7YuOCuxG)Cte)YeQ4XNI?@b%cQ=f)&OI#sCoQ?!6x{ zZ`Rg~O)WnTP5u7ehD$Z~(ddOkQ}q4F7f$>Sm#ncvQtz3)_te|h=NYs?f)>-d;=djXE^dmT zjfN!-6^GWy=@!xlpAUUHN*510%jc+(lyH}}PmsG{NnF)0PMDLt(eo;RVGdMZlq8>a z9YTx2U|mn)Tqh~XZ0A)@J#va5@5h&$e@T>?a<}lYS|_k#g%z4$4tKSwO!se+fq;OH zBd+Xu(em>U^4Gwg)f(Tofu|+7Xrv=s6*{&Tx;f zP|P{4?PWfMe^ug<#Z=QW&9)V|I@|km`J@!@V*7xND=n$hZ8jSwp*(XI69KV7&Mf^o zAfsmb^gX-IiAvGCb6%@8v0h71T46&kHPd zGz8VLU+s+Ti5w2g@7=UGyQq0j2mo%<4aX<^CAmwl_m_m!4PZ5SUp@ku*JZ|f`@#d4 zWa7w7t1TR^>f!yPv!cIK#Fbmch+A0AefiZ<3W)BsT_0PkPbB>q#0V5uE8?W))Sux= zTUqcmU~G^nAq8#mQ1Egkz~Yl8Z1?cCPE*jO{e@%V8Cef*3~yEQ`}&Th0;<0#;EB!t z$4R+?M1!$$D;XWmAXz=;`=ipj-L+KOJoG>%9<_$<&s@P_o2Ig1K^3&Ka-SuAeWGg) zN<=sI_e+kAlg9{4PoFRue!bRbqEJ&J?Cl){NfB#pSN3FG-a+j%MkK+(CIWeSTFV1E4AE;NBThH zG;TB`NWK`cyLjMTwW#K~PjPQmY**^`F%LsK(c5{M8F}N@SY`n9GIsyPZgYK0=vC*Rl`>J^(#fmD$~Tn1_1^U z9sTknRe6tfSS*qcAxx)!k(HO;t7-F4Tx=WOG(}|BmX~8pe2=Q4a;MNe!%{J;*vxI< zz~x~~>2s}qjVGTPaLCsh`=e8uHP1mepzcUALim`K5!TG(ng04KQ!b%`m4XER3}w~~ z{UUPF?_r!A@?Cl6HCFVuo^E&|b5;XfDj!$snicO00+poMP`L~-49WIb}4y8r8U$#R*xHtH%&kS^ksxOkCZk$Gg)l9?X zYf`Gsh3X9I$P*+fl1Oe)7>UQU)~7PeBB`P7X;%oK!U)@+s}ZIT^ECzc75P~PNchhD zOT9iP>QlyZI#WF$As>fn@IW*tC4FG>^t7fe1;!7v(}b&yr*{_byH?&}c-}%)BKj%5 zEG}7ErX6u9KfaetDLVQ4mSxrkY^}M&ba2;cetGSn0uWC=a{aa6Zh#)8+{wVt?2Q=& zu~sCaCsq)Ru}l1XE_B%kGm&=E>7NRhDf~nb+c~Q_eWvuppN<0fq*YgI`5V#)%cl9b z*?F~T|L<&^?#)7B4}OQ)!#IS^GD@~9A%bWdUlhtgVQlS*#iuBmtXl&8(#c_7*FlRU zFF2|tz-U-ZyFv907kkAJQA|OYc;SId#W1(nZ5#2^%R=K=Ux93E+yVbcbddA9+eCQ<%UlsK@r#EO;7LF3ysTr68Bna_Na?!0SpD|BkihCg9020ec=1W5VCEeoncZ^++&bGdt_~pS96wZyxp`e~(E(Cs#~%P1$cQ-=^sT&e(AeUjYM&HP!mM`p>s20-u*&Gp%{%g}eeGt?XFr zD=Y9+R6M?VP}`W`WYXFaw3w#gZJB65{o?^={HVHT4HAYt5qxy);&=&rFW>9x%>-nnkA6JwJjiB$%h{?J;bW5(TGLl;R1d|Ihg4H;Jg(2Zw z16H=Y=zZ?k5C$&O@k99Kh!j?v-8kxZlfJ`tD&WF#pUrr{_j$w{>drOfFIiM6 z-)ITf64o#t;fjBM*E*~on8jk;5pLMwRrWMB5E?`WaB&)Jw>K|(xo)Ho*ni3WoJM6W zC>-fO+Dh3ygdj$)NgkKl#a?m{rKQR#W{XUiy_g1gz8#mcXRBAN&OtD2`tue!VGhe?r}6<(qH7>mL)e_&_fX-J$TGrp3Ul- zto^k+1D&r!qvE}ss(JkdnAc1NPjY>f1%DmkGV(jxvx{=;)6nE5XB&fUWbwpAx zd1(U=E3i;pz~EK8Ex%WS%WZYp?&sa>e05b&s3cix>7l`+N%zF>&3G5g=CD>9rlTnC z&+Lnld)g9H#kTjqKG@>)6r^n$>fDG@qv5MTASVbgywIbNCS(nA7^|t|dI?AHg|e@f za%rX%=uZ*^vU6IUa9IsuGK=2(l@ZbU`mr_&JIvILZuCb>6pt5p-kj3kTs!PdVZaIh ziWGzBf17wT@Uk}Vshd!?<@itqeeh+*tlV=kwf;FtP?*C;uc(3FRal*`8b8|T3SZOL z2?@SL)U8i3f>Eq4sZ@*1pGhb!zB`vf{qa>ta5yhWA?vZcf`zE*w9xx=3Z9_U6S%YU-J7gm0HA1;|Z8tLe zA|&?Ae}BDSC7j;Saz{q3V7Rd^YhJ_3<1PB$E7?6}h%z3kfMiG%`Sk}kS@WSRalgh{ zwgeF(GX#JB>}JL_)0YD?k<6jj9SV2NFT}RtGe1%S!^=iQK7?s>#W363$py4V5yX36 ztVNmBX&w=nFt9YFH!uy2dHFCRG0-v_e1%r_?*06*y^Xfl5>6K1^#AB(EOA;|#_jpc zz@@9Gr1^XJHapgEK9|LQM5SG;asyw>DLDz|OTH9C6Z(yLN4=2iv8u=r3B<4JFWUO3;IA<-RwR8I^I?CS z7CArW%h=*{rw(Gs9=c$Yu=W1^iM+!2^2`ZrMOk$(SZqA|Egjv%3C1|3)s|)6Whc)< z`pLsTbH>;E{PuYgNnQPVsVH2-UyC)=Y7VqU5X)8;9}F19kN8Wp#jiaieKtVC#^-wR zR^8+ZLs6qt5--C{dzk-<5<$DhwpD$>5)KzYiK)(23tLMTzczW(&?P1+(^|fkQP5q- zGfI%V*)x!7oju>n%lEBr+y{Je1-8Em&!0C-Gj>hgmf%vF9To~O;(2Na%l$#&*6FO| z#2q-9Uk?%x$mWyvSDO`&V`Jn3bE~-I=wN(F`5D z=G}%5TH8Hj+etU|L2cwVO&z>QYICr+eYzeyRqn!V7na!NL}TOH8RRC&H}e|rjx+w% zE{3Ijfc;s-9>2lm((gOIVm%zlW9DeTRC+sfyda$^V|)6s3`m8KX^3-loRi> z1bbM{<@LB3$zA!H-M_|h<-wBJxc70%YSDhvnp2z#tY3;|9&h>c!Up37(2uXUeQST= z#P|SrXTgYNYj~thOB~rDoj^A4n^U`CJ`djg(W2|13PXN6=|JvXVl+?@Dw~=#+ar?c zE&vEPEbi!&-K>(|n-h#xi^*4Vw|2G>e8?C-lS*l%+36I`P8-5g$XQ1lA<@)}W-Vb* z|ELpdFDNT0WFkl(>`%0-3%6B2|>dTkX{Fmh0N^;*j z<%c?9*QzeO2|VKmu84f^jR}75TxORF!^n9xks%9P1crwV5ks}kJ(qfg=^M-z?*tGo zq3l6o9kSic=gAB%6tcsYpS*lhZ<=+A2Yk({d(Y043#T&Mvq+S`0P->9u_(^bl*moU zA&`R~;>9p=*0Bfkk1;ZUr^&1z_)}bOa0sNndhDycV%1e+ z)hpQdGACAUyNO@bz92j9H1=7)!`J5{on24-=Q-tf{#ewBOb;Y#T_{4x-~c~DNext1 z_95+^1e%dh7cao5TT;~e^-DivMj6S&DgrafkB>%e4S~);|pYo-sg1FP=sYwkqyxp}kWCJhN}Z#9b%+ zN-_-ijgoAxKB4$kq;S>Q+3|MC*}KgC=T>nt)^TweJ(|#p-Q&8U)W;t<#Wj1MBO9Rm z?rRK5@cguxt)@m~JCprznd2*5Fx>aq_e??3t@`2yl7?GP@IF!`O+|E(bCIIcl4{D} z+R#A5iPD)rv@pi_{u;StzNqH-lD~jG{t<6Yd;b>|w%P%l*JUw9VY>TOiRG@^3;jUtoJPn0)q$$UfgNuYAiqku(-Fr-cEQY#HVe zx* zzNZ9lcHfYFkqi=Fc7Z2~Zz(w}hh~IBc?fsfummd&wi$Uw1S_&h0dDIf&Ee+e#`mr@ z@qV`8hiV!Ui_|M>X}XF_4#*qO6~QqG-16+4*Jd>hWom&rsH)wm{8+lgib2$Pm+a&x zw28fFun$w+h@HIOt^Y)k=}%l`6#Ccly9BmV>+)R2JI zO{aSn@bT#@aCV0U@w+COWV@bJio6Nc?UMo+;~c(zbN5FUWW=j`$_L$w0(oV{+>7r# zIzDE!&hXW_kR-6ZR&BGGaKDh4RJjuwV4j-Z&ASX{0oG3Ua zHlI1w#FhQAg*wgrT$Bv`P_!ShMsL;Qgvs)7yxxFpte=p-;YjHD@_xv@VoAO(qqpEm zLxPK5wx-&2C`Xsxv0T5`?qe3M%ADOy< z^1G0)r0Rn_(;{sbVgz<9Z%Giea&|HIu(Tz-&H9A)bB(0V!mz(uzlD)NX z{+1$!v&T!9Y{T~h-j(kSIyEpFljiFPqvmFfdAL*(!}im5^LTsO3Pxdme<^cvgvB@YjH^yu3|n0w!Z zrFMcn`l$>!Sl6x-XC9fV49$+pEVrrbpcqA_>b)|Bfj2ZHDP#zE1Q5I) z@8v9S27|6@@P)!xo#IxowIwB86stD!^nL#S8BWOLP%)ZgUN3NsH}`JcZ%n2#OAEsCuK zAzxKTc3}b^7TkUF?Xq`Y@{=!}a9(>=s7S7!UsM6V3*Z9$iq(Rq2r~>KshAD|F?u<2 zHzU4H%NODG!Ir-}darvu8xwCorl=kxvX{Y)6{4P^+S*OO|nGWptF+%S+TEq6U(g?QpaV>{a-PWpZ)>ct)$l^o^9R5bPyX{ zKOfuOJ$P-rw?NI=e@)%rL3u5I1tH9=Mp>0bnK-ZioYbXrR4W5Y!^q>9xi+5?-iJzi z)#$|hThaDs#;1e5l0@A?Rwa`9t4AuYBCc}w>o_kFo{q~c8rh7?gE&L|XkiTZB;{|F zT3R+IulT*?PNlNZ`Xf%do=gA(C8B5zsVQ&}trInV-t?CxZOyYa{VCtlmWvmrFsHr< z*}>P+paZoM8oiEjw}`{WkCvYpIxqcbQ$)i}!nI=5grWY`+tMQvQ_HAY7);{u8N4-2 zfUZvWSG|tXvFkJGpM$*WY33bSe9T1YN=&gmv9qbuf(M)qAQuCgI;yu8zYI)axL0Mmq~*R2NLAvE*D8-|?A-W;_| zxUY|z-u>}9VxaVj6bGypMy&dCauB@XPOL+o8pTQK!=}#D{%5NEfqoj3XyAVXWg-;u{9Om-c`TNf1%&O`4W#7ojL)M2LkzJw01Y&GA%4TQ0-L4RL<~-v(}gL z&1g$#}*`k}NJyW|R9l-``r&=OjPAg>64+Ppu8-4(P4C9_n>|Po$H_jA!~mv9Yk|w{3+X^xFI9uF%upb4y8lIRB8yK?uY+_3gH`fvJUw;>E9RxQ^!bl|Hae zQoO5O~uFbE%t#vYrV~1l6I(y$;b{=1t_k?M*&ovB69+Cv<@s-I`zH?P1FPG zE?0A6G=K9`U!Ld@L>yWEl6-sv`j<)Ex5d%RwX<=z4%02Y^S>lX|MuA*nqRk}vINaR zKr@1t!BkvQ@cm0dHVN%^Lz|vo(E`YB(37h7!!L_|Cc`TU+kjpMk32g84G;z{rDYmEx^S~iU#`Y9>c%v`s_FDCW$Zpr@Njy#kI!7R`>=6Rv2gUxpBW;cc)H7YbZh@ z#)u4;IYy}?O%Apvw$}TWTFj;T&t0?caeHlTn6c)iKEK0}A{AXEKmD!n7-(d13}omc zFmhy>2_fwP=R$VZ7#=>Dl9ZdCh#afP&Bl#Pq4fQHd(UEZQ)UlS%^uHiQn;DUKm6gZ z^uAW9Rr+#0d;X-M?kKNMB{%3HM}%t)c=C^C>~gbS6vPeCmV+7Ro3ldDNk&P#Eyd8L zk}j`;&7%*GX@@w(5bB>|(aBqBMp){7h7}X)J~KS85*XGL9!;I)>Fwuez~Lx38d2i<8h8IdVDX18~GaVslpdx>e__h7>44m5B49?Ch!^vvB(!6S< zmTdisWVFeaJn0MA@*?-C#mn0_WiYQ96`)SQoP#@7fwzfv@(PIJp17#qB~U z=i}gWwXaVSX4%N!vR9HJf)JOnkvBqLdD&W}n3PwR?%1A8clgtKVg~#JB~T8xjS6B^ z%Z^gUJe<7R-B_agd?9!7q-)R=5Ul|ICKhDjE#|A@AQKK{_M8z7GRm3QOjsU)d+WRg zHQle*R?I$plZEpUk~x2f8e7=i-MF;-BS*M-;R@l$;*!vpPMro{c8)@;jK5AbwpmvT z$8LH@=!m3~3lXg{e!o>U&g4Kzz0=^#GT8;8cF_uDwJ48fqe^{A-~G*umATeFpHtHy zp68=&beUn*=3(W{mr}<~J$r~tbttT-LA?#FIRL%_aY&&`0z)fC-1+%OO{G#$O?YM>iisTUToS`4C{H5m3H)Ur0#^E-%1t!l@z>db;u^r+_nvaoq&4*6l(>L+MV$ty)W)V#M-878j~Wl^8EHG*A*S@BC_@K z7Z%CtFBftjmy);W0)~>oPH^^4+U2XE$bQ{(+|qLAscl4I6$MUKCm_?S8F#8s#bf-b zjJa&`2}chJ`WIoWg$D*$2s{PRhQ4NC^~8o&ST)pRZ^rus&GIGqCT`c=MK~;`)Oi@N z!H0y$jX^Ks;Sb{r?+DI?_ZNMgc@6Pg-FNS2=bE}KZ5?)KZ^j4-kPCh7p>Jq?CyW{% zPTxOzUL(8CBQlAQlEMRm`H-?cZ?VI3AZsT1v&=*sM_m6UCjsvJ>d7@-R{Wc6Z()Zm zIMS7Rk8UV%BRk0bD9`#vRh;Pk@Im>U%E)ODgB{T<-S9k}spD6efecaiUD zZEDO8x)`mpcS93EtORzcZp|CbS%GB+5@kJJiHd6VHlsJX`$P(aq-gK$gL3#3@7FWy zUhUj`0iC04*F>MSa~I?5arErga5#LrMq~7<-J7TaF+cskBszKtF?BlEm??D9TqKXB z7nl8tW|r=OVc_0fz$puk-pum$uZ&20TsWdJk5kR9(G2>0LrKim--K;52*tq8DgHC7Ua=Q)>Sf<7MJg) z9W*p)c6deF3wqA&Ro=f;PfKTRD)D+gOyy|LmTT9emOfueL#BAB*8lj)@bc_3`k+do z;jE76~T+BzTH(wf3FA zBn(a+kvS^*NT7~Lwa7YA{o;eymi42ct!=Zp!wz`rh;x{u4nu>7&8k?_T&W~0`JE>! z46(}~h#1zYH-{I5$;$pD#Asu?BL)=`%?>~<-wyW_jM&IEjLOqp_{{OVf~q0UapmW!ZhP{pSt9>^b8yk^F8`(-F{9$&b;kic4b&ccvmdRgWHIUnQ5PJEI zVjw*Bz&&HhMAm4$B+<9R)z(%&e*P6@9--U5P<8^?WWaVe@XMdxgMFc}U8nSY?T=+2 z&F^-9RWq;YLKiVO;uBXD^Ta=ef+P{jh7SS3UJT_$~c;f3;4pG{D%$^MkUN0q6)1)W3 z!e}BA)iuV$gc!p}UG9S$Wgaxh8mL~eA=}8Rf$ieyE1q50VTlO$oj8K{q7^5x^BWM& z3ZO(~y?@S$uh>EO>)^j617b&y>@Q(N6+G0w=&_1IjzU3ASLtY}o&DK#m5c97_&9CB zaGYqHtF^>h&>1AJ{PFtp<;M^%{Nk!+yd&_Olh}517vHBr#RbnS>OCbt9v!f(aMx3q zN^ba$iB|Qu8Ijdi->nrM)^1%aOnB`n;r z3*(7c)upMVj-H*lUPVyB``6Cyas|UaHez;9&6d4LP(RDVtK1Ttv+^wNWJuH5-OzWN zEoIb+kbWQNB!X|im5dWPLV;2=sBf?sL(D0^0&Or(l3nr|`|%{@StBVV;nDu}FlOCa z;h5KruLLV$Q8}bzTLF(z$cz*9jSr(a)>7@=poX+PLiSfXXg43HgoImT^CIOucW6bP zpU{yJv$Lnj{jBvgpA-QxHX(9{rRWtY@=)*IhoNx8@q^S#i7&HF>)T<0nse#!_}1EaL}$d-hbQ7{`T&v&sP2rK3qpt zS%+_0VN%Hx4GhS-n%S@r1$2 z>!b_jNjGZ~bxuxX?c84fz=j7XzU;@ak)rve)x=O5lnvoAt{*d_uHOzI*UL^7vly z{6ag2*Mr}Y+R_D=8hUpb+EEz50FKMOnfbTlM$ z_4{Xcv(1i+)x*9~o=|RFC#Um2z*J6HmK z_j!I$$HmrimB&q0(z=;YGz<%DRWkIQb0Tt{gUXyRZ-2jn4*b(FBOqO6OBy)8cf1Gf zMlvC;6O8c-a5i2J{K;xZpy_zK6r}Ll%*mYZS{*y8Hpk<*>F@)O7>~45;}})8dR{NYFyVIzbRrWPK)50n71Ak2R{)X07m1x(4_l3 zv@wJ=Pryzv}xN7F#klZ89a2XQF%}H*n>=P?Z{=CGtp{RV?aE~bEET#@1P4)V}JrWu)Qevi4-MP zZ=i?=Lb_(ksZ-`{`@>f9gJ*=17S*t%<9*Pk?bdci#?tJ7lqFZhPsCl z`3Pdutm8%}U3hfh=2F8N(@b=zpGz6*lI-Rk-v1672__+&67(6hvQCM07n zg-I?~Jjh@DNA`@^)2*iJ06X1GF-~ESUzjduyl7N_Y)NnEP{o0c&Aj`@))rn`Odwcu&zs4AnDd>=sI8#W5k5*HJ!%xk(vxAq68AKiK z`bk}Xtk9fu>#eAX_>b&#gMiFenO{g027gXNBGwL5TAO3s%80{6)yby3k(pTcHpHZ_ zCBz}3gZH3o_+HXgM5Do{LOybvY%9{MB)?nYSeAal!_^gE?v^KQ6a`TWXNwl-SJ+TmG_M@i8RGVfaI-cP-LB{EUtER`nR?fI9@956 zm%l`5^hC;Xj>0TuTu?w@a6WGcHbyhb)QfL5xp@Vd31FmgbT^cP+^hci{w4neOg)0w zd#wK$#0s4R=-|1R8!lU^qklYJcof=ySB>YT6}x>-+OrG>ik*gg``k4R==8*|=Xdy` z50w8@Z~H%!3`}MbtTKW*<$*~}#1=D);_{;w)2@HM7DhH4{g)(NjJ!{i5}<={;e&U( zHAH7z5ED{w2mHN-3$AQvPg#psWvO$T(g708w~&c`4HdlJN)d-`sH}ZBHss|mx*myz zuk*Cu5Tky6LN*HB4aRCM!x5X*A1S6h5PT4!%vFnJnxYFsiHB?+?L&?Ss(N|+iRfa3 zA8lM(EMtfz*$gUuzKDQWmx>OCR;SJ9NVl!i+KO*Fm>nX^RqBLkdB-U6=BW8(r2(FY z@SYAW45*K0cDq3QFUj7?1_oHGsr}?HO}p#7?|jhHO}z@<|5{6(vH9jELa8lDo; z%*iJ!tF57OMO2IS!|vX)!}18V*>PfU=rCQ%j)-u8GAFrU8PB)0W9@^Sv!qjpyb0h} zZ)w|s7#HEm0TZYh=nB2y1B?axP}mpRIdFx|Ts!QRj%BN+Z)_El z_ceIET9fQ{U%nm?t|?fuPjI)PP3~74eJ_n*Ng5y$}6DlWC}K+R1>w?L>J07O-K_$vN^O z5MjSS|Md!FL-U{KABYV=Ofcjtr^zR6MND|ueWQ!rchY&~`2kOd`}jRH!;(UzC?2@J z(%mR|XuxMxXMUGgHta$J!ar$pii^#d_+TzEju{Gj(U0Dz;SheCSAWXHu{IZA@ZQH> z^SiW@@nu0IpM<;y*WXql1)(nGLcMhUE7rA8`eH*2awj z!9>$tiUe{8pH)qG_^W1>E4!j}JMu=%v;qTFT1j5v^juknflzLB&UNYZS!SBrJH?1s z#fjWL?=ofq;*0iwl!X)lR?Hch6SM=^z)E9hdSF)}dK0S?gvUOBnLtjgb*V>rLVVY4 z0d%Y$oywgZ`fyN;4;sWux(wfE-GW476)???1a*`b7K}I z8Db?C-P`83fQM0YvKP+pqbp8Nx201Q=42m;uqw0NQE zl>+@%%t-ajOgg8=IDJQJSO$y7zz=LCPvUqS3O>7Bj1EXtIIG~m$n7f=0XtqfFX7q(riUu>)k zdOGWsht`@JU=&~fkz#ZVZO|F_m5_G;49VMLg$*``C=Q=1D)I%%fA~xY#G6#unTFu# z{X$D7x5idIFmX_8=6+*-)G`+pwB;GD{CA*!Ro!RdUQPFR+zwJf^ol^75 zA8=A6YMt!Z9#*7};oCZi^w`U4OAVFrnxNb#fkK|)%s)T(y6`Chui8q^^m~GgU&vlD zkX(!226lxs#G4Q?e3zdv=F75{;GFG_dCzA%6F0f+kdL`HE%GehL739Ne--WI6)UL2 zpJK!w{!-IgHM)JsJJZvg1XX66ILsf{%TU^CHYun`)r-|hDWUdvR1%cBwCqPMj5S{% zj~Hsl5zDVte;ueugVGEYm3|MTzMsi2Lfu@^YOrhJHqR@GJ=nx?RM!d>n8tR8G=Df4 zGt0ea!V^y(aKZD@O;_+C2nd+elJeUSbuurhTiH9PrN<5LyTSt9*guFXjK|E2BOGm* z9=Ot6DEjz-b_x~4|LGHuW7*c}jpnmERBKVc+$+Z4%0|EKjiC2eDB{teU+gJc^qF*y ze*CSq&Y-m6gG=JkP`Qn7xpPaZn|GLnn2YdT>P`DLEBqS9$e^WeqBkceq_wOjWihL) zsd0QXkoRNe5JPrUz{tkDi6Ct0{ypn!GSDxfQ`4H^e@Ucnp3=K`9=e~hm0&cZow44b z=Mth*K?4pcpRc^L&+1>`ly2h=?SE-yvuTtJ_6ytbj+;ct6K+owm{IlE=6asLQ8B+W zYp;}#i)cv%<;buuD85xFzqYFI-T;gpSn&lQA~$?esl9`fXI**jj_>|fm@9FXsQ}P} z9X`t((Q2ftRPuR(Mz9RnS=~@uKD&N`G9q&9Mc?57q?MzB@7tJP1_aut!mt%9u((E} zMQ|_kk`46PA}x18nC~V#(*n0s^ZrqHwi5hDOpYjt+B(;c8feWljwm z7^{?bFT43LdR};61JvD;MBj2RH&f z0M=4FJ>uHGOzx0V#z8fqmxqMt-AVY&f!eR3+uTNVH;zo@zW+JI>oh_S07oF`KZYj% z@ir5!2LAUCv=i5Uc*o)k{3(liI{c2dr*C~~$XZ-{{pCw?hQSJnve2J@jBe3vV}D6n z0GlFU`-TCuWKKP{YEBJXDWGAAza-4w2#$3g-B3P^)`9BuJ0&|#ny>HQ`{aisk3>Ru zeC28(!dR-^m9!Khzuwv{qQGyQ*>^eoiI{6!)7!;?QirDf&o*4<>#LRm_gLYdPzvwm zLznxOX(6051e4X##8!r6HBD&sF53vs&Z4!xxoH0$ii3~Yv3)?{*DyuMJxwVwQ7X#B zFGb%X`S6+On^8nQt(88E$cM;yxovsXcg@YFbput41!9&=jjkhk@AAm%-)XgNDm@xn zBv@S*{eh*v%;`!A2dq_dCxV4?s82AY+^T?==-^&njdYkq!+k$+?9cvyDm778RoK}t1A z=TIdPjPOM|y|1CX zQgg`FhYq7a7n%Gzi5GWc9P#&k%H6g3L}9An`_VcdQ;Y5hT*-&`Rua(>36!d?3y&4* zN%cdcu6r@=OoUeS9ESL0=+TWt__(GB_n%lwWid2`kc4%P8Xge2A@?zC-!$Nrp0>Dx zAaaN9V*OgQo3QU1g{w4=RZUS$5p5b_SVfOW(gomPkL=KT-Eh^S?sMhy*}G7j;cpQmsEn>tQ-BgjpF2kH$X&;U6{BCzpWzWSHsmo0#=kpVf`>X!k6 zsOg)(B!9#_fMlJN`XBg=AnFtY0xm57+qsiGv0R%7MG}BNHmhnjf{rq+cWo6u@AKw_ zQ4aG_zL6hQS z>6Wk;%V+E-=Z;YSO*yLyz&WSnFNr@eWr=eC>HWX|C-uyB*yTX(m?MzeU1?6c><35) zX?S3o{ohWAm23YNxSfE&{XgGb@4O=A5Y+LPE<&?%N=|+$9*3dfz{qtyr zZxB0lhbV~FE173>~67+rd$tR_0Z>tF+2fEKHMfaO&9MZdh z-e?upuiUOzX~5c^ZiAYSNwMRwSS&Oko5`z6@(>P}Hk=RqDVe=yqfO#2cI#_u|}O?YMRz zOJL-IS@I8|1O%p^l#>-V`tLOBNBDH?BY;Ij^XGSv6xR}WP%BjON=eFQ{xhDp>HVDZ z9vkr-=0mk=k-S-6%oPqObzD=bj{{ebry;9D$#UJ<#)TYlQ01~bDy$^*J8D;V+;t56 z=jm2`1Y*a;A5S-#E#PlRx zu(az+a*PthcjdV6#rak*G_H`-fhauFxUa;?pQNa0u6;55%Gl`m>scvHAGKvyzyG6b z^ygn6KgETlo^~AkCEI!2hQS`BTNi{6 zEny{RTu;T6mz))Kiqi#C-hSL2@q^sMGhu1hth!qew4u8XeFZNh9TdEw*FDGeaBVqt z*|L(p#`lyc7s#w#X$5@^?4Dh_a@t|Eme!TR;nZBWy3h8@^M6tI-eFC>+tw%uh*Fi_ zLr1zu6@iFI6A?vv3rO#vR0)J4NJl_GK|rZWmEMW;4$>ub5Rei~C?SyIUBA7*eRlcI zx##|S|HzXkgePmgD=YJzbIdWvc$#22t?=m*wd-PlK-P<-j^%Z`}J*%Q{wZ!xWDLIIS<$~TE<6cl`wGqyd$tFC-KE7 zz}=0`bGJG(vGs1lZBoQl-lL0q#_YOw>8CcI(2(_*)Do$d@}P9R##$HKW>$ukd#P+* zdJ@vjU#)91>^Pli)X9`S0>GOip}}#zYeP~Rr(xJ+Iid$?)ZDSMY^{^1DM3!K8#&Q^ z{6h68NFMB}d)SDpJhvg7!XJmo1EB+g8i)uYTbt%kMA; z_;sv?ep@5TG$Bhr$NX!{w7+V$TSM!jp08@Jo;|gG!c$Mo`?PCFWly^fw$GJG(iqFx z`jEHvHE0^}j|-D99P-a$YI{mwK*3?|$VLQRjR|n~;7ev58vA`QrXY&+(&`jn&7d zX?)x?vQh!(ZvOC)w|3!1%AlyYhdSCRNs7F(Q`6%2qUZ-2RI*TJ*{i01DTZo!BYoC@wf_g0+XZsr%CrbNhe?B_% z)y|!uDre-cqI=*#!!-?H263yjc=2+WEnrxz?3MGG)482x-qcq^dZCcU#xuFAz2Qn< zKIh%1>0JdjFkrTBL+%3Ldr%G^h4m7-XNBkdfT3O+gc~BDt$e}lEXGsaIf9i za_S9#*CQ^WGdRHe!8io|g zJ_VzZ@)Ci}4ufRK1a5cT8RsK8+-GMyrWeSh*^~?gXwx^5tZgua5yM`Ma zoNgME5pCz~fryt)atw~`q```oLA59S^LgB!n7hjc^CKpI_kEqYoGaS;N|)5ui=w_I zgA(;&M}WUwdZRxBr-Uuo?43shCZAZsuDLauD)QJAmKS9zq{zPwNP|#2rkdTdU^l?Qsp>`ABMU_M5iVu4 z^%!5eV_|cemjeTmw}`X2nfRBg^IPs=-iNCBPqEZE1%oxD_`n%r=(WzI#L1>>Im|^% z@|x@9gg5i}uEsY9hnDw(0ZUvuT{T2USVixyC*!o$k;lttnziUG?qPkV{EsXZSB z&lr}4ji!6CEE;0bZ}?ml`44${+462Rp_>v^?i65^r*&Fe&@2&Ob`d>ErXj9pTK#qc zd@~HB*PoaAdGUWsy*ofYKgPpINKg~{;ZCNHmTQ##HDNnB%z(^R+R8FW`;ic&5*<_= z@06(Nzy0Pb-;--jgOx@(dhpX@g35OhR_4)DB777#ZHS#j1!~U2*4gG%%nL2tLUPrH zV+Uy^ z_B>XsJi1V`6J-M!VwfQMvncX%DDKE3WQYzRaWR1kIJU?cOsdcIAgfsw)N^?$mVROS>pHe&jr_7!CohTaD5D=YEHWde zVU{OX)3~OiVWy&BM#4&u=2xxO{$xgsI!`axY}s8vUWMepd%RG)1%Ui|#sDYgHjuAh zt{wsa-&tu~;koI)cOv|!;Vf)_97Z55KY9Q@)g%5Q8j+&s|2;d~1F->JiI3KR3ceEf z#Be_AJ0+kC{l8~x$0I~4&h_d96JNz%NKEG3ys3;Uy{m(F6CwUi@7rO9twNgbeLhs5 z-ilL*T_umEJW>2VT4-&7?D7LCT?5SbjD!#JcfX7!_Gn+%ol0+*=z}eUtbr3%oImc* z$yl+|eOV`18qD~|J$c2-5%YzE_lk16FSC-dkiDky_@VeOz&1h=Nw#5rNaWYHD{&!8 z**RW*he71%(WYBks4(c=Oq+L{VN|I~v%GcbnX$kB+Z5>&A6~XTa5FP|z-1+6szk@I zrek6~3pLHx8DplylyULZ1=yx`Y^7AYv{ao`G(BGEWmSvkXAA3|+f7lA_0`pzBJ~VX z>-_ROp0`KD5UP|aCt`WXu?c9>Uaua>xf2QnaQ?|J;hC?pDAbJpt}!O@XixxUEF@Q1_z74}6q zcRg3^Hf`BrkOij(?b`<{x@L!xHs%>dq6U1!G2|PCK-OVDkW!G{NL7ncv|l@aq-11~7G+VZKOoT$Jg zH5%>Wp!^N5WXYWx)Og0aW7OT`P!qkV(l1VVPRUnL5pg<9pTwvc)I$qLD&dasBn+HZ zMSETRhFo#WjEXdUbmg|*eB2g`q0N$J(9cCB?To|g4?U{4WKu_T!z(X>0`jhWq+8gI zE`du=;D7wS2;&UX-foHkJ540c6(_(YTEP=ZjW;$%zjVPwtU_M zc42j_R?xog%72%PtVKDAb&b9c$!Ywx1}B^^UmbJWp<&f9V{t;L#V(mQ)ca)IyiWhl zK~h%s0le)=!bCD6m3A0l31Y&WDnw?d<)ht?}Qbe22{FxKqJfvMQng= zh;PRj)VG&xxWCs45@<4Q=@6H?KiU4|eCqa5EU5)J4o0me1a&}rCqPZta)K-CQ<$;0 z+ZFg`KK$6&12>V(E2JapQ&_wh?FAF?<|b?tzf5igvlWq$*84ut%YlLW_fN%c`3BlR zq706;hgRzby6D-U?CAVhKU_&Yj7m7a*euB3K6PaeF*ma`h^dWv+-XIxHWFYW*cWX} z<=ENyxvOhCD@?3dczJUq}obNl75vlc}2 zu!s1c=+}5Yn=9?#dD~H~HWvGe3K*?zQPWYqxC_tl+@o;PV5-!bTLVf;XixAayo)nO zlEzILh^4T~k()T?<<(5glf?>2zP4qyo5OIIBxuP)$A}fy?dngb1hOuTftU1gr?6{7 zMf!c#&_^aN-=)?M-EbY}*OuGf% z+E>_r;qNTr6UW!QSReYfkBQy*es_2vbky>|Q90gVWr{E{!S^5`rGqjQ!;jADU5y}; zVVhT}@p=Uoixmnr%S)>rqi&MxK~+MsDirOj{4!o9x}AHnfZbW@_Z7Sr;hi6x&CP~- zCU1&5mw#DGs@UZxPMJ05V1w_*aQ%uC>xfm2AZhv=xiH=!#`XMI{fYi_e@oZvm4#^{ zZJjzNx`OedYcb~L#mnMKx*tZ}h9yO_ zmqJK7b1N!qdyhKa2ivRP68|Wrrr*bI>4fak5hT$JgrlmUS66kL@Hu$*;dXX+)SYe{ zVIGl56=P?~@#5lwKF}gkAW~21s@h8puNt5=c&pJ#>_?ZeSPS!BYjtn_YYVyxIoiNp z4#y1Kt&fe(?J=SW3JMXNN**5)X`E9n`pdW4vNm@1o(W{TKgk0Y>tQ?#8Q6L&ZeO2o2#M@?A_QUH(^f`GM zOSZ|hhM!kP$EF&8VH~_0Bv3qM1 zo{4OItTXD8!Hw_UappqTX@d8c%3)#zyZkONBhS_Aer>kqX7neMGl{SXv2%${!nVOs znDz>g%i-Rd=HMqnu>hLR&k_gZb}XwGlI~xt0Qk~#JZariQM-L{$HJnA8hP7#^5fH} z0e0r^^O+%bnoN&L|CAU7_iy7>xNHDAsGH4X$5D5HDhZY{u{hjjCkr;s% z+Z(ZnMmpVGY}UO@H_fp|%-DpuHW9HO>HkvR{qMk+zy9`L0L;$`#4#pzj>x=E9UWJ)y~tF_Xf7gX=-&S?d6t>($i{ILMhnMn>1`Xk2Sw1 zPf{lZ;VCkG$goY(+R#?p!j`tOeVJE}@ue3zgO86H56yV)%Xh9l^x_8a9W2+oU%-gF zRziJBN*HC>!s@jK*i~#6WX{Sxz~@(XNkhT1Re^?JkAC89>7QlNsS4_`(W~`dw`pWf zD0{sTmIFL2Xl`U~ba8HDLXmcDNV2aj(a?=C_(iwP4uqL z?@lV>$O=2w5|e;*TbD0iKqkd+lhRfwB$BukzN$m58{H0gc9-gAyD7vKCx@r_`aP@1 zB#ddg@T+;>Kw1s7VnhCw*Ut761_?_}@vYAfJxPol<ziNhDkkOq%lwFT^ErbC@(09*j=gx*yk)7|WEP5mm83m-6faasuZ}^7d-g6gucwVuc zf_Y5DUgY<2f-dP5{kTtl=}yMArB(xfn${1dlC(eguS~+~dcXGZzjxBMz4(gT-JOfIK z9Yd21$nIVk^yiKoamF_RW`_g~JLnhv80B^j zVD{QObY1n<+0&V^jivid_c%w&gZRzf&smBDAvttMS6rK1nH7d&1Yq|G1z{rlrkTq1 z0SJ$m2C)qZNcu;wx^A~O$VuLl+ z&!km)^ro3)cZtx0F*#CcX{ou2JuL8@3&Od=Xn3_$PJhgI3m3k_pV zXZJge8e5}ALV}&+Z*!Ql6&H zS7UNM(f*XNsayP!W2QU0ZCNDKn$ikaoS!SiQlrWr;Uq3t(!EQqT_=YdO%;Brj5xm? z>^Gw0rfDk)fn3WM8%>5nc}KU&rt=Y2uvY|iA9`$3W{R64ohK@0|kx~^{Dt*nDtTE}{C7PJrf zatx!3_WAsbv}_;WAlEf-4MYl{1A7B)B&P6S?*et-HHVoh@9g7oqmKBg!|fHp*sHRa zPa9H;a!0=~-U?4Sw%=2|1GKLI>CZOLJ3Vuc`$7S$^~u3@4H|Iu!@VLSkj<(&_P@ zp&PP+0D*M4K;df!i;R7F0@(PKt7zfZu7K@RP_j7prKfl5vk;IUo0;p&S9q+K>wnj5 zP<-zl;swi!g@yO)GH++WDb<+vUN+`b@7ZuoS4r1gp!V-pVg7`eYz9f$OK;+Sl$?BM6aZf<59Nl$*gi!#dM$(JER*@J1BUPH{= z(nq+G&XcgaP;HcHTmvA%tR{rwKxo`C?{f?dR9y-)YBYCpacp#WHrke8Il@?|Ex^6- zusn!+q6?g00q=32Am4u(_D9Dp`Z-APA^iq(YGFq^?!-Rw@Ran&RbIwW$I7kNccW|)ZfqIFn43KD8LjQ(@^UyDXE%W?&o>@FU@(N% z&QBM*A3Kj$Brq~%$@!h`J70j<-8;q6&=nTQ73{O&V7yL@<;FV!LuWqS4RQ)k5{u?0r(Uy7|42(!G;5O)*&?1lK)%XltzqryoW4*o%8SoQm z(3R)=sly?yu;GHNHoQ^FD7N!Lx$(Q@G?W4bF-LVqtZ{}u!>m4@saMs){2?)KZ*DO! z$`gwHp!_;T;1$txh8W)|6oU)F)=J@*PC*RB z@2gkZSy6f8i+WK{C)*!q`7DeOGUpkJTCLvDE0fbT-&j-dX{(FMK55I30XJkJ z92`hnd=N6l(i&I6%o2pJ=9A3S*$Tg;r`xt|ThSh{0I>LoZFx!ct4$j@cuD8I&~rk^ zX37Pf2$8h(4+-ldj-6yPXzRv@KP1y1V0hy&PoQH<^@qfrysCWH*oMmW}srCkPEoe@2^yLv(q&iD#9%+9V;66Vv*OAht zT)f?~PAg(Db47N=+;W2NNrT>C{8&VKMEX?cEtF{w_%dE)PQD|gyD_cDw|a>)r}Ah> zOq{{2=*G_qnfL5lGX0m{b1x;GpRGx7)7?vViV# z?>8$d%-Da3xS`>g^*05t{8A^J&y#fHs`RZlMB_E0bj*$@#T_mW2TacJm&ZHn(GVX`~|P zh4I&!Q)gPt?cb+Llh>~${*LFTo3-v%`Zh7TM4;U8j?Cx6KYbC57G70voPs{qOzOp_G-Z^69OnYz^ z@uRqY+(v1Sq}+;B@rI-nS?PnX%ja2dN>jym+;p?f+1Az+u69)oTjJOpV%VLMIM!0H zNsvA#wXYBCPDC%}Wt>-k4jTorLG|o{*EQQiufe*l%l6+usxhIX*VJJ=1bz% zv#7o5K-%MNlPhc`=2=r|wbQVDVA4Rgy6P{}5L%6ayepF@#}#hbeMt@316=*VUiLb@ zFBobX0b)!Gx#&1sBF%J4GL%K$(vxPN<=zDbRByRRC;%&%`B=UbMvmsF1MZh3d9&xG zD0j?#Y!XV*WcoMKPZDG_+0OVS7u568xPF9jVBJzrA;DnaJzJ8Or(>SqbrveVXBshg zL|a)8VCWZ`Xd@m47of8ycE_DUS?+1S=XXM>@!K9Iq?eHKbTl63EH=b6T$RKSStL?a z(5eQ{+T9J2@$)_;b^`hu1Ub}BLc8cDwWvJ(Rkwib;&$(?`o{F4UsWV`^!RwWuKCVi z&ISAssUbPVbu*Pt3bF~7`BDzv!U4QjR%wq~1W78R(v-FVr$o+m>1%kVrm1#aH0wQ2 z)`Kv%DdO!!x?2 z6GfvwMopL-%OyV(8G@zq$^H$c$jS6<;J zn9?O+%Ek>*nOh;l&b;C}CI6IF>gqERQM@^>WB(e|82ikrrV)TScF-Qdh)E@pJrE6^36R!DGYo7}#>$0oE6;HUTFeVgBW3;NKd_p}Sn~G^ zB%Y^_quIC3w8+!m&?IHObQ_Z-lI4<9pKJtr0;Cqd@eBu;82 z1k50~qn|#mndzi7xcoX0wSigGo-$E=I215&?`!gr>GXC4jnLekxWafA2K?M6-PNaX z<>#Jij^5Z{=^>G0PF)xT2%w5*pb|suNVfu_mk4T^^%Q_NhA-=TOn=N)qBLi}y7U>` zuBE!CjrNY_Lz9Ow*am=KCpG`&Nm6F0wh z&H-j2s?lRT23KlZIy~7=`m<%WI6rUaJx;<6IYcUPcWJ$)tS$=!1%WnRf$HL^_TuNg4d6}c9FM61!YO1j#ML1gA^W=u<; zxG!s1$n}EPY(q$s)ht42%u>P@-b;q6XQy|H-DEU$x&HGmsKQ^o>{oN8V3DvQ$TAx6Vx_>YVxvk`snbtui@FT10VNZD)zz;ket0d zlJzL7R;f=fz*(Hut2sHk{44i;_>f#}&+rsFJn=q(mFXQGv9#)y{UMP* zcOwG9dInFr_8Z99Y4A2G+6CC`>uSYCz5Mk!{oICfghmu5qp2bJ%F+i|wb-JmSoG7a zYPVUUFrcAHtdQe=0h#uwepi+dlF(f6HmiTK=AGhLYVz%|u$S66p`a{7?5B?_OD^BA zjuOacol%C_RE&ho6G^^?g;+e=J*Qt@pZjyj77&B=2=cMj2u)_dm}V&z}-JLM^?~;Pt+ZUQ8a} zw4?dqSfZxXy62e&d3O=`mTlBm=*sL0S$fI?d=C}|j9NTHWrep>IP$zRh{KC=?84pJ zT5GbqByy1!8)QrYzibxp;gh-%L23M3@paB8yLTp^Jv|I_B+mO{g&~U~YwytW=@>{& zP=a@!8O`BWA~LkR5RkRIy}_+2Z_Ut+*hAdwa!7jr_U*F@*|xe%^q-hfuaQ|_+~y#S ztKZy1sJ(ApMXKmw;_zd652SxQc++83XKQOqnr1*^`b)7}`xf@jn0?wjB4UTwSzILZ zdTpXd;19{oSQv0SZhQt%c3IRQY?L1~2FI42T-+cVg{|vDWdYHp``{cH+6dO2jRojX zcmMwX<amBiL~ z`0s6`f3%Z;=S#_-{*dsb!{!x;TJ4Yhn2|64&AS6IL4_fE{*VaG0d~d+*e~WIiclEN z2k40VXA!$F`4<$>h`;On{v&|==ktF7&y5)Z{^Gpk*CT-t(~A{3Tn>GDF+U7%$U4T5VR`SovjZZwtw2as;;|vije7-}UVqnPf|F&AZkhH^WL(2fMi5 zd(n!9C4;!Sb5 zd*1r&vge9n#LovhM<0Zdg}#H1)gjm_+%riVFw9i?^wn}smNlM1Dv*#G82W)Z{Pry_(dv&x zK|HhtYl*^j!x`G`N(Suh%G#gol?$$qMJ-wAN<63vAK-J(NM5~7^)Mph7ro`AbuP<% z)&)S`ys+|^;4;K#oDZO0W4f@P&}r6`-2pt@Q}wWGD~PVkHC$P>%UwnLH#H?%Gv14H z>4O*0m-=A=4h&hgpM316V4e_;Jup*6C(n|Lf{#*T-%Q9MTgS{TwH7!9q=}IpGcVhStN=ep@EBOg_h>Jp=W@en- zMvmD=^YhgO(?Hoi{;{_}CznY9dEq4;}m*sjeVl5|`YY#)xleU`^sEX-On z7UpC@TzPpG2Qr$*3l;4CA#tm3ZpplyjoeDM7Iq)8)W3y0bA6KqcX4G7+7Kc6;MC## z5c?Y)iQ5>@LA4%^Xhx@-wol26W|Y2>sjrVGIEG)6+3r$tn;d+>6w&F*>MC8Ce@d{v zJWQZ(^l90D^o{wbwC^{<@*&n5o!$+iUNY0AYfzEdtF^MJ?C5*@@>R@SSHY{qaW83^ zt51>&CDRi@?C5Ibmkk&tRs=9>$9f@-AETv`+`sH*iu?VVXAl~+z9(rz|XDT%&`dw>Yaz6&_zIR&D2J!Wtl;2FUxd~0Nn=_UM z&FMxmS;vlflC-_gZaKU*R0yOf(!doT4bpLC?5W7&Y}`AgJt_MYE_f%~S*w=yye}7j z(e$ZuY6@BlaW#I*GOgP6UJ~KGJZeu>>KB)W02H+^h}jIY)?o1d&(K459||Ra^s-}` z0Cty7?Bu{&mH-oCDk3F&^J>!5j89RR?5r_HuS|Exc)-=IR^+`z;0@@Ht|TEq#Rsvo z2Z{u4dQRi*CCrz!y;9Z#!rdkz4?OAiW;K9pnFv#4;lK>b1 zWdqV9f#jk-I|u1os0BU*Ab#=p(%;Ls22{V5fx zvhULHgv>&(#!1aPXD6IL!S7S*yw|u$hIZi&;j2cX_F=v>jD ztqt}aj!&aV(~`2#dYLg+i6eqgXJXd34GdE-HK^k8EHv00D2YoNu|`J&pY^uHv|oNl zP`9Y{j}OPpJ!!qcC2Wr-NQ`Kf3ub?`AkMO0FDk!06j^{=VIy0ZUs<33B{GLdzUHZZE zS7HZAF_6xW)hiUQxe{^mnH|EV&UD3g{yXZ*5~+h7>}C$v2kwb)sM2GUv7(AFY)dV^&M|L>e92e$c|+fi09sXg}&{ z?bN|Wtj#i{^5>NHwWHoTE7`PzzDK+4wPPPXY&T3pfFMGq)KmbQTMQEch6!9&fmwB@ zV&XI)0#avs6_kP}yLFXszJ9>OKlawjH}ybvV{-6{z{kB16J$(fv0IGe)ZwQVg()_t zYlF3HKW#qabP+wRAFk_-mP|x|-4+e5?L!(?yY5d_5*KbEf?o3u*VuY}JzO&VHPMaa zH2n?*vE*0X=WS7BeJba+p7?Ir4$HJXbuzO2;evp!&#RmbN&7)Abv1bsnrLCv2Sf4p01a8$USp)WwOFRhYolxA{$?zk z&WG;CSCF0q&yB(A9AJP-8SR{kfg_ji3||l4A`UB@Yh#Zw9{{4H`yp1_b=|0;v-S_?Km}FY?`DNH$UXMYeL}jw)Ke0 zw|e}?!+b4EJyCyoaW?!e8p1E*}ah;I8GvtKfMLlf3?~*F+@xRzBbcj#ahd$Ue zL-{vW2QcaRJESYwFv+FHC{1|e-LB3(EjIuSRd=^iY_-3%9!x>f@^8C)-`HyZco{SS z5?#{H472;N5e^oEd4bNQrU3gXwP_$ftbQYYK);w6h2D4uu&M9X?-IA5Fd$Q#|DP!W zzvLDiUTb!yvE1k?=z1D8KVuq$O&<8R$^_Le!?O0jq|Ixi=t%Tk&3(kSqbtxnWTw($ zoS2&044@Mx^dln<)BxB`lq~(W8p*?p{cxeW-7a@7F@In}=HHtK9bz%E>pUb3B!|y$ zy8}?$Q#q{;Yvayq*mp}6PJD`QRk}F2GM2ZTU3s~j*^6&{5!bbPBw8*PvKn7sDXodt z`cnK0GihNdnmwpNb!C%f{F0oy5$RTKc(F`S_~h>*yqRm9j`2{L{MW_67LE4 zPQJNM9`Z=*8*3>u`gJVao3YA$KD!r2r=rKP{BY`(3tI;5k5m?M5Rvn_!G%a3kcGp6 zE3$#1S*OZ3N^BtjBBU56S)Cz4>PV zXn>uS#w+~HKQT}NNNZsLT%Kgy)-i0Oy@Wxjy8>l^hXsF6Am(fqitS%eTa~cUSUO}= z3(>?iX5ATv7H!7IEE;^RYfK&=zp@d-9DV)zy|T)>JNHPq$vq_!^OvQ1?R_VDR#A(! zut^m(h=7F=c!h)flSg>qaDQ9Y6I->ZX%+ePF-GFu*3gDL36J`TrTeKsL9<(7Gqt)!cBU{o z`d#~sU+^tSx;1mg?08N<(}iD>xAKyeb)3DBl@-8Rt#`2@p+NHaw}rqN zV1Hi+9JQGC&vokWmy@@Gg52tKtCmM#RiMVDhkQU^k8qGE`J?@AonsAW+R()CfIxP# ztrSQvZSyRtAQ-;T*P;;ilC9`SmRJt;c+zP9JdIAh4+HJSr+41k*c_N)3# zcH&NC{0%JbyW1ZUUd&KkU1kVrD=m^t)2B zwDN}B%D@Pl@wo29T^BOUoz4876t;b!ljZlDC5Zl^`60CAYkNy;_HspMzhi)ZkmF^) zTt$|q>>r73o;f!zt38|n1IT`Y8Xp!CyjBV8nw#9afJC1FN{lXnU#(mxnA~XsQbPD{ zIpP=J+Z}yN6OPrUv1<-wz6e1pZqlWo3eNPs8Fm9}U9+RR7l%H7<9G4gdiy5g`=E%& zTH3R??$1j<9_t+W5o(CsJ?kUZ-sY43In?E2-rAG=(>93P-qiaspt?sU?A+wkTpQ*e zI_>U$=ki^dX~&9;JcK1V8xCWZ6ChI3`R2$~g}-jv>syruR&h8>2%UeeEiD z%P=q43w#({?V@n{dUo;(x;%;Mk*Lv0 z8w3(sui--~vJP|2mep!Kd_H9G-N5~<0$d}WeteZDZ14{Wn~GK|IgvZXAXHIjx?x}3 z^Nz7?R;Fjg4~Vv}6fW4I-npeU@20{HGhR-TZyqObnm;6t^xT$vD;%>>OR5Eg#s1T= za!Yne=px~^yw?c$x{C=PZ;h(pv$3On`(V@)7&?+Ar%vRXVoD3O;guF86&%DLX&E;@Y;IqIR zmq5b~c==M6tK=CG{`3eTy|CI(jIW;Q?izI)PtiA zH^ChXap?t3R=AXdoiz{(UYTP%`ipCMiHoS9fNpYd<)OE+_B%)Iyd`=Cav_Q~gPad6 z$5+?#TcE`!pbY7^c~M7SN?P~}8g->xgf3V`Mh|gKXN`YIo}yo1jygNO5c+8U4DGow_7q9^+Nrk>`vuwwY)dCJ?iUa=cA#$MHoxGO5qzx|KJFW6^Hk(VvWJ| z2H$f1{X)i^M_>2)o-5q2P*3^F^@em4Rh?siYynijvx7Ile-A0|Qod7Lzp$u@@bswK zjuHBm+M>zbdWph$LL$o$qUYDIJ+p2Z(!Jx`9Buqoum$&v`5lEvI9u|o`L4fjYX8%2 z^O*9N5Ik0XUq_&!zOy<0M#hutf{w<KOQc#EX=F4{aamzq{bIH`tz;*}@Qp-?ga$veT$Gnp8q-Xgp=TGMD6Aq#J=q}Axjeh8Yd{&#nF+JJ&xpX4OEl!@&&R*@0L-V|{ho zU+~6% zNT4uG8Oin^5-(@h&O5_L_TU9fR5&Fn#B?Bsr313T+7IZbxTdgKgC zFyX}gE9!&3YVL{X?97dcW_bhRb=oIB)Y|9ysE_m+ouW{`*)Sgb6Vya_8rxV&o!#d> zMaAcpOA|~lUo4i33DrNpAyFsmosmlV__kzUI_@!4oFk(oO`du};ZW|qZezw1zL5JJ z$J~olWs*Nly#nd(l9${6qF#+=;mB;GkE=#jZo?bvo4+PIOXxJVIk*-M+^e#G_!HEs z9EffM{m5F&%PoOD(^$s})hGs>O#$LM{=VdO2(35YrnDuhYOmPGF$npLusx}#Vu=GB z@4s$!%%7H?=b4v<&9BhTgt!bqQ#%)*Xl~ckyHh8PxykN}LeOfZPnNy1`*)@5k``dia}Y zu(4U#_zL4RlnYK@&~RMu@-xz?p{2d0#cZsu=*=tFqCh~S=3Z>YHP5wxDzGWx4Uv0! zpmL(vwQ(-X;=oiiKxMzs)n{Hy+uqlmYf}3ry?AR>WMc29doT2yR}BV;Tnq*q#D+g4 z0%L-Ns!oosGEV9*4dTIoeU(F9UiY^6Y`b|uET8G6n=S9dp8dE@&LC@U0&A@9ZkBfh z^0x#)msC#jUZ@35uc@h}DZn+ef7fGd@Mz@O<%}7xy_ON%OWF`XmL9T>$yw*t?&(yF zQ_)asJS#P9P7C=AfJ%oc@@-DRLN1&mnhu%ffzjt9bmCLMh*0(GMjk#Mv8fZZV_2QO zVvc8sKP4W@cl-#5UX9-s z#)xMx#p1fRPvw>k`BZD#BlUD`gmKkd56H^CZUvI)Y`5-K9SZ2L*q=T*OL4FVh zPgG<4UePYT-fUYpeez47u`zUQgb2WG4i& zBPMB!IO<*ihFZ5Pm=Jarj#ivyX)OvktTkb zolQxJH)0Rs(hSJ#rU1?zAas>%*9z&33dJYw^$D6F*yH$N#@Nj~f>(P33CZVq)?2qe zl2F{eS8Xf)1qVu$O*!Dq$Kkwz>sMCnjw=#B6>@Kj&JZ|;Y%k-r8^s{?R6`e6bg1>ONL zyH#zx3r=$Ye**iP6o=m`>h&J%cuM-OS7=~P1@r~L^mG7~5P&iFol+K)ka7;Z^EbJj zf4M2F74du*`S_Cr;12nVq4%$!K{`q(*d)FLLL;odeRGrijB?$py+oTAp`dP0*s2C@ zjaT5Oz|~0wB!htQ@M;7p)nl2#I)VsF=AxYLn$>4a1<5|g&Uc@P}vI@a@2~m_? zy-;nk_%I~+V?^hmt${d!Lv^cg316q6BaeXkV%;|gu#L{RP0*!ZW1)UPN@v@ex2I<^ zJvEuTa3)xayt$;sR?o0_i95$()7CH_ve8=HsVTZt)us8H&p7$(_}}^`*{9ix*v`wwIb~hlZbLGUx`Jx^lS-k6lTuGj z6qUnryhB5LJ6)zWFx#hwY_AtQ*W*{jLmV{(o#e^JpDk*zIPIzEZQNN+~h`@{0q2iX~z|eb;X)r=Bn}h!JG&b0R z-rZIx$0)MGLZ((%x_%H@(A=okTrsflI;-p+hg@U#yGITqx(XJ7^8ne#!CbT0d%tN7 zz6L55-88tO6RC5mr(if79=FL`7dm;?n%C3ev9M-& zsIf4YKHROK>K+~;)OJl#kegsr4LT)GJ|tsmW12cA^nt0nE4Np72~}$Q0PciKbWi*j zY#{;AZUdTJK<@PWfHZg!$*n;8%;oZl`37SDhMJSL=dEgqSA>WA}v6uO79?`ARyAaln{F89g!}bgeD~rdLSX* zwa?k-%-lWqp4oHnADIvsW-@Em`o8yj-sk;2fzT5NJob`1uMa)@dbD!&{@y6 zr_sA(utgNTmX+&M1~vk10(!hXqi(53bE0kf`a^#EI5}iczesIdY0GU#{wsg1|CtZl zfQhGf;-bMF17#K5$eSs%fdCpFfHDEtX{jrZ=& z=3cAL?zFyBxBs}J#*;Rzwn;sG(ezNFcEQlhlXceY%3O~if=*95^0FfIpypL@xMwL5 zl+nSZU_(ETuYPF%S~4V__$()3^Vy+{B%cM}{nkTdFyl|kCw1RnrzR`s-y+)Ltz?_i z-fd4oZyPwDKHI)4!ZT^v>u|<;=E}hc36wPra zH>gcgU)QeEj#Wz-D6_fr4BEsFq6TUO3xG%a)v&1s0IJ7dyC`S|?JMi!{m=`6XqAaw z&q#@HqkB8GI0=bwZ1aI{KwnmZqwZEUBt2YDrm&5B$l{iMHiE7z+)wu$N1<~B_hSM$ z#tU0;$7{pY*6nWI-rv+YYndy=MrPI6K4dh7Ch^dl0IB1@3y%UY1T=T2-|3VfX=t=? z@=krDU#QlyGp5c+=I76EuGjOL==E8Z5V7tAbvOg!2LgBy(R{V(?0AT)c)(8!uPo8u zSui$%So5!n_nUK_9;l1Gdsr}aN@`K&-ehV5&I9AShstsY&5IK3-uk{=zEI=~1|2`M z)hOMXcc=85rZx`RWx1*@O>6fRk@w#?RgChXIjc^Dc8<#z%(feU1A$f~I{Dghr7jYW zU-mXU1jtVWkm;_|0?SCp)Tyr{>T>lHSJsu;HtU`Wc)i_75f~9U=iMf^*tj}!s zX678qV@=?}jTU(IO2-l+ty`Jl&mt8*qoDI5IR}N`cBnnd&@GxYW6|vCp-ek+RVB&acsjtd{WOedQlGft_zXIp3+B@(_=J6Io|7Z zJm{>cyBCo=CxstaLT3jbEeO4w*bD90M&6%w?zb=K_7v!zbD>DU<5$&O2&=Ya-jphlWDMs`QMasu9>+-VVV)101O?dvCM}mL)YXl@x<39QZ zlSC^U+(r2oT)h}<%|0nnTiT-W@YoE}+~rfobUvUFJ#;wZ$+<7quIAQZ8_6G2`Xxx& z5Fg&Hd{t52zs%`8E9ZXmFISm4P!WTg1Sz1bTij4gB~teX&*wprTdH&)w1px`v_cVk za6anciSx`)dVF)8IjpR!vAjuIwxXBJg<=I`^CE9}QYR^ybT&>3zC^g#OrfU22Ye-i z10hS);6~+`Ks0wc08r}W;u(c2zfE6_rD$9lcA3&I2G!ot8^anqE+{8bNze^y^8{>k z2HsQ>g=y;G96wgb#9FTy;R7rFk~#e8_ZOZPlav8RV(sNkO>1`2uuN0!E7}XG_K6%6 zpXK)+I~w2l>l}mJ=F`=iWSdq3nX9FK5 zdyC*w%voF?Uib}iMj>XuWGiyr zT#&awtbL&vZ4)`0rG_^Q-p{^XvNl+%X70!|)s0r)k82Q$5D4?ru5g8p^;hJD^*neJ z!n4ix`4+!uOYY4v`tEo>a_Ds6IEgzjJ6Xa6k$MqGi&qZ+vi(RBYriZ>M|`{w?dH6R z0Sz;E&@Je>i#Ml^D~`_cHPyOlN^5;N_|22~+F|SJc={x68vA&e+OOSo@xkP=%&%8t zGgIf{I({BWOLnm=0^d`$zF19^K{#S6+KLp~U#!p4?&RYL&vBAba8~;gYkgZxIb*Y8 zjbviRV@Y{&j(cxo_|Eh^h3R!TGlJHtM%eR718gQ~`^@UidaA>twS#`!gLP7>SFA=n ziaD;=C8#B+LY)x>F$K&&-~XPsic0Ht!a_5FRlC*dv&O9a5{{I$F_y)Bk=x3E&+YUI zkQ{BcD$p);VSDS_Oy4r0ox%D<^gfl!%d;SU+>Q42mPfr^&=RxGVrL}S4e~x(%&^ON zOwBFD{&j*1N5})-R>o1~*FZ(~h$m~yUtN}cSYwPbu+Bx!gqfMk9#PBm9cI~+&)$y= zf8YCYKBw(!D+*VT4Ms2B+&rh>dcRfr8p0uXe&Ctz(`5_I5X)r!9b(>8=dyr69s@>^ zyN9ASfp|(1?Y!60Y6^8Pa1~zu z`Xh95KyGTbz3#`3+b;r_^KH1>ej~;&eok|_Ik|3~KpUYExa`GvE+1@@vBU~^o(@|n zfzwr5MBtIa%?sYV%ded}#zvpq2+gI`RVNcV;4PX)Z;-%9hJyhcZ&6+W1`lQ#T3EoQiJ(GtX~ z>F9D0K{2`3Std6{vb}t3SRwCq!~^lKTq)8B#P6cTlemqZbncz_eN7c@MfrbV>$Cmy z7wb}-Wez~K^0&;K(mx^V`fs~kqD#z4+rggF$%zJa3Y+9!q_he;r!&VMm2zt8)k7m{ zGWB1c?Y?atE`G))f50|>Mf`gY?_V;_Pj5`kpXHrl<2JM}%2y_9MHjrNX`QNn;IONS z842C?(-NyS=}aOYU2BsB=QX?&a3NL;r>M4iAp=F!aEQmuTZfj1_mvI{g9_F&Q zhhnLy8Jn9@%4cuTYhRXdxK03;+x|d{Qj#2z>N{q4`Z)JO0!*~WtFHj?1FCT)?wM}o zunM9=(Tu`YV{A)v>TGDxa9!?ggITNHn1va|epXN8$V_(Ix5AdD9jF;mjAOefUeOq> zXQMT7EUt8FhO9xHf(j`txZ*c<+p^GXz|+$G$Ze+eAJj9TB{>x|5*t{nCb8NACFrxFI@N{@LrnN z{0UU}?++>@WcF^&^?N@qov9v3E>9{Sj_s};S9jreLT3upi<6g7ma*qJ+^K%N*G^&q z=6nd<-r2xxN}f_f@wSOYK;nr%#rFd-tjFjb(^TD=@^p}WQZHnw}$BSl( z-bhy&us)?m$GA@Yge#99ChZ41NPAbnQJ<&lm39u(F#r3ey+roGS$7))_+5M@Mv@s8 zZiLf}J^C!uV*0%L$Hc^pC1Rm=Dm^GcV_@&LL`Ai#NcHVMjKOCSrsWGnlntPYMY35e zP3uqf4l6S%+v+pIHY_I>7(b| zAGmy;(`jRCu8${A{nRTd6%IDnG|urH;qV2)Us3K6ozhtUCid{@Kqs4U5p~}Q*=y9?e>NL8vkyY zZ+Il}Mgf=_!}@jxC{2(a!_vCO-?Zkxjzr|`JKr{Z@+DyOe`QT%FxB5Di*-pwm^h+ zvn0CS5j6NzLY#Vh^R&?uf#~l1`68D=?b(<{VnNS9k2mtOv4o!0(z?zJ2#V)k_1z00 zUV@-S_2d+qwuj&7@pT?LrG^l+644qB$?yt;dxC;5qwX+aff+)n`~1_)X#EhAMb=i_ zC~|^}(u&foY3hsE5o>v-V@s;GK-k3D9%oDG-pzfM{P$OGp)=keJW}qFWUz8!v*hB% z#fKfq)@M=k9*EA##W_jt)@~k?*T3q9*V8{39}4bGmsheg{4DyNXunTsGZP4fsXhrC zn>y49+mFk_>HC8#wvwHvOyx$kbV0&qAF&*Z;Cqm774r0(vKOvO@krwR^Gmp`sjb0w zHv5)UeX8ioyV_S--G5%+@h?SGP@+F-Ukmam;x#soBP%Jn_wDi%GOsHL@7gR3kSLKr zar?y+3Loy?>@&VrKES-g3Z542BPjxV4+r2yg6R^AuB^q!qiEp96~tGgMEm6y>SNs^ zOi~91T!-*iVmD10tUt801QLi@KIhe$m?M4MsdFrp<7Jg+dNItAN^DzPoKqx@>xc`{ zj#Mu0F)00G!IvpXqME3bRlnFhfzB=-H9!(fcpYJ`7w{FXgFi#>g)K1<3a!h zQ89-%NiNc@)j0apt?7o)T@bv3=b6C=KD6%{O(6jcgQ|@io^~nGPt-QpHcd?(Ebf8@ zk`{ZPwnY5|m-Q+p_VG)1%NXQ-c?JnHPGH%xrCg9e0TcYMg}f{Er_HqPY|%k-r5oo$H(X&@g4Y!02iitro5@ zhA2*vTIg6fBXc_Ik&65w9$S|JalQyk9F}cPZgb695~F!j2SeUVP%zzko=Z@fby8x( z2baN;pEe+q(Z|tS@+PChREr+G*Xq?Zsra;Y1TEzGUfJd^lReot~_!{pVOSp`R!tu zj{u5Cb#J`*E>TtxRS;;q2`+mso53rF0b?aZVHJ?1^~KTj*?Z}Ens#181HWoyK6q4{ zF3tjVF5v;#f-$UFQP0*XJuZW${I<}x%~nIb^E@EgCg`SoP{Sl=jiOMr@~zX zrSVnaQM;!G#Uh=2VPx?+WO2t9(F82rQTc64wR4Z@ zf=R@~L7%V}ZUVWF=J|cp!!U0o> zysvtH+Df?C{YzGS*1MmPJtx21s=P7SrF_mA8jv`>fx=!d$*x;!H~7H)hwIMfKlIK& z>i7TSQToT@^nZDA2Lp~QK)gD8@I`rt9SqPxO*@WHHHo*CNzVoK30DCPc4rdT0%&9Z z2JX=X$_amrGwsThqGsq#hg;;wwhYNfL*B_h9S7C@nHg8Ga3)}M#C-)gUtl0g)U(mHlTDukp%2EsbxB99qYxL-)ZpY3< zT}sZN4;Tb`=>hE$89>0PGvj2*jFo?yVKWk3uR3^#nVMt3(a||y;(*C}(fj@|+!(zC zv_9D#0r8E*0|^+hYzXhPu=*;`ly8@Q-up6r>16`T$3)2t*Q#M@u`p%fPEiPX3>d{p zG)nFWcm$jxqRpqVYpSSr2;cmSY|kfacZyEMTYdH|QoHKN&%&uJ*pE5Se(HHt&z+Of z5*4Riw~cl$yqB+q!u|6#(cIBDpcrnN&GWGA;!R(>)JP~}%FpC?X>zV%yp%rlnygP? zXIE8N85xl~`+`I0uW^g4 z45D0US&1e<3M7;oKz(4g;sgEMuf}?V!Eaq9c#}c#I|%?+l=sQ^v0W|uF90UAo2_ri z>oRzqHq81FzM-@tD?r($RcIXkuyEc6=52tAqRhC~IMMu=!>eO0UBKM-gTdzoeU7SN z&i1%l9Hk;!d#E%b<@rShvB}vNxS6JH4g(`kk2iLPadM~HR+o>E7fr{(Ptpp3lGr)u%C^GRAvvgEvB zZSz{J{MkRPjJkn7HDmtKBHzUB@t-^KI$+qwi${av6OG1mbD9!abtE-CneVJwYoMX1 zHKxnKcCsgFJ4+QV$i+L;<+a25hLdn1&aBjvG>aNNVAF9{gr4-F)0c19McfUJ(-qO# zE$O77n<*gKR30Cy36>*|FO=J}_L)1N;Xho$&7*oDgD$<64)pzD6I9#|hgu7@r0unv z8f^h@UCE|%gKobvU4?d|n2FxF^zB(lY83|b5wWjSC7$1=Vb1m7EkvLW8PtBcYIosK58kZr5WrA!we2hsc~! z%rVt-&m;!lcT7@Uzf?rFTw6`g&a=ydbF)NZWpnXOCZM9)jK^>OlIfDh8bI|jf?r&R z2RB9P^ggrNT}M&`y!{wEpn!f^efI1G7{cq$rr+eEzz63?TPdjRizAa^kczQIz(!*A z2q$eg z_R^}S9FMlhN@J#yzmJs-wq>49@Q2ThO>Yed?kFl^uJh_Ih~ z3kCWE^=HxD*^~j&&H;3-t&?KM(S65Ow%yMXoUa~3r@Z}g@)So-!UYBReV%=P@MeMT z9iz9TeFdHYV-q0*P--8&<+`I?*(`l4w4A?XTo>#`^1BzDuI_A?^o&8al%MuS9B(0O zj;voGD6oxaaOS_p^_OfNHG=bxcR}I#a$E{jOYuVb&CazaV{718sfX#h@=KpTw|>`^ z;CJiizxL_@zey;n^R7px=JS0K{PW358D%+L-7hl7sZQ|Dmb#`bu_Zq-spmKNUnN_+ z?f15k9z|nZ5two*S~{yg=V2-6qGOh~uCDp94>_U_a6*HYt8))lGa9~@^**KnsviJK4yQ&;^RI-OCEfq5yhBx z+Jti5!KdZH!h0SK{&w-^b}%bbn&A*G3bcO?<>3N_?7aH?kfp2He~c#8LvHDEo#bi#Z4gNd>UrU1 zQ%b5kpL>pX#8nS2LnE3MO7JBR&GuHl>LyUecE+H6EKe(qP>SpKIK`STEVb2&u}#-J z-`yjU;{)@Fnq!Jke+)~(cx@@Qrb&I)`dPrFzxa_`o2$Te-eB=^^b=bTOX#bmI}}+< z-F}ixa1FepGcH5FdLpCCB(2dQM=`B#{eCoiQH8E5ybd+fv-UJBhVec#JF9QvdNe@* zAGV_QE3T%%3^e~6|KJZ2lF#xH>kjpI_H<+ce-xH+i(r43c50V;>2d>ES~oIBU%W^xa(041%%6D@3fTTKC=V@Ex&G3(Ys? z?fpWsI7gy1zzy8HmVF6d&;j@Yn9kSpoqagF?w5es{!F~DsZ3?>L&O~pguXsAYignr zxw(M%BnZOJJeMe8A9{XPsTXc1FSy+E|1Ok%(q{1#rwMx<_P`N_ct8(p&ilx(Q}P}@ zd=B;opd(GBti^z`}s8IMka#Z@eLc#@Q|1vw4dzFjH6FuNt}Ui|D<=9SpwlGoh?L%fkLr~=WuuXUZ}JOQ&RPPEdIFuG zXOBlhP{(Kt_XSM{N@e?SR3{6BcV|26;ke|Z1*CBA=$3JUH9 z`uo5otp&w1z)DUy74&PZ#x3U`#L18^+s9qw54!gL3CEP=Ka0NifgDQQ_zxJ`urgj; zUR{6jpV{)TN(d(oT0L}PbVIRN=kfr@Wu_-(w*Hk>by%L;(mun9?bo!!NbbC}5Q9kz%$wayfu{UrMryZ)jsib>j%2gI8BYMokK5T7JSytY# zv0H0FQkL2>+Bc6=@}7K|mtOUyZL{Hu=cJ7w-KihPR_u9}URa#`99>Uhnevx={D@qe z)u4~?MqA*0;%d@m;j^(hwT#^p%idR7^6pIn{F%Y>PCkHSuO>`KDbsNGE-@vZcaUMx0J8h-a%BLiDf9u zBH;L)v!;Jw#S#oag8>uO@#~G*mM+}2wC&WA22(OEy4~qkEhFKboEq{C#$c-gS?E2x zSEYKl_==)12;2|2^}6b$0-fQ;@*0>89TlaF{IjX5-#wfKFuODtwMx;5mks3b__{Y{wfI%|HNnC>xR=M*(zoyT z=EDlxp_gGz9oiUizg`<#J)^GKZxas4@r5;HkU)9M_mX~IwFCM(^h^vmWbVnG_x?>R z+uIgWMqWzV_)?6`WxTx6WV8NCi-x$!9M;(-%Q&Tbe(W=r-ur#(j%CCO)lZW3Z;am1 z6Y|NwWSr)BKA=%4K@!4mbjhWA1;4&IukVY~8ZC2uVkkG%jbys?G(?++1~CkG!h4qD z2d(Pse^qXbdMfDl4>3#TTe{rS1_u`A=l#C5!#+Xm^Tzdu8B)W?i%H$SUWMObH7fBv zzSxg!XsOM9@OK^C@l5emL@c>`yHx*WSAxlqsDGVy0o9f7YM2GYfjFr2@%9URG|u+f zge*1JN>_+(`6H9yoN(QiFS`PMyuF zrc}3BJ*SmES|PL!ruJfJPJ&H^vQb?O$}5_t?kV<-&RW%*Aiw3j@|d|dm5aaLZ||xL zTWSuv(-TdYGKM%frters47~|Fj;Uh#$gV+oRrbp_Ac%=EEYo&*ShM@lZ`^tsf11hu zjMI3hk9hZlLGSxCdR;19KSVe-K6y2c|L%~nj|3ni0r}f+OubQn_kzloD|#P!@@`+^ zLmmvNw%?NSGrrRK7TD`i6mTSr1F@HSHH9`GkPMsa{)}rf+)K%24@n$`&cc?^8hIXl zZzd(a$Yw0<)JHp6M3pSEvhhJ5z9Of;YSbY3`zf)Df@eCopTH5l6n>ITiKt|8@jka| zHj#(ANrvw-?H@xP&6I!pT&vCTbcqX8dzq&@Azm7MODPE_AM`vD)1uJBb#KDeQ}i?R zSkHQ$MMn4cYsk|J+c05X>gMn5Z>!bAB!1e7rx0p)_tq{f72}HnxEJ*Tms_OPJ!38B zzVQmou*}{+sCYiwdqs%x_cQ};eQ|xdoy|_(O;PGPvoPSN`}zE4mdf$9L@R{u{N?|> zblZ#gOSau(aXfIK9It$!2?q>`)>6N&G4C_4h;h>p9RcSmmCIDxm?}nhu<2!M^Z-FT z7X%vifMb91pXCOhM63j8KB#2C^#<{^bomIS_e zA?bZWvmU0}-u%GZMVT1mIMXTsvjw7^_9eo(VKd08N)6_ASRDm_bIQ<>x+p%krd{NgEpTJ_? zOC?!rh^N1RTDzY6`lJXTV!bb#;0(0EaP5`TaaqUc&gq01@mgNi3W>EWbH3|QNsMtX z=@gSyrTG)ouJql$SE+-~Am{_?1d0F85_Pi*V^`AFXdcQb&gr&V(zDm|cf1*NsuJuz zcg3!$QMTSTjkOBs&aUx}hI7Lgc^Vq0jtJPtW#z}$CN5_v&@R%~9nSga#%Q${CiIII zUDjr~+nY-?z}w>ryGYXGisl%I8$_bE>4A{$wN@lDWNS&k9#n5(lE~#o|GdNs<4J=O z%a}-!DN997WpB*w3l(Hf6@rQbCY7&x#Iw!8F8?L#HYqK#avtAmPdQcLEN^V*lSvAC zL8r;O8yG&5vAF~rv@n;|ofFqyvywXyjN7GtY9{!aekg8S6?_6V4(=eMXQYvn zzH>Zs11^R)!fo`jc}JGP)qdCrr2Nd}EE#-lOFcu|YMM%^1?f8IYFl~7OK(<^<0z0{ z9S~SaB%3Zw+XNC+&f^IhK!*BudOVgLkfe5>qn;Fsh>VuuCHo_Dk(^fZlH>&D-TlZP;-u5X@H zoLz?`77g~bn|(AwnExmQ{S$wfzyFskh!xACY)sBZ%0^Dymjl>gUh8uPh%k)bpx0ai_`{ zzr4x#q-~#jr<9-w>uvW{4r z`SoI78sA<$fO*J)!io|M3U^itZLCmXdy3i~jhySHg=U)vJ;+C~-Cuuny+xbLeM=!8 z**mVtx1(a~etU|;WePk>RDy<+(be4h5f(_zZvA>@0SzAiW%k^buopKc_R+uDdAljS!^ z1+y<7jgi_#tkNZzX`8Qdr_GJu)|+h4qYIQGsl0qsCJYA)wbnI}=`SR|h@^?lcu7#o zvADjC)~LLIK&LQ%PO#NLB%+|W7#26(vB>qoFkHiImcB7TJ1|iEtJnH2-zBhdJ;D55 z_1RqWmrGN8Ryo_(#@FDe!FOw`8}y;9prFTi=;8A_S}>& z>&BMC#R$AD=M)*qJ!__q&nqRQm(pO7AKy>?^c2 zN_KG?8RK?QxxJNU`8%cp0<|e!Mj0<7KRwdpTE`AR0X?p>OWfXyO6%q=#mjr~T3l`wUz)W`F{t|AHge=G!1Wt((( z=4U9QV)eW~9ZSzMhO+#kx4|r{R{JGZ>d}cZ!>)Lw0h7bix$r_*ASNudV(Vm3BJ`W{ z?DO)x+H;M3|AJbkQ*W<*R!Q>M`V43dqyMjgD?c>ukPN{;TJZPLegv$MVKeR{y?$wx z&;0DXy=7lAb2PJZgS*&^J`2tmE4rE#a`C{rdWuh6%tSDr<2*H}g@*Rj*| z8CX$7WJWjS-W;yAOD|{&)IGmYa<;PgOE!g?J~}!;;1QJ|^CejPOB{DN_GpjR`P?g~ zSY5oS+uaS~=jZu$Y%V=v!vqDCWXr_v^x6^xut_p zh_jz#wWz3y&|H{Dp8?4|DKtxvKB!>~vq6(p~1GdT>W&Rl@g5 zALAd-0T_EH3Ks!{AL4!f6Iwov zXj{Dq(>ZSJ-M{)F51*^vh&ySzJU_G8^=p`6gC_2tN)N44PY?hnTmi%?Hek!*-=Y1kBConZSWe3MinqC zw1bpfc9ErIW)op9@3Cl4AUYgw1HiZ)0r?y+l>v<67q40X4&L^^WODMQsN->BVhd(i zR2^T8OSZnqhMRt0aA9sNi1cxMT!1Ku6w7!1{;E`F9~3KlV6II^-=Z=9!CJn6NQ+-U z+eD_3M5@gtcPg=E@YT58iNZwZ0!^p7Dphv|p=?e}M$tyXnZWxpsV>vpz)l)GiUpw8 z)nEW504uQAcN+q~g8{qqu=#!PYo!56Gcd0Xbhh6>s&Z6Y_MR*)M4R)~IwYRvyo{luy-MoJW`YCt#r2JF zRE^9RnFVRkWTmx&GoM_NreCFc$qlH|ShZC@e@XQ-V7xt%wSt{~_vz&E2=U%VH0nad-6G!Ix_2b<7(F6@JQeTSUX6@8i_XFLO4EgJiMH($9(@$ z?Vx|bMw9)^=Sz6NM@B&l=u?r&r1m-#K^87);}O`d3YgU}c0ec|q>?mjLkR@RP5n>q zd%9EC0Kk1e40LAn>G9DWOSIomqY@P|bLO53l>SBrweiDQBl_u{=Ibhm6Rc5KLNVPP zXBs&>-~Y0cS?yQGD>C3#R0A1^Dw8`$kEM7SclS1Qoo;G(UrH5V}7BiIPOqeN!|vWD&E}lf^eKlU57z z%7jX+gyw>JJc53Bk>5yv$fS625X1aHX3rjAuGKGmg`5Ri(IzRsU80Bkj2?5+wsVtv zDqsbH2k0LaYq$)7;HTovl~Vea`JMGZflw@+IJ(jBkd7{S@R{_Z56fB17dg$2`+V(= zn2~tI-Yq<{&c0xzdnL?ed`F+UuR)5%76G%qnxwsAmYbv?XL!gSKA`lA;i##@SE!?s z?DrEzS_lB!NuMwJbrEYDrqEzTDQxwb{(h`BOTUQpFJr6%2HF{G(IBE3U{xtGfpM9$_ zE0}t)pPcvV;cIQ)3|+!g%?!9jhSO@p-MRI{6>mseckroW#}eCoJQ;q;X7V_L@Ry9z zSw2lud&*i`U?LrEGTJYen8@YIBs|nlmBZ3kqy(I87iWMIZWQnX+~(U7EV~1^f0F5izs*)}bB5I%@#sQHYX$I9Q+pfl3w1VlW@=iH zW}`49^vSm3g`ZeEC)7HN7?q>DZkn4Rsag$G{i*YtsY33My z!ZQmgaC|<1bV1s7qMVouRyT!DI5KP&BS_;&A>Mlgs%*in146muNg2KdqsQIWDmJ=4 zUS1nW57L-UR4@9iQfwBhwD+?iOV@|*ZX}GHk_KC?=mua5O44PypTqTlPzX3+@8TgZaGzw}gBEGyBV|kaTS!|5{E0x9sK;W$VK&=0hJ4w2i$2~Gu&O3p_T4M7O2=)kw+7aNj8tDb$GVz?;*4MB|XU3 zg$KGY^lNtZMuT_d0Wb?YTzS%5BOBA5qFl4|GF829QnAzzK%+hGyeNH3E<<0@ucP#5 z#%oP0XU-3}m&Z6N#pB~3kbwK#6brqjy_j*Y+3<)y0zD8}eUUdda*<%?Of!CU9((^y zAGhRC$4;dqp7vUgz&>~8zPe{TR%&XnGGkBC8Cz0OKzi|mp0qtckoS9CGk9=HT#2`d z?^(R;Bje<%XnJBUi}~I!Q7#K|e9ae;D^a}w#OEb^jLg4YU3D`wvRgeDK)2ggSUDTP zyt_{t2d%ce<>a1@O&g$0n#b+v=HaF8BEjSzuO;?$J?Ou!^1gO&Y$>$$FWG@rL`pia zh?FiJ5l3oQdfMZ4{NIx(Co4TiZvNhCj2lsM`k=iQQUuCr6NupO=s%x+p&j#IBUJyd z??hnp_*ds~Wux+P4)A^4qym4-O#{zSW$2$P{}Y_-zcI4e_`_w0Ee6`2%?MY-9VM#z zmv5Sy$n8B8rSDg?zujQabyw)C@B^&XwSP7%+?P07`CM$Sw=!=lGss{i39mgA%q1NW z%$H|oufJ^C@iH;QbBomd(l@!C4|b7(2k%2q^!As!A*vY7P|s9kMkkk2)0lvV|5J4p zitoRB-<7|uJ2U?xo_DZjKJv1Uh2I?*6m{HBAv()? zt&4V8fEbP3_C0!3$JACs;W@GKsfe0VeC1O|n7+LDyX@`GP|-j)vyd0fBtA_YxDWl|;8bUZu31v#UowVaA=7ZH#z+bK zB!5GexXf=qZ&iyWwihV$!dO$X)b&0}=mmts95L&K^YXRb}ye%Xi`h2oE-&fUD$09QmF2 zNbc^Wi(`#?kUO;0(axkC)p%bYD1@QBo1%2?AE6T8eht>&^fWHp)- zkIs}mRGqg@eP)C{KX3C2QwT4?TOE%8#}u%YQ|oQqMR`Hugq z$&%j-zg4(|BD+PK{D+&P_EHg^L2Ez!qX)=g-!4{QK4H;z$4kEEackm(IH#2)Rev7! z`^zlRiK%l*M{}t4X(DZ#>2${VueM)-j?oJu>#};rkhikzrP`G{*rSX?#?01?;!KUH z#ZKSM#1IR48oAi++?QlL*a{{3MG$OHd%rHTXjuNYqDt9TB^_c?tUuZ~iJ7u>iGo-~ zN0oFn>e)f=6)tZyG(NpDE`R|XpoE(aKDt!w95P&9R^iwX>bg2%1VPk`y9m&@-oNqX zjh|P)o%h|XYJB)f;uu`^8~9NtUS6%Sb-Y>UoL`RGO|~%OYNlfNi?5dYZ-4aRW4fnji?EeF3=fCzYmyLKha}Ln;bSPftzRlNPuq{_TGhgOv>wbH{IHWP*DJcghzOG(Ein$vZeX0tDFG`odzvcpd@j-#&JDV;$2HuLy9t$6f-_D zZ{hw5{U;Z(fsVj&dyXShGiO!41)8_icBlIyQBleKerTk@1l-_Nd}e%^H>m0x zxAbOUEp6PMqEF$LcYqMVHM(?*4N;fu`08ZaO$-clLXjF*KenuWP-iPjUzq5w8G_2m zOKAd)`reV)=~h}`P#}YwY2M00JFmAGA9bS8#QBs(yJaacRhkenNz~E+W^wJd~M|1kz3w$W`Vq~7)$sKQs)XKLgSM&U{ z=M&(&u4x`Ie2Wn%{~&0#q9oxZtP}$cF2r|Mz)*%b!B`9}?H~qveQth6o$6H3*-i_R zCH&3VK8Zhyi69cJkWoO+@PjwHk%THop}WNyFO+Sjca>F+XhW^}anFlR7jI3czAhT( zNY85=ZUWZ8>&CX~+O}B>yp(JS(hNz@SCYS;#fJyJmYgZ=bbd`y5bN#+Ep(nGC3Ch+ z<+!_1rDh2CFff2lMoRZe{mn_0->5`HV4B2jxmnCFFd(n({ zH1YHNvd0%SDlkueH}d_Dd6~rAp88K3*M{1vQWQF~8};`#w;j*orm`hpVh{Cpd{43o zazv#ljJT?GhQYK1Ch4+-tU_IbJ7cY3H;4FFQ8gwXZ(&0_2eGw`t1asTW7JAxqLjun zw?(SH{d`wKcIBWlQ`sS&*@YzW%X8CJws2oA#lCD1k_VRZQCl)UyVs8z3g(M4NLH-6 zrcU;;y`MZgU!P#C@3D03uMJ%3Mkjd2zHU1g`#!0>81LORYcN&L*AlmJGp(vws^$CB z{V(D{^VLtg?eylXAKrYFpjGh{`eBhg6@xibgRrYl?-Wq3dJU(Kr2&#XUPP^Ccixo)a-Cy{6XhVa~ z=+3SsItTJ;W4fE;J53X3gwotO+v9hb$3XLXx39lCr1a1JnW#LT+w_91hYt?ObzLMu zJeK;nIh&Fllx-TN#a@9W~)Sh_F?MnMu^OO$5c$)BEManX98^vtL6%g*W)01?L55_dMa3xWsdIG%FapqD+DJ z)55!#!X_&`#jSrXuAdy7njo@%lQT0z?|Q`uJLNc?q#W3$KPf5Ewm77{hW9%Oo^)+y zJFh&+0ldhwJJT45tx32X6V^+-8T-xWCRzMIs2h2%Gyi^);nZ-TMW7Au!;87f?ZEBz zfG{}!*ij2DIxF=1pA3r3%un~8ROPBga)1%WFG%(`uadcHJgii!U`7`xqbbJk|0Pq@ zJd|B{`2!f13HtweDPI93!+$wOZSThaYlD;Qe+FaxQ<^K8kSr@>pIU%QCX>w8dZCv5rbx@@Xx9mZHqydlPM zFNkIlUq&ezlYLFrvr12)`;lzl6hBO?%M_*wz(SB3QIcNO*>W+8@`LTQAJdmzO0fvS*pWe~nr`9iOIA3$4r`A1A~;B zQBx<%Dju9d==hE~zd>s59Sa$d`q}{N)7MJNiKj@)M5J3=U$4dJloCJj5$CtSL*k3*Q)m(cQ39+TK9_&bM*JBTS9pJdIM*l}|@v=W28^xkd zZj;<}Mfa)DF($*OvJwjqr(d7-=g0N^$@_bzg-0D*y8KR5Mf2P3!qOG*-wCNTy8Ozy zmy@^cX>%O6&~{F3FVU8qR<{pz>waYCRzB#hk8MA06|(L6Jgcj>1EY1VfG0j3yJXg; zo#R_Hk;7%uPq)ay`O3cPi&DX>JR0Jjfy=w+OiAOZ166<3IVBKe|6MGapoU=&lXB^y=@Uwll0v7V5ia zWlR;(YUP$*aZ7~NhL0`#28T-M-Z$*0_6zNCeOS+)6?MMrPwXRZf1U`{Ct=63JKsGP zI;9&Fbi}z>sAgkRM9a_Qp3O2$F8hqL`P;vRng7%K7*ZDas+>DLeOIjOtVd<;6I8OM zuQ_)w%K8EGrmB91P`q^LIQ~k*DJx=P1u8yg<_UWmvR~F8Fd+&D8Wuc2tE_d1i;Ij+1J?OIsRa?PylTew+$^7cvk zz>Q-kmF}L1?5>>T%~#XfD$tz=Tz3(Ft#RRd|Bw6+e|bM$-=|P!{(h-~+g159dx6JQ zDE8+*y!Y{EP(J6}+0tbOne#tv{3j>gXY%oikmUQPmmYH5TKw~RLH_5Ye)0A*8Gp9l z->hi1?U6xt@1=b_Km2aJcAWS7$n`bAX)4jIe@eRq?qvn$_5Hc`GvM&n<5R@nYCc<) z+vmG>LXp<|_BxIq(O$bgtOd?Pb-tJXEgpZ(boWNyXG_Irm@*eXb>+J*7m}eKZ=`x# zdg{sGqt?6M{;dxe`Ep5pdEAf72d!#m^#uQTbhB@|Rdh^cQC1LR!6yzE?@eE~pHNjY z>WG|r?h@mpb(#CrKlH79Z2aNZ`D54Cu3560J8I@u(Fr~m)}H8owwL+rC)w0boA;l# z-q`iI+MYLSnmkw956e~h?O*>I`zDpF%&WVdsnYvgGHF`AmYAV0gTnjHnM)_+vndrA z0B?A`exyommr1-x#j^7u7yY&`ef?dkXJgF1<;8Y0lQZ*HoI0WGtn%$&&SdtaWHsBF zNe^95&y3BmjJh6w{p-T-`G>=L|8Op?EL`Idz2H`BS5M)=vNZb-YU)>Jz0+>}k=8el z`O=ZDQ@*?A7)`aXR90wkm+e^hs(lBjdjej0?wauH0zbe|JS!^OnbZz5X+l zEw&YZ?R1B8#iA~Uusbz}jQvgr^Zz>Rw}AC@;It)+8pL%yB6mFNa=sG4+IS)85?|9a Oit0O1T}gDEcM||z$Zm}Q literal 0 HcmV?d00001 From f1ddfa17cce5da3b23b8c0822bf5c06666fdb717 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 25 Jun 2023 00:20:53 +0200 Subject: [PATCH 10/41] rename constants to make them unique --- src/settings/README.md | 2 +- src/settings/SettingsStorage.cpp | 10 +++++----- src/settings/SettingsStorage.h | 8 ++++---- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/settings/README.md b/src/settings/README.md index d8a44de..9bd70fb 100644 --- a/src/settings/README.md +++ b/src/settings/README.md @@ -59,7 +59,7 @@ void setup() { motor.initFOC(); // and if the settings were not loaded earlier, then save them - if (motor.motor_status == FOCMotorStatus::motor_ready && loadStatus != SETTINGS_SUCCESS) { + if (motor.motor_status == FOCMotorStatus::motor_ready && loadStatus != SFOC_SETTINGS_SUCCESS) { settings.saveSettings(); } diff --git a/src/settings/SettingsStorage.cpp b/src/settings/SettingsStorage.cpp index 6c3c4cd..5df2bcf 100644 --- a/src/settings/SettingsStorage.cpp +++ b/src/settings/SettingsStorage.cpp @@ -50,17 +50,17 @@ SettingsStatus SettingsStorage::loadSettings() { uint8_t magic; readByte(&magic); if (magic != SIMPLEFOC_SETTINGS_MAGIC_BYTE) { SimpleFOCDebug::println("No settings found "); - return NO_SETTINGS; + return SFOC_SETTINGS_NONE; } uint8_t rversion; readByte(&rversion); if (rversion != SIMPLEFOC_REGISTERS_VERSION) { SimpleFOCDebug::println("Registers version mismatch"); - return OLD_SETTINGS; + return SFOC_SETTINGS_OLD; } uint8_t version; readByte(&version); if (version != settings_version) { SimpleFOCDebug::println("Settings version mismatch"); - return OLD_SETTINGS; + return SFOC_SETTINGS_OLD; } for (int m = 0; m < numMotors; m++) { if (numMotors>1) @@ -76,7 +76,7 @@ SettingsStatus SettingsStorage::loadSettings() { } afterLoad(); SimpleFOCDebug::println("Settings loaded"); - return SETTINGS_SUCCESS; + return SFOC_SETTINGS_SUCCESS; }; @@ -100,7 +100,7 @@ SettingsStatus SettingsStorage::saveSettings() { } afterSave(); SimpleFOCDebug::println("Settings saved"); - return SETTINGS_SUCCESS; + return SFOC_SETTINGS_SUCCESS; }; // empty implementation for these diff --git a/src/settings/SettingsStorage.h b/src/settings/SettingsStorage.h index 4c9e172..8506c3b 100644 --- a/src/settings/SettingsStorage.h +++ b/src/settings/SettingsStorage.h @@ -19,10 +19,10 @@ typedef enum : uint8_t { - SETTINGS_SUCCESS = 0, // settings loaded/saved successfully - NO_SETTINGS = 1, // no settings found to load - OLD_SETTINGS = 2, // settings found, but version is old - ERROR = 3 // other error + SFOC_SETTINGS_SUCCESS = 0, // settings loaded/saved successfully + SFOC_SETTINGS_NONE = 1, // no settings found to load + SFOC_SETTINGS_OLD = 2, // settings found, but version is old + SFOC_SETTINGS_ERROR = 3 // other error } SettingsStatus; From b3ec23a1edb0316470f7007aa4c380b1bbdcaecc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 25 Jun 2023 00:29:30 +0200 Subject: [PATCH 11/41] disambiguate call to Wire.requestFrom() --- src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp index 1ed50f7..e5c255a 100644 --- a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp @@ -78,7 +78,7 @@ void CAT24I2CFlashSettingsStorage::reset() { int CAT24I2CFlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { - int read = _wire->requestFrom(_address, numBytes, true); + int read = _wire->requestFrom(_address, (size_t)numBytes, true); _currptr += read; if (read==numBytes) _wire->readBytes((uint8_t*)valueToSet, numBytes); From 941048bf2285a8815283c08a57feebd7998a433d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 25 Jun 2023 00:43:59 +0200 Subject: [PATCH 12/41] improve READMEs --- src/comms/serial/README.md | 4 ++++ src/comms/telemetry/README.md | 42 +++++++++++++++++++++++++++++++++++ src/settings/i2c/README.md | 2 +- src/settings/samd/README.md | 2 +- 4 files changed, 48 insertions(+), 2 deletions(-) create mode 100644 src/comms/telemetry/README.md diff --git a/src/comms/serial/README.md b/src/comms/serial/README.md index 7096e6f..2e1b733 100644 --- a/src/comms/serial/README.md +++ b/src/comms/serial/README.md @@ -5,6 +5,8 @@ Serial communications classes for register-based control and telemetry from Simp ## SerialASCIITelemetry +:warning: unfinished, untested + Telemetry class that sends telemetry as ASCII on a serial port. Similar to the classic "monitoring" functionality of SimpleFOC, but allows you to configure telemetry based on most of the defined registers. Usage: @@ -40,6 +42,8 @@ Some options are supported: ## SerialBinaryCommander +:warning: unfinished, untested! + Control SimpleFOC via a binary protocol over the serial port. The standard SimpleFOC registers are used. TODO document the protocol \ No newline at end of file diff --git a/src/comms/telemetry/README.md b/src/comms/telemetry/README.md new file mode 100644 index 0000000..384098f --- /dev/null +++ b/src/comms/telemetry/README.md @@ -0,0 +1,42 @@ + +# SimpleFOC Telemetry + +:warning: unfinished, untested + +A flexible abstraction for telemetry (monitoring) of SimpleFOC systems. + +The telemetry implementation is based on the SimpleFOC registers, and allows you to send telemetry for any (readable) register. Telemetry supports multiple motors. + +The concept allows you to choose registers which are then sent by the telemetry automatically, on a regular schedule. + +The method of sending depends on the type of telemetry you add to your program. There are telemetry drivers for: + +- Serial ASCII telemetry +- Serial Binary telemetry +- and more drivers will be added in the future + +Multiple motors can be added to the same telemetry, to monitor several motors at the same time. The registers reported by telemetry can be changed at run-time. Multiple instances of telemetry can be used to monitor different sets of values at different time intervals, or to send to multiple channels at the same time. + +## Usage + +Using telemetry is simple: + +```c++ + +SerialASCIITelemetry telemetry = SerialASCIITelemetry(); +... + +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY }); + telemetry.init(); + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` \ No newline at end of file diff --git a/src/settings/i2c/README.md b/src/settings/i2c/README.md index cf2740f..9cb33bd 100644 --- a/src/settings/i2c/README.md +++ b/src/settings/i2c/README.md @@ -19,7 +19,7 @@ If you want to use a different I2C bus than the default, you can pass it to the You can use multiple settings objects in the same EEPROM, by providing an offset to the constructor: -``` +```c++ CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0, 80); CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0, 160); diff --git a/src/settings/samd/README.md b/src/settings/samd/README.md index 7316011..7382751 100644 --- a/src/settings/samd/README.md +++ b/src/settings/samd/README.md @@ -21,7 +21,7 @@ Otherwise there is nothing special to do. When reprogramming the chip, be carefu You can use multiple settings objects to store to different areas of the RWW. To do so, specify an offset to the constructor: -``` +```c++ SAMDNVMSettingsStorage settings0 = SAMDNVMSettingsStorage(); SAMDNVMSettingsStorage settings1 = SAMDNVMSettingsStorage(NVMCTRL_ROW_SIZE); SAMDNVMSettingsStorage settings2 = SAMDNVMSettingsStorage(NVMCTRL_ROW_SIZE*2); From ef319e2dd9214993592495fc8b946d88e00b202c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 25 Jun 2023 00:56:05 +0200 Subject: [PATCH 13/41] placate ESP32 compiler --- src/comms/telemetry/Telemetry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h index 953e354..256036a 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -16,7 +16,7 @@ #define DEF_TELEMETRY_DOWNSAMPLE 100 -enum : uint8_t { +typedef enum : uint8_t { TELEMETRY_FRAMETYPE_DATA = 0x01, TELEMETRY_FRAMETYPE_HEADER = 0x02 } TelemetryFrameType; From 796251b9cf142e79db6b9cf6b6c39d0dc64037ae Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 25 Jun 2023 01:10:48 +0200 Subject: [PATCH 14/41] placate AVR and SAM compiler --- src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp index e5c255a..5d4b336 100644 --- a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp @@ -78,7 +78,7 @@ void CAT24I2CFlashSettingsStorage::reset() { int CAT24I2CFlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { - int read = _wire->requestFrom(_address, (size_t)numBytes, true); + int read = _wire->requestFrom((uint8_t)_address, (uint8_t)numBytes, (uint8_t)true); _currptr += read; if (read==numBytes) _wire->readBytes((uint8_t*)valueToSet, numBytes); From f6782d5f076c93579677ae536bb94b43cdd1b340 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 25 Jun 2023 01:22:13 +0200 Subject: [PATCH 15/41] prepare for 1.0.5 release version --- README.md | 20 +++++++++++++------- library.properties | 2 +- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 9452157..195f2fc 100644 --- a/README.md +++ b/README.md @@ -10,14 +10,13 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.4 - Released June 2023, for Simple FOC 2.3.0 +v1.0.5 - Released July 2023, for Simple FOC 2.3.0 -What's changed since 1.0.3? -- New Comms/Input: STM32SpeedDirCommander -- New Utility: STM32PWMInput -- Fixed MT6835 driver bugs -- Improved AS5047 driver, fixed bugs -- Improved AS5047U driver, fixed bugs +What's changed since 1.0.4? +- New Settings abstraction to load and save SimpleFOC settings and calibration +- New Settings driver: SAMDNVMSettingsStorage +- SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" +- Updated I2CCommander to use the new registers abstraction ## What is included @@ -90,6 +89,13 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi ## Release History +What's changed since 1.0.3? +- New Comms/Input: STM32SpeedDirCommander +- New Utility: STM32PWMInput +- Fixed MT6835 driver bugs +- Improved AS5047 driver, fixed bugs +- Improved AS5047U driver, fixed bugs + What's changed since 1.0.2? - New Sensor: MT6835 - Fixed bugs diff --git a/library.properties b/library.properties index d60e428..d3565e4 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.4 +version=1.0.5 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. From e9b657ce2ede80e8351894ad1a76a76d3e7c489d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 26 Jun 2023 13:38:38 +0200 Subject: [PATCH 16/41] exclude smoothing example except for due --- .github/workflows/ccpp.yml | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 162be4d..5d0fb1a 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,21 +26,21 @@ jobs: - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:uno - sketches-exclude: calibrated mt6816_spi + sketches-exclude: calibrated mt6816_spi smoothing required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC sketches-exclude: calibrated - arduino-boards-fqbn: arduino:samd:nano_33_iot required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: arduino:mbed_rp2040:pico required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # platform-url: https://dl.espressif.com/dl/package_esp32_index.json # required-libraries: Simple FOC @@ -48,18 +48,19 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - sketches-exclude: calibrated mt6816_spi + sketches-exclude: calibrated mt6816_spi smoothing - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC + sketches-exclude: smoothing # Do not cancel all jobs / architectures if one job fails fail-fast: false steps: From a049307b0f84bb1dc0a17924ac2d2b3ff93e82f7 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 26 Jun 2023 13:39:04 +0200 Subject: [PATCH 17/41] Add SmoothingSensor to main README --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 195f2fc..c792a66 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,7 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un v1.0.5 - Released July 2023, for Simple FOC 2.3.0 What's changed since 1.0.4? +- Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) - New Settings abstraction to load and save SimpleFOC settings and calibration - New Settings driver: SAMDNVMSettingsStorage - SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" @@ -42,6 +43,7 @@ What is here? See the sections below. Each driver or function should come with i - [MT6701 SSI driver](src/encoders/mt6701/) - SSI driver for the MagnTek MT6701 absolute position magnetic rotary encoder IC. - [MT6835 SPI driver](src/encoders/mt6835/) - SPI driver for the MagnTek MT6835 21 bit magnetic rotary encoder IC. - [STM32 PWM sensor driver](src/encoders/stm32pwmsensor/) - STM32 native timer-based driver for PWM angle sensors. + - [SmoothingSensor](src/encoders/smoothing/) - A SimpleFOC Sensor wrapper implementation which adds angle extrapolation. ### Communications From dc1192ceda000a9b95034f61467a2fe84cf6e4ee Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 26 Jun 2023 13:40:11 +0200 Subject: [PATCH 18/41] Update SimpleFOC version requirement in README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c792a66..64281e1 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.5 - Released July 2023, for Simple FOC 2.3.0 +v1.0.5 - Released July 2023, for Simple FOC 2.3.1 What's changed since 1.0.4? - Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) From 47d60e6a0774b87a41ed6e325a8fb3cd6f40657f Mon Sep 17 00:00:00 2001 From: "Yaseen M. Twati" Date: Thu, 29 Jun 2023 01:28:45 +0200 Subject: [PATCH 19/41] Add TMC6200 Driver --- src/drivers/tmc6200/README.md | 145 +++++++++++++ src/drivers/tmc6200/TMC6200.cpp | 243 ++++++++++++++++++++++ src/drivers/tmc6200/TMC6200.hpp | 145 +++++++++++++ src/drivers/tmc6200/TMC6200_Registers.hpp | 113 ++++++++++ 4 files changed, 646 insertions(+) create mode 100644 src/drivers/tmc6200/README.md create mode 100644 src/drivers/tmc6200/TMC6200.cpp create mode 100644 src/drivers/tmc6200/TMC6200.hpp create mode 100644 src/drivers/tmc6200/TMC6200_Registers.hpp diff --git a/src/drivers/tmc6200/README.md b/src/drivers/tmc6200/README.md new file mode 100644 index 0000000..8d8450a --- /dev/null +++ b/src/drivers/tmc6200/README.md @@ -0,0 +1,145 @@ + +# TMC6200 SimpleFOC Driver + +The TMC6200 is a Universal high voltage BLDC/PMSM/Servo MOSFET 3-halfbridge gate-driver with in line motor current +sensing. External MOSFETs for up to 100A motor current. + +See https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC6200_datasheet_Rev1.05.pdf for more information. + +## Hardware setup + +To use the TMC6200 you will have to connect the following: + +- GND +- VCC_IO - 3.3V or 5V +- SPE - pull up to VCC to enable SPI mode +- SPI MOSI ( SDI ) +- SPI MISO ( SDO ) +- SPI CLK +- SPI nCS +- DRV_EN - connect to digital out (or pull up to VCC) +- UH - connect to motor PWM pin +- VH - connect to motor PWM pin +- WH - connect to motor PWM pin +- UL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation +- VL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation +- WL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation + +Optionally, but useful: + +- FAULT - digital in, active high + +For current sensing: + +- CURU - connect to analog in +- CURV - connect to analog in +- CURW - connect to analog in + +## Usage + +Usage is quite easy, especially if you already know SimpleFOC. See also the [examples](https://github.com/simplefoc/Arduino-FOC-drivers/examples/drivers/drv8316/) + +```c++ +#include "Arduino.h" +#include +#include "SimpleFOCDrivers.h" +#include "drivers/tmc6200/TMC6200.hpp" + +BLDCMotor motor = BLDCMotor(15); +TMC6200Driver6PWM driver = DRV8316Driver6PWM(UH, UL, VH, VL, WH, WL, nCS, DRV_EN); + +//... normal simpleFOC init code... +``` + +Or, for 3-PWM: + +```c++ +#include "Arduino.h" +#include +#include "SimpleFOCDrivers.h" +#include "drivers/tmc6200/TMC6200.hpp" + +BLDCMotor motor = BLDCMotor(15); +TMC6200Driver3PWM driver = TMC6200Driver3PWM(UH, MOTOR_VH, MOTOR_WH, nCS, DRV_EN); + +void setup() { + + pinMode(UL, OUTPUT); + pinMode(VL, OUTPUT); + pinMode(WL, OUTPUT); + + digitalWrite(WL, HIGH); + digitalWrite(UL, HIGH); + digitalWrite(VL, HIGH); + + //... normal simpleFOC init code... +} + +``` + +### Validating the SPI Connection +You can validate the SPI connection by checking the value of VERSION field in IOIN register. The value should be 0x10. + +```c++ + if(driver.getInputs().VERSION != TMC6200_VERSION){ + // something is wrong with the spi connection + } +``` + +### Current Sensing +The gain of the internal current amplifiers can be set to 5, 10 or 20 through `setCurrentSenseGain()` + +```c++ + driver.setCurrentSenseGain(TMC6200_AmplificationGain::_5); + //driver.setCurrentSenseGain(TMC6200_AmplificationGain::_10); + //driver.setCurrentSenseGain(TMC6200_AmplificationGain::_20); +``` +The sense amplifiers can also be turned off ( they are on by default ), through `setCurrentSenseAmplifierState()` + +```c++ + driver.setCurrentSenseAmplifierState(false); +``` +### Driver Strength +The strength of the mosfet drivers can be controlled through `setDriverStrength()` + +```c++ + driver.setDriverStrength(TMC6200_DRVStrength::Weak); + //driver.setDriverStrength(TMC6200_DRVStrength::WeakTC); // (medium above OTPW level) + //driver.setDriverStrength(TMC6200_DRVStrength::Medium); + //driver.setDriverStrength(TMC6200_DRVStrength::Strong); +``` + +### Handling Faults +The fault line will go high if a fault occurs such as a short, an interrupt can be used to handle it. +Note that some faults will disable the driver and will require the DRV_EN to be cycled to clear the fault. + +```c++ + // somewhere in setup + attachInterrupt(digitalPinToInterrupt(FAULT), handleFault, RISING); +``` + +```c++ + void handleFault() + { + // you can read the status register to see what happened + TMC6200GStatus status = driver.getStatus(); + Serial.print("hasUShorted: "); Serial.println(status.hasUShorted()); + Serial.print("hasVShorted: "); Serial.println(status.hasVShorted()); + Serial.print("hasWShorted: "); Serial.println(status.hasWShorted()); + Serial.print("isUShortedToGround: "); Serial.println(status.isUShortedToGround()); + Serial.print("isUShortedToSupply: "); Serial.println(status.isUShortedToSupply()); + Serial.print("isVShortedToGround: "); Serial.println(status.isVShortedToGround()); + Serial.print("isVShortedToSupply: "); Serial.println(status.isVShortedToSupply()); + Serial.print("isWShortedToGround: "); Serial.println(status.isWShortedToGround()); + Serial.print("isWShortedToSupply: "); Serial.println(status.isWShortedToSupply()); + Serial.print("isOverTemperaturePreWarning: "); Serial.println(status.isOverTemperaturePreWarning()); + Serial.print("isChargePumpUnderVoltage: "); Serial.println(status.isChargePumpUnderVoltage()); + + // the driver must be cycled to clear the fault + digitalWrite(DRV_EN, LOW); + delayMicrosockets(someSmallDelay); + digitalWrite(DRV_EN, HIGH); + } +``` + +The driver provides other features such as controlling the tolerences of short detection and the BBM cycle time and so on, setting the options can be conveniently done via the provided setter methods. All documented registers and options are available via the driver, and the option values can be accessed via enums. diff --git a/src/drivers/tmc6200/TMC6200.cpp b/src/drivers/tmc6200/TMC6200.cpp new file mode 100644 index 0000000..54871db --- /dev/null +++ b/src/drivers/tmc6200/TMC6200.cpp @@ -0,0 +1,243 @@ +#include "TMC6200.hpp" + +void TMC6200Driver::init(SPIClass *_spi) +{ + spi = _spi; + settings = SPISettings(500000, MSBFIRST, SPI_MODE3); + + pinMode(csPin, OUTPUT); + digitalWrite(csPin, HIGH); +} + +// TMC6200_GCONF ------ + +void TMC6200Driver::setDriverState(bool state) +{ + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.DISABLE = state ? 0 : 1; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setPWMMode(TMC6200_PWMMode pwmMode) +{ + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.SINGLE_LINE = pwmMode; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setFaultDirect(TMC6200_FaultDirect faultDirect) +{ + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.FAULT_DIRECT = faultDirect; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setCurrentSenseAmplifierState(bool state) +{ + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.AMPLIFIER_OFF = state ? 0 : 1; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain) +{ + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.AMPLIFICATION = amplificationGain; + writeRegister(TMC6200_GCONF_REG, gConf.reg); + + gConf.reg = readRegister(TMC6200_GCONF_REG); +} + +// TMC6200_DRV_CONF ------ + +void TMC6200Driver::setOverTemperatureThreshold(TMC6200_OTSelect otLevel) +{ + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); + + drvConf.OT_SELECT = otLevel; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); +} + +void TMC6200Driver::setDriverStrength(TMC6200_DRVStrength strength) +{ + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); + + drvConf.DRV_STRENGTH = strength; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); +} + +void TMC6200Driver::setBBMCycles(uint8_t clockCycles) +{ + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); + + drvConf.BBMCLKS = clockCycles; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); +} + +// TMC6200_SHORT_CONF ------ + +void TMC6200Driver::setShortDelay(TMC6200_ShortDelay shortDelay) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.SHORT_DELAY = shortDelay; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::shortFilter(TMC6200_ShortFilter shortFilter) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.SHORT_FILTER = shortFilter; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToSupplySensitivityLevel(uint8_t level) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.S2VS_LEVEL = level; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToGroundSensitivityLevel(uint8_t level) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.S2G_LEVEL = level; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortRetries(uint8_t retries) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.RETRY = retries; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setParallelProtect(TMC6200_ParallelProtect parallelProtect) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.PROTECT_PARALLEL = parallelProtect; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToGroundDetectionState(bool state) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.DISABLE_S2G = state ? 0 : 1; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToSupplyDetectionState(bool state) +{ + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.DISABLE_S2VS = state ? 0 : 1; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +// TMC6200_IOIN ------ + +TMC6200_IOIN TMC6200Driver::getInputs() +{ + TMC6200_IOIN ioin; + ioin.reg = readRegister(TMC6200_IOIN_REG); + + return ioin; +} + +// TMC6200_GSTAT ------ + +TMC6200GStatus TMC6200Driver::getStatus() +{ + TMC6200_GSTAT gstat; + gstat.reg = readRegister(TMC6200_GSTAT_REG); + + return {gstat}; +} + +void TMC6200Driver::setStatus(TMC6200_GSTAT status) +{ + writeRegister(TMC6200_GSTAT_REG, status.reg); +} + +uint32_t TMC6200Driver:: readRegister(uint8_t addr) +{ + digitalWrite(csPin, LOW); + spi->beginTransaction(settings); + + // Address + spi->transfer(TMC_ADDRESS(addr)); + + // 32bit Data + uint32_t value = 0; + value |= (spi->transfer(0x00) << 24); + value |= (spi->transfer(0x00) << 16); + value |= (spi->transfer(0x00) << 8); + value |= (spi->transfer(0x00) << 0); + + spi->end(); + digitalWrite(csPin, HIGH); + + return value; +} + +void TMC6200Driver::writeRegister(uint8_t addr, uint32_t data) +{ + digitalWrite(csPin, LOW); + spi->beginTransaction(settings); + + // Address + spi->transfer(addr | TMC_WRITE_BIT); + + // 32bit Data + spi->transfer(0xFF & (data >> 24)); + spi->transfer(0xFF & (data >> 16)); + spi->transfer(0xFF & (data >> 8)); + spi->transfer(0xFF & (data >> 0)); + + spi->end(); + digitalWrite(csPin, HIGH); +} + +// -------------------- + +void TMC6200Driver3PWM::init(SPIClass* _spi) { + TMC6200Driver::init(_spi); + delayMicroseconds(1); + TMC6200Driver::setPWMMode(TMC6200_PWMMode::SingleLine); + BLDCDriver3PWM::init(); +}; + + +void TMC6200Driver6PWM::init(SPIClass* _spi) { + TMC6200Driver::init(_spi); + delayMicroseconds(1); + TMC6200Driver::setPWMMode(TMC6200_PWMMode::Individual); + BLDCDriver6PWM::init(); +}; diff --git a/src/drivers/tmc6200/TMC6200.hpp b/src/drivers/tmc6200/TMC6200.hpp new file mode 100644 index 0000000..fcc5ff3 --- /dev/null +++ b/src/drivers/tmc6200/TMC6200.hpp @@ -0,0 +1,145 @@ +#pragma once + +#include +#include +#include +#include +#include "TMC6200_Registers.hpp" + +#define TMC6200_VERSION 0x10 + +enum TMC6200_PWMMode { + Individual = 0, // 6PWM + SingleLine = 1, // 3PWM +}; + +enum TMC6200_AmplificationGain { + _5 = 0, + _10 = 1, + _Also_10 = 2, // maybe just remove this + _20 = 3, +}; + +enum TMC6200_ShortDelay { + _750nS = 0, + _1500nS = 1, +}; + +enum TMC6200_ShortFilter { + _100nS = 0, + _1uS = 1, + _2uS = 2, + _3uS = 3, +}; + +enum TMC6200_ParallelProtect { + Detected = 0, + All = 1, +}; + +enum TMC6200_FaultDirect { + AtLeastOneBridgeDown = 0, + EachProtectiveAction = 1, +}; + +enum TMC6200_OTSelect { + _150 = 0b00, + _143 = 0b01, + _136 = 0b10, + _120 = 0b11, +}; + +enum TMC6200_DRVStrength { + Weak = 0b00, + WeakTC = 0b01, // (medium above OTPW level) + Medium = 0b10, + Strong = 0b11, +}; + +class TMC6200GStatus { +public: + TMC6200GStatus(TMC6200_GSTAT status) : status(status) {}; + + bool isReset() const { return status.RESET == 0b1; }; + + bool isOverTemperaturePreWarning() const { return status.DRV_OTPW == 0b1; }; + bool isOverTemperature() const { return status.DRV_OT == 0b1; }; + bool isChargePumpUnderVoltage() const { return status.UV_CP == 0b1; }; + + bool hasUShorted() const { return status.SHORT_DET_U == 0b1; }; + bool hasVShorted() const { return status.SHORT_DET_V == 0b1; }; + bool hasWShorted() const { return status.SHORT_DET_W == 0b1; }; + + bool isUShortedToGround() const { return status.S2GU == 0b1; }; + bool isUShortedToSupply() const { return status.S2VSU == 0b1; }; + + bool isVShortedToGround() const { return status.S2GV == 0b1; }; + bool isVShortedToSupply() const { return status.S2VSV == 0b1; }; + + bool isWShortedToGround() const { return status.S2GW == 0b1; }; + bool isWShortedToSupply() const { return status.S2VSW == 0b1; }; + + TMC6200_GSTAT status; +}; + +class TMC6200Driver { + + public: + TMC6200Driver(int csPin) : csPin(csPin), spi(&SPI), settings(1000000, MSBFIRST, SPI_MODE3) {}; + virtual ~TMC6200Driver() {}; + + virtual void init(SPIClass* _spi); + + void setDriverState(bool state); + void setPWMMode(TMC6200_PWMMode pwmMode); + void setFaultDirect(TMC6200_FaultDirect faultDirect); + void setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain); + void setOverTemperatureThreshold(TMC6200_OTSelect otLevel); + void setDriverStrength(TMC6200_DRVStrength strength); + void setCurrentSenseAmplifierState(bool state); + void setShortDelay(TMC6200_ShortDelay shortDelay); + void shortFilter(TMC6200_ShortFilter shortFilter); + void setShortToSupplySensitivityLevel(uint8_t level); + void setShortToGroundSensitivityLevel(uint8_t level); + void setShortRetries(uint8_t retries); + void setParallelProtect(TMC6200_ParallelProtect parallelProtect); + void setShortToGroundDetectionState(bool state); + void setShortToSupplyDetectionState(bool state); + void setBBMCycles(uint8_t clockCycles); + + void setStatus(TMC6200_GSTAT status); + TMC6200GStatus getStatus(); + + TMC6200_IOIN getInputs(); + + private: + uint32_t readRegister(uint8_t addr); + void writeRegister(uint8_t addr, uint32_t data); + + int csPin; + SPIClass* spi; + SPISettings settings; +}; + + +class TMC6200Driver3PWM : public TMC6200Driver, public BLDCDriver3PWM { + + public: + TMC6200Driver3PWM(int phA,int phB,int phC, int cs, int en = NOT_SET) : + TMC6200Driver(cs), BLDCDriver3PWM(phA, phB, phC, en) { }; + ~TMC6200Driver3PWM() override = default; + + virtual void init(SPIClass* _spi = &SPI) override; +}; + + + +class TMC6200Driver6PWM : public TMC6200Driver, public BLDCDriver6PWM { + + public: + TMC6200Driver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int cs, int en = NOT_SET) : + TMC6200Driver(cs), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) { }; + ~TMC6200Driver6PWM() override = default; + + virtual void init(SPIClass* _spi = &SPI) override; +}; \ No newline at end of file diff --git a/src/drivers/tmc6200/TMC6200_Registers.hpp b/src/drivers/tmc6200/TMC6200_Registers.hpp new file mode 100644 index 0000000..8c7052b --- /dev/null +++ b/src/drivers/tmc6200/TMC6200_Registers.hpp @@ -0,0 +1,113 @@ +#pragma once + +#include + +// ------ + +#define TMC_WRITE_BIT 0x80 +#define TMC_ADDRESS_MASK 0x7F +#define TMC_ADDRESS(x) ((x) & (TMC_ADDRESS_MASK)) + +// ------ + +#define TMC6200_GCONF_REG 0x00 // RW +#define TMC6200_GSTAT_REG 0x01 // RW + WC +#define TMC6200_IOIN_REG 0x04 // R + +#define TMC6200_OTP_PROG_REG 0x06 // not used +#define TMC6200_OTP_READ_REG 0x07 // not used +#define TMC6200_FACTORY_CONF_REG 0x08 // not used + +#define TMC6200_SHORT_CONF_REG 0x09 // RW +#define TMC6200_DRV_CONF_REG 0x0A // RW + +typedef union { + struct { + uint32_t DISABLE:1; + uint32_t SINGLE_LINE:1; + uint32_t FAULT_DIRECT:1; + uint32_t :1; + uint32_t AMPLIFICATION:2; + uint32_t AMPLIFIER_OFF:1; + uint32_t TEST_MODE:1; + uint32_t :24; + }; + uint32_t reg; +} TMC6200_GCONF; + +typedef union { + struct { + uint32_t RESET:1; + uint32_t DRV_OTPW:1; + uint32_t DRV_OT:1; + uint32_t UV_CP:1; + + uint32_t SHORT_DET_U:1; + uint32_t S2GU:1; + uint32_t S2VSU:1; + uint32_t :1; + + uint32_t SHORT_DET_V:1; + uint32_t S2GV:1; + uint32_t S2VSV:1; + uint32_t :1; + + uint32_t SHORT_DET_W:1; + uint32_t S2GW:1; + uint32_t S2VSW:1; + uint32_t :1; + }; + uint32_t reg; +} TMC6200_GSTAT; + +typedef union { + struct { + unsigned UL: 1; + uint32_t UH: 1; + uint32_t VL: 1; + uint32_t VH: 1; + uint32_t WL: 1; + uint32_t WH: 1; + uint32_t DRV_EN: 1; + uint32_t _reserved1: 1; + uint32_t OTPW: 1; + uint32_t OT136C: 1; + uint32_t OT143C: 1; + uint32_t OT150C: 1; + uint32_t _reserved2: 12; + uint32_t VERSION: 8; + }; + + uint32_t reg; +} TMC6200_IOIN; + +typedef union { + struct { + uint32_t S2VS_LEVEL:4; + uint32_t :4; + uint32_t S2G_LEVEL:4; + uint32_t :4; + uint32_t SHORT_FILTER:2; + uint32_t :2; + uint32_t SHORT_DELAY:1; + uint32_t :4; + uint32_t RETRY:2; + uint32_t :2; + uint32_t PROTECT_PARALLEL:1; + uint32_t DISABLE_S2G:1; + uint32_t DISABLE_S2VS:1; + uint32_t :2; + }; + uint32_t reg; +} TMC6200_SHORT_CONF; + +typedef union { + struct { + uint32_t BBMCLKS:5; + uint32_t :11; + uint32_t OT_SELECT:2; + uint32_t DRV_STRENGTH:2; + uint32_t :12; + }; + uint32_t reg; +} TMC6200_DRV_CONF; \ No newline at end of file From 5c407eacdbd68429682e653a16aa80dfe36cab36 Mon Sep 17 00:00:00 2001 From: "Yaseen M. Twati" Date: Thu, 29 Jun 2023 09:36:29 +0200 Subject: [PATCH 20/41] Reformat Code --- src/drivers/tmc6200/TMC6200.cpp | 270 ++++++++++------------ src/drivers/tmc6200/TMC6200.hpp | 194 +++++++++------- src/drivers/tmc6200/TMC6200_Registers.hpp | 132 +++++------ 3 files changed, 300 insertions(+), 296 deletions(-) diff --git a/src/drivers/tmc6200/TMC6200.cpp b/src/drivers/tmc6200/TMC6200.cpp index 54871db..659ab5d 100644 --- a/src/drivers/tmc6200/TMC6200.cpp +++ b/src/drivers/tmc6200/TMC6200.cpp @@ -1,243 +1,221 @@ #include "TMC6200.hpp" -void TMC6200Driver::init(SPIClass *_spi) -{ - spi = _spi; - settings = SPISettings(500000, MSBFIRST, SPI_MODE3); +void TMC6200Driver::init(SPIClass *_spi) { + spi = _spi; + settings = SPISettings(500000, MSBFIRST, SPI_MODE3); - pinMode(csPin, OUTPUT); - digitalWrite(csPin, HIGH); + pinMode(csPin, OUTPUT); + digitalWrite(csPin, HIGH); } // TMC6200_GCONF ------ -void TMC6200Driver::setDriverState(bool state) -{ - TMC6200_GCONF gConf; - gConf.reg = readRegister(TMC6200_GCONF_REG); +void TMC6200Driver::setDriverState(bool state) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); - gConf.DISABLE = state ? 0 : 1; - writeRegister(TMC6200_GCONF_REG, gConf.reg); + gConf.DISABLE = state ? 0 : 1; + writeRegister(TMC6200_GCONF_REG, gConf.reg); } -void TMC6200Driver::setPWMMode(TMC6200_PWMMode pwmMode) -{ - TMC6200_GCONF gConf; - gConf.reg = readRegister(TMC6200_GCONF_REG); +void TMC6200Driver::setPWMMode(TMC6200_PWMMode pwmMode) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); - gConf.SINGLE_LINE = pwmMode; - writeRegister(TMC6200_GCONF_REG, gConf.reg); + gConf.SINGLE_LINE = pwmMode; + writeRegister(TMC6200_GCONF_REG, gConf.reg); } -void TMC6200Driver::setFaultDirect(TMC6200_FaultDirect faultDirect) -{ - TMC6200_GCONF gConf; - gConf.reg = readRegister(TMC6200_GCONF_REG); +void TMC6200Driver::setFaultDirect(TMC6200_FaultDirect faultDirect) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); - gConf.FAULT_DIRECT = faultDirect; - writeRegister(TMC6200_GCONF_REG, gConf.reg); + gConf.FAULT_DIRECT = faultDirect; + writeRegister(TMC6200_GCONF_REG, gConf.reg); } -void TMC6200Driver::setCurrentSenseAmplifierState(bool state) -{ - TMC6200_GCONF gConf; - gConf.reg = readRegister(TMC6200_GCONF_REG); +void TMC6200Driver::setCurrentSenseAmplifierState(bool state) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); - gConf.AMPLIFIER_OFF = state ? 0 : 1; - writeRegister(TMC6200_GCONF_REG, gConf.reg); + gConf.AMPLIFIER_OFF = state ? 0 : 1; + writeRegister(TMC6200_GCONF_REG, gConf.reg); } -void TMC6200Driver::setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain) -{ - TMC6200_GCONF gConf; - gConf.reg = readRegister(TMC6200_GCONF_REG); +void TMC6200Driver::setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); - gConf.AMPLIFICATION = amplificationGain; - writeRegister(TMC6200_GCONF_REG, gConf.reg); + gConf.AMPLIFICATION = amplificationGain; + writeRegister(TMC6200_GCONF_REG, gConf.reg); gConf.reg = readRegister(TMC6200_GCONF_REG); } // TMC6200_DRV_CONF ------ -void TMC6200Driver::setOverTemperatureThreshold(TMC6200_OTSelect otLevel) -{ - TMC6200_DRV_CONF drvConf; - drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); +void TMC6200Driver::setOverTemperatureThreshold(TMC6200_OTSelect otLevel) { + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); - drvConf.OT_SELECT = otLevel; - writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); + drvConf.OT_SELECT = otLevel; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); } -void TMC6200Driver::setDriverStrength(TMC6200_DRVStrength strength) -{ - TMC6200_DRV_CONF drvConf; - drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); +void TMC6200Driver::setDriverStrength(TMC6200_DRVStrength strength) { + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); - drvConf.DRV_STRENGTH = strength; - writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); + drvConf.DRV_STRENGTH = strength; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); } -void TMC6200Driver::setBBMCycles(uint8_t clockCycles) -{ - TMC6200_DRV_CONF drvConf; - drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); +void TMC6200Driver::setBBMCycles(uint8_t clockCycles) { + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); - drvConf.BBMCLKS = clockCycles; - writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); + drvConf.BBMCLKS = clockCycles; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); } // TMC6200_SHORT_CONF ------ -void TMC6200Driver::setShortDelay(TMC6200_ShortDelay shortDelay) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setShortDelay(TMC6200_ShortDelay shortDelay) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.SHORT_DELAY = shortDelay; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.SHORT_DELAY = shortDelay; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::shortFilter(TMC6200_ShortFilter shortFilter) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::shortFilter(TMC6200_ShortFilter shortFilter) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.SHORT_FILTER = shortFilter; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.SHORT_FILTER = shortFilter; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::setShortToSupplySensitivityLevel(uint8_t level) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setShortToSupplySensitivityLevel(uint8_t level) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.S2VS_LEVEL = level; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.S2VS_LEVEL = level; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::setShortToGroundSensitivityLevel(uint8_t level) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setShortToGroundSensitivityLevel(uint8_t level) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.S2G_LEVEL = level; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.S2G_LEVEL = level; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::setShortRetries(uint8_t retries) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setShortRetries(uint8_t retries) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.RETRY = retries; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.RETRY = retries; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::setParallelProtect(TMC6200_ParallelProtect parallelProtect) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setParallelProtect(TMC6200_ParallelProtect parallelProtect) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.PROTECT_PARALLEL = parallelProtect; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.PROTECT_PARALLEL = parallelProtect; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::setShortToGroundDetectionState(bool state) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setShortToGroundDetectionState(bool state) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.DISABLE_S2G = state ? 0 : 1; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.DISABLE_S2G = state ? 0 : 1; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } -void TMC6200Driver::setShortToSupplyDetectionState(bool state) -{ - TMC6200_SHORT_CONF shortConf; - shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); +void TMC6200Driver::setShortToSupplyDetectionState(bool state) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); - shortConf.DISABLE_S2VS = state ? 0 : 1; - writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); + shortConf.DISABLE_S2VS = state ? 0 : 1; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); } // TMC6200_IOIN ------ -TMC6200_IOIN TMC6200Driver::getInputs() -{ +TMC6200_IOIN TMC6200Driver::getInputs() { TMC6200_IOIN ioin; ioin.reg = readRegister(TMC6200_IOIN_REG); - return ioin; + return ioin; } // TMC6200_GSTAT ------ -TMC6200GStatus TMC6200Driver::getStatus() -{ - TMC6200_GSTAT gstat; - gstat.reg = readRegister(TMC6200_GSTAT_REG); +TMC6200GStatus TMC6200Driver::getStatus() { + TMC6200_GSTAT gstat; + gstat.reg = readRegister(TMC6200_GSTAT_REG); - return {gstat}; + return {gstat}; } -void TMC6200Driver::setStatus(TMC6200_GSTAT status) -{ - writeRegister(TMC6200_GSTAT_REG, status.reg); +void TMC6200Driver::setStatus(TMC6200_GSTAT status) { + writeRegister(TMC6200_GSTAT_REG, status.reg); } -uint32_t TMC6200Driver:: readRegister(uint8_t addr) -{ - digitalWrite(csPin, LOW); - spi->beginTransaction(settings); +uint32_t TMC6200Driver::readRegister(uint8_t addr) { + digitalWrite(csPin, LOW); + spi->beginTransaction(settings); - // Address - spi->transfer(TMC_ADDRESS(addr)); + // Address + spi->transfer(TMC_ADDRESS(addr)); - // 32bit Data + // 32bit Data uint32_t value = 0; value |= (spi->transfer(0x00) << 24); value |= (spi->transfer(0x00) << 16); value |= (spi->transfer(0x00) << 8); value |= (spi->transfer(0x00) << 0); - spi->end(); - digitalWrite(csPin, HIGH); + spi->end(); + digitalWrite(csPin, HIGH); - return value; + return value; } -void TMC6200Driver::writeRegister(uint8_t addr, uint32_t data) -{ - digitalWrite(csPin, LOW); - spi->beginTransaction(settings); +void TMC6200Driver::writeRegister(uint8_t addr, uint32_t data) { + digitalWrite(csPin, LOW); + spi->beginTransaction(settings); - // Address - spi->transfer(addr | TMC_WRITE_BIT); + // Address + spi->transfer(addr | TMC_WRITE_BIT); - // 32bit Data - spi->transfer(0xFF & (data >> 24)); - spi->transfer(0xFF & (data >> 16)); - spi->transfer(0xFF & (data >> 8)); - spi->transfer(0xFF & (data >> 0)); + // 32bit Data + spi->transfer(0xFF & (data >> 24)); + spi->transfer(0xFF & (data >> 16)); + spi->transfer(0xFF & (data >> 8)); + spi->transfer(0xFF & (data >> 0)); - spi->end(); - digitalWrite(csPin, HIGH); + spi->end(); + digitalWrite(csPin, HIGH); } // -------------------- -void TMC6200Driver3PWM::init(SPIClass* _spi) { - TMC6200Driver::init(_spi); - delayMicroseconds(1); - TMC6200Driver::setPWMMode(TMC6200_PWMMode::SingleLine); - BLDCDriver3PWM::init(); +void TMC6200Driver3PWM::init(SPIClass *_spi) { + TMC6200Driver::init(_spi); + delayMicroseconds(1); + TMC6200Driver::setPWMMode(TMC6200_PWMMode::SingleLine); + BLDCDriver3PWM::init(); }; -void TMC6200Driver6PWM::init(SPIClass* _spi) { - TMC6200Driver::init(_spi); - delayMicroseconds(1); - TMC6200Driver::setPWMMode(TMC6200_PWMMode::Individual); - BLDCDriver6PWM::init(); +void TMC6200Driver6PWM::init(SPIClass *_spi) { + TMC6200Driver::init(_spi); + delayMicroseconds(1); + TMC6200Driver::setPWMMode(TMC6200_PWMMode::Individual); + BLDCDriver6PWM::init(); }; diff --git a/src/drivers/tmc6200/TMC6200.hpp b/src/drivers/tmc6200/TMC6200.hpp index fcc5ff3..7a5215c 100644 --- a/src/drivers/tmc6200/TMC6200.hpp +++ b/src/drivers/tmc6200/TMC6200.hpp @@ -9,137 +9,163 @@ #define TMC6200_VERSION 0x10 enum TMC6200_PWMMode { - Individual = 0, // 6PWM - SingleLine = 1, // 3PWM + Individual = 0, // 6PWM + SingleLine = 1, // 3PWM }; enum TMC6200_AmplificationGain { - _5 = 0, - _10 = 1, - _Also_10 = 2, // maybe just remove this - _20 = 3, + _5 = 0, + _10 = 1, + _Also_10 = 2, // maybe just remove this + _20 = 3, }; enum TMC6200_ShortDelay { - _750nS = 0, - _1500nS = 1, + _750nS = 0, + _1500nS = 1, }; enum TMC6200_ShortFilter { - _100nS = 0, - _1uS = 1, - _2uS = 2, - _3uS = 3, + _100nS = 0, + _1uS = 1, + _2uS = 2, + _3uS = 3, }; enum TMC6200_ParallelProtect { - Detected = 0, - All = 1, + Detected = 0, + All = 1, }; enum TMC6200_FaultDirect { - AtLeastOneBridgeDown = 0, - EachProtectiveAction = 1, + AtLeastOneBridgeDown = 0, + EachProtectiveAction = 1, }; enum TMC6200_OTSelect { - _150 = 0b00, - _143 = 0b01, - _136 = 0b10, - _120 = 0b11, + _150 = 0b00, + _143 = 0b01, + _136 = 0b10, + _120 = 0b11, }; enum TMC6200_DRVStrength { - Weak = 0b00, - WeakTC = 0b01, // (medium above OTPW level) + Weak = 0b00, + WeakTC = 0b01, // (medium above OTPW level) Medium = 0b10, - Strong = 0b11, + Strong = 0b11, }; class TMC6200GStatus { public: - TMC6200GStatus(TMC6200_GSTAT status) : status(status) {}; + TMC6200GStatus(TMC6200_GSTAT status) : status(status) {}; - bool isReset() const { return status.RESET == 0b1; }; + bool isReset() const { return status.RESET == 0b1; }; - bool isOverTemperaturePreWarning() const { return status.DRV_OTPW == 0b1; }; - bool isOverTemperature() const { return status.DRV_OT == 0b1; }; - bool isChargePumpUnderVoltage() const { return status.UV_CP == 0b1; }; + bool isOverTemperaturePreWarning() const { return status.DRV_OTPW == 0b1; }; - bool hasUShorted() const { return status.SHORT_DET_U == 0b1; }; - bool hasVShorted() const { return status.SHORT_DET_V == 0b1; }; - bool hasWShorted() const { return status.SHORT_DET_W == 0b1; }; + bool isOverTemperature() const { return status.DRV_OT == 0b1; }; - bool isUShortedToGround() const { return status.S2GU == 0b1; }; - bool isUShortedToSupply() const { return status.S2VSU == 0b1; }; + bool isChargePumpUnderVoltage() const { return status.UV_CP == 0b1; }; - bool isVShortedToGround() const { return status.S2GV == 0b1; }; - bool isVShortedToSupply() const { return status.S2VSV == 0b1; }; + bool hasUShorted() const { return status.SHORT_DET_U == 0b1; }; - bool isWShortedToGround() const { return status.S2GW == 0b1; }; - bool isWShortedToSupply() const { return status.S2VSW == 0b1; }; + bool hasVShorted() const { return status.SHORT_DET_V == 0b1; }; - TMC6200_GSTAT status; + bool hasWShorted() const { return status.SHORT_DET_W == 0b1; }; + + bool isUShortedToGround() const { return status.S2GU == 0b1; }; + + bool isUShortedToSupply() const { return status.S2VSU == 0b1; }; + + bool isVShortedToGround() const { return status.S2GV == 0b1; }; + + bool isVShortedToSupply() const { return status.S2VSV == 0b1; }; + + bool isWShortedToGround() const { return status.S2GW == 0b1; }; + + bool isWShortedToSupply() const { return status.S2VSW == 0b1; }; + + TMC6200_GSTAT status; }; class TMC6200Driver { - public: - TMC6200Driver(int csPin) : csPin(csPin), spi(&SPI), settings(1000000, MSBFIRST, SPI_MODE3) {}; - virtual ~TMC6200Driver() {}; - - virtual void init(SPIClass* _spi); - - void setDriverState(bool state); - void setPWMMode(TMC6200_PWMMode pwmMode); - void setFaultDirect(TMC6200_FaultDirect faultDirect); - void setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain); - void setOverTemperatureThreshold(TMC6200_OTSelect otLevel); - void setDriverStrength(TMC6200_DRVStrength strength); - void setCurrentSenseAmplifierState(bool state); - void setShortDelay(TMC6200_ShortDelay shortDelay); - void shortFilter(TMC6200_ShortFilter shortFilter); - void setShortToSupplySensitivityLevel(uint8_t level); - void setShortToGroundSensitivityLevel(uint8_t level); - void setShortRetries(uint8_t retries); - void setParallelProtect(TMC6200_ParallelProtect parallelProtect); - void setShortToGroundDetectionState(bool state); - void setShortToSupplyDetectionState(bool state); - void setBBMCycles(uint8_t clockCycles); - - void setStatus(TMC6200_GSTAT status); - TMC6200GStatus getStatus(); - - TMC6200_IOIN getInputs(); - - private: - uint32_t readRegister(uint8_t addr); - void writeRegister(uint8_t addr, uint32_t data); - - int csPin; - SPIClass* spi; - SPISettings settings; +public: + TMC6200Driver(int csPin) : csPin(csPin), spi(&SPI), settings(1000000, MSBFIRST, SPI_MODE3) {}; + + virtual ~TMC6200Driver() {}; + + virtual void init(SPIClass *_spi); + + void setDriverState(bool state); + + void setPWMMode(TMC6200_PWMMode pwmMode); + + void setFaultDirect(TMC6200_FaultDirect faultDirect); + + void setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain); + + void setOverTemperatureThreshold(TMC6200_OTSelect otLevel); + + void setDriverStrength(TMC6200_DRVStrength strength); + + void setCurrentSenseAmplifierState(bool state); + + void setShortDelay(TMC6200_ShortDelay shortDelay); + + void shortFilter(TMC6200_ShortFilter shortFilter); + + void setShortToSupplySensitivityLevel(uint8_t level); + + void setShortToGroundSensitivityLevel(uint8_t level); + + void setShortRetries(uint8_t retries); + + void setParallelProtect(TMC6200_ParallelProtect parallelProtect); + + void setShortToGroundDetectionState(bool state); + + void setShortToSupplyDetectionState(bool state); + + void setBBMCycles(uint8_t clockCycles); + + void setStatus(TMC6200_GSTAT status); + + TMC6200GStatus getStatus(); + + TMC6200_IOIN getInputs(); + +private: + uint32_t readRegister(uint8_t addr); + + void writeRegister(uint8_t addr, uint32_t data); + + int csPin; + SPIClass *spi; + SPISettings settings; }; class TMC6200Driver3PWM : public TMC6200Driver, public BLDCDriver3PWM { - public: - TMC6200Driver3PWM(int phA,int phB,int phC, int cs, int en = NOT_SET) : - TMC6200Driver(cs), BLDCDriver3PWM(phA, phB, phC, en) { }; - ~TMC6200Driver3PWM() override = default; +public: + TMC6200Driver3PWM(int phA, int phB, int phC, int cs, int en = NOT_SET) : + TMC6200Driver(cs), BLDCDriver3PWM(phA, phB, phC, en) {}; - virtual void init(SPIClass* _spi = &SPI) override; -}; + ~TMC6200Driver3PWM() override = default; + virtual void init(SPIClass *_spi = &SPI) override; +}; class TMC6200Driver6PWM : public TMC6200Driver, public BLDCDriver6PWM { - public: - TMC6200Driver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int cs, int en = NOT_SET) : - TMC6200Driver(cs), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) { }; - ~TMC6200Driver6PWM() override = default; +public: + TMC6200Driver6PWM(int phA_h, int phA_l, int phB_h, int phB_l, int phC_h, int phC_l, int cs, int en = NOT_SET) : + TMC6200Driver(cs), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) {}; + + ~TMC6200Driver6PWM() override = default; - virtual void init(SPIClass* _spi = &SPI) override; + virtual void init(SPIClass *_spi = &SPI) override; }; \ No newline at end of file diff --git a/src/drivers/tmc6200/TMC6200_Registers.hpp b/src/drivers/tmc6200/TMC6200_Registers.hpp index 8c7052b..242e37f 100644 --- a/src/drivers/tmc6200/TMC6200_Registers.hpp +++ b/src/drivers/tmc6200/TMC6200_Registers.hpp @@ -10,54 +10,54 @@ // ------ -#define TMC6200_GCONF_REG 0x00 // RW -#define TMC6200_GSTAT_REG 0x01 // RW + WC -#define TMC6200_IOIN_REG 0x04 // R +#define TMC6200_GCONF_REG 0x00 // RW +#define TMC6200_GSTAT_REG 0x01 // RW + WC +#define TMC6200_IOIN_REG 0x04 // R -#define TMC6200_OTP_PROG_REG 0x06 // not used -#define TMC6200_OTP_READ_REG 0x07 // not used -#define TMC6200_FACTORY_CONF_REG 0x08 // not used +#define TMC6200_OTP_PROG_REG 0x06 // not used +#define TMC6200_OTP_READ_REG 0x07 // not used +#define TMC6200_FACTORY_CONF_REG 0x08 // not used -#define TMC6200_SHORT_CONF_REG 0x09 // RW -#define TMC6200_DRV_CONF_REG 0x0A // RW +#define TMC6200_SHORT_CONF_REG 0x09 // RW +#define TMC6200_DRV_CONF_REG 0x0A // RW typedef union { - struct { - uint32_t DISABLE:1; - uint32_t SINGLE_LINE:1; - uint32_t FAULT_DIRECT:1; - uint32_t :1; - uint32_t AMPLIFICATION:2; - uint32_t AMPLIFIER_OFF:1; - uint32_t TEST_MODE:1; - uint32_t :24; - }; - uint32_t reg; + struct { + uint32_t DISABLE: 1; + uint32_t SINGLE_LINE: 1; + uint32_t FAULT_DIRECT: 1; + uint32_t : 1; + uint32_t AMPLIFICATION: 2; + uint32_t AMPLIFIER_OFF: 1; + uint32_t TEST_MODE: 1; + uint32_t : 24; + }; + uint32_t reg; } TMC6200_GCONF; typedef union { - struct { - uint32_t RESET:1; - uint32_t DRV_OTPW:1; - uint32_t DRV_OT:1; - uint32_t UV_CP:1; - - uint32_t SHORT_DET_U:1; - uint32_t S2GU:1; - uint32_t S2VSU:1; - uint32_t :1; - - uint32_t SHORT_DET_V:1; - uint32_t S2GV:1; - uint32_t S2VSV:1; - uint32_t :1; - - uint32_t SHORT_DET_W:1; - uint32_t S2GW:1; - uint32_t S2VSW:1; - uint32_t :1; - }; - uint32_t reg; + struct { + uint32_t RESET: 1; + uint32_t DRV_OTPW: 1; + uint32_t DRV_OT: 1; + uint32_t UV_CP: 1; + + uint32_t SHORT_DET_U: 1; + uint32_t S2GU: 1; + uint32_t S2VSU: 1; + uint32_t : 1; + + uint32_t SHORT_DET_V: 1; + uint32_t S2GV: 1; + uint32_t S2VSV: 1; + uint32_t : 1; + + uint32_t SHORT_DET_W: 1; + uint32_t S2GW: 1; + uint32_t S2VSW: 1; + uint32_t : 1; + }; + uint32_t reg; } TMC6200_GSTAT; typedef union { @@ -82,32 +82,32 @@ typedef union { } TMC6200_IOIN; typedef union { - struct { - uint32_t S2VS_LEVEL:4; - uint32_t :4; - uint32_t S2G_LEVEL:4; - uint32_t :4; - uint32_t SHORT_FILTER:2; - uint32_t :2; - uint32_t SHORT_DELAY:1; - uint32_t :4; - uint32_t RETRY:2; - uint32_t :2; - uint32_t PROTECT_PARALLEL:1; - uint32_t DISABLE_S2G:1; - uint32_t DISABLE_S2VS:1; - uint32_t :2; - }; - uint32_t reg; + struct { + uint32_t S2VS_LEVEL: 4; + uint32_t : 4; + uint32_t S2G_LEVEL: 4; + uint32_t : 4; + uint32_t SHORT_FILTER: 2; + uint32_t : 2; + uint32_t SHORT_DELAY: 1; + uint32_t : 4; + uint32_t RETRY: 2; + uint32_t : 2; + uint32_t PROTECT_PARALLEL: 1; + uint32_t DISABLE_S2G: 1; + uint32_t DISABLE_S2VS: 1; + uint32_t : 2; + }; + uint32_t reg; } TMC6200_SHORT_CONF; typedef union { - struct { - uint32_t BBMCLKS:5; - uint32_t :11; - uint32_t OT_SELECT:2; - uint32_t DRV_STRENGTH:2; - uint32_t :12; - }; - uint32_t reg; + struct { + uint32_t BBMCLKS: 5; + uint32_t : 11; + uint32_t OT_SELECT: 2; + uint32_t DRV_STRENGTH: 2; + uint32_t : 12; + }; + uint32_t reg; } TMC6200_DRV_CONF; \ No newline at end of file From e0c8712690c78231ad7fdb968253e96e98a2982e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 29 Jun 2023 18:05:29 +0200 Subject: [PATCH 21/41] update readmes with TMC6200 contribution --- README.md | 2 ++ src/drivers/tmc6200/README.md | 2 ++ 2 files changed, 4 insertions(+) diff --git a/README.md b/README.md index 64281e1..79cb89d 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ v1.0.5 - Released July 2023, for Simple FOC 2.3.1 What's changed since 1.0.4? - Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) +- Added TMD6200 SPI driver by [@YaseenTwati](https://github.com/YaseenTwati) - New Settings abstraction to load and save SimpleFOC settings and calibration - New Settings driver: SAMDNVMSettingsStorage - SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" @@ -25,6 +26,7 @@ What is here? See the sections below. Each driver or function should come with i ### Motor/Gate driver ICs + - [TMC6200 driver](src/drivers/tmc6200/) - SPI driver for Trinamics TMC6200 motor driver IC. - [DRV8316 driver](src/drivers/drv8316/) - SPI driver for TI's DRV8316 motor driver IC. ### Encoders diff --git a/src/drivers/tmc6200/README.md b/src/drivers/tmc6200/README.md index 8d8450a..b34ba7a 100644 --- a/src/drivers/tmc6200/README.md +++ b/src/drivers/tmc6200/README.md @@ -1,6 +1,8 @@ # TMC6200 SimpleFOC Driver +by [@YaseenTwati](https://github.com/YaseenTwati) + The TMC6200 is a Universal high voltage BLDC/PMSM/Servo MOSFET 3-halfbridge gate-driver with in line motor current sensing. External MOSFETs for up to 100A motor current. From 43313c3164a1038ddaf666e109045b0f9d21460e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 29 Jun 2023 22:51:13 +0200 Subject: [PATCH 22/41] fix AVR compile by removing unneeded header --- src/drivers/tmc6200/TMC6200_Registers.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/tmc6200/TMC6200_Registers.hpp b/src/drivers/tmc6200/TMC6200_Registers.hpp index 242e37f..43a04bc 100644 --- a/src/drivers/tmc6200/TMC6200_Registers.hpp +++ b/src/drivers/tmc6200/TMC6200_Registers.hpp @@ -1,7 +1,5 @@ #pragma once -#include - // ------ #define TMC_WRITE_BIT 0x80 From 7dc77bfab1a96ad9c395ba275c43d1cf5854a565 Mon Sep 17 00:00:00 2001 From: Swapnil Pande Date: Mon, 24 Jul 2023 10:43:47 -0400 Subject: [PATCH 23/41] Negate zero electrical angle if sensor direction is CCW --- src/encoders/calibrated/CalibratedSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 859e919..e3d579c 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -153,7 +153,7 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ if(i==(k*128+96)) { _delay(50); - avg_elec_angle += _normalizeAngle(_wrapped.getMechanicalAngle()*_NPP); + avg_elec_angle += _normalizeAngle(directionSensor*_wrapped.getMechanicalAngle()*_NPP); k += 1; } } From 53b04afe0bc9c315e333987eb17589688e639bbd Mon Sep 17 00:00:00 2001 From: Candas1 Date: Thu, 27 Jul 2023 18:45:46 +0200 Subject: [PATCH 24/41] Update SmoothingSensor.cpp Temporary fix --- src/encoders/smoothing/SmoothingSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/smoothing/SmoothingSensor.cpp b/src/encoders/smoothing/SmoothingSensor.cpp index 86f8282..889432c 100644 --- a/src/encoders/smoothing/SmoothingSensor.cpp +++ b/src/encoders/smoothing/SmoothingSensor.cpp @@ -23,7 +23,7 @@ void SmoothingSensor::update() { // Perform angle prediction, using low-pass filtered velocity. But don't advance more than // pi/3 (equivalent to one step of block commutation) from the last true angle reading. float dt = (_micros() - angle_prev_ts) * 1e-6f; - angle_prev += _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs); + angle_prev -= _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs); // Apply phase correction if needed if (phase_correction != 0) { From 20598c6ee8f3887ae4bc9cf2f78dd3f18b82de36 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 28 Jul 2023 10:25:09 +0200 Subject: [PATCH 25/41] fix SSI data position for mt6701 on teensy --- src/encoders/mt6701/MagneticSensorMT6701SSI.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h index 61d1e6b..32abd1d 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.h +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -10,7 +10,7 @@ #define MT6701_BITORDER MSBFIRST -#if defined(TARGET_RP2040)||defined(ESP_H) +#if defined(TARGET_RP2040)||defined(ESP_H)||defined(CORE_TEENSY) #define MT6701_DATA_POS 1 #else #define MT6701_DATA_POS 2 From b7b81a3e8146a6c7053e5c6976e2f5f2cbcf22cb Mon Sep 17 00:00:00 2001 From: candas Date: Sat, 29 Jul 2023 20:20:44 +0200 Subject: [PATCH 26/41] Fix to take sensor direction into account --- src/encoders/smoothing/SmoothingSensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/encoders/smoothing/SmoothingSensor.cpp b/src/encoders/smoothing/SmoothingSensor.cpp index 889432c..ef0d322 100644 --- a/src/encoders/smoothing/SmoothingSensor.cpp +++ b/src/encoders/smoothing/SmoothingSensor.cpp @@ -23,12 +23,12 @@ void SmoothingSensor::update() { // Perform angle prediction, using low-pass filtered velocity. But don't advance more than // pi/3 (equivalent to one step of block commutation) from the last true angle reading. float dt = (_micros() - angle_prev_ts) * 1e-6f; - angle_prev -= _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs); + angle_prev += _motor.sensor_direction * _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs); // Apply phase correction if needed if (phase_correction != 0) { - if (_motor.shaft_velocity < -0) angle_prev -= phase_correction / _motor.pole_pairs; - else if (_motor.shaft_velocity > 0) angle_prev += phase_correction / _motor.pole_pairs; + if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs; + else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs; } // Handle wraparound of the projected angle From 4ffd626ec3b8d64450f5f17b9a917dbc5a5a4d71 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 17 Aug 2023 21:24:15 +0200 Subject: [PATCH 27/41] fix PWM input to work on channel 2 --- src/utilities/stm32pwm/STM32PWMInput.cpp | 17 +++++++++++++---- src/utilities/stm32pwm/STM32PWMInput.h | 1 + 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/utilities/stm32pwm/STM32PWMInput.cpp b/src/utilities/stm32pwm/STM32PWMInput.cpp index 5d04b4c..c530c04 100644 --- a/src/utilities/stm32pwm/STM32PWMInput.cpp +++ b/src/utilities/stm32pwm/STM32PWMInput.cpp @@ -25,6 +25,9 @@ int STM32PWMInput::initialize(){ timer.Init.Period = 4.294967295E9; // TODO max period, depends on which timer is used - 32 bits or 16 bits timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; timer.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (channel!=1 && channel!=2) // only channels 1 & 2 supported + return -10; + useChannel2 = (channel==2);// remember the channel if (HAL_TIM_Base_Init(&timer) != HAL_OK) { return -1; } @@ -47,7 +50,7 @@ int STM32PWMInput::initialize(){ return -4; } TIM_IC_InitTypeDef icCfg = { - .ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING, + .ICPolarity = (channel==1)?TIM_INPUTCHANNELPOLARITY_RISING:TIM_INPUTCHANNELPOLARITY_FALLING, .ICSelection = (channel==1)?TIM_ICSELECTION_DIRECTTI:TIM_ICSELECTION_INDIRECTTI, .ICPrescaler = TIM_ICPSC_DIV1, .ICFilter = 0 @@ -55,7 +58,7 @@ int STM32PWMInput::initialize(){ if (HAL_TIM_IC_ConfigChannel(&timer, &icCfg, TIM_CHANNEL_1) != HAL_OK) { return -5; } - icCfg.ICPolarity = TIM_INPUTCHANNELPOLARITY_FALLING; + icCfg.ICPolarity = (channel==1)?TIM_INPUTCHANNELPOLARITY_FALLING:TIM_INPUTCHANNELPOLARITY_RISING; icCfg.ICSelection = (channel==1)?TIM_ICSELECTION_INDIRECTTI:TIM_ICSELECTION_DIRECTTI; if (HAL_TIM_IC_ConfigChannel(&timer, &icCfg, TIM_CHANNEL_2) != HAL_OK) { return -6; @@ -86,12 +89,18 @@ float STM32PWMInput::getDutyCyclePercent(){ uint32_t STM32PWMInput::getDutyCycleTicks(){ - return timer.Instance->CCR2; + if (useChannel2) + return timer.Instance->CCR1; + else + return timer.Instance->CCR2; }; uint32_t STM32PWMInput::getPeriodTicks(){ - return timer.Instance->CCR1; + if (useChannel2) + return timer.Instance->CCR2; + else + return timer.Instance->CCR1; }; diff --git a/src/utilities/stm32pwm/STM32PWMInput.h b/src/utilities/stm32pwm/STM32PWMInput.h index 4a1a04d..fcf3acd 100644 --- a/src/utilities/stm32pwm/STM32PWMInput.h +++ b/src/utilities/stm32pwm/STM32PWMInput.h @@ -19,6 +19,7 @@ class STM32PWMInput { PinName _pin; protected: TIM_HandleTypeDef timer; + bool useChannel2 = false; }; From 6f265027c0da67c0cb12fe1814e9c691c6b87d36 Mon Sep 17 00:00:00 2001 From: candas Date: Tue, 29 Aug 2023 19:51:08 +0200 Subject: [PATCH 28/41] Fix --- src/voltage/GenericVoltageSense.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/voltage/GenericVoltageSense.cpp b/src/voltage/GenericVoltageSense.cpp index 20f8ffd..dde1c9b 100644 --- a/src/voltage/GenericVoltageSense.cpp +++ b/src/voltage/GenericVoltageSense.cpp @@ -37,7 +37,7 @@ bool GenericVoltageSense::init(int resolution){ float GenericVoltageSense::readRawVoltage(){ - uint32_t val = analogRead(pin); + float val = analogRead(pin); val = (val / (float)maxValue) * fullScaleVoltage; return val; }; From 06aca924fdb70bf0cc9e4c2ea8ce0ff16741a365 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 10 Sep 2023 20:27:59 +0200 Subject: [PATCH 29/41] swap pow() for _powtwo() --- src/voltage/GenericVoltageSense.cpp | 6 +++--- src/voltage/GenericVoltageSense.h | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/voltage/GenericVoltageSense.cpp b/src/voltage/GenericVoltageSense.cpp index dde1c9b..8dcbe14 100644 --- a/src/voltage/GenericVoltageSense.cpp +++ b/src/voltage/GenericVoltageSense.cpp @@ -21,13 +21,13 @@ bool GenericVoltageSense::init(int resolution){ #ifndef ARDUINO_ARCH_AVR if (resolution>0) { analogReadResolution(resolution); - maxValue = pow(2, resolution); + maxValue = _powtwo(resolution); } else { - maxValue = pow(2, ADC_RESOLUTION); + maxValue = _powtwo(ADC_RESOLUTION); } #else - maxValue = pow(2, ADC_RESOLUTION); + maxValue = _powtwo(ADC_RESOLUTION); #endif return true; }; diff --git a/src/voltage/GenericVoltageSense.h b/src/voltage/GenericVoltageSense.h index 52e4259..673162a 100644 --- a/src/voltage/GenericVoltageSense.h +++ b/src/voltage/GenericVoltageSense.h @@ -3,6 +3,7 @@ #define __GENERICVOLTAGESENSE_H__ #include "./VoltageSense.h" +#include "common/foc_utils.h" class GenericVoltageSense : public VoltageSense { public: From d41cfdf16a6494c9e4f7ab641b56fd9a55635805 Mon Sep 17 00:00:00 2001 From: matei jordache Date: Thu, 14 Sep 2023 17:41:22 -0400 Subject: [PATCH 30/41] add hybrid stepper class --- src/motors/HybridStepperMotor.cpp | 551 ++++++++++++++++++++++++++++++ src/motors/HybridStepperMotor.h | 118 +++++++ 2 files changed, 669 insertions(+) create mode 100644 src/motors/HybridStepperMotor.cpp create mode 100644 src/motors/HybridStepperMotor.h diff --git a/src/motors/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor.cpp new file mode 100644 index 0000000..815ce38 --- /dev/null +++ b/src/motors/HybridStepperMotor.cpp @@ -0,0 +1,551 @@ +#include "HybridStepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + +// HybridStepperMotor(int pp) +// - pp - pole pair number +// - R - motor phase resistance +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance [H] +HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance) + : FOCMotor() +{ + // number od pole pairs + pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + // usually used rms + KV_rating = _KV * _SQRT2; + // save phase inductance + phase_inductance = _inductance; + + // torque control type is voltage by default + // current and foc_current not supported yet + torque_controller = TorqueControlType::voltage; +} + +/** + Link the driver which controls the motor +*/ +void HybridStepperMotor::linkDriver(BLDCDriver *_driver) +{ + driver = _driver; +} + +// init hardware pins +void HybridStepperMotor::init() +{ + if (!driver || !driver->initialized) + { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return; + } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + + // sanity check for the voltage limit configuration + if (voltage_limit > driver->voltage_limit) + voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if (voltage_sensor_align > voltage_limit) + voltage_sensor_align = voltage_limit; + + // update the controller limits + if (_isset(phase_resistance)) + { + // velocity control loop controls current + PID_velocity.limit = current_limit; + } + else + { + PID_velocity.limit = voltage_limit; + } + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + SIMPLEFOC_DEBUG("MOT: Enable driver."); + enable(); + _delay(500); + + motor_status = FOCMotorStatus::motor_uncalibrated; +} + +// disable motor driver +void HybridStepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable driver + driver->disable(); + // motor status update + enabled = 0; +} +// enable motor driver +void HybridStepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + // motor status update + enabled = 1; +} + +/** + FOC functions +*/ +// FOC initialization function +int HybridStepperMotor::initFOC(float zero_electric_offset, Direction _sensor_direction) +{ + int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + + // align motor if necessary + // alignment necessary for encoders! + if (_isset(zero_electric_offset)) + { + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor_direction = _sensor_direction; + } + + // sensor and motor alignment - can be skipped + // by setting motor.sensor_direction and motor.zero_electric_angle + _delay(500); + if (sensor) + { + exit_flag *= alignSensor(); + // added the shaft_angle update + sensor->update(); + shaft_angle = sensor->getAngle(); + } + else + SIMPLEFOC_DEBUG("MOT: No sensor."); + + if (exit_flag) + { + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; + } + else + { + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; + disable(); + } + + return exit_flag; +} + +// Encoder alignment to electrical 0 angle +int HybridStepperMotor::alignSensor() +{ + int exit_flag = 1; // success + SIMPLEFOC_DEBUG("MOT: Align sensor."); + + // if unknown natural direction + if (!_isset(sensor_direction)) + { + // check if sensor needs zero search + if (sensor->needsSearch()) + exit_flag = absoluteZeroSearch(); + // stop init if not found index + if (!exit_flag) + return exit_flag; + + // find natural direction + // move one electrical revolution forward + for (int i = 0; i <= 500; i++) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + // take and angle in the middle + sensor->update(); + float mid_angle = sensor->getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >= 0; i--) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + sensor->update(); + float end_angle = sensor->getAngle(); + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle == end_angle) + { + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); + return 0; // failed calibration + } + else if (mid_angle < end_angle) + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); + sensor_direction = Direction::CCW; + } + else + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); + sensor_direction = Direction::CW; + } + // check pole pair number + float moved = fabs(mid_angle - end_angle); + if (fabs(moved * pole_pairs - _2PI) > 0.5f) + { // 0.5f is arbitrary number it can be lower or higher! + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI / moved); + } + else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } + else + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + + // zero electric angle not known + if (!_isset(zero_electric_angle)) + { + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + _delay(700); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); + _delay(20); + if (monitor_port) + { + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); + } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); + } + else + SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + return exit_flag; +} + +// Encoder alignment the absolute zero angle +// - to the index +int HybridStepperMotor::absoluteZeroSearch() +{ + + SIMPLEFOC_DEBUG("MOT: Index search..."); + // search the absolute zero with small velocity + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; + velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; + shaft_angle = 0; + while (sensor->needsSearch() && shaft_angle < _2PI) + { + angleOpenloop(1.5f * _2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->update(); + } + // disable motor + setPhaseVoltage(0, 0, 0); + // reinit the limits + velocity_limit = limit_vel; + voltage_limit = limit_volt; + // check if the zero found + if (monitor_port) + { + if (sensor->needsSearch()) + SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else + SIMPLEFOC_DEBUG("MOT: Success!"); + } + return !sensor->needsSearch(); +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void HybridStepperMotor::loopFOC() +{ + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) + sensor->update(); + + // if open-loop do nothing + if (controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop) + return; + // shaft angle + shaft_angle = shaftAngle(); + + // if disabled do nothing + if (!enabled) + return; + + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void HybridStepperMotor::move(float new_target) +{ + + // downsampling (optional) + if (motion_cnt++ < motion_downsample) + return; + motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if (controller != MotionControlType::angle_openloop && controller != MotionControlType::velocity_openloop) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if (!enabled) + return; + + // set internal target variable + if (_isset(new_target)) + target = new_target; + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + if (_isset(KV_rating)) + voltage_bemf = shaft_velocity / KV_rating / _RPM_TO_RADS; + // estimate the motor current if phase reistance available and current_sense not available + if (!current_sense && _isset(phase_resistance)) + current.q = (voltage.q - voltage_bemf) / phase_resistance; + + // choose control loop + switch (controller) + { + case MotionControlType::torque: + if (!_isset(phase_resistance)) + voltage.q = target; // if voltage torque control + else + voltage.q = target * phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if (!_isset(phase_inductance)) + voltage.d = 0; + else + voltage.d = _constrain(-target * shaft_velocity * pole_pairs * phase_inductance, -voltage_limit, voltage_limit); + break; + case MotionControlType::angle: + // angle set point + shaft_angle_sp = target; + // calculate velocity set point + shaft_velocity_sp = P_angle(shaft_angle_sp - shaft_angle); + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + // if torque controlled through voltage + // use voltage if phase-resistance not provided + if (!_isset(phase_resistance)) + voltage.q = current_sp; + else + voltage.q = _constrain(current_sp * phase_resistance + voltage_bemf, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if (!_isset(phase_inductance)) + voltage.d = 0; + else + voltage.d = _constrain(-current_sp * shaft_velocity * pole_pairs * phase_inductance, -voltage_limit, voltage_limit); + break; + case MotionControlType::velocity: + // velocity set point + shaft_velocity_sp = target; + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control + // use voltage if phase-resistance not provided + if (!_isset(phase_resistance)) + voltage.q = current_sp; + else + voltage.q = _constrain(current_sp * phase_resistance + voltage_bemf, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if (!_isset(phase_inductance)) + voltage.d = 0; + else + voltage.d = _constrain(-current_sp * shaft_velocity * pole_pairs * phase_inductance, -voltage_limit, voltage_limit); + break; + case MotionControlType::velocity_openloop: + // velocity control in open loop + shaft_velocity_sp = target; + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor + voltage.d = 0; // TODO d-component lag-compensation + break; + case MotionControlType::angle_openloop: + // angle control in open loop + shaft_angle_sp = target; + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor + voltage.d = 0; // TODO d-component lag-compensation + break; + } +} + +void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) +{ + angle_el = _normalizeAngle(angle_el); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + float center; + + switch (foc_modulation) + { + case FOCModulationType::SinePWM: + // C phase is fixed at half-rail to provide bias point for A, B legs + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + center = driver->voltage_limit / 2; + + Ua += center; + Ub += center; + Uc = center; + break; + + case FOCModulationType::SpaceVectorPWM: + // C phase moves in order to increase max bias on coils + uint8_t sector = floor(4.0 * angle_el / _PI) + 1; + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + // Ua = _ca * Ud; + // Ub = _sa * Uq; + + switch (sector) + { + case 1: + case 8: + center = (driver->voltage_limit - Ua - Ub) / 2; + break; + + case 2: + center = (driver->voltage_limit - Ua - 0) / 2; + break; + + case 3: + center = (driver->voltage_limit - Ub - 0) / 2; + break; + + case 4: + case 5: + center = (driver->voltage_limit - Ub - Ua) / 2; + break; + + case 6: + center = (driver->voltage_limit - 0 - Ua) / 2; + break; + + case 7: + center = (driver->voltage_limit - 0 - Ub) / 2; + break; + } + + Ua += center; + Ub += center; + Uc = center; + break; + } + + driver->setPwm(Ua, Ub, Uc); +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +float HybridStepperMotor::velocityOpenloop(float target_velocity) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to achieve target velocity + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts); + // for display purposes + shaft_velocity = target_velocity; + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +float HybridStepperMotor::angleOpenloop(float target_angle) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if (abs(target_angle - shaft_angle) > abs(velocity_limit * Ts)) + { + shaft_angle += _sign(target_angle - shaft_angle) * abs(velocity_limit) * Ts; + shaft_velocity = velocity_limit; + } + else + { + shaft_angle = target_angle; + shaft_velocity = 0; + } + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} diff --git a/src/motors/HybridStepperMotor.h b/src/motors/HybridStepperMotor.h new file mode 100644 index 0000000..214a6ff --- /dev/null +++ b/src/motors/HybridStepperMotor.h @@ -0,0 +1,118 @@ +/** + * @file HybridStepperMotor.h + * + */ + +#ifndef HybridStepperMotor_h +#define HybridStepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class HybridStepperMotor : public FOCMotor +{ +public: + /** + HybridStepperMotor class constructor + @param pp pole pair number + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] + */ + HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver handle for hardware peripheral control + */ + void linkDriver(BLDCDriver *driver); + + /** + * BLDCDriver link: + */ + BLDCDriver *driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + int initFOC(float zero_electric_offset = NOT_SET, Direction sensor_direction = Direction::CW) override; + + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the HybridStepperMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua, Ub, Uc; //!< Phase voltages used for inverse Park and Clarke transform + + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + +private: + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroSearch(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + float velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + float angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + +#endif From bc4b8bcecb14101b60a0d2a8ac7c4985a68eaea8 Mon Sep 17 00:00:00 2001 From: matei jordache Date: Thu, 14 Sep 2023 17:46:03 -0400 Subject: [PATCH 31/41] clean up comments --- src/motors/HybridStepperMotor.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/motors/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor.cpp index 815ce38..e3f6fad 100644 --- a/src/motors/HybridStepperMotor.cpp +++ b/src/motors/HybridStepperMotor.cpp @@ -430,9 +430,8 @@ void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) uint8_t sector = floor(4.0 * angle_el / _PI) + 1; Ua = (_ca * Ud) - (_sa * Uq); Ub = (_sa * Ud) + (_ca * Uq); - // Ua = _ca * Ud; - // Ub = _sa * Uq; + // Determine max/ min of [Ua, Ub, 0] based on phase angle. switch (sector) { case 1: From 50843b55d02939e55063f24fbe198a7c12251e86 Mon Sep 17 00:00:00 2001 From: matei jordache Date: Thu, 14 Sep 2023 19:08:45 -0400 Subject: [PATCH 32/41] add subfolder and readme --- .../HybridStepperMotor.cpp | 0 .../HybridStepperMotor.h | 0 src/motors/HybridStepperMotor/README.md | 45 +++++++++++++++++++ 3 files changed, 45 insertions(+) rename src/motors/{ => HybridStepperMotor}/HybridStepperMotor.cpp (100%) rename src/motors/{ => HybridStepperMotor}/HybridStepperMotor.h (100%) create mode 100644 src/motors/HybridStepperMotor/README.md diff --git a/src/motors/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp similarity index 100% rename from src/motors/HybridStepperMotor.cpp rename to src/motors/HybridStepperMotor/HybridStepperMotor.cpp diff --git a/src/motors/HybridStepperMotor.h b/src/motors/HybridStepperMotor/HybridStepperMotor.h similarity index 100% rename from src/motors/HybridStepperMotor.h rename to src/motors/HybridStepperMotor/HybridStepperMotor.h diff --git a/src/motors/HybridStepperMotor/README.md b/src/motors/HybridStepperMotor/README.md new file mode 100644 index 0000000..4176539 --- /dev/null +++ b/src/motors/HybridStepperMotor/README.md @@ -0,0 +1,45 @@ +# Three Phase / Hybrid Stepper Motor Class + +By tying two phases together, you can use a standard bipolar stepper motor with three-phase drivers! +Of course, it requires some different math for commutation. Another limitation is that with SinePWM the motor voltage is limited to 50% of the supply voltage. Using some tricks in SpaceVectorPWM, this max bias can be increased to about 70%. + +## Warning + +This code has not been tested much! +It has killed one B-G431B-ESC1 board before. +However, it has been succesfully used on a few different hardware platforms, so it does work. +BEMF may be the danger here, so take caution with fast decelerations. +Keep in mind that potentially twice the current can be sunk in the shorted phase as the two other legs. + +## Hardware setup + +Tie two of the phases of the motor together. This is phase "C". The other two phases are A & B. Phase C must go last in the driver constructor. + + +## Software setup + +Usage is quite simple: + +```c++ +#include "Arduino.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "motors/HybridStepperMotor/HybridStepperMotor.h" + +HybridStepperMotor motor = HybridStepperMotor(pole_pairs, phase_resistance, kv, winding_inductance); +BLDCDriver3PWM driver = BLDCDriver3PWM(phase_A, phase_B, phase_C, enable); + +void setup() { + driver.init(); + motor.linkDriver(&driver); + + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; //or SinePWM + motor.init(); + motor.initFOC(); +} + +void loop(){ + motor.loopFOC(); + motor.move(); +} +``` \ No newline at end of file From d4e8318a168280986a75bffa4c339c360f81be29 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Sep 2023 22:19:59 +0200 Subject: [PATCH 33/41] initial driver for AS5600, to be tested --- src/encoders/as5600/AS5600.cpp | 171 +++++++++++++++++++ src/encoders/as5600/AS5600.h | 103 +++++++++++ src/encoders/as5600/MagneticSensorAS5600.cpp | 18 ++ src/encoders/as5600/MagneticSensorAS5600.h | 22 +++ src/encoders/as5600/README.md | 131 ++++++++++++++ 5 files changed, 445 insertions(+) create mode 100644 src/encoders/as5600/AS5600.cpp create mode 100644 src/encoders/as5600/AS5600.h create mode 100644 src/encoders/as5600/MagneticSensorAS5600.cpp create mode 100644 src/encoders/as5600/MagneticSensorAS5600.h create mode 100644 src/encoders/as5600/README.md diff --git a/src/encoders/as5600/AS5600.cpp b/src/encoders/as5600/AS5600.cpp new file mode 100644 index 0000000..f358d4f --- /dev/null +++ b/src/encoders/as5600/AS5600.cpp @@ -0,0 +1,171 @@ + +#include "AS5600.h" + + +AS5600::AS5600(uint8_t address) : _address(address) {}; +AS5600::~AS5600() {}; + +void AS5600::init(TwoWire* wire){ + _wire = wire; + _wire->begin(); + if (!closeTransactions) { + setAngleRegister(); + } +}; + + +void AS5600::setAngleRegister() { + _wire->beginTransmission(_address); + if (useHysteresis) + _wire->write(AS5600_REG_ANGLE); + else + _wire->write(AS5600_REG_ANGLE_RAW); + _wire->endTransmission(false); +} + + +uint16_t AS5600::angle() { + uint16_t result = 0; + if (!closeTransactions) { + setAngleRegister(); + } + _wire->requestFrom(_address, 2, closeTransactions); + result = _wire->read()<<8; + result |= _wire->read(); + return result; +}; + + +uint16_t AS5600::readRawAngle() { + return readRegister(AS5600_REG_ANGLE_RAW, 2); +}; + + +uint16_t AS5600::readAngle() { + return readRegister(AS5600_REG_ANGLE, 2); +}; + + + +uint16_t AS5600::readMagnitude() { + return readRegister(AS5600_REG_MAGNITUDE, 2); +}; + + +AS5600Status AS5600::readStatus() { + AS5600Status result; + result.reg = (uint8_t)readRegister(AS5600_REG_STATUS, 1); // TODO: shift bits around + return result; +}; + + +uint8_t AS5600::readAGC() { + return (uint8_t)readRegister(AS5600_REG_AGC, 1); +}; + + + +AS5600Conf AS5600::readConf() { + AS5600Conf result; + result.reg = readRegister(AS5600_REG_CONF, 2); + return result; +}; + + +uint16_t AS5600::readMang() { + return readRegister(AS5600_REG_MANG, 2); +}; + + +uint16_t AS5600::readMPos() { + return readRegister(AS5600_REG_MPOS, 2); +}; + + +uint16_t AS5600::readZPos() { + return readRegister(AS5600_REG_ZPOS, 2); +}; + + +uint8_t AS5600::readZMCO() { + return (readRegister(AS5600_REG_ZMCO, 1)&0x03); +}; + +uint8_t AS5600::readI2CAddr() { + return (readRegister(AS5600_REG_I2C_ADDR, 1)>>1); +}; + + +// set registers +void AS5600::setConf(AS5600Conf value) { + writeRegister(AS5600_REG_CONF, value.reg); +}; + + +void AS5600::setMang(uint16_t value) { + writeRegister(AS5600_REG_MANG, value); +}; + + +void AS5600::setMPos(uint16_t value) { + writeRegister(AS5600_REG_MPOS, value); +}; + + +void AS5600::setZPos(uint16_t value) { + writeRegister(AS5600_REG_ZPOS, value); +}; + +void AS5600::setI2CAddr(uint8_t value) { + uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CADDR, 1); + val = (value<<1) | (val&0x01); + writeRegister(AS5600_REG_I2CADDR, val); +}; + +void AS5600::setI2CUpdt(uint8_t value) { + uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CUPDT, 1); + val = (value<<1) | (val&0x01); + writeRegister(AS5600_REG_I2CUPDT, val); +}; + + +void AS5600::burnSettings(){ + writeRegister(AS5600_REG_BURN, 0x40, 1); + delay(50); +} + + + +uint16_t AS5600::readRegister(uint8_t reg, uint8_t len){ + uint16_t result = 0; + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->endTransmission(false); + _wire->requestFrom(_address, len, closeTransactions); + if (!closeTransactions) { + setAngleRegister(); + } + result = _wire->read(); + if (len == 2) { + result <<= 8; + result |= _wire->read(); + } + return result; +}; + + + +void AS5600::writeRegister(uint8_t reg, uint16_t val, uint8_t len){ + _wire->beginTransmission(_address); + _wire->write(reg); + if (len == 2) { + _wire->write(val>>8); + } + _wire->write(val&0xFF); + _wire->endTransmission(closeTransactions); + if (!closeTransactions) { + setAngleRegister(); + } +}; + + diff --git a/src/encoders/as5600/AS5600.h b/src/encoders/as5600/AS5600.h new file mode 100644 index 0000000..0242993 --- /dev/null +++ b/src/encoders/as5600/AS5600.h @@ -0,0 +1,103 @@ + +#pragma once + + +#include "Wire.h" + + +#define AS5600_REG_ZMCO 0x00 +#define AS5600_REG_ZPOS 0x01 +#define AS5600_REG_MPOS 0x03 +#define AS5600_REG_MANG 0x05 +#define AS5600_REG_CONF 0x07 + +#define AS5600_REG_I2CADDR 0x20 +#define AS5600_REG_I2CUPDT 0x21 + +#define AS5600_REG_ANGLE 0x0E +#define AS5600_REG_ANGLE_RAW 0x0C + +#define AS5600_REG_STATUS 0x0B +#define AS5600_REG_AGC 0x1A +#define AS5600_REG_MAGNITUDE 0x1B + +#define AS5600_REG_BURN 0xFF + +#define AS5600_CPR (4096.0f) + + +union AS5600Conf { + struct { + uint16_t pm:2; + uint16_t hyst:2; + uint16_t outs:2; + uint16_t pwmf:2; + uint16_t sf:2; + uint16_t fth:3; + uint16_t wd:1; + uint16_t unused:2; + }; + uint16_t reg; +}; + +union AS5600Status { + struct { + uint8_t unused:3; + uint8_t mh:1; + uint8_t ml:1; + uint8_t md:1; + uint8_t unused2:2; + }; + uint8_t reg; +}; + + + + +class AS5600 { +public: + AS5600(uint8_t address = 0x36); + ~AS5600(); + + virtual void init(TwoWire* wire = &Wire); + + // read an angle, either with or without hysteresis, depending on the useHysteresis flag + // and using fast mode (not closing transactions) if so configured + uint16_t angle(); + + // read registers + uint16_t readRawAngle(); + uint16_t readAngle(); + + uint16_t readMagnitude(); + AS5600Status readStatus(); + uint8_t readAGC(); + + AS5600Conf readConf(); + uint16_t readMang(); + uint16_t readMPos(); + uint16_t readZPos(); + uint8_t readZMCO(); + uint8_t readI2CAddr(); + + // set registers + void setConf(AS5600Conf value); + void setMang(uint16_t value); + void setMPos(uint16_t value); + void setZPos(uint16_t value); + void setI2CAddr(uint8_t value); + void setI2CUpdt(uint8_t value); + + void burnSettings(); + + bool closeTransactions = true; + bool useHysteresis = true; + uint8_t _address; +protected: + TwoWire* _wire; + + void setAngleRegister(); + uint16_t readRegister(uint8_t reg, uint8_t len); + void writeRegister(uint8_t reg, uint16_t val, uint8_t len = 2); +}; + diff --git a/src/encoders/as5600/MagneticSensorAS5600.cpp b/src/encoders/as5600/MagneticSensorAS5600.cpp new file mode 100644 index 0000000..8add24a --- /dev/null +++ b/src/encoders/as5600/MagneticSensorAS5600.cpp @@ -0,0 +1,18 @@ + + +#include "./MagneticSensorAS5600.h" +#include "common/foc_utils.h" + +MagneticSensorAS5600::MagneticSensorAS5600() : AS5600(_address) {}; +MagneticSensorAS5600::~MagneticSensorAS5600() {}; + +void MagneticSensorAS5600::init(TwoWire* wire) { + AS5600::init(wire); + Sensor::init(); +}; + + +float MagneticSensorAS5600::getSensorAngle() { + uint16_t raw = readRawAngle(); + return raw / AS5600_CPR * _2PI; +}; \ No newline at end of file diff --git a/src/encoders/as5600/MagneticSensorAS5600.h b/src/encoders/as5600/MagneticSensorAS5600.h new file mode 100644 index 0000000..82a8419 --- /dev/null +++ b/src/encoders/as5600/MagneticSensorAS5600.h @@ -0,0 +1,22 @@ + +#pragma once + + +#include "./AS5600.h" +#include "common/base_classes/Sensor.h" + + +class MagneticSensorAS5600 : public Sensor, public AS5600 { + +public: + MagneticSensorAS5600(uint8_t _address = 0x36); + ~MagneticSensorAS5600(); + + virtual void init(TwoWire* wire = &Wire); + + virtual float getSensorAngle() override; + +protected: + +}; + diff --git a/src/encoders/as5600/README.md b/src/encoders/as5600/README.md new file mode 100644 index 0000000..eb0f37a --- /dev/null +++ b/src/encoders/as5600/README.md @@ -0,0 +1,131 @@ +# AS5600 SimpleFOC driver + +I2C protocol driver for the AMS AS5600 magnetic encoder (digital potentiometer). Also supports the newer AS5600L variant. + +:warning: work in progress + +## Hardware setup + +Connect the sensor to 3.3V or 5V power as appropriate, and to I2C (SDA and SCL). + +Important: please make sure the direction pin (DIR) is either pulled up to VDD, or down to GND. Do not leave the direction pin floating. + +## Software setup + +The sensor driver is easy to use. + +```c++ +#include +#include +#include +#include "encoders/as5600/MagneticSensorAS5600.h" + +MagneticSensorAS5600 sensor; + +void setup() { + sensor.init(); +} + +long ts; + +void loop() { + sensor.update(); + if (millis() - ts > 1000) { + Serial.println(sensor.getAngle(), 3); + ts = millis(); + } + delay(1); +} +``` + +You can use a different I2C bus by passing a pointer to its TwoWire object in the init method: + +```c++ +MagneticSensorAS5600 sensor1; +MagneticSensorAS5600 sensor2; +TwoWire myI2C; + +void setup() { + + ... + + sensor1.init(&myI2C); + sensor2.init(&Wire2); +} +``` + +Using the sensor without releasing the bus should give you considerably more speed. Setting closeTransactions to false means the sensor will not release the bus between angle reads, and also will not re-write the register to the device, which boosts angle reading throughput significantly. + +```c++ +MagneticSensorAS5600 sensor; + +void setup() { + sensor.closeTransactions = false; + sensor.init(); +} +``` + +The sensor's other registers are exposed. Note that using the setter functions to set register values only performs a normal write, not a permanent programing of the register. Permanent programming (BURN function) is not supported by this driver. + +```c++ + +MagneticSensorAS5600 sensor; + +void setup() { + sensor.closeTransactions = false; + sensor.init(); + + uint16_t magnitude = sensor.readMagnitude(); + AS5600Status status = sensor.readStatus(); + // print the values or something +} + +``` + +The AS5600L has a default address of 0x40, and you can set the I2C address. To temporarily change the address (resets to default on restart): + +```c++ + +MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40 + +void setup() { + Serial.begin(115200); + delay(2000); + + sensor.closeTransactions = true; // use normal transactons + sensor.init(); + sensor.setI2CAddr(0x41); // set address to 0x41 + sensor.setI2CUpdt(0x41); // i2c address is now 0x41 + sensor._address = 0x41; + delay(10); + // sensor is using new address + uint16_t magnitude = sensor.readMagnitude(); +} +``` + + +You can program an I2C address permanently (AS5600L only). The address will remain after reset. This programming operation can only be done once. + +```c++ + +MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40 + +void setup() { + Serial.begin(115200); + delay(2000); + + sensor.closeTransactions = true; // use normal transactons + sensor.init(); + + uint8_t addr = sensor.readI2CAddr(); // read I2C address, should be 0x40 + Serial.print("Current Address: "); + Serial.println(addr); + + sensor.setI2CAddr(0x41); // set address to 0x41 + sensor.burnSettings(); // permanently set new address + Serial.println("Permanently changed address to 0x41. Restart the system."); + while (1); +} + + +``` \ No newline at end of file From 6f2ae83a036837412eef9fa6216a4164a9f63052 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Sep 2023 22:21:08 +0200 Subject: [PATCH 34/41] consider MEGAAVR in GenericVoltageSense --- src/voltage/GenericVoltageSense.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/voltage/GenericVoltageSense.cpp b/src/voltage/GenericVoltageSense.cpp index 20f8ffd..f18eaec 100644 --- a/src/voltage/GenericVoltageSense.cpp +++ b/src/voltage/GenericVoltageSense.cpp @@ -18,7 +18,7 @@ GenericVoltageSense::GenericVoltageSense(int pin, float gain, float offset, floa bool GenericVoltageSense::init(int resolution){ pinMode(pin, INPUT_ANALOG); -#ifndef ARDUINO_ARCH_AVR +#if !defined(ARDUINO_ARCH_AVR) && !defined(ARDUINO_ARCH_MEGAAVR) if (resolution>0) { analogReadResolution(resolution); maxValue = pow(2, resolution); From e6a1a7bf888b4d8dff4506768a2f74249bb2fa91 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Sep 2023 22:21:33 +0200 Subject: [PATCH 35/41] fix collision with existing define --- src/drivers/tmc6200/TMC6200_Registers.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/tmc6200/TMC6200_Registers.hpp b/src/drivers/tmc6200/TMC6200_Registers.hpp index 43a04bc..aff9fed 100644 --- a/src/drivers/tmc6200/TMC6200_Registers.hpp +++ b/src/drivers/tmc6200/TMC6200_Registers.hpp @@ -27,7 +27,7 @@ typedef union { uint32_t : 1; uint32_t AMPLIFICATION: 2; uint32_t AMPLIFIER_OFF: 1; - uint32_t TEST_MODE: 1; + uint32_t TMC6200_TEST_MODE: 1; uint32_t : 24; }; uint32_t reg; From 8504629e0c958c85626ede17ec93ed8823c4588a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 22 Sep 2023 22:46:48 +0200 Subject: [PATCH 36/41] update README before release --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 79cb89d..e5f571d 100644 --- a/README.md +++ b/README.md @@ -15,10 +15,12 @@ v1.0.5 - Released July 2023, for Simple FOC 2.3.1 What's changed since 1.0.4? - Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) - Added TMD6200 SPI driver by [@YaseenTwati](https://github.com/YaseenTwati) +- Added HybridStepperMotor by [@VIPQualityPost](https://github.com/VIPQualityPost) - New Settings abstraction to load and save SimpleFOC settings and calibration - New Settings driver: SAMDNVMSettingsStorage - SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" - Updated I2CCommander to use the new registers abstraction +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5+) ## What is included From 3819e044685bc280ffbd50e11838f9f89607da8b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 22 Sep 2023 22:58:38 +0200 Subject: [PATCH 37/41] fix AS5600 compile errors --- src/encoders/as5600/AS5600.cpp | 14 +++++++++----- src/encoders/as5600/MagneticSensorAS5600.cpp | 2 +- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/encoders/as5600/AS5600.cpp b/src/encoders/as5600/AS5600.cpp index f358d4f..84e66e2 100644 --- a/src/encoders/as5600/AS5600.cpp +++ b/src/encoders/as5600/AS5600.cpp @@ -1,5 +1,6 @@ -#include "AS5600.h" + +#include "./AS5600.h" AS5600::AS5600(uint8_t address) : _address(address) {}; @@ -29,7 +30,7 @@ uint16_t AS5600::angle() { if (!closeTransactions) { setAngleRegister(); } - _wire->requestFrom(_address, 2, closeTransactions); + _wire->requestFrom(_address, (uint8_t)2, (uint8_t)closeTransactions); result = _wire->read()<<8; result |= _wire->read(); return result; @@ -92,27 +93,31 @@ uint8_t AS5600::readZMCO() { }; uint8_t AS5600::readI2CAddr() { - return (readRegister(AS5600_REG_I2C_ADDR, 1)>>1); + return (readRegister(AS5600_REG_I2CADDR, 1)>>1); }; // set registers void AS5600::setConf(AS5600Conf value) { + // TODO: read before write writeRegister(AS5600_REG_CONF, value.reg); }; void AS5600::setMang(uint16_t value) { + // TODO: read before write writeRegister(AS5600_REG_MANG, value); }; void AS5600::setMPos(uint16_t value) { + // TODO: read before write writeRegister(AS5600_REG_MPOS, value); }; void AS5600::setZPos(uint16_t value) { + // TODO: read before write writeRegister(AS5600_REG_ZPOS, value); }; @@ -131,7 +136,6 @@ void AS5600::setI2CUpdt(uint8_t value) { void AS5600::burnSettings(){ writeRegister(AS5600_REG_BURN, 0x40, 1); - delay(50); } @@ -141,7 +145,7 @@ uint16_t AS5600::readRegister(uint8_t reg, uint8_t len){ _wire->beginTransmission(_address); _wire->write(reg); _wire->endTransmission(false); - _wire->requestFrom(_address, len, closeTransactions); + _wire->requestFrom(_address, len, (uint8_t)closeTransactions); if (!closeTransactions) { setAngleRegister(); } diff --git a/src/encoders/as5600/MagneticSensorAS5600.cpp b/src/encoders/as5600/MagneticSensorAS5600.cpp index 8add24a..4cea5cf 100644 --- a/src/encoders/as5600/MagneticSensorAS5600.cpp +++ b/src/encoders/as5600/MagneticSensorAS5600.cpp @@ -3,7 +3,7 @@ #include "./MagneticSensorAS5600.h" #include "common/foc_utils.h" -MagneticSensorAS5600::MagneticSensorAS5600() : AS5600(_address) {}; +MagneticSensorAS5600::MagneticSensorAS5600(uint8_t _address) : AS5600(_address) {}; MagneticSensorAS5600::~MagneticSensorAS5600() {}; void MagneticSensorAS5600::init(TwoWire* wire) { From 5b7c7b40c39d8814699a39dd53a10b35504aaac2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 22 Sep 2023 23:09:16 +0200 Subject: [PATCH 38/41] HybridStepper changes for 2.3.1 release --- .../HybridStepperMotor/HybridStepperMotor.cpp | 30 ++++++++----------- .../HybridStepperMotor/HybridStepperMotor.h | 8 +---- 2 files changed, 13 insertions(+), 25 deletions(-) diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp index e3f6fad..8119f7e 100644 --- a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp +++ b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp @@ -94,25 +94,14 @@ void HybridStepperMotor::enable() } /** - FOC functions -*/ -// FOC initialization function -int HybridStepperMotor::initFOC(float zero_electric_offset, Direction _sensor_direction) + * FOC functions + */ +int HybridStepperMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; - // align motor if necessary - // alignment necessary for encoders! - if (_isset(zero_electric_offset)) - { - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); @@ -148,8 +137,7 @@ int HybridStepperMotor::alignSensor() SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction - if (!_isset(sensor_direction)) - { + if(sensor_direction==Direction::UNKNOWN) { // check if sensor needs zero search if (sensor->needsSearch()) exit_flag = absoluteZeroSearch(); @@ -411,8 +399,14 @@ void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) float _sa = _sin(angle_el); float center; - switch (foc_modulation) - { + switch (foc_modulation) { + case FOCModulationType::Trapezoid_120: + case FOCModulationType::Trapezoid_150: + // not handled + Ua = 0; + Ub = 0; + Uc = 0; + break; case FOCModulationType::SinePWM: // C phase is fixed at half-rail to provide bias point for A, B legs Ua = (_ca * Ud) - (_sa * Uq); diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.h b/src/motors/HybridStepperMotor/HybridStepperMotor.h index 214a6ff..71e8792 100644 --- a/src/motors/HybridStepperMotor/HybridStepperMotor.h +++ b/src/motors/HybridStepperMotor/HybridStepperMotor.h @@ -51,14 +51,8 @@ class HybridStepperMotor : public FOCMotor /** * Function initializing FOC algorithm * and aligning sensor's and motors' zero position - * - * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - int initFOC(float zero_electric_offset = NOT_SET, Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time From e096b048f59a6d6f51403697664cd86d1d5a18fb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 16:55:11 +0200 Subject: [PATCH 39/41] omit smoothing example from due test compile --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 5d0fb1a..bd74689 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -30,7 +30,7 @@ jobs: required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: arduino:samd:nano_33_iot required-libraries: Simple FOC sketches-exclude: calibrated smoothing From 081cdd45cfb17159d34762383a0921f17c2efc3c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 16:55:49 +0200 Subject: [PATCH 40/41] include arduino header to prevent error on samd --- src/encoders/as5600/AS5600.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/encoders/as5600/AS5600.h b/src/encoders/as5600/AS5600.h index 0242993..8518c17 100644 --- a/src/encoders/as5600/AS5600.h +++ b/src/encoders/as5600/AS5600.h @@ -2,7 +2,8 @@ #pragma once -#include "Wire.h" +#include +#include #define AS5600_REG_ZMCO 0x00 From ecb3866fbdf57cf43660677a239dbb7c7c6f127a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Sep 2023 17:10:50 +0200 Subject: [PATCH 41/41] solve uninitialized variable error on esp32 --- src/motors/HybridStepperMotor/HybridStepperMotor.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp index 8119f7e..b7fea27 100644 --- a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp +++ b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp @@ -453,6 +453,10 @@ void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) case 7: center = (driver->voltage_limit - 0 - Ub) / 2; break; + + default: // this case does not occur, but compilers complain about uninitialized variables + center = (driver->voltage_limit - 0) / 2; + break; } Ua += center;