From 14b67b0f1352fd9784903a99f0ca063fa1ce18a6 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 1 Dec 2023 23:42:59 +0100 Subject: [PATCH 01/37] prepare for next 1.0.7 release --- README.md | 26 +++++++++++++++++++------- library.properties | 2 +- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 69923ec..d91fcaf 100644 --- a/README.md +++ b/README.md @@ -10,14 +10,19 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.6 - Released December 2023, for Simple FOC 2.3.2 or later +v1.0.7 - Released ??? 2023, for Simple FOC 2.3.3 or later + + +What's changed since 1.0.6? +- Added AS5600 sensor driver +- Added STM32 LPTIM encoder driver +- Refactored communications code +- Working telemetry abstraction +- Working streams communications, ASCII or Binary based +- Refactored settings storage code +- Refactored I2CCommander +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.7+) -What's changed since 1.0.5? -- Added STSPIN32G4 driver -- Added STM32G4 CORDIC code, for greatly accellerated trig functions on supported MCUs -- Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs -- Improvements in the MT6835 sensor driver -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+) ## What is included @@ -102,6 +107,13 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi ## Release History +What's changed since 1.0.5? +- Added STSPIN32G4 driver +- Added STM32G4 CORDIC code, for greatly accellerated trig functions on supported MCUs +- Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs +- Improvements in the MT6835 sensor driver +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+) + 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) diff --git a/library.properties b/library.properties index 8b4fcb8..94db11b 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.6 +version=1.0.7 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. From 3c4ff83dd897c4d0436f3c9fbc9abf2d6411b72e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 13:30:24 +0100 Subject: [PATCH 02/37] fix double precision constant --- src/motors/HybridStepperMotor/HybridStepperMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp index b7fea27..f69a252 100644 --- a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp +++ b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp @@ -421,7 +421,7 @@ void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) case FOCModulationType::SpaceVectorPWM: // C phase moves in order to increase max bias on coils - uint8_t sector = floor(4.0 * angle_el / _PI) + 1; + uint8_t sector = floor(4.0f * angle_el / _PI) + 1; Ua = (_ca * Ud) - (_sa * Uq); Ub = (_sa * Ud) + (_ca * Uq); From c467cb042704b13f72d0444210492f3bfaa68711 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 20 Dec 2023 13:13:44 +0100 Subject: [PATCH 03/37] major refactoring of registers and IO code --- src/comms/RegisterIO.h | 68 +++ src/comms/RegisterReceiver.cpp | 131 ----- src/comms/RegisterReceiver.h | 15 - src/comms/RegisterSender.cpp | 193 -------- src/comms/RegisterSender.h | 18 - src/comms/SimpleFOCRegisters.cpp | 454 ++++++++++++++++++ src/comms/SimpleFOCRegisters.h | 22 + src/comms/i2c/I2CCommander.cpp | 301 ++---------- src/comms/i2c/I2CCommander.h | 16 +- src/comms/i2c/I2CCommanderMaster.h | 7 +- src/comms/serial/README.md | 49 +- src/comms/serial/SerialASCIITelemetry.cpp | 65 --- src/comms/serial/SerialASCIITelemetry.h | 27 -- src/comms/serial/SerialBinaryCommander.cpp | 153 ------ src/comms/serial/SerialBinaryCommander.h | 57 --- src/comms/streams/BinaryIO.cpp | 83 ++++ src/comms/streams/BinaryIO.h | 32 ++ src/comms/streams/PacketCommander.cpp | 107 +++++ src/comms/streams/PacketCommander.h | 44 ++ src/comms/streams/README.md | 75 +++ src/comms/streams/TextIO.cpp | 138 ++++++ src/comms/streams/TextIO.h | 34 ++ src/comms/telemetry/README.md | 12 +- src/comms/telemetry/Telemetry.cpp | 40 +- src/comms/telemetry/Telemetry.h | 26 +- src/settings/SettingsStorage.cpp | 19 +- src/settings/SettingsStorage.h | 10 +- .../i2c/CAT24I2CFlashSettingsStorage.cpp | 32 +- .../i2c/CAT24I2CFlashSettingsStorage.h | 15 +- .../rp2040/RP2040FlashSettingsStorage.cpp | 95 ++++ .../rp2040/RP2040FlashSettingsStorage.h | 33 ++ src/settings/samd/README.md | 37 +- src/settings/samd/SAMDNVMSettingsStorage.cpp | 66 ++- src/settings/samd/SAMDNVMSettingsStorage.h | 19 +- .../stm32/STM32FlashSettingsStorage.cpp | 35 +- .../stm32/STM32FlashSettingsStorage.h | 17 +- 36 files changed, 1447 insertions(+), 1098 deletions(-) create mode 100644 src/comms/RegisterIO.h delete mode 100644 src/comms/RegisterReceiver.cpp delete mode 100644 src/comms/RegisterReceiver.h delete mode 100644 src/comms/RegisterSender.cpp delete mode 100644 src/comms/RegisterSender.h create mode 100644 src/comms/SimpleFOCRegisters.cpp delete mode 100644 src/comms/serial/SerialASCIITelemetry.cpp delete mode 100644 src/comms/serial/SerialASCIITelemetry.h delete mode 100644 src/comms/serial/SerialBinaryCommander.cpp delete mode 100644 src/comms/serial/SerialBinaryCommander.h create mode 100644 src/comms/streams/BinaryIO.cpp create mode 100644 src/comms/streams/BinaryIO.h create mode 100644 src/comms/streams/PacketCommander.cpp create mode 100644 src/comms/streams/PacketCommander.h create mode 100644 src/comms/streams/README.md create mode 100644 src/comms/streams/TextIO.cpp create mode 100644 src/comms/streams/TextIO.h create mode 100644 src/settings/rp2040/RP2040FlashSettingsStorage.cpp create mode 100644 src/settings/rp2040/RP2040FlashSettingsStorage.h diff --git a/src/comms/RegisterIO.h b/src/comms/RegisterIO.h new file mode 100644 index 0000000..203cf37 --- /dev/null +++ b/src/comms/RegisterIO.h @@ -0,0 +1,68 @@ + +#pragma once + +#include + + +class RegisterIO { + public: + virtual ~RegisterIO() = default; + virtual RegisterIO& operator<<(float value) = 0; + virtual RegisterIO& operator<<(uint32_t value) = 0; + virtual RegisterIO& operator<<(uint8_t value) = 0; + virtual RegisterIO& operator>>(float& value) = 0; + virtual RegisterIO& operator>>(uint32_t& value) = 0; + virtual RegisterIO& operator>>(uint8_t& value) = 0; +}; + + +class Packet { +public: + Packet(uint8_t type, uint8_t size = 0) : type(type), payload_size(size) {}; + uint8_t type; + uint8_t payload_size; +}; + + +class Separator { +public: + Separator(char value) : value(value) {}; + char value; +}; + + +enum PacketType : uint8_t { + TELEMETRY_HEADER = 'H', + TELEMETRY = 'T', + REGISTER = 'R', + RESPONSE = 'r', + ALERT = 'A', + DEBUG = 'D', + SYNC = 'S' +}; + + +#define START_PACKET(type, size) Packet(type, size) +#define END_PACKET Packet(0x00,0x00) + + + +class PacketIO : public RegisterIO { + public: + virtual ~PacketIO() = default; + + virtual PacketIO& operator<<(float value) = 0; + virtual PacketIO& operator<<(uint32_t value) = 0; + virtual PacketIO& operator<<(uint8_t value) = 0; + virtual PacketIO& operator>>(float& value) = 0; + virtual PacketIO& operator>>(uint32_t& value) = 0; + virtual PacketIO& operator>>(uint8_t& value) = 0; + + virtual PacketIO& operator<<(char value) = 0; + virtual PacketIO& operator<<(Separator value) = 0; + virtual PacketIO& operator<<(Packet value) = 0; + virtual PacketIO& operator>>(Packet& value) = 0; + virtual bool is_complete() = 0; + + bool in_sync = true; // start in-sync? +}; diff --git a/src/comms/RegisterReceiver.cpp b/src/comms/RegisterReceiver.cpp deleted file mode 100644 index 44f7c54..0000000 --- a/src/comms/RegisterReceiver.cpp +++ /dev/null @@ -1,131 +0,0 @@ - -#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; - - 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 deleted file mode 100644 index fc2af1c..0000000 --- a/src/comms/RegisterReceiver.h +++ /dev/null @@ -1,15 +0,0 @@ - -#pragma once - -#include "./SimpleFOCRegisters.h" -#include "common/base_classes/FOCMotor.h" - - - -class RegisterReceiver { -protected: - 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 deleted file mode 100644 index 2a163ce..0000000 --- a/src/comms/RegisterSender.cpp +++ /dev/null @@ -1,193 +0,0 @@ - -#include "./RegisterSender.h" -#include "common/foc_utils.h" -#include "BLDCMotor.h" - -bool RegisterSender::sendRegister(SimpleFOCRegister reg, FOCMotor* motor){ - // write a register value for the given motor - switch(reg) { - case REG_STATUS: - writeByte(motor->motor_status); - break; - case REG_ENABLE: - writeByte(motor->enabled); - break; - case REG_MODULATION_MODE: - writeByte(motor->foc_modulation); - break; - case REG_TORQUE_MODE: - writeByte(motor->torque_controller); - break; - case REG_CONTROL_MODE: - writeByte(motor->controller); - break; - case REG_TARGET: - writeFloat(motor->target); - break; - case REG_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(motor->shaft_velocity); - break; - case REG_SENSOR_ANGLE: - if (motor->sensor) - writeFloat(motor->sensor->getAngle()); // stored angle - else - writeFloat(motor->shaft_angle); - break; - - case REG_VOLTAGE_Q: - writeFloat(motor->voltage.q); - break; - case REG_VOLTAGE_D: - writeFloat(motor->voltage.d); - break; - case REG_CURRENT_Q: - writeFloat(motor->current.q); - break; - case REG_CURRENT_D: - writeFloat(motor->current.d); - break; - case REG_CURRENT_A: - if (motor->current_sense) - writeFloat(motor->current_sense->getPhaseCurrents().a); - else - writeFloat(0.0f); - break; - case REG_CURRENT_B: - if (motor->current_sense) - writeFloat(motor->current_sense->getPhaseCurrents().b); - else - writeFloat(0.0f); - break; - case REG_CURRENT_C: - if (motor->current_sense) - writeFloat(motor->current_sense->getPhaseCurrents().c); - else - writeFloat(0.0f); - break; - case REG_CURRENT_ABC: - if (motor->current_sense) { - PhaseCurrent_s currents = motor->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(motor->PID_velocity.P); - break; - case REG_VEL_PID_I: - writeFloat(motor->PID_velocity.I); - break; - case REG_VEL_PID_D: - writeFloat(motor->PID_velocity.D); - break; - case REG_VEL_LPF_T: - writeFloat(motor->LPF_velocity.Tf); - break; - case REG_ANG_PID_P: - writeFloat(motor->P_angle.P); - break; - case REG_VEL_LIMIT: - writeFloat(motor->velocity_limit); - break; - case REG_VEL_MAX_RAMP: - writeFloat(motor->PID_velocity.output_ramp); - break; - - case REG_CURQ_PID_P: - writeFloat(motor->PID_current_q.P); - break; - case REG_CURQ_PID_I: - writeFloat(motor->PID_current_q.I); - break; - case REG_CURQ_PID_D: - writeFloat(motor->PID_current_q.D); - break; - case REG_CURQ_LPF_T: - writeFloat(motor->LPF_current_q.Tf); - break; - case REG_CURD_PID_P: - writeFloat(motor->PID_current_d.P); - break; - case REG_CURD_PID_I: - writeFloat(motor->PID_current_d.I); - break; - case REG_CURD_PID_D: - writeFloat(motor->PID_current_d.D); - break; - case REG_CURD_LPF_T: - writeFloat(motor->LPF_current_d.Tf); - break; - - case REG_VOLTAGE_LIMIT: - writeFloat(motor->voltage_limit); - break; - case REG_CURRENT_LIMIT: - writeFloat(motor->current_limit); - break; - case REG_MOTION_DOWNSAMPLE: - writeByte((uint8_t)motor->motion_downsample); - break; - case REG_DRIVER_VOLTAGE_LIMIT: - writeFloat(((BLDCMotor*)motor)->driver->voltage_limit); - break; - case REG_PWM_FREQUENCY: - writeInt(((BLDCMotor*)motor)->driver->pwm_frequency); - break; - - case REG_ZERO_ELECTRIC_ANGLE: - writeFloat(motor->zero_electric_angle); - break; - case REG_SENSOR_DIRECTION: - writeByte((int8_t)motor->sensor_direction); - break; - case REG_ZERO_OFFSET: - writeFloat(motor->sensor_offset); - break; - case REG_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)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: - case REG_REPORT: - case REG_MOTOR_ADDRESS: - case REG_ENABLE_ALL: - default: - writeByte(0); // TODO what to send in this case? - return false; - } - return true; -}; - diff --git a/src/comms/RegisterSender.h b/src/comms/RegisterSender.h deleted file mode 100644 index b2486a1..0000000 --- a/src/comms/RegisterSender.h +++ /dev/null @@ -1,18 +0,0 @@ - -#pragma once - -#include "./SimpleFOCRegisters.h" -#include "common/base_classes/FOCMotor.h" - -/** - * 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 { -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; -}; diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp new file mode 100644 index 0000000..3a8072d --- /dev/null +++ b/src/comms/SimpleFOCRegisters.cpp @@ -0,0 +1,454 @@ + +#include "./SimpleFOCRegisters.h" +#include "BLDCMotor.h" + + +SimpleFOCRegisters::SimpleFOCRegisters(){}; +SimpleFOCRegisters::~SimpleFOCRegisters(){}; + + +// write the register value(s) for the given motor to the given comms object +bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMotor* motor){ + switch(reg) { + case SimpleFOCRegister::REG_STATUS: + comms << (uint8_t)motor->motor_status; + break; + case SimpleFOCRegister::REG_ENABLE: + comms << (uint8_t)motor->enabled; + break; + case SimpleFOCRegister::REG_MODULATION_MODE: + comms << (uint8_t)motor->foc_modulation; + break; + case SimpleFOCRegister::REG_TORQUE_MODE: + comms << (uint8_t)motor->torque_controller; + break; + case SimpleFOCRegister::REG_CONTROL_MODE: + comms << (uint8_t)motor->controller; + break; + case SimpleFOCRegister::REG_TARGET: + comms << motor->target; + break; + case SimpleFOCRegister::REG_ANGLE: + comms << motor->shaft_angle; + break; + case SimpleFOCRegister::REG_POSITION: + if (motor->sensor) { + comms << (uint32_t)motor->sensor->getFullRotations(); + comms << motor->sensor->getMechanicalAngle(); + } + else { + comms << (uint32_t)motor->shaft_angle/_2PI; + comms << (float)fmod(motor->shaft_angle, _2PI); + } + break; + case SimpleFOCRegister::REG_VELOCITY: + comms << motor->shaft_velocity; + break; + case SimpleFOCRegister::REG_SENSOR_ANGLE: + if (motor->sensor) + comms << motor->sensor->getAngle(); // stored angle + else + comms << motor->shaft_angle; + break; + + case SimpleFOCRegister::REG_PHASE_VOLTAGE: + comms << ((BLDCMotor*)motor)->Ua; + comms << ((BLDCMotor*)motor)->Ub; + comms << ((BLDCMotor*)motor)->Uc; + break; + case SimpleFOCRegister::REG_PHASE_STATE: + // TODO implement - need to refactor BLDCDriver to make it accessible + break; + case SimpleFOCRegister::REG_DRIVER_ENABLE: // write only at the moment + return false; + + case SimpleFOCRegister::REG_VOLTAGE_Q: + comms << motor->voltage.q; + break; + case SimpleFOCRegister::REG_VOLTAGE_D: + comms << motor->voltage.d; + break; + case SimpleFOCRegister::REG_CURRENT_Q: + comms << motor->current.q; + break; + case SimpleFOCRegister::REG_CURRENT_D: + comms << motor->current.d; + break; + case SimpleFOCRegister::REG_CURRENT_A: + if (motor->current_sense) + comms << motor->current_sense->getPhaseCurrents().a; + else + comms << 0.0f; + break; + case SimpleFOCRegister::REG_CURRENT_B: + if (motor->current_sense) + comms << motor->current_sense->getPhaseCurrents().b; + else + comms << 0.0f; + break; + case SimpleFOCRegister::REG_CURRENT_C: + if (motor->current_sense) + comms << motor->current_sense->getPhaseCurrents().c; + else + comms << 0.0f; + break; + case SimpleFOCRegister::REG_CURRENT_ABC: + if (motor->current_sense) { + PhaseCurrent_s currents = motor->current_sense->getPhaseCurrents(); + comms << currents.a; + comms << currents.b; + comms << currents.c; + } + else { + comms << 0.0f; + comms << 0.0f; + comms << 0.0f; + } + break; + case SimpleFOCRegister::REG_VEL_PID_P: + comms << motor->PID_velocity.P; + break; + case SimpleFOCRegister::REG_VEL_PID_I: + comms << motor->PID_velocity.I; + break; + case SimpleFOCRegister::REG_VEL_PID_D: + comms << motor->PID_velocity.D; + break; + case SimpleFOCRegister::REG_VEL_LPF_T: + comms << motor->LPF_velocity.Tf; + break; + case SimpleFOCRegister::REG_ANG_PID_P: + comms << motor->P_angle.P; + break; + case SimpleFOCRegister::REG_VEL_LIMIT: + comms << motor->velocity_limit; + break; + case SimpleFOCRegister::REG_VEL_MAX_RAMP: + comms << motor->PID_velocity.output_ramp; + break; + + case SimpleFOCRegister::REG_CURQ_PID_P: + comms << motor->PID_current_q.P; + break; + case SimpleFOCRegister::REG_CURQ_PID_I: + comms << motor->PID_current_q.I; + break; + case SimpleFOCRegister::REG_CURQ_PID_D: + comms << motor->PID_current_q.D; + break; + case SimpleFOCRegister::REG_CURQ_LPF_T: + comms << motor->LPF_current_q.Tf; + break; + case SimpleFOCRegister::REG_CURD_PID_P: + comms << motor->PID_current_d.P; + break; + case SimpleFOCRegister::REG_CURD_PID_I: + comms << motor->PID_current_d.I; + break; + case SimpleFOCRegister::REG_CURD_PID_D: + comms << motor->PID_current_d.D; + break; + case SimpleFOCRegister::REG_CURD_LPF_T: + comms << motor->LPF_current_d.Tf; + break; + + case SimpleFOCRegister::REG_VOLTAGE_LIMIT: + comms << motor->voltage_limit; + break; + case SimpleFOCRegister::REG_CURRENT_LIMIT: + comms << motor->current_limit; + break; + case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE: + comms << (uint8_t)motor->motion_downsample; + break; + case SimpleFOCRegister::REG_DRIVER_VOLTAGE_LIMIT: + comms << ((BLDCMotor*)motor)->driver->voltage_limit; // TODO handle stepper motors + break; + case SimpleFOCRegister::REG_PWM_FREQUENCY: + comms << (uint32_t)((BLDCMotor*)motor)->driver->pwm_frequency; // TODO handle stepper motors + break; + + case SimpleFOCRegister::REG_ZERO_ELECTRIC_ANGLE: + comms << motor->zero_electric_angle; + break; + case SimpleFOCRegister::REG_SENSOR_DIRECTION: + comms << (uint8_t)motor->sensor_direction; + break; + case SimpleFOCRegister::REG_ZERO_OFFSET: + comms << motor->sensor_offset; + break; + case SimpleFOCRegister::REG_PHASE_RESISTANCE: + comms << motor->phase_resistance; + break; + case SimpleFOCRegister::REG_KV: + comms << motor->KV_rating; + break; + case SimpleFOCRegister::REG_INDUCTANCE: + comms << motor->phase_inductance; + break; + case SimpleFOCRegister::REG_POLE_PAIRS: + comms << (uint8_t)motor->pole_pairs; + break; + + case SimpleFOCRegister::REG_SYS_TIME: + // TODO how big is millis()? Same on all platforms? + comms << (uint32_t)(int)millis(); + break; + // unknown register or write only register (no read) or can't handle in superclass + case SimpleFOCRegister::REG_NUM_MOTORS: + case SimpleFOCRegister::REG_REPORT: + case SimpleFOCRegister::REG_MOTOR_ADDRESS: + case SimpleFOCRegister::REG_ENABLE_ALL: + default: + return false; + } + return true; +}; + + +// read the register value(s) for the given motor from the given comms object +bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMotor* motor) { + uint32_t val32; + uint8_t val8 = 0; + switch(reg) { + case REG_ENABLE: + comms >> val8; + (val8>0)?motor->enable():motor->disable(); + return true; + case SimpleFOCRegister::REG_MODULATION_MODE: + comms >> val8; + motor->foc_modulation = static_cast(val8); + return true; + case SimpleFOCRegister::REG_TORQUE_MODE: + comms >> val8; + motor->torque_controller = static_cast(val8); + return true; + case SimpleFOCRegister::REG_CONTROL_MODE: + comms >> val8; + motor->controller = static_cast(val8); + return true; + case SimpleFOCRegister::REG_TARGET: + comms >> (motor->target); + return true; + + case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE: + comms >> val32; + // TODO implement + return true; + case SimpleFOCRegister::REG_TELEMETRY_CTRL: + comms >> val8; + // TODO implement - do we actually need it? + return true; + case SimpleFOCRegister::REG_TELEMETRY_REG: + // TODO implement - difficult to implement it here + return true; + + case SimpleFOCRegister::REG_PHASE_VOLTAGE: + float va, vb, vc; + comms >> va >> vb >> vc; + if (motor->enabled) { + motor->disable(); + ((BLDCMotor*)motor)->driver->enable(); + } + ((BLDCMotor*)motor)->driver->setPwm(va, vb, vc); + return true; + case SimpleFOCRegister::REG_PHASE_STATE: + uint8_t sa, sb, sc; + comms >> sa >> sb >> sc; + ((BLDCMotor*)motor)->driver->setPhaseState((PhaseState)sa, (PhaseState)sb, (PhaseState)sc); + return true; + case SimpleFOCRegister::REG_DRIVER_ENABLE: + comms >> val8; + if (val8>0) + ((BLDCMotor*)motor)->driver->enable(); + else + ((BLDCMotor*)motor)->driver->disable(); + return true; + + case SimpleFOCRegister::REG_VEL_PID_P: + comms >> (motor->PID_velocity.P); + return true; + case SimpleFOCRegister::REG_VEL_PID_I: + comms >> (motor->PID_velocity.I); + return true; + case SimpleFOCRegister::REG_VEL_PID_D: + comms >> (motor->PID_velocity.D); + return true; + case SimpleFOCRegister::REG_VEL_LPF_T: + comms >> (motor->LPF_velocity.Tf); + return true; + case SimpleFOCRegister::REG_ANG_PID_P: + comms >> (motor->P_angle.P); + return true; + case SimpleFOCRegister::REG_VEL_LIMIT: + comms >> (motor->velocity_limit); + return true; + case SimpleFOCRegister::REG_VEL_MAX_RAMP: + comms >> (motor->PID_velocity.output_ramp); + return true; + + case SimpleFOCRegister::REG_CURQ_PID_P: + comms >> (motor->PID_current_q.P); + return true; + case SimpleFOCRegister::REG_CURQ_PID_I: + comms >> (motor->PID_current_q.I); + return true; + case SimpleFOCRegister::REG_CURQ_PID_D: + comms >> (motor->PID_current_q.D); + return true; + case SimpleFOCRegister::REG_CURQ_LPF_T: + comms >> (motor->LPF_current_q.Tf); + return true; + case SimpleFOCRegister::REG_CURD_PID_P: + comms >> (motor->PID_current_d.P); + return true; + case SimpleFOCRegister::REG_CURD_PID_I: + comms >> (motor->PID_current_d.I); + return true; + case SimpleFOCRegister::REG_CURD_PID_D: + comms >> (motor->PID_current_d.D); + return true; + case SimpleFOCRegister::REG_CURD_LPF_T: + comms >> (motor->LPF_current_d.Tf); + return true; + + case SimpleFOCRegister::REG_VOLTAGE_LIMIT: + comms >> (motor->voltage_limit); + return true; + case SimpleFOCRegister::REG_CURRENT_LIMIT: + comms >> (motor->current_limit); + return true; + case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE: + comms >> val8; + motor->motion_downsample = val8; // change to use 4 bytes? + return true; + case SimpleFOCRegister::REG_DRIVER_VOLTAGE_LIMIT: + comms >> (((BLDCMotor*)motor)->driver->voltage_limit); + return true; + case SimpleFOCRegister::REG_PWM_FREQUENCY: + comms >> val32; + ((BLDCMotor*)motor)->driver->pwm_frequency = val32; + return true; + + case SimpleFOCRegister::REG_ZERO_ELECTRIC_ANGLE: + comms >> (motor->zero_electric_angle); + return true; + case SimpleFOCRegister::REG_SENSOR_DIRECTION: + comms >> val8; + motor->sensor_direction = static_cast(val8); + return true; + case SimpleFOCRegister::REG_ZERO_OFFSET: + comms >> (motor->sensor_offset); + return true; + case SimpleFOCRegister::REG_PHASE_RESISTANCE: + comms >> (motor->phase_resistance); + return true; + case SimpleFOCRegister::REG_KV: + comms >> (motor->KV_rating); + return true; + case SimpleFOCRegister::REG_INDUCTANCE: + comms >> (motor->phase_inductance); + return true; + case SimpleFOCRegister::REG_POLE_PAIRS: + comms >> val8; + motor->pole_pairs = val8; + return true; + // unknown register or read-only register (no write) or can't handle in superclass + case SimpleFOCRegister::REG_STATUS: + case SimpleFOCRegister::REG_ANGLE: + case SimpleFOCRegister::REG_POSITION: + case SimpleFOCRegister::REG_VELOCITY: + case SimpleFOCRegister::REG_SENSOR_ANGLE: + case SimpleFOCRegister::REG_VOLTAGE_Q: + case SimpleFOCRegister::REG_VOLTAGE_D: + case SimpleFOCRegister::REG_CURRENT_Q: + case SimpleFOCRegister::REG_CURRENT_D: + case SimpleFOCRegister::REG_CURRENT_A: + case SimpleFOCRegister::REG_CURRENT_B: + case SimpleFOCRegister::REG_CURRENT_C: + case SimpleFOCRegister::REG_CURRENT_ABC: + case SimpleFOCRegister::REG_SYS_TIME: + case SimpleFOCRegister::REG_NUM_MOTORS: + case SimpleFOCRegister::REG_MOTOR_ADDRESS: + case SimpleFOCRegister::REG_ENABLE_ALL: + case SimpleFOCRegister::REG_REPORT: + default: + return false; + } + return false; +}; + + + +// return the size of the register values, when output to comms, in bytes +uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ + switch (reg) { + case SimpleFOCRegister::REG_TARGET: + case SimpleFOCRegister::REG_ANGLE: + case SimpleFOCRegister::REG_VELOCITY: + case SimpleFOCRegister::REG_SENSOR_ANGLE: + case SimpleFOCRegister::REG_VOLTAGE_Q: + case SimpleFOCRegister::REG_VOLTAGE_D: + case SimpleFOCRegister::REG_CURRENT_Q: + case SimpleFOCRegister::REG_CURRENT_D: + case SimpleFOCRegister::REG_CURRENT_A: + case SimpleFOCRegister::REG_CURRENT_B: + case SimpleFOCRegister::REG_CURRENT_C: + case SimpleFOCRegister::REG_VEL_PID_P: + case SimpleFOCRegister::REG_VEL_PID_I: + case SimpleFOCRegister::REG_VEL_PID_D: + case SimpleFOCRegister::REG_VEL_LPF_T: + case SimpleFOCRegister::REG_ANG_PID_P: + case SimpleFOCRegister::REG_VEL_LIMIT: + case SimpleFOCRegister::REG_VEL_MAX_RAMP: + case SimpleFOCRegister::REG_CURQ_PID_P: + case SimpleFOCRegister::REG_CURQ_PID_I: + case SimpleFOCRegister::REG_CURQ_PID_D: + case SimpleFOCRegister::REG_CURQ_LPF_T: + case SimpleFOCRegister::REG_CURD_PID_P: + case SimpleFOCRegister::REG_CURD_PID_I: + case SimpleFOCRegister::REG_CURD_PID_D: + case SimpleFOCRegister::REG_CURD_LPF_T: + case SimpleFOCRegister::REG_VOLTAGE_LIMIT: + case SimpleFOCRegister::REG_CURRENT_LIMIT: + case SimpleFOCRegister::REG_DRIVER_VOLTAGE_LIMIT: + case SimpleFOCRegister::REG_PWM_FREQUENCY: + case SimpleFOCRegister::REG_ZERO_ELECTRIC_ANGLE: + case SimpleFOCRegister::REG_ZERO_OFFSET: + case SimpleFOCRegister::REG_PHASE_RESISTANCE: + case SimpleFOCRegister::REG_KV: + case SimpleFOCRegister::REG_INDUCTANCE: + case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE: + return 4; + case SimpleFOCRegister::REG_SYS_TIME: + return 4; // TODO how big is millis()? Same on all platforms? + case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE: + case SimpleFOCRegister::REG_SENSOR_DIRECTION: + case SimpleFOCRegister::REG_POLE_PAIRS: + case SimpleFOCRegister::REG_STATUS: + case SimpleFOCRegister::REG_ENABLE: + case SimpleFOCRegister::REG_MODULATION_MODE: + case SimpleFOCRegister::REG_TORQUE_MODE: + case SimpleFOCRegister::REG_CONTROL_MODE: + case SimpleFOCRegister::REG_NUM_MOTORS: + case SimpleFOCRegister::REG_MOTOR_ADDRESS: + case SimpleFOCRegister::REG_TELEMETRY_CTRL: + return 1; + case SimpleFOCRegister::REG_POSITION: + return 8; + case SimpleFOCRegister::REG_CURRENT_ABC: + case SimpleFOCRegister::REG_PHASE_VOLTAGE: + return 12; + case SimpleFOCRegister::REG_PHASE_STATE: + return 3; + case SimpleFOCRegister::REG_DRIVER_ENABLE: + case SimpleFOCRegister::REG_TELEMETRY_REG: // TODO depends on telemetry id + case SimpleFOCRegister::REG_REPORT: // size can vary, handled in Commander if used - may discontinue this feature + case SimpleFOCRegister::REG_ENABLE_ALL: // write-only + default: // unknown register or write only register (no output) or can't handle in superclass + return 0; + } +}; + + +SimpleFOCRegisters* SimpleFOCRegisters::regs = new SimpleFOCRegisters(); diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index 6fea825..ae0e993 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -2,6 +2,8 @@ #pragma once #include +#include "common/base_classes/FOCMotor.h" +#include "./RegisterIO.h" // this constant is changed each time the registers definition are changed *in an incompatible way*. This means that just adding new registers @@ -26,6 +28,14 @@ typedef enum : uint8_t { REG_VELOCITY = 0x11, // RO - float REG_SENSOR_ANGLE = 0x12, // RO - float + REG_PHASE_VOLTAGE = 0x16, // R/W - 3xfloat = 12 bytes + REG_PHASE_STATE = 0x17, // R/W - 3 bytes (1 byte per phase) + REG_DRIVER_ENABLE = 0x18, // R/W - 1 byte + + REG_TELEMETRY_REG = 0x1A, // R/W - 1 + n*2 bytes, number of registers (n) = 1 byte + n * (motor number + register number, 1 byte each) + REG_TELEMETRY_CTRL = 0x1B, // R/W - 1 byte + REG_TELEMETRY_DOWNSAMPLE = 0x1C, // R/W - uint32_t + REG_VOLTAGE_Q = 0x20, // RO - float REG_VOLTAGE_D = 0x21, // RO - float REG_CURRENT_Q = 0x22, // RO - float @@ -70,3 +80,15 @@ typedef enum : uint8_t { REG_NUM_MOTORS = 0x70, // RO - 1 byte REG_SYS_TIME = 0x71, // RO - uint32_t } SimpleFOCRegister; + + +class SimpleFOCRegisters { +public: + SimpleFOCRegisters(); + virtual ~SimpleFOCRegisters(); + virtual uint8_t sizeOfRegister(uint8_t reg); + virtual bool registerToComms(RegisterIO& comms, uint8_t reg, FOCMotor* motor); + virtual bool commsToRegister(RegisterIO& comms, uint8_t reg, FOCMotor* motor); + + static SimpleFOCRegisters* regs; +}; diff --git a/src/comms/i2c/I2CCommander.cpp b/src/comms/i2c/I2CCommander.cpp index 737b2ee..2aa8b04 100644 --- a/src/comms/i2c/I2CCommander.cpp +++ b/src/comms/i2c/I2CCommander.cpp @@ -24,24 +24,43 @@ void I2CCommander::addMotor(FOCMotor* motor){ -bool I2CCommander::readBytes(void* valueToSet, uint8_t numBytes){ +uint8_t I2CCommander::readBytes(void* valueToSet, uint8_t numBytes){ if (_wire->available()>=numBytes){ byte* bytes = (byte*)valueToSet; for (int i=0;iread(); - return true; + return numBytes; } commanderror = true; - return false; + return 0; }; +RegisterIO& I2CCommander::operator>>(uint8_t& valueToSet) { + readBytes(&valueToSet, 1); + return *this; +}; +RegisterIO& I2CCommander::operator>>(float& valueToSet) { + readBytes(&valueToSet, 4); + return *this; +}; +RegisterIO& I2CCommander::operator>>(uint32_t& valueToSet) { + readBytes(&valueToSet, 4); + return *this; +}; -void I2CCommander::writeFloat(float value){ +RegisterIO& I2CCommander::operator<<(uint8_t value) { + _wire->write(value); + return *this; +}; +RegisterIO& I2CCommander::operator<<(float value) { _wire->write((byte*)&value, 4); + return *this; }; - - +RegisterIO& I2CCommander::operator<<(uint32_t value) { + _wire->write((byte*)&value, 4); + return *this; +}; void I2CCommander::onReceive(int numBytes){ @@ -109,113 +128,8 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu for (int i=0; i0)?motors[i]->enable():motors[i]->disable(); break; - case REG_ENABLE: - val = _wire->read(); - (val>0)?motors[motorNum]->enable():motors[motorNum]->disable(); - break; - case REG_CONTROL_MODE: - val = _wire->read(); - if (val>=0 && val<=4) // TODO these enums don't have assigned constants - motors[motorNum]->controller = static_cast(val); - else - commanderror = true; - break; - case REG_TORQUE_MODE: - val = _wire->read(); - if (val>=0 && val<=2) - motors[motorNum]->torque_controller = static_cast(val); - else - commanderror = true; - break; - case REG_MODULATION_MODE: - val = _wire->read(); - if (val>=0 && val<=3) - motors[motorNum]->foc_modulation = static_cast(val); - else - commanderror = true; - break; - case REG_TARGET: - readBytes(&(motors[motorNum]->target), 4); - break; - case REG_VEL_PID_P: - readBytes(&(motors[motorNum]->PID_velocity.P), 4); - break; - case REG_VEL_PID_I: - readBytes(&(motors[motorNum]->PID_velocity.I), 4); - break; - case REG_VEL_PID_D: - readBytes(&(motors[motorNum]->PID_velocity.D), 4); - break; - case REG_VEL_LPF_T: - readBytes(&(motors[motorNum]->LPF_velocity.Tf), 4); - break; - case REG_ANG_PID_P: - readBytes(&(motors[motorNum]->P_angle.P), 4); - break; - case REG_VEL_LIMIT: - readBytes(&(motors[motorNum]->velocity_limit), 4); - break; - case REG_VEL_MAX_RAMP: - readBytes(&(motors[motorNum]->PID_velocity.output_ramp), 4); - break; - case REG_CURQ_PID_P: - readBytes(&(motors[motorNum]->PID_current_q.P), 4); - break; - case REG_CURQ_PID_I: - readBytes(&(motors[motorNum]->PID_current_q.I), 4); - break; - case REG_CURQ_PID_D: - readBytes(&(motors[motorNum]->PID_current_q.D), 4); - break; - case REG_CURQ_LPF_T: - readBytes(&(motors[motorNum]->LPF_current_q.Tf), 4); - break; - case REG_CURD_PID_P: - readBytes(&(motors[motorNum]->PID_current_d.P), 4); - break; - case REG_CURD_PID_I: - readBytes(&(motors[motorNum]->PID_current_d.I), 4); - break; - case REG_CURD_PID_D: - readBytes(&(motors[motorNum]->PID_current_d.D), 4); - break; - case REG_CURD_LPF_T: - readBytes(&(motors[motorNum]->LPF_current_d.Tf), 4); - break; - case REG_VOLTAGE_LIMIT: - readBytes(&(motors[motorNum]->voltage_limit), 4); - break; - case REG_CURRENT_LIMIT: - readBytes(&(motors[motorNum]->current_limit), 4); - break; - case REG_MOTION_DOWNSAMPLE: - readBytes(&(motors[motorNum]->motion_downsample), 4); - break; - case REG_ZERO_OFFSET: - readBytes(&(motors[motorNum]->sensor_offset), 4); - break; - // RO registers - 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_ZERO_ELECTRIC_ANGLE: - case REG_SENSOR_DIRECTION: - case REG_POLE_PAIRS: - case REG_PHASE_RESISTANCE: - case REG_NUM_MOTORS: - case REG_SYS_TIME: default: // unknown register - return false; + return SimpleFOCRegisters::regs->commsToRegister(*this, registerNum, motors[motorNum]); } return true; } @@ -257,168 +171,17 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { break; case REG_REPORT: for (int i=0;iwrite(motors[motorNum]->enabled); - break; - case REG_MODULATION_MODE: - _wire->write(motors[motorNum]->foc_modulation); - break; - case REG_TORQUE_MODE: - _wire->write(motors[motorNum]->torque_controller); - break; - case REG_CONTROL_MODE: - _wire->write(motors[motorNum]->controller); - break; - - case REG_TARGET: - writeFloat(motors[motorNum]->target); - break; - case REG_ANGLE: - writeFloat(motors[motorNum]->shaft_angle); + SimpleFOCRegisters::regs->registerToComms(*this, reportRegisters[i], motors[reportMotors[i]]); // send any normal register 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_VOLTAGE_LIMIT: - writeFloat(motors[motorNum]->voltage_limit); - break; - case REG_CURRENT_LIMIT: - writeFloat(motors[motorNum]->current_limit); - break; - case REG_MOTION_DOWNSAMPLE: - _wire->write((int)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: - _wire->write((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: - _wire->write((int)motors[motorNum]->pole_pairs); - break; - - case REG_SYS_TIME: - // TODO how big is millis()? Same on all platforms? - _wire->write((int)millis()); - break; - case REG_NUM_MOTORS: + case REG_NUM_MOTORS: _wire->write(numMotors); break; - - // unknown register or write only register (no read) + // write only register case REG_ENABLE_ALL: - default: - _wire->write(0); // TODO what to send in this case? return false; + // unknown register - not handled here + default: + return SimpleFOCRegisters::regs->registerToComms(*this, registerNum, motors[motorNum]); } return true; } diff --git a/src/comms/i2c/I2CCommander.h b/src/comms/i2c/I2CCommander.h index 65cefde..b978a65 100644 --- a/src/comms/i2c/I2CCommander.h +++ b/src/comms/i2c/I2CCommander.h @@ -1,6 +1,5 @@ -#ifndef SIMPLEFOC_I2CCOMMANDER_H -#define SIMPLEFOC_I2CCOMMANDER_H +#pragma once #include "Arduino.h" #include "Wire.h" @@ -15,7 +14,7 @@ #define I2CCOMMANDER_MAX_REPORT_REGISTERS 8 -class I2CCommander { +class I2CCommander : public RegisterIO { public: I2CCommander(TwoWire* wire = &Wire); ~I2CCommander(); @@ -27,11 +26,17 @@ class I2CCommander { void onRequest(); protected: - void writeFloat(float value); - bool readBytes(void* valueToSet, uint8_t numBytes); virtual bool sendRegister(uint8_t motorNum, uint8_t registerNum); virtual bool receiveRegister(uint8_t motorNum, uint8_t registerNum, int numBytes); + uint8_t readBytes(void* valueToSet, uint8_t numBytes); + RegisterIO& operator<<(float value) override; + RegisterIO& operator<<(uint32_t value) override; + RegisterIO& operator<<(uint8_t value) override; + RegisterIO& operator>>(float& value) override; + RegisterIO& operator>>(uint32_t& value) override; + RegisterIO& operator>>(uint8_t& value) override; + uint8_t _address; TwoWire* _wire; uint8_t numMotors = 0; @@ -46,4 +51,3 @@ class I2CCommander { uint8_t reportRegisters[I2CCOMMANDER_MAX_REPORT_REGISTERS]; }; -#endif diff --git a/src/comms/i2c/I2CCommanderMaster.h b/src/comms/i2c/I2CCommanderMaster.h index 9b52ae7..7fbe9af 100644 --- a/src/comms/i2c/I2CCommanderMaster.h +++ b/src/comms/i2c/I2CCommanderMaster.h @@ -5,6 +5,7 @@ #include #include "../SimpleFOCRegisters.h" +#include "../CommanderMaster.h" #define I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS 4 @@ -15,7 +16,7 @@ typedef struct { } I2CRemoteMotor; -class I2CCommanderMaster { +class I2CCommanderMaster : public CommanderMaster { public: I2CCommanderMaster(int maxMotors = I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS); @@ -23,8 +24,8 @@ class I2CCommanderMaster { void init(); void addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, TwoWire *wire = &Wire); - int writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size); - int readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size); + int writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size) override; + int readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size) override; 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/README.md b/src/comms/serial/README.md index 2e1b733..99660d6 100644 --- a/src/comms/serial/README.md +++ b/src/comms/serial/README.md @@ -1,49 +1,26 @@ -# Serial communications classes +# Serial communications -Serial communications classes for register-based control and telemetry from SimpleFOC. +There are different options for serial communications with SimpleFOC. -## SerialASCIITelemetry +## Commander -:warning: unfinished, untested +Commander is part of the main SimpleFOC library, and enables a simple ASCII command interface to SimpleFOC. It also allows control over the standard motor "monitoring" functionality which can be used for basic telemetry. -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. +Please see our docs on [Commander]() and [Monitoring]() -Usage: +## Telemetry -```c++ +A more complex abstraction which allows you to monitor most of SimpleFOCs parameters and state - anything that can be accessed via one of our [Registers](../). Telemetry data can be output to the serial port, in either ASCII or binary formats. -SerialASCIITelemetry telemetry = SerialASCIITelemetry(); // number of float digits to display +See [Telemetry](../telemetry/) and [Streams Comms](../streams/) for more documentation. -void setup() { - ... - telemetry.addMotor(&motor); - telemetry.setTelemetryRegisters(2, [REG_VELOCITY, REG_VOLTAGE_Q]); - telemetry.init(); - ... -} +## PacketCommander -void loop() { - motor.move(); - motor.loopFOC(); - telemetry.run(); -} -``` +PacketCommander is a serial communications class for register-based control of SimpleFOC. It works with our [Registers](../) and can operate in ASCII or binary mode. It can be used in combination with the Telemetry class. -Some options are supported: +See [PacketCommander](../streams/) -```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 -``` +## Python API - - -## 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 +Our Python API can work via serial port on your PC with any of the above options. See [pysimplefoc](https://github.com/simplefoc/pysimplefoc) \ No newline at end of file diff --git a/src/comms/serial/SerialASCIITelemetry.cpp b/src/comms/serial/SerialASCIITelemetry.cpp deleted file mode 100644 index 084b940..0000000 --- a/src/comms/serial/SerialASCIITelemetry.cpp +++ /dev/null @@ -1,65 +0,0 @@ - -#include "./SerialASCIITelemetry.h" - -SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : Telemetry() { - this->floatPrecision = floatPrecision; -}; - -SerialASCIITelemetry::~SerialASCIITelemetry(){ - -}; - -void SerialASCIITelemetry::init(Print* 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 deleted file mode 100644 index e218f3b..0000000 --- a/src/comms/serial/SerialASCIITelemetry.h +++ /dev/null @@ -1,27 +0,0 @@ - -#pragma once - -#include "Arduino.h" -#include "../telemetry/Telemetry.h" - -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; -}; - - diff --git a/src/comms/serial/SerialBinaryCommander.cpp b/src/comms/serial/SerialBinaryCommander.cpp deleted file mode 100644 index ffa2406..0000000 --- a/src/comms/serial/SerialBinaryCommander.cpp +++ /dev/null @@ -1,153 +0,0 @@ - -#include "SerialBinaryCommander.h" - - - -SerialBinaryCommander::SerialBinaryCommander(bool echo) : Telemetry(), echo(echo) { - -}; - - - -SerialBinaryCommander::~SerialBinaryCommander(){ - -}; - - - -void SerialBinaryCommander::init(Stream &serial) { - _serial = &serial; - this->Telemetry::init(); -}; - - - -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); // TODO call is incorrect - sendRegister(static_cast(registerNum), motors[motorNum]); - endFrame(); - } - else if (command==SERIALBINARY_COMMAND_WRITE) { - setRegister(static_cast(registerNum), motors[motorNum]); - if (echo) { - startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect - sendRegister(static_cast(registerNum), motors[motorNum]); - endFrame(); - } - } - } - // and handle the telemetry - this->Telemetry::run(); -}; - - - -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); -}; - - - - -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(uint8_t* valueToSet){ - return readBytes(valueToSet, 1); -}; - - - -uint8_t SerialBinaryCommander::readFloat(float* valueToSet){ - return readBytes(valueToSet, 4); -}; - - - -uint8_t SerialBinaryCommander::readInt(uint32_t* 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 deleted file mode 100644 index f80ccc4..0000000 --- a/src/comms/serial/SerialBinaryCommander.h +++ /dev/null @@ -1,57 +0,0 @@ - -#pragma once - -#include -#include "../RegisterReceiver.h" -#include "../telemetry/Telemetry.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 Telemetry, public RegisterReceiver { -public: - SerialBinaryCommander(bool echo = false); - virtual ~SerialBinaryCommander(); - - void init(Stream &serial = Serial); - void run(); - - - bool echo; -protected: - 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); - - uint8_t writeByte(uint8_t value) override; - uint8_t writeFloat(float value) override; - uint8_t writeInt(uint32_t value) 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; - - 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]; - - Stream* _serial; -}; diff --git a/src/comms/streams/BinaryIO.cpp b/src/comms/streams/BinaryIO.cpp new file mode 100644 index 0000000..4555c52 --- /dev/null +++ b/src/comms/streams/BinaryIO.cpp @@ -0,0 +1,83 @@ + + +#include "./BinaryIO.h" + +BinaryIO::BinaryIO(Stream& io) : _io(io) { + // nothing to do here +}; + +BinaryIO::~BinaryIO(){ + // nothing to do here +}; + + +BinaryIO& BinaryIO::operator<<(float value) { + _io.write(value); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(uint32_t value) { + _io.print(value); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(uint8_t value) { + _io.write(value); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(char value) { + _io.write((uint8_t)value); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(Packet value) { + if (value.type!=0x00) { + _io.write(MARKER_BYTE); + _io.write(value.payload_size+1); + _io.write(value.type); + } + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(Separator value) { + // separator is ignored in binary mode + return *this; +}; + + + +BinaryIO& BinaryIO::operator>>(float &value) { + _io.readBytes((uint8_t*)&value, 4); + return *this; +}; + + + +BinaryIO& BinaryIO::operator>>(uint32_t &value) { + _io.readBytes((uint8_t*)&value, 4); + return *this; +}; + + + +BinaryIO& BinaryIO::operator>>(uint8_t &value) { + value = (uint8_t)_io.read(); + return *this; +}; + + +bool BinaryIO::is_complete() { + //return remaining <= 0; // TODO write me! + return false; +}; diff --git a/src/comms/streams/BinaryIO.h b/src/comms/streams/BinaryIO.h new file mode 100644 index 0000000..818cff8 --- /dev/null +++ b/src/comms/streams/BinaryIO.h @@ -0,0 +1,32 @@ + + +#pragma once + +#include "../RegisterIO.h" +#include + + +#define MARKER_BYTE 0xA5 + + +class BinaryIO : public PacketIO { + public: + BinaryIO(Stream& io); + virtual ~BinaryIO(); + BinaryIO& operator<<(float value) override; + BinaryIO& operator<<(uint32_t value) override; + BinaryIO& operator<<(uint8_t value) override; + BinaryIO& operator<<(char value) override; + BinaryIO& operator<<(Packet value) override; + BinaryIO& operator<<(Separator value) override; + BinaryIO& operator>>(float& value) override; + BinaryIO& operator>>(uint32_t& value) override; + BinaryIO& operator>>(uint8_t& value) override; + bool is_complete() override; + protected: + Stream& _io; +}; + + + + diff --git a/src/comms/streams/PacketCommander.cpp b/src/comms/streams/PacketCommander.cpp new file mode 100644 index 0000000..4e68a64 --- /dev/null +++ b/src/comms/streams/PacketCommander.cpp @@ -0,0 +1,107 @@ + +#include "PacketCommander.h" + +PacketCommander::PacketCommander(bool echo) : echo(echo) { + // nothing to do +}; + + + +PacketCommander::~PacketCommander() { + // nothing to do +}; + + + + +void PacketCommander::addMotor(FOCMotor* motor) { + if (numMotors < PACKETCOMMANDER_MAX_MOTORS) + motors[numMotors++] = motor; +}; + + + + +void PacketCommander::init(PacketIO& io) { + _io = &io; +}; + + + +void PacketCommander::run() { + // is a packet available? + *_io >> curr_packet; + if (curr_packet.type != 0x00) { + // new packet arrived - the only types of packets we expect to receive are REGISTER packets + if (curr_packet.type == PacketType::REGISTER) { + commanderror = false; + *_io >> curRegister; + handleRegisterPacket(!_io->is_complete(), curRegister); + lastcommanderror = commanderror; + lastcommandregister = curRegister; + } + else if (curr_packet.type == PacketType::SYNC) { + *_io << START_PACKET(PacketType::SYNC, 1); + *_io << (uint8_t)0x01; + *_io << END_PACKET; + } + else + _io->in_sync = false; // TODO flag in another way? + + if (! _io->is_complete()) + _io->in_sync = false; + } +}; + + +void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) { + if (write) { + bool ok = commsToRegister(reg); + commanderror = commanderror && !ok; + } + if (!write || echo) { + uint8_t size = SimpleFOCRegisters::regs->sizeOfRegister(reg); + if (size > 0) { // sendable register + *_io << START_PACKET(PacketType::RESPONSE, size+1) << reg << Separator('='); + // TODO status? + registerToComms(reg); + *_io << END_PACKET; + } + } +} + + + +bool PacketCommander::commsToRegister(uint8_t reg){ + switch (reg) { + case REG_MOTOR_ADDRESS: + uint8_t val; + *_io >> val; + if (val >= numMotors) + commanderror = true; + else + curMotor = val; + return true; + default: + return SimpleFOCRegisters::regs->commsToRegister(*_io, reg, motors[curMotor]); + } +} + + + +bool PacketCommander::registerToComms(uint8_t reg){ + switch (reg) { + case REG_STATUS: + // TODO implement status register + return true; + case REG_MOTOR_ADDRESS: + *_io << curMotor; + return true; + case REG_NUM_MOTORS: + *_io << numMotors; + return true; + default: + return SimpleFOCRegisters::regs->registerToComms(*_io, reg, motors[curMotor]); + } +} + diff --git a/src/comms/streams/PacketCommander.h b/src/comms/streams/PacketCommander.h new file mode 100644 index 0000000..95d81ab --- /dev/null +++ b/src/comms/streams/PacketCommander.h @@ -0,0 +1,44 @@ + + +#pragma once + + +#include "../SimpleFOCRegisters.h" + + +#if !defined(PACKETCOMMANDER_MAX_MOTORS) +#define PACKETCOMMANDER_MAX_MOTORS 4 +#endif + + + +class PacketCommander { +public: + PacketCommander(bool echo = false); + virtual ~PacketCommander(); + + void addMotor(FOCMotor* motor); + + void init(PacketIO& io); + void run(); + + bool echo = true; +protected: + + bool commsToRegister(uint8_t reg); + bool registerToComms(uint8_t reg); + void handleRegisterPacket(bool write, uint8_t reg); + + FOCMotor* motors[PACKETCOMMANDER_MAX_MOTORS]; + uint8_t numMotors = 0; + + uint8_t curMotor = 0; + uint8_t curRegister = REG_STATUS; + + bool commanderror = false; + bool lastcommanderror = false; + uint8_t lastcommandregister = REG_STATUS; + + PacketIO* _io; + Packet curr_packet = Packet(0x00, 0x00); +}; diff --git a/src/comms/streams/README.md b/src/comms/streams/README.md new file mode 100644 index 0000000..b40916b --- /dev/null +++ b/src/comms/streams/README.md @@ -0,0 +1,75 @@ + +# Streams IO + +IO objects which work with Arduino Streams. Used for the Telemetry abstraction, or packet based control of SimpleFOC via PacketCommander. + +This can be used in conjunction with Serial, probably the way most people will use it, but also with other Streams like TCP network connections. + +There are Binary and Text versions of the Streams implementation. Binary will be a bit more compact, but isn't easily readable. + +## Usage + +Use of the IO classes is shown in conjunction with their use in PacketCommander or Telemetry. Please see the docs for those classes. + +### TextIO + +In TextIO, you can set the float precision: + +```c++ +TextIO comms = TextIO(Serial); + +void setup(){ + comms.precision = 6; // print floats with 6 digits after the decimal place +} +``` + +### BinaryIO + +BinaryIO has no options: + +```c++ +BinaryIO comms = BinaryIO(Serial); +``` + +### PacketCommander + +Use PacketCommander in combination with either TextIO (for ASCII text based protocol) or BinaryIO (for a binary protocol) to send control messages to SimpleFOC motors in the form of register reads and writes. + +```c++ +BLDCMotor motor = BLDCMotor(7); // 7 pole pairs +TextIO comms = TextIO(Serial); +PacketCommander packetCommander = PacketCommander(); + +void setup() { + ... + packetCommander.addMotor(&motor); + packetCommander.echo = true; + packetCommander.init(comms); + ... +} + +void run() { + ... + packetCommander.run(); +} +``` + +You can then send messages to PacketCommander like this: + +``` +R2=0 # set motor 0 (note this is default if you only have 1 motor...) +R8=10.0 # set target to 10.0 +R4=1 # enable motor + +R0 # read register 0, the status register +``` + +The PacketCommander will respond with response messages like this: + +``` +r2=0 # motor address is 0 +r8=10.0000 # target is 10 +r4=1 # motor is enabled + +r0=4 # motor status is FOCMotorStatus::motor_ready +``` \ No newline at end of file diff --git a/src/comms/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp new file mode 100644 index 0000000..f692292 --- /dev/null +++ b/src/comms/streams/TextIO.cpp @@ -0,0 +1,138 @@ + +#include "./TextIO.h" +#include "Arduino.h" + +TextIO::TextIO(Stream& io) : _io(io) { + // nothing to do here +}; + +TextIO::~TextIO(){ + // nothing to do here +}; + + +TextIO& TextIO::operator<<(float value) { + if (sep) { + _io.print(","); + } + _io.print(value, precision); + sep = true; + return *this; +}; + + + +TextIO& TextIO::operator<<(uint32_t value) { + if (sep) { + _io.print(","); + } + _io.print(value); + sep = true; + return *this; +}; + + + +TextIO& TextIO::operator<<(uint8_t value) { + if (sep) { + _io.print(","); + } + _io.print(value); + sep = true; + return *this; +}; + + + +TextIO& TextIO::operator<<(char value) { + _io.print(value); + sep = false; + return *this; +}; + + + +TextIO& TextIO::operator<<(Packet value) { + if (value.type==0x00) + _io.println(); + else { + _io.print((char)value.type); + } + sep = false; + return *this; +}; + + + +TextIO& TextIO::operator<<(Separator value) { + _io.print(value.value); + sep = false; + return *this; +}; + + + +TextIO& TextIO::operator>>(float &value) { + if (_io.peek() == '\n') { + return *this; // TODO flag this error + } + if (in_sep) { + _io.read(); // discard the separator + } + value = _io.parseFloat(LookaheadMode::SKIP_NONE); + in_sep = true; + return *this; +}; + +TextIO& TextIO::operator>>(uint32_t &value) { + if (_io.peek() == '\n') { + return *this; // TODO flag this error + } + if (in_sep) { + _io.read(); // discard the separator + } + value = (uint32_t)_io.parseInt(LookaheadMode::SKIP_NONE); + in_sep = true; + return *this; +}; + + + +TextIO& TextIO::operator>>(uint8_t &value) { + if (_io.peek() == '\n') { + return *this; // TODO flag this error + } + if (in_sep) { + _io.read(); // discard the separator + } + value = (uint8_t)_io.parseInt(LookaheadMode::SKIP_NONE); + in_sep = true; + return *this; +}; + + + +TextIO& TextIO::operator>>(Packet &value) { + while (!in_sync && _io.available() > 0) { + if (_io.read() == '\n') + in_sync = true; + } + if (_io.peek() == '\n') { + _io.read(); // discard the \n + } + if (!in_sync || _io.available() < 3) { // frame type, register id, \n to end frame = 3 bytes minimum frame size + value.type = 0x00; + value.payload_size = 0; + return *this; + } + value.type = _io.read(); + value.payload_size = 0; + in_sep = false; + return *this; +}; + + + +bool TextIO::is_complete() { + return _io.peek() == '\n'; +}; \ No newline at end of file diff --git a/src/comms/streams/TextIO.h b/src/comms/streams/TextIO.h new file mode 100644 index 0000000..6651a3a --- /dev/null +++ b/src/comms/streams/TextIO.h @@ -0,0 +1,34 @@ + + +#pragma once + +#include "../RegisterIO.h" +#include + + +class TextIO : public PacketIO { + public: + TextIO(Stream& io); + virtual ~TextIO(); + TextIO& operator<<(float value) override; + TextIO& operator<<(uint32_t value) override; + TextIO& operator<<(uint8_t value) override; + TextIO& operator<<(char value) override; + TextIO& operator<<(Packet value) override; + TextIO& operator<<(Separator value) override; + TextIO& operator>>(float& value) override; + TextIO& operator>>(uint32_t& value) override; + TextIO& operator>>(uint8_t& value) override; + TextIO& operator>>(Packet &value) override; + bool is_complete() override; + + uint8_t precision = 4; + protected: + Stream& _io; + bool sep = false; + bool in_sep = false; +}; + + + + diff --git a/src/comms/telemetry/README.md b/src/comms/telemetry/README.md index 384098f..89e7f8e 100644 --- a/src/comms/telemetry/README.md +++ b/src/comms/telemetry/README.md @@ -1,7 +1,7 @@ # SimpleFOC Telemetry -:warning: unfinished, untested +:warning: new code, not yet fully tested A flexible abstraction for telemetry (monitoring) of SimpleFOC systems. @@ -9,11 +9,7 @@ The telemetry implementation is based on the SimpleFOC registers, and allows you 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 +The telemetry uses the packet based IO, so you can initialize it in either Text or Binary mode. 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. @@ -22,8 +18,8 @@ Multiple motors can be added to the same telemetry, to monitor several motors at Using telemetry is simple: ```c++ - -SerialASCIITelemetry telemetry = SerialASCIITelemetry(); +TextIO io = TextIO(Serial); +Telemetry telemetry = Telemetry(io); ... void setup() { diff --git a/src/comms/telemetry/Telemetry.cpp b/src/comms/telemetry/Telemetry.cpp index 6a6c446..7e8c993 100644 --- a/src/comms/telemetry/Telemetry.cpp +++ b/src/comms/telemetry/Telemetry.cpp @@ -4,7 +4,7 @@ -Telemetry::Telemetry() { +Telemetry::Telemetry(PacketIO& _comms) : comms(_comms) { this->numRegisters = 0; }; @@ -15,7 +15,7 @@ Telemetry::~Telemetry(){ }; - +uint8_t Telemetry::id_seed = 0; void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){ @@ -29,12 +29,17 @@ void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, this->registers_motor[i] = 0; } } + headerSent = false; }; void Telemetry::init() { + this->id = Telemetry::id_seed++; headerSent = false; + if (SimpleFOCRegisters::regs == NULL) { + SimpleFOCRegisters::regs = new SimpleFOCRegisters(); + } }; @@ -46,10 +51,10 @@ void Telemetry::run() { sendHeader(); headerSent = true; } - if (downsampleCnt++ < downsample) return; + if (downsample==0 || downsampleCnt++ < downsample) return; downsampleCnt = 0; if (min_elapsed_time > 0) { - long now = _micros(); + unsigned long now = _micros(); if (now - last_run_time < min_elapsed_time) return; last_run_time = now; } @@ -65,3 +70,30 @@ void Telemetry::addMotor(FOCMotor* motor) { } }; + + +void Telemetry::sendHeader() { + if (numRegisters > 0) { + comms << START_PACKET(PacketType::TELEMETRY_HEADER, 2*numRegisters + 1) << id << Separator('='); + for (uint8_t i = 0; i < numRegisters; i++) { + comms << registers_motor[i] << Separator(':') << registers[i]; + } + comms << END_PACKET; + }; +}; + + + +void Telemetry::sendTelemetry(){ + if (numRegisters > 0) { + uint8_t size = 1; + for (uint8_t i = 0; i < numRegisters; i++) { + size += SimpleFOCRegisters::regs->sizeOfRegister(registers[i]); + } + comms << START_PACKET(PacketType::TELEMETRY, size) << id << Separator('='); + for (uint8_t i = 0; i < numRegisters; i++) { + SimpleFOCRegisters::regs->registerToComms(comms, registers[i], motors[registers_motor[i]]); + }; + comms << END_PACKET; + } +}; diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h index 256036a..6ed63ca 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -1,8 +1,9 @@ #pragma once +#include "common/base_classes/FOCMotor.h" #include "../SimpleFOCRegisters.h" -#include "../RegisterSender.h" +#include "../RegisterIO.h" #ifndef TELEMETRY_MAX_REGISTERS #define TELEMETRY_MAX_REGISTERS 8 @@ -17,16 +18,16 @@ typedef enum : uint8_t { - TELEMETRY_FRAMETYPE_DATA = 0x01, - TELEMETRY_FRAMETYPE_HEADER = 0x02 + TELEMETRY_PACKETTYPE_DATA = 'T', + TELEMETRY_PACKETTYPE_HEADER = 'H' } TelemetryFrameType; -class Telemetry : public RegisterSender { +class Telemetry { public: - Telemetry(); + Telemetry(PacketIO& _comms); virtual ~Telemetry(); virtual void init(); void addMotor(FOCMotor* motor); @@ -36,8 +37,10 @@ class Telemetry : public RegisterSender { uint16_t downsample = DEF_TELEMETRY_DOWNSAMPLE; uint32_t min_elapsed_time = 0; protected: - virtual void sendTelemetry() = 0; - virtual void sendHeader() = 0; + virtual void sendTelemetry(); + virtual void sendHeader(); + + uint8_t id; FOCMotor* motors[TELEMETRY_MAX_MOTORS]; uint8_t numMotors = 0; @@ -45,9 +48,12 @@ class Telemetry : public RegisterSender { uint8_t numRegisters; uint8_t registers[TELEMETRY_MAX_REGISTERS]; uint8_t registers_motor[TELEMETRY_MAX_REGISTERS]; - uint8_t frameSize; + uint8_t packetSize; bool headerSent; - long last_run_time = 0; + unsigned long last_run_time = 0; uint16_t downsampleCnt = 0; -}; + PacketIO& comms; + + static uint8_t id_seed; // TODO how to initialize this? +}; diff --git a/src/settings/SettingsStorage.cpp b/src/settings/SettingsStorage.cpp index 5df2bcf..63a6109 100644 --- a/src/settings/SettingsStorage.cpp +++ b/src/settings/SettingsStorage.cpp @@ -30,7 +30,8 @@ void SettingsStorage::setRegisters(SimpleFOCRegister* registers, int numRegister }; -void SettingsStorage::init() { +void SettingsStorage::init(RegisterIO* comms) { + this->_io = comms; // make sure we have motors and registers if (numMotors < 1) { SimpleFOCDebug::println("SS: no motors"); @@ -47,17 +48,17 @@ void SettingsStorage::init() { SettingsStatus SettingsStorage::loadSettings() { SimpleFOCDebug::println("Loading settings..."); beforeLoad(); - uint8_t magic; readByte(&magic); + uint8_t magic; *_io >> magic; if (magic != SIMPLEFOC_SETTINGS_MAGIC_BYTE) { SimpleFOCDebug::println("No settings found "); return SFOC_SETTINGS_NONE; } - uint8_t rversion; readByte(&rversion); + uint8_t rversion; *_io >> rversion; if (rversion != SIMPLEFOC_REGISTERS_VERSION) { SimpleFOCDebug::println("Registers version mismatch"); return SFOC_SETTINGS_OLD; } - uint8_t version; readByte(&version); + uint8_t version; *_io >> version; if (version != settings_version) { SimpleFOCDebug::println("Settings version mismatch"); return SFOC_SETTINGS_OLD; @@ -69,7 +70,7 @@ SettingsStatus SettingsStorage::loadSettings() { for (int i = 0; i < numRegisters; i++) { SimpleFOCRegister reg = registers[i]; startLoadRegister(reg); - setRegister(reg, motors[m]); + SimpleFOCRegisters::regs->commsToRegister(*_io, reg, motors[m]); endLoadRegister(); } endLoadMotor(); @@ -83,9 +84,9 @@ SettingsStatus SettingsStorage::loadSettings() { SettingsStatus SettingsStorage::saveSettings() { SimpleFOCDebug::println("Saving settings..."); beforeSave(); - writeByte(SIMPLEFOC_SETTINGS_MAGIC_BYTE); - writeByte(SIMPLEFOC_REGISTERS_VERSION); - writeByte(settings_version); + *_io << (uint8_t)SIMPLEFOC_SETTINGS_MAGIC_BYTE; + *_io << (uint8_t)SIMPLEFOC_REGISTERS_VERSION; + *_io << (uint8_t)settings_version; for (int m = 0; m < numMotors; m++) { if (numMotors>1) SimpleFOCDebug::println("Saving settings for motor ", m); @@ -93,7 +94,7 @@ SettingsStatus SettingsStorage::saveSettings() { for (int i = 0; i < numRegisters; i++) { SimpleFOCRegister reg = registers[i]; startSaveRegister(reg); - sendRegister(reg, motors[m]); + SimpleFOCRegisters::regs->registerToComms(*_io, reg, motors[m]); endSaveRegister(); } endSaveMotor(); diff --git a/src/settings/SettingsStorage.h b/src/settings/SettingsStorage.h index 8506c3b..ede1e2e 100644 --- a/src/settings/SettingsStorage.h +++ b/src/settings/SettingsStorage.h @@ -3,8 +3,6 @@ #include "../comms/SimpleFOCRegisters.h" -#include "../comms/RegisterReceiver.h" -#include "../comms/RegisterSender.h" #include "BLDCMotor.h" #define SIMPLEFOC_SETTINGS_MAGIC_BYTE 0x42 @@ -28,7 +26,7 @@ typedef enum : uint8_t { -class SettingsStorage : public RegisterReceiver, public RegisterSender { +class SettingsStorage { public: SettingsStorage(); ~SettingsStorage(); @@ -36,7 +34,7 @@ class SettingsStorage : public RegisterReceiver, public RegisterSender { void addMotor(BLDCMotor* motor); void setRegisters(SimpleFOCRegister* registers, int numRegisters); - virtual void init(); + virtual void init(RegisterIO* comms); SettingsStatus loadSettings(); SettingsStatus saveSettings(); @@ -57,7 +55,6 @@ class SettingsStorage : public RegisterReceiver, public RegisterSender { virtual void startLoadRegister(SimpleFOCRegister reg); virtual void endLoadRegister(); - virtual void beforeLoad(); virtual void afterLoad(); virtual void beforeSave(); @@ -66,5 +63,6 @@ class SettingsStorage : public RegisterReceiver, public RegisterSender { FOCMotor* motors[SIMPLEFOC_SETTINGS_MAX_MOTORS]; int numMotors = 0; SimpleFOCRegister* registers = NULL; - int numRegisters = 0; + int numRegisters = 0; + RegisterIO* _io; }; diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp index 5d4b336..24ebadb 100644 --- a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp @@ -14,45 +14,51 @@ CAT24I2CFlashSettingsStorage::~CAT24I2CFlashSettingsStorage() {}; void CAT24I2CFlashSettingsStorage::init(TwoWire* wire) { - SettingsStorage::init(); + SettingsStorage::init(this); _wire = wire; reset(); }; -uint8_t CAT24I2CFlashSettingsStorage::readByte(uint8_t* valueToSet) { - return readBytes(valueToSet, 1); +RegisterIO& CAT24I2CFlashSettingsStorage::operator>>(uint8_t& valueToSet) { + readBytes(&valueToSet, 1); + return *this; }; -uint8_t CAT24I2CFlashSettingsStorage::readFloat(float* valueToSet) { - return readBytes(valueToSet, 4); +RegisterIO& CAT24I2CFlashSettingsStorage::operator>>(float& valueToSet) { + readBytes(&valueToSet, 4); + return *this; }; -uint8_t CAT24I2CFlashSettingsStorage::readInt(uint32_t* valueToSet) { - return readBytes(valueToSet, 4); +RegisterIO& CAT24I2CFlashSettingsStorage::operator>>(uint32_t& valueToSet) { + readBytes(&valueToSet, 4); + return *this; }; -uint8_t CAT24I2CFlashSettingsStorage::writeByte(uint8_t value) { - return writeBytes(&value, 1); +RegisterIO& CAT24I2CFlashSettingsStorage::operator<<(uint8_t value) { + writeBytes(&value, 1); + return *this; }; -uint8_t CAT24I2CFlashSettingsStorage::writeFloat(float value) { - return writeBytes(&value, 4); +RegisterIO& CAT24I2CFlashSettingsStorage::operator<<(float value) { + writeBytes(&value, 4); + return *this; }; -uint8_t CAT24I2CFlashSettingsStorage::writeInt(uint32_t value) { - return writeBytes(&value, 4); +RegisterIO& CAT24I2CFlashSettingsStorage::operator<<(uint32_t value) { + writeBytes(&value, 4); + return *this; }; diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.h b/src/settings/i2c/CAT24I2CFlashSettingsStorage.h index 4f6c511..2187c2d 100644 --- a/src/settings/i2c/CAT24I2CFlashSettingsStorage.h +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.h @@ -4,7 +4,7 @@ #include "../SettingsStorage.h" #include "Wire.h" -class CAT24I2CFlashSettingsStorage : public SettingsStorage { +class CAT24I2CFlashSettingsStorage : public SettingsStorage, public RegisterIO { public: CAT24I2CFlashSettingsStorage(uint8_t address = 0xA0, uint16_t offset = 0x0); ~CAT24I2CFlashSettingsStorage(); @@ -12,13 +12,12 @@ class CAT24I2CFlashSettingsStorage : public SettingsStorage { 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; + RegisterIO& operator<<(float value) override; + RegisterIO& operator<<(uint32_t value) override; + RegisterIO& operator<<(uint8_t value) override; + RegisterIO& operator>>(float& value) override; + RegisterIO& operator>>(uint32_t& value) override; + RegisterIO& operator>>(uint8_t& value) override; void beforeSave() override; void beforeLoad() override; diff --git a/src/settings/rp2040/RP2040FlashSettingsStorage.cpp b/src/settings/rp2040/RP2040FlashSettingsStorage.cpp new file mode 100644 index 0000000..108b97f --- /dev/null +++ b/src/settings/rp2040/RP2040FlashSettingsStorage.cpp @@ -0,0 +1,95 @@ + +#include "./RP2040FlashSettingsStorage.h" + +#if defined(TARGET_RP2040) + +#include "communication/SimpleFOCDebug.h" + + +RP2040FlashSettingsStorage::RP2040FlashSettingsStorage(uint32_t offset = 0x0) { + _offset = offset; +}; + +RP2040FlashSettingsStorage::~RP2040FlashSettingsStorage() { + +}; + +void RP2040FlashSettingsStorage::init() { + SettingsStorage::init(this); + reset(); +}; + + +void RP2040FlashSettingsStorage::reset() { + _currptr = (uint8_t*)_offset; +}; + + + +void RP2040FlashSettingsStorage::beforeSave() { + reset(); +}; + + +void RP2040FlashSettingsStorage::afterSave() { + +}; + + +void RP2040FlashSettingsStorage::beforeLoad() { + reset(); +}; + + + + + +int RP2040FlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { + uint8_t* bytes = (uint8_t*)valueToSet; + for (int i=0;i>(uint8_t& value) { + uint8_t val; + uint8_t num = readBytes(&val, 1); + if (num==1) + value = val; + return num; +}; + +RegisterIO& RP2040FlashSettingsStorage::operator>>(float& value) { + float val; + uint8_t num = readBytes(&val, 4); + if (num==4) + value = val; + return num; +}; + +RegisterIO& RP2040FlashSettingsStorage::operator>>(uint32_t& value) { + uint32_t val; + uint8_t num = readBytes(&val, 4); + if (num==4) + value = val; + return num; +}; + + +RegisterIO& RP2040FlashSettingsStorage::operator<<(uint8_t value) { + +}; + +RegisterIO& RP2040FlashSettingsStorage::operator<<(float value) { + +}; + +RegisterIO& RP2040FlashSettingsStorage::operator<<(uint32_t value) { + +}; + +#endif \ No newline at end of file diff --git a/src/settings/rp2040/RP2040FlashSettingsStorage.h b/src/settings/rp2040/RP2040FlashSettingsStorage.h new file mode 100644 index 0000000..3cb4543 --- /dev/null +++ b/src/settings/rp2040/RP2040FlashSettingsStorage.h @@ -0,0 +1,33 @@ + +#pragma once + +#if defined(TARGET_RP2040) + +#include "../SettingsStorage.h" + +class RP2040FlashSettingsStorage : public SettingsStorage, public RegisterIO { +public: + RP2040FlashSettingsStorage(uint32_t offset = 0x0); + ~RP2040FlashSettingsStorage(); + + void init(); + +protected: + RegisterIO& operator<<(float value) override; + RegisterIO& operator<<(uint32_t value) override; + RegisterIO& operator<<(uint8_t value) override; + RegisterIO& operator>>(float& value) override; + RegisterIO& operator>>(uint32_t& value) override; + RegisterIO& operator>>(uint8_t& value) override; + + void beforeSave() override; + void afterSave() override; + void beforeLoad() override; + + int readBytes(void* valueToSet, int numBytes); + void reset(); + + uint8_t* _currptr; +}; + +#endif \ No newline at end of file diff --git a/src/settings/samd/README.md b/src/settings/samd/README.md index 7382751..6db5279 100644 --- a/src/settings/samd/README.md +++ b/src/settings/samd/README.md @@ -2,15 +2,18 @@ Driver for storing settings on SAMD21 chips. -There are two options: +There are three 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** \ +2. **Standard Flash** \ +You can write to any part of the flash memory as long as the region is not locked. The disadvantage compared to the RWW Flash is twofold: firstly, writing to this memory cannot be done in parallel to reading from the flash. Secondly, the normal flash memory will be overwritten when flashing new program code, unless you modify your board description files to reserve the memory. + +3. **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. +Note: It's not clear how this method is different to just writing to the flash. It may have to do with the memory locking settings. ## Using RWW flash @@ -30,20 +33,34 @@ 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 +## Using normal flash + +Add a build-flag to your build to set the base address where you want to store to, e.g.: + +`-DSIMPLEFOC_SAMDNVMSETTINGS_BASE=0x3FF00` + +The base address has to be a multiple of the ROWSIZE, which is 256 bytes. Normally, you would use memory at the end of the flash, so (NVMCTRL_FLASH_SIZE - n * ROWSIZE) would be a good setting to use, e.g.: + +`-DSIMPLEFOC_SAMDNVMSETTINGS_BASE=(NVMCTRL_FLASH_SIZE - ROWSIZE)` + +or -TODO implement and test this method +`-DSIMPLEFOC_SAMDNVMSETTINGS_BASE=(NVMCTRL_FLASH_SIZE - 2*ROWSIZE)` + +Then use the settings as normal. + +You'll have to adjust your board files to exclude the chosen flash area in the ldscript to prevent it being erased when you re-program, if you care to keep your settings when reflashing. + +## Using EEPROM emulation 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. +Normally 256 bytes (one flash row) 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. +`-DSIMPLEFOC_SAMDNVMSETTINGS_BASE=0x3FF00` -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. +like for the "Using normal flash" described above. \ No newline at end of file diff --git a/src/settings/samd/SAMDNVMSettingsStorage.cpp b/src/settings/samd/SAMDNVMSettingsStorage.cpp index 5569600..3dfb22e 100644 --- a/src/settings/samd/SAMDNVMSettingsStorage.cpp +++ b/src/settings/samd/SAMDNVMSettingsStorage.cpp @@ -16,7 +16,7 @@ SAMDNVMSettingsStorage::SAMDNVMSettingsStorage(uint32_t offset) { SAMDNVMSettingsStorage::~SAMDNVMSettingsStorage(){}; void SAMDNVMSettingsStorage::init(){ - SettingsStorage::init(); + SettingsStorage::init(this); reset(); }; @@ -42,12 +42,24 @@ void SAMDNVMSettingsStorage::beforeSave(){ NVMCTRL->CTRLB.reg |= NVMCTRL_CTRLB_MANW; while (NVMCTRL->INTFLAG.bit.READY == 0) {}; // unlock region - user can do it with fuses + // ret_status_code = nvm_execute_command(NVM_COMMAND_UNLOCK_REGION, SIMPLEFOC_SAMDNVMSETTINGS_BASE, 0); + // if (ret_status_code != STATUS_OK) + // SimpleFOCDebug::println("NVM unlock error ", ret_status_code); + // SimpleFOCDebug::println("NVM lock: ", NVMCTRL->LOCK.reg); // erase rows // TODO handle case that it is more than one row - NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + uint8_t cmd; + if (SIMPLEFOC_SAMDNVMSETTINGS_BASE >= RWWEE_BASE) { + cmd = 0x1A; + NVMCTRL->ADDR.reg = ((uint32_t)_currptr - SIMPLEFOC_SAMDNVMSETTINGS_BASE) / 2; + } + else { + cmd = NVMCTRL_CTRLA_CMD_ER; + NVMCTRL->ADDR.reg = ((uint32_t)_currptr) / 2; + } NVMCTRL->STATUS.reg |= NVMCTRL_STATUS_MASK; NVMCTRL->INTFLAG.bit.ERROR = 0; - NVMCTRL->CTRLA.reg = 0x1A | NVMCTRL_CTRLA_CMDEX_KEY; + NVMCTRL->CTRLA.reg = cmd | NVMCTRL_CTRLA_CMDEX_KEY; delay(1); while (NVMCTRL->INTFLAG.bit.READY == 0) {}; if (NVMCTRL->INTFLAG.bit.ERROR == 1) @@ -73,7 +85,7 @@ void SAMDNVMSettingsStorage::flushPage(){ _writeBuffer[_writeIndex++] = 0xFF; } // erase page buffer - // NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + // NVMCTRL->ADDR.reg = ((uint32_t)_currptr - SIMPLEFOC_SAMDNVMSETTINGS_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 @@ -83,10 +95,18 @@ void SAMDNVMSettingsStorage::flushPage(){ *dst++ = *src++; } // write page - NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + uint8_t cmd; + if (SIMPLEFOC_SAMDNVMSETTINGS_BASE >= RWWEE_BASE) { + cmd = 0x1C; + NVMCTRL->ADDR.reg = ((uint32_t)_currptr - SIMPLEFOC_SAMDNVMSETTINGS_BASE) / 2; + } + else { + cmd = NVMCTRL_CTRLA_CMD_WP; + NVMCTRL->ADDR.reg = ((uint32_t)_currptr) / 2; + } NVMCTRL->STATUS.reg |= NVMCTRL_STATUS_MASK; NVMCTRL->INTFLAG.bit.ERROR = 0; - NVMCTRL->CTRLA.reg = 0x1C | NVMCTRL_CTRLA_CMDEX_KEY; + NVMCTRL->CTRLA.reg = cmd | NVMCTRL_CTRLA_CMDEX_KEY; delay(1); while (NVMCTRL->INTFLAG.bit.READY == 0) {}; if (NVMCTRL->INTFLAG.bit.ERROR == 1) @@ -121,44 +141,48 @@ int SAMDNVMSettingsStorage::writeBytes(void* value, int numBytes) { -uint8_t SAMDNVMSettingsStorage::readByte(uint8_t* valueToSet){ + +RegisterIO& SAMDNVMSettingsStorage::operator>>(uint8_t& value){ uint8_t val; uint8_t num = readBytes(&val, 1); if (num==1) - *valueToSet = val; - return num; + value = val; + return *this; }; -uint8_t SAMDNVMSettingsStorage::readFloat(float* valueToSet){ +RegisterIO& SAMDNVMSettingsStorage::operator>>(float& value){ float val; uint8_t num = readBytes(&val, 4); if (num==4) - *valueToSet = val; - return num; + value = val; + return *this; }; -uint8_t SAMDNVMSettingsStorage::readInt(uint32_t* valueToSet){ +RegisterIO& SAMDNVMSettingsStorage::operator>>(uint32_t& value){ uint32_t val; uint8_t num = readBytes(&val, 4); if (num==4) - *valueToSet = val; - return num; + value = val; + return *this; }; -uint8_t SAMDNVMSettingsStorage::writeByte(uint8_t value){ - return writeBytes(&value, 1); +RegisterIO& SAMDNVMSettingsStorage::operator<<(uint8_t value){ + writeBytes(&value, 1); + return *this; }; -uint8_t SAMDNVMSettingsStorage::writeFloat(float value){ - return writeBytes(&value, 4); +RegisterIO& SAMDNVMSettingsStorage::operator<<(float value){ + writeBytes(&value, 4); + return *this; }; -uint8_t SAMDNVMSettingsStorage::writeInt(uint32_t value){ - return writeBytes(&value, 4); +RegisterIO& SAMDNVMSettingsStorage::operator<<(uint32_t value){ + writeBytes(&value, 4); + return *this; }; #endif diff --git a/src/settings/samd/SAMDNVMSettingsStorage.h b/src/settings/samd/SAMDNVMSettingsStorage.h index 6e8a855..0977057 100644 --- a/src/settings/samd/SAMDNVMSettingsStorage.h +++ b/src/settings/samd/SAMDNVMSettingsStorage.h @@ -30,8 +30,6 @@ * * Don't save to NVM unnecessarily. It has a limited number of write cycles, and writing takes time. * - * - * */ @@ -48,21 +46,20 @@ #endif -class SAMDNVMSettingsStorage : public SettingsStorage { +class SAMDNVMSettingsStorage : public SettingsStorage, public RegisterIO { public: SAMDNVMSettingsStorage(uint32_t offset = 0x0); ~SAMDNVMSettingsStorage(); - void init() override; + void init(); 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; + RegisterIO& operator<<(float value) override; + RegisterIO& operator<<(uint32_t value) override; + RegisterIO& operator<<(uint8_t value) override; + RegisterIO& operator>>(float& value) override; + RegisterIO& operator>>(uint32_t& value) override; + RegisterIO& operator>>(uint8_t& value) override; void beforeSave() override; void afterSave() override; diff --git a/src/settings/stm32/STM32FlashSettingsStorage.cpp b/src/settings/stm32/STM32FlashSettingsStorage.cpp index 27899e0..1402127 100644 --- a/src/settings/stm32/STM32FlashSettingsStorage.cpp +++ b/src/settings/stm32/STM32FlashSettingsStorage.cpp @@ -17,7 +17,7 @@ STM32FlashSettingsStorage::~STM32FlashSettingsStorage(){}; void STM32FlashSettingsStorage::init(){ if (!IS_FLASH_PROGRAM_ADDRESS(_address)) SimpleFOCDebug::println("SS: Invalid address"); - SettingsStorage::init(); + SettingsStorage::init(this); reset(); }; @@ -111,44 +111,47 @@ int STM32FlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { } -uint8_t STM32FlashSettingsStorage::readByte(uint8_t* valueToSet) { +RegisterIO& STM32FlashSettingsStorage::operator>>(uint8_t& value) { uint8_t val; uint8_t num = readBytes(&val, 1); if (num==1) - *valueToSet = val; - return num; + value = val; + return *this; }; -uint8_t STM32FlashSettingsStorage::readFloat(float* valueToSet) { +RegisterIO& STM32FlashSettingsStorage::operator>>(float& value) { float val; uint8_t num = readBytes(&val, 4); if (num==4) - *valueToSet = val; - return num; + value = val; + return *this; }; -uint8_t STM32FlashSettingsStorage::readInt(uint32_t* valueToSet) { +RegisterIO& STM32FlashSettingsStorage::operator>>(uint32_t& value) { uint32_t val; uint8_t num = readBytes(&val, 4); if (num==4) - *valueToSet = val; - return num; + value = val; + return *this; }; -uint8_t STM32FlashSettingsStorage::writeByte(uint8_t value) { - return writeBytes(&value, 1); +RegisterIO& STM32FlashSettingsStorage::operator<<(uint8_t value) { + writeBytes(&value, 1); + return *this; }; -uint8_t STM32FlashSettingsStorage::writeFloat(float value) { - return writeBytes(&value, 4); +RegisterIO& STM32FlashSettingsStorage::operator<<(float value) { + writeBytes(&value, 4); + return *this; }; -uint8_t STM32FlashSettingsStorage::writeInt(uint32_t value) { - return writeBytes(&value, 4); +RegisterIO& STM32FlashSettingsStorage::operator<<(uint32_t value) { + writeBytes(&value, 4); + return *this; }; #endif \ No newline at end of file diff --git a/src/settings/stm32/STM32FlashSettingsStorage.h b/src/settings/stm32/STM32FlashSettingsStorage.h index e66e68e..2e7c52f 100644 --- a/src/settings/stm32/STM32FlashSettingsStorage.h +++ b/src/settings/stm32/STM32FlashSettingsStorage.h @@ -8,23 +8,22 @@ #define PAGE_OF(x) (((uint32_t)x - FLASH_BASE) / FLASH_PAGE_SIZE) -class STM32FlashSettingsStorage : public SettingsStorage { +class STM32FlashSettingsStorage : public SettingsStorage, public RegisterIO { public: STM32FlashSettingsStorage(uint32_t address = FLASH_BASE + FLASH_SIZE - FLASH_PAGE_SIZE); ~STM32FlashSettingsStorage(); - void init() override; + void init(); uint32_t _bank = FLASH_BANK_1; // TODO also support bank 2 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; + RegisterIO& operator<<(float value) override; + RegisterIO& operator<<(uint32_t value) override; + RegisterIO& operator<<(uint8_t value) override; + RegisterIO& operator>>(float& value) override; + RegisterIO& operator>>(uint32_t& value) override; + RegisterIO& operator>>(uint8_t& value) override; void beforeLoad() override; void beforeSave() override; From c7e8f897c8f676c2ac8da206141fa20ea97ae7f8 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 21 Dec 2023 23:32:09 +0100 Subject: [PATCH 04/37] added teleplot telemetry --- src/comms/telemetry/README.md | 4 +-- src/comms/telemetry/Telemetry.cpp | 17 ++++++----- src/comms/telemetry/Telemetry.h | 6 ++-- src/comms/telemetry/TeleplotTelemetry.cpp | 36 +++++++++++++++++++++++ src/comms/telemetry/TeleplotTelemetry.h | 18 ++++++++++++ 5 files changed, 68 insertions(+), 13 deletions(-) create mode 100644 src/comms/telemetry/TeleplotTelemetry.cpp create mode 100644 src/comms/telemetry/TeleplotTelemetry.h diff --git a/src/comms/telemetry/README.md b/src/comms/telemetry/README.md index 89e7f8e..8f8a650 100644 --- a/src/comms/telemetry/README.md +++ b/src/comms/telemetry/README.md @@ -19,14 +19,14 @@ Using telemetry is simple: ```c++ TextIO io = TextIO(Serial); -Telemetry telemetry = Telemetry(io); +Telemetry telemetry = Telemetry(); ... void setup() { ... telemetry.addMotor(&motor); telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY }); - telemetry.init(); + telemetry.init(io); ... } diff --git a/src/comms/telemetry/Telemetry.cpp b/src/comms/telemetry/Telemetry.cpp index 7e8c993..3b13ee9 100644 --- a/src/comms/telemetry/Telemetry.cpp +++ b/src/comms/telemetry/Telemetry.cpp @@ -4,7 +4,7 @@ -Telemetry::Telemetry(PacketIO& _comms) : comms(_comms) { +Telemetry::Telemetry() { this->numRegisters = 0; }; @@ -34,7 +34,8 @@ void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, -void Telemetry::init() { +void Telemetry::init(PacketIO& _comms) { + comms = &_comms; this->id = Telemetry::id_seed++; headerSent = false; if (SimpleFOCRegisters::regs == NULL) { @@ -74,11 +75,11 @@ void Telemetry::addMotor(FOCMotor* motor) { void Telemetry::sendHeader() { if (numRegisters > 0) { - comms << START_PACKET(PacketType::TELEMETRY_HEADER, 2*numRegisters + 1) << id << Separator('='); + *comms << START_PACKET(PacketType::TELEMETRY_HEADER, 2*numRegisters + 1) << id << Separator('='); for (uint8_t i = 0; i < numRegisters; i++) { - comms << registers_motor[i] << Separator(':') << registers[i]; + *comms << registers_motor[i] << Separator(':') << registers[i]; } - comms << END_PACKET; + *comms << END_PACKET; }; }; @@ -90,10 +91,10 @@ void Telemetry::sendTelemetry(){ for (uint8_t i = 0; i < numRegisters; i++) { size += SimpleFOCRegisters::regs->sizeOfRegister(registers[i]); } - comms << START_PACKET(PacketType::TELEMETRY, size) << id << Separator('='); + *comms << START_PACKET(PacketType::TELEMETRY, size) << id << Separator('='); for (uint8_t i = 0; i < numRegisters; i++) { - SimpleFOCRegisters::regs->registerToComms(comms, registers[i], motors[registers_motor[i]]); + SimpleFOCRegisters::regs->registerToComms(*comms, registers[i], motors[registers_motor[i]]); }; - comms << END_PACKET; + *comms << END_PACKET; } }; diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h index 6ed63ca..70b618e 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -27,9 +27,9 @@ typedef enum : uint8_t { class Telemetry { public: - Telemetry(PacketIO& _comms); + Telemetry(); virtual ~Telemetry(); - virtual void init(); + virtual void init(PacketIO& _comms); void addMotor(FOCMotor* motor); void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); void run(); @@ -53,7 +53,7 @@ class Telemetry { unsigned long last_run_time = 0; uint16_t downsampleCnt = 0; - PacketIO& comms; + PacketIO* comms; static uint8_t id_seed; // TODO how to initialize this? }; diff --git a/src/comms/telemetry/TeleplotTelemetry.cpp b/src/comms/telemetry/TeleplotTelemetry.cpp new file mode 100644 index 0000000..3329645 --- /dev/null +++ b/src/comms/telemetry/TeleplotTelemetry.cpp @@ -0,0 +1,36 @@ + +#include "./TeleplotTelemetry.h" + + +TeleplotTelemetry::TeleplotTelemetry(){ +}; + + +TeleplotTelemetry::~TeleplotTelemetry(){ + // nothing to do here +}; + + +void TeleplotTelemetry::init(TextIO& _comms){ + this->Telemetry::init(_comms); +}; + + +void TeleplotTelemetry::sendTelemetry(){ + if (numRegisters > 0) { + for (uint8_t i = 0; i < numRegisters; i++) { + *comms << '>'; + if (registers_motor[i] != 0) { + *comms << registers_motor[i] << '_'; + } + *comms << registers[i] << ':'; + SimpleFOCRegisters::regs->registerToComms(*comms, registers[i], motors[registers_motor[i]]); + *comms << '\n'; + }; + } +}; + + +void TeleplotTelemetry::sendHeader(){ + // don't do anything +}; diff --git a/src/comms/telemetry/TeleplotTelemetry.h b/src/comms/telemetry/TeleplotTelemetry.h new file mode 100644 index 0000000..4a4e763 --- /dev/null +++ b/src/comms/telemetry/TeleplotTelemetry.h @@ -0,0 +1,18 @@ + + +#pragma once + +#include "./Telemetry.h" +#include "../streams/TextIO.h" + +class TeleplotTelemetry : public Telemetry { + public: + TeleplotTelemetry(); + virtual ~TeleplotTelemetry(); + + void init(TextIO& _comms); + + protected: + void sendTelemetry() override; + void sendHeader() override; +}; \ No newline at end of file From 852dea4b951e7d9e4de9e7c83ef9835e7aa8c4a3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Dec 2023 04:19:51 +0100 Subject: [PATCH 05/37] fixing bugs in stream based IO --- src/comms/SimpleFOCRegisters.cpp | 61 +++++++++++++++++-- src/comms/SimpleFOCRegisters.h | 1 + src/comms/streams/BinaryIO.cpp | 78 +++++++++++++++++++++---- src/comms/streams/BinaryIO.h | 14 +++++ src/comms/streams/PacketCommander.cpp | 2 + src/comms/telemetry/README.md | 69 +++++++++++++++++++++- src/comms/telemetry/SimpleTelemetry.cpp | 42 +++++++++++++ src/comms/telemetry/SimpleTelemetry.h | 18 ++++++ src/comms/telemetry/Telemetry.cpp | 32 +++++++--- src/comms/telemetry/Telemetry.h | 14 ++++- 10 files changed, 304 insertions(+), 27 deletions(-) create mode 100644 src/comms/telemetry/SimpleTelemetry.cpp create mode 100644 src/comms/telemetry/SimpleTelemetry.h diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index 3a8072d..9b31229 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -1,6 +1,7 @@ #include "./SimpleFOCRegisters.h" #include "BLDCMotor.h" +#include "./telemetry/Telemetry.h" SimpleFOCRegisters::SimpleFOCRegisters(){}; @@ -51,6 +52,37 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto comms << motor->shaft_angle; break; + case SimpleFOCRegister::REG_TELEMETRY_REG: + if (Telemetry::num_telemetry > 0){ + Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl]; + comms << telemetry->numRegisters; + for (uint8_t i=0; inumRegisters; i++) { + comms << telemetry->registers[i]; + comms << telemetry->registers_motor[i]; + } + telemetry->headerSent = false; + } + else { + comms << (uint32_t)0; + } + break; + case SimpleFOCRegister::REG_TELEMETRY_CTRL: + comms << (Telemetry::telemetry_ctrl); + break; + case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE: + if (Telemetry::num_telemetry > 0) + comms << (uint32_t)(Telemetry::telemetries[Telemetry::telemetry_ctrl]->downsample); + else + comms << (uint32_t)0; + break; + case SimpleFOCRegister::REG_ITERATIONS_SEC: + if (Telemetry::num_telemetry > 0) + comms << (Telemetry::telemetries[0]->last_iterations); + else + comms << (uint32_t)0; + break; + + case SimpleFOCRegister::REG_PHASE_VOLTAGE: comms << ((BLDCMotor*)motor)->Ua; comms << ((BLDCMotor*)motor)->Ub; @@ -233,14 +265,27 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE: comms >> val32; - // TODO implement + if (Telemetry::telemetry_ctrl < Telemetry::num_telemetry) + Telemetry::telemetries[Telemetry::telemetry_ctrl]->downsample = (uint16_t)val32; return true; case SimpleFOCRegister::REG_TELEMETRY_CTRL: comms >> val8; - // TODO implement - do we actually need it? + if (val8 < Telemetry::num_telemetry) + Telemetry::telemetry_ctrl = val8; return true; case SimpleFOCRegister::REG_TELEMETRY_REG: - // TODO implement - difficult to implement it here + if (Telemetry::telemetry_ctrl < Telemetry::num_telemetry){ + Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl]; + uint8_t numRegisters; + comms >> numRegisters; + uint8_t registers[numRegisters]; + uint8_t motors[numRegisters]; + for (uint8_t i=0; i> registers[i]; + comms >> motors[i]; + } + telemetry->setTelemetryRegisters(numRegisters, registers, motors); + } return true; case SimpleFOCRegister::REG_PHASE_VOLTAGE: @@ -354,6 +399,7 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto motor->pole_pairs = val8; return true; // unknown register or read-only register (no write) or can't handle in superclass + case SimpleFOCRegister::REG_ITERATIONS_SEC: case SimpleFOCRegister::REG_STATUS: case SimpleFOCRegister::REG_ANGLE: case SimpleFOCRegister::REG_POSITION: @@ -419,6 +465,7 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ case SimpleFOCRegister::REG_KV: case SimpleFOCRegister::REG_INDUCTANCE: case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE: + case SimpleFOCRegister::REG_ITERATIONS_SEC: return 4; case SimpleFOCRegister::REG_SYS_TIME: return 4; // TODO how big is millis()? Same on all platforms? @@ -441,8 +488,14 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ return 12; case SimpleFOCRegister::REG_PHASE_STATE: return 3; + case SimpleFOCRegister::REG_TELEMETRY_REG: + if (Telemetry::num_telemetry > 0) { + Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl]; + return 2*telemetry->numRegisters + 1; + } + else + return 0; case SimpleFOCRegister::REG_DRIVER_ENABLE: - case SimpleFOCRegister::REG_TELEMETRY_REG: // TODO depends on telemetry id case SimpleFOCRegister::REG_REPORT: // size can vary, handled in Commander if used - may discontinue this feature case SimpleFOCRegister::REG_ENABLE_ALL: // write-only default: // unknown register or write only register (no output) or can't handle in superclass diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index ae0e993..0f59edb 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -35,6 +35,7 @@ typedef enum : uint8_t { REG_TELEMETRY_REG = 0x1A, // R/W - 1 + n*2 bytes, number of registers (n) = 1 byte + n * (motor number + register number, 1 byte each) REG_TELEMETRY_CTRL = 0x1B, // R/W - 1 byte REG_TELEMETRY_DOWNSAMPLE = 0x1C, // R/W - uint32_t + REG_ITERATIONS_SEC = 0x1D, // RO - uint32_t REG_VOLTAGE_Q = 0x20, // RO - float REG_VOLTAGE_D = 0x21, // RO - float diff --git a/src/comms/streams/BinaryIO.cpp b/src/comms/streams/BinaryIO.cpp index 4555c52..ed8a681 100644 --- a/src/comms/streams/BinaryIO.cpp +++ b/src/comms/streams/BinaryIO.cpp @@ -12,28 +12,28 @@ BinaryIO::~BinaryIO(){ BinaryIO& BinaryIO::operator<<(float value) { - _io.write(value); + _buff((uint8_t*)&value, 4); return *this; }; BinaryIO& BinaryIO::operator<<(uint32_t value) { - _io.print(value); + _buff((uint8_t*)&value, 4); return *this; }; BinaryIO& BinaryIO::operator<<(uint8_t value) { - _io.write(value); + _buff(value); return *this; }; BinaryIO& BinaryIO::operator<<(char value) { - _io.write((uint8_t)value); + _buff((uint8_t)value); return *this; }; @@ -41,9 +41,9 @@ BinaryIO& BinaryIO::operator<<(char value) { BinaryIO& BinaryIO::operator<<(Packet value) { if (value.type!=0x00) { - _io.write(MARKER_BYTE); - _io.write(value.payload_size+1); - _io.write(value.type); + _buff(MARKER_BYTE); + _buff(value.payload_size+1); + _buff(value.type); } return *this; }; @@ -57,15 +57,48 @@ BinaryIO& BinaryIO::operator<<(Separator value) { + + +void BinaryIO::_buff(uint8_t* data, uint8_t size) { + for (uint8_t i=0; i= BINARYIO_BUFFER_SIZE) { + _flush(); + } + _buffer[_pos++] = data[i]; + } +}; + + +void BinaryIO::_buff(uint8_t data) { + if (_pos >= BINARYIO_BUFFER_SIZE) { + _flush(); + } + _buffer[_pos++] = data; +}; + + + +void BinaryIO::_flush() { + if (_pos>0) { + _io.write(_buffer, _pos); + _pos = 0; + } +}; + + + + + + BinaryIO& BinaryIO::operator>>(float &value) { - _io.readBytes((uint8_t*)&value, 4); + remaining -= _io.readBytes((uint8_t*)&value, 4); return *this; }; BinaryIO& BinaryIO::operator>>(uint32_t &value) { - _io.readBytes((uint8_t*)&value, 4); + remaining -= _io.readBytes((uint8_t*)&value, 4); return *this; }; @@ -73,11 +106,34 @@ BinaryIO& BinaryIO::operator>>(uint32_t &value) { BinaryIO& BinaryIO::operator>>(uint8_t &value) { value = (uint8_t)_io.read(); + remaining--; + return *this; +}; + + + +PacketIO& BinaryIO::operator>>(Packet& value) { + while (!in_sync && _io.available() > 0) { + if (_io.read() == MARKER_BYTE) + in_sync = true; + } + if (_io.peek() == MARKER_BYTE) { + _io.read(); // discard the \n + } + if (!in_sync || _io.available() < 3) { // size, frame type, payload = 3 bytes minimum frame size + value.type = 0x00; + value.payload_size = 0; + return *this; + } + value.payload_size = (uint8_t)_io.read() - 1; + value.type = (uint8_t)_io.read(); + remaining = value.payload_size; return *this; }; + bool BinaryIO::is_complete() { - //return remaining <= 0; // TODO write me! - return false; + return (remaining <= 0); }; + diff --git a/src/comms/streams/BinaryIO.h b/src/comms/streams/BinaryIO.h index 818cff8..b406318 100644 --- a/src/comms/streams/BinaryIO.h +++ b/src/comms/streams/BinaryIO.h @@ -8,6 +8,13 @@ #define MARKER_BYTE 0xA5 +// standard buffer size is 64 bytes, to match the FS USB packet transfer size +// the BinaryIO will send the data in chunks of this size, which should greatly +// improve the performance compared to the ASCII version, which will do a lot of +// converting data to ASCII, and then write in many small writes. +#ifndef BINARYIO_BUFFER_SIZE +#define BINARYIO_BUFFER_SIZE 64 +#endif class BinaryIO : public PacketIO { public: @@ -22,9 +29,16 @@ class BinaryIO : public PacketIO { BinaryIO& operator>>(float& value) override; BinaryIO& operator>>(uint32_t& value) override; BinaryIO& operator>>(uint8_t& value) override; + PacketIO& operator>>(Packet& value) override; bool is_complete() override; protected: + void _buff(uint8_t* data, uint8_t size); + void _buff(uint8_t data); + void _flush(); Stream& _io; + uint8_t remaining = 0; + uint8_t _pos = 0; + uint8_t _buffer[BINARYIO_BUFFER_SIZE]; }; diff --git a/src/comms/streams/PacketCommander.cpp b/src/comms/streams/PacketCommander.cpp index 4e68a64..5fa2cba 100644 --- a/src/comms/streams/PacketCommander.cpp +++ b/src/comms/streams/PacketCommander.cpp @@ -44,6 +44,7 @@ void PacketCommander::run() { *_io << START_PACKET(PacketType::SYNC, 1); *_io << (uint8_t)0x01; *_io << END_PACKET; + // TODO flush packet } else _io->in_sync = false; // TODO flag in another way? @@ -66,6 +67,7 @@ void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) { // TODO status? registerToComms(reg); *_io << END_PACKET; + // TODO flush packet } } } diff --git a/src/comms/telemetry/README.md b/src/comms/telemetry/README.md index 8f8a650..e8858d6 100644 --- a/src/comms/telemetry/README.md +++ b/src/comms/telemetry/README.md @@ -9,10 +9,46 @@ The telemetry implementation is based on the SimpleFOC registers, and allows you The concept allows you to choose registers which are then sent by the telemetry automatically, on a regular schedule. -The telemetry uses the packet based IO, so you can initialize it in either Text or Binary mode. - 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. +# SimpleTelemetry + +Suitable when you want to plot your telemetry values using the arduino serial plotter, or similar tools. + +The chosen register values will be printed as ASCII text, all on the same line, seperated by tabs and terminated by a newline character. + +Note that this will most likely not work with registers that return more than one value, as these will be comma-separated. + +## Usage + +Using SimpleTelemetry: + +```c++ +TextIO io = TextIO(Serial); +SimpleTelemetry telemetry = SimpleTelemetry(); +... + +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY }); + telemetry.init(io); + telemetry.downsample = 500; + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` + + +# Telemetry + +The telemetry uses the packet based IO, so you can initialize it in either Text or Binary mode. + ## Usage Using telemetry is simple: @@ -35,4 +71,31 @@ void loop() { motor.loopFOC(); telemetry.run(); } -``` \ No newline at end of file +``` + +# TeleplotTelemetry + +Formats the telemetry for plotting with [Teleplot](https://github.com/nesnes/teleplot/tree/main/vscode), from inside Visual Studio Code. + +Using TeleplotTelemetry: + +```c++ +TextIO io = TextIO(Serial); +TeleplotTelemetry telemetry = TeleplotTelemetry(); +... + +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY }); + telemetry.init(io); + telemetry.downsample = 500; + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` diff --git a/src/comms/telemetry/SimpleTelemetry.cpp b/src/comms/telemetry/SimpleTelemetry.cpp new file mode 100644 index 0000000..fe8777b --- /dev/null +++ b/src/comms/telemetry/SimpleTelemetry.cpp @@ -0,0 +1,42 @@ + +#include "./SimpleTelemetry.h" + + +SimpleTelemetry::SimpleTelemetry(){ +}; + + +SimpleTelemetry::~SimpleTelemetry(){ + // nothing to do here +}; + + +void SimpleTelemetry::init(TextIO& _comms){ + this->Telemetry::init(_comms); +}; + + +void SimpleTelemetry::sendTelemetry(){ + if (numRegisters > 0) { + for (uint8_t i = 0; i < numRegisters; i++) { + SimpleFOCRegisters::regs->registerToComms(*comms, registers[i], motors[registers_motor[i]]); + if (i 0) { + for (uint8_t i = 0; i < numRegisters; i++) { + *comms << 'R'; + if (registers_motor[i] != 0) { + *comms << registers_motor[i] << '_'; + } + *comms << registers[i]; + if (iid = Telemetry::id_seed++; + if (Telemetry::num_telemetry < TELEMETRY_MAX_TELEMETRIES) { + Telemetry::telemetries[Telemetry::num_telemetry] = this; + } + this->id = Telemetry::num_telemetry++; headerSent = false; if (SimpleFOCRegisters::regs == NULL) { SimpleFOCRegisters::regs = new SimpleFOCRegisters(); @@ -48,17 +52,29 @@ void Telemetry::init(PacketIO& _comms) { void Telemetry::run() { if (numRegisters<1) return; - if (!headerSent) { - sendHeader(); - headerSent = true; + unsigned long now = 0; + if (id==0) { + now = _micros(); + iterations++; + if (now - last_iter_time >= 1000000) { // TODO add overflow protection + last_iter_time = now; + last_iterations = iterations; + iterations = 0; + } } + if (downsample==0) + return; if (downsample==0 || downsampleCnt++ < downsample) return; downsampleCnt = 0; if (min_elapsed_time > 0) { - unsigned long now = _micros(); - if (now - last_run_time < min_elapsed_time) return; + if (id!=0) now = _micros(); + if (now - last_run_time < min_elapsed_time) return; // TODO add overflow protection last_run_time = now; } + if (!headerSent) { + sendHeader(); + headerSent = true; + } sendTelemetry(); } diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h index 70b618e..c0f4006 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -5,6 +5,10 @@ #include "../SimpleFOCRegisters.h" #include "../RegisterIO.h" +#ifndef TELEMETRY_MAX_TELEMETRIES +#define TELEMETRY_MAX_TELEMETRIES 4 +#endif + #ifndef TELEMETRY_MAX_REGISTERS #define TELEMETRY_MAX_REGISTERS 8 #endif @@ -26,6 +30,7 @@ typedef enum : uint8_t { class Telemetry { + friend class SimpleFOCRegisters; public: Telemetry(); virtual ~Telemetry(); @@ -36,6 +41,12 @@ class Telemetry { uint16_t downsample = DEF_TELEMETRY_DOWNSAMPLE; uint32_t min_elapsed_time = 0; + + uint32_t last_iterations = 0; + + static uint8_t num_telemetry; + static uint8_t telemetry_ctrl; + static Telemetry* telemetries[]; protected: virtual void sendTelemetry(); virtual void sendHeader(); @@ -51,9 +62,10 @@ class Telemetry { uint8_t packetSize; bool headerSent; unsigned long last_run_time = 0; + unsigned long last_iter_time = 0; uint16_t downsampleCnt = 0; + uint32_t iterations = 0; PacketIO* comms; - static uint8_t id_seed; // TODO how to initialize this? }; From 8decf46cdf23897089f1107105905ae7f6788d24 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Dec 2023 22:37:44 +0100 Subject: [PATCH 06/37] Binary io is working in first tests --- src/comms/SimpleFOCRegisters.cpp | 4 ++-- src/comms/streams/BinaryIO.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index 9b31229..df6ac4c 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -57,8 +57,8 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl]; comms << telemetry->numRegisters; for (uint8_t i=0; inumRegisters; i++) { - comms << telemetry->registers[i]; comms << telemetry->registers_motor[i]; + comms << telemetry->registers[i]; } telemetry->headerSent = false; } @@ -281,8 +281,8 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto uint8_t registers[numRegisters]; uint8_t motors[numRegisters]; for (uint8_t i=0; i> registers[i]; comms >> motors[i]; + comms >> registers[i]; } telemetry->setTelemetryRegisters(numRegisters, registers, motors); } diff --git a/src/comms/streams/BinaryIO.h b/src/comms/streams/BinaryIO.h index b406318..5604b16 100644 --- a/src/comms/streams/BinaryIO.h +++ b/src/comms/streams/BinaryIO.h @@ -13,7 +13,7 @@ // improve the performance compared to the ASCII version, which will do a lot of // converting data to ASCII, and then write in many small writes. #ifndef BINARYIO_BUFFER_SIZE -#define BINARYIO_BUFFER_SIZE 64 +#define BINARYIO_BUFFER_SIZE 58 #endif class BinaryIO : public PacketIO { From bb863cf2af489ef5b7b979d8bd284f92e9567726 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Dec 2023 22:58:07 +0100 Subject: [PATCH 07/37] updated main README --- README.md | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index d91fcaf..23125e1 100644 --- a/README.md +++ b/README.md @@ -30,12 +30,16 @@ What is here? See the sections below. Each driver or function should come with i ### Motor/Gate driver ICs +Software to control gate driver ICs or integrated driver ICs which have advanced configuration and status interfaces, e.g. via I2C or SPI. + - [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. - [STSPIN32G4 driver](src/drivers/stspin32g4/) - I2C and BLDCDriver for the STSPIN32G4 integrated gate driver MCU. ### Encoders +Drivers for various position sensor ICs. In many cases these hardware-specific drivers often support more functionality than the generic SimpleFOC sensor drivers, including reading status registers, setting configurations and more. + - [AS5048A SPI driver](src/encoders/as5048a/) - SPI driver for the AMS AS5048A absolute position magnetic rotary encoder IC. - [AS5047 SPI driver](src/encoders/as5047/) - SPI driver for the AMS AS5047P and AS5047D absolute position magnetic rotary encoder ICs. - [AS5047U SPI driver](src/encoders/as5047u/) - SPI driver for the AMS AS5047U absolute position magnetic rotary encoder ICs. @@ -51,21 +55,25 @@ 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. + - [CalibratedSensor](src/encoders/calibrated/) - A sensor which can calibrate for eccentricity on the magnet placement. - [SmoothingSensor](src/encoders/smoothing/) - A SimpleFOC Sensor wrapper implementation which adds angle extrapolation. ### Communications - - [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 +Extended communications classes for SimpleFOC. In particular the Telemetry and PacketCommander classes, which implement ASCII or Binary communications and allow monitoring and control of multiple motors, via an easy to use "Registers" abstraction. The Binary and ASCII packet based protocols are directly supported in [PySimpleFOC](https://github.com/simplefoc/pysimplefoc). + - [PacketCommander](src/comms/streams/) - Serial communications with binary protocol, based on register abstraction - get or set any value in SimpleFOC + - [Telemetry](src/comms/telemetry/) - Telemetry based on registers - monitor any value in SimpleFOC, and log in either ASCII or Binary, compatible with PacketCommander + - [SimpleTelemetry](src/comms/telemetry/) - Register telemetry for use with Arduino Serial Plotter tool + - [TeleplotTelemetry](src/comms/telemetry/) - Register telemetry for use with VSCode Teleplot extension + - [I2CCommander I2C driver](src/comms/i2c/) - I2C communications based on register abstraction + - [STM32 SpeedDir Input](src/comms/stm32speeddir/) - Control target velocity with PWM speed and direction inputs + ### Settings -Load and store SimpleFOC motor settings, based on register abstraction. +Load and store SimpleFOC motor settings, based on register abstraction. Storing the motor calibration allows you to skip the calibration phase during setup. - - [SAMD NVM storage driver](src/settings/samd/) - Store settings to the NVM flash memory in your SAMD MCU + - [SAMD NVM storage driver](src/settings/samd/) - Store settings to either the main flash memory or the RWWEE memory (if available) in your SAMD MCU - [CAT24 I2C EEPROM storage driver](src/settings/i2c/) - Store settings to CAT24 I2C EEPROMs - [STM32 flash storage driver](src/settings/stm32/) - Store settings directly to STM32 on-board flash, currently supporting STM32G4 MCUs. @@ -78,6 +86,8 @@ Drive different kinds of motors, or use alternate algorithms to SimpleFOC's defa ### Utilities +Other support code not fitting in the above categories. + - [STM32 PWM Input driver](src/utilities/stm32pwm/) - PWM Input driver for STM32 MCUs. Accurately measure PWM inputs with zero MCU overhead. - [STM32 CORDIC trig driver](src/utilities/stm32math/) - CORDIC driver to accellerate sine and cosine calculations in SimpleFOC, on STM32 MCUs which have a CORDIC unit. From 0f8454d20b9a58e6957967d2319417cf80a15b26 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Dec 2023 23:39:10 +0100 Subject: [PATCH 08/37] Documentation updates --- src/comms/streams/README.md | 65 ++++++++++++++++++----------------- src/comms/telemetry/README.md | 30 +++++++++++++++- 2 files changed, 63 insertions(+), 32 deletions(-) diff --git a/src/comms/streams/README.md b/src/comms/streams/README.md index b40916b..2234aad 100644 --- a/src/comms/streams/README.md +++ b/src/comms/streams/README.md @@ -1,35 +1,6 @@ -# Streams IO +# PacketCommander & Streams IO -IO objects which work with Arduino Streams. Used for the Telemetry abstraction, or packet based control of SimpleFOC via PacketCommander. - -This can be used in conjunction with Serial, probably the way most people will use it, but also with other Streams like TCP network connections. - -There are Binary and Text versions of the Streams implementation. Binary will be a bit more compact, but isn't easily readable. - -## Usage - -Use of the IO classes is shown in conjunction with their use in PacketCommander or Telemetry. Please see the docs for those classes. - -### TextIO - -In TextIO, you can set the float precision: - -```c++ -TextIO comms = TextIO(Serial); - -void setup(){ - comms.precision = 6; // print floats with 6 digits after the decimal place -} -``` - -### BinaryIO - -BinaryIO has no options: - -```c++ -BinaryIO comms = BinaryIO(Serial); -``` ### PacketCommander @@ -72,4 +43,36 @@ r8=10.0000 # target is 10 r4=1 # motor is enabled r0=4 # motor status is FOCMotorStatus::motor_ready -``` \ No newline at end of file +``` + +## Streams IO + +IO objects which work with Arduino Streams. Used for the Telemetry abstraction, or packet based control of SimpleFOC via PacketCommander. + +This can be used in conjunction with Serial, probably the way most people will use it, but also with other Streams like TCP network connections. + +There are Binary and Text versions of the Streams implementation. Binary will be a bit more compact, but isn't easily readable. + +## Usage + +Use of the IO classes is shown in conjunction with their use in PacketCommander or Telemetry. Please see the docs for those classes. + +### TextIO + +In TextIO, you can set the float precision: + +```c++ +TextIO comms = TextIO(Serial); + +void setup(){ + comms.precision = 6; // print floats with 6 digits after the decimal place +} +``` + +### BinaryIO + +BinaryIO has no options: + +```c++ +BinaryIO comms = BinaryIO(Serial); +``` diff --git a/src/comms/telemetry/README.md b/src/comms/telemetry/README.md index e8858d6..743a66e 100644 --- a/src/comms/telemetry/README.md +++ b/src/comms/telemetry/README.md @@ -9,7 +9,21 @@ The telemetry implementation is based on the SimpleFOC registers, and allows you The concept allows you to choose registers which are then sent by the telemetry automatically, on a regular schedule. -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. +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. + +In addition, 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. + +Each telemetry instance has an id (starting at 0) and can be controlled via register commands, for example using PacketCommander. + +Each telemetry has the following configuration options: + +- Motors added (at least one motor must be added) +- Register list to output, each register is associated with a motor +- Downsample rate, downsample=0 switches off the telemetry instance +- min_elapsed_time, telemetry is not sent more often than this + +Further details and usage examples are provided for the individual implementations, below. + # SimpleTelemetry @@ -49,6 +63,20 @@ void loop() { The telemetry uses the packet based IO, so you can initialize it in either Text or Binary mode. +The telemetry data will be sent in packets at the configured rate. In ASCII mode, each packet will look something like this: + +`T0=17.0982,1.1026` + +This would be for telemetry id 0, with two values (floating point). If you don't know in advance, then to know the meaning of the values you need the telemetry header information. + +The telemetry will send a header packet describing the registers contained in the subsequent telemetry packets. The header packet is sent again each time you change the registers. You can also force sending of the header by reading the REG_TELEMETRY_REG register (e.g. via PacketCommander). This will send the registers as a response packet, and additionally cause the telemetry to re-send the header packet. + +In ASCII mode, the Header Packet will look something like this: + +`H0=0:9,0:17` + +Meaning header for telemetry id 0, with registers 9 (angle) and 17 (velocity), both for motor 0. + ## Usage Using telemetry is simple: From 4c0031a6f44c8295f0afd6866b62ddb346372a05 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 23 Dec 2023 23:40:17 +0100 Subject: [PATCH 09/37] add missing class --- src/comms/CommanderMaster.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 src/comms/CommanderMaster.h diff --git a/src/comms/CommanderMaster.h b/src/comms/CommanderMaster.h new file mode 100644 index 0000000..32d2a23 --- /dev/null +++ b/src/comms/CommanderMaster.h @@ -0,0 +1,14 @@ + + +#pragma once + + +class CommanderMaster { +public: + CommanderMaster(); + ~CommanderMaster(); + + virtual int writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size) = 0; + virtual int readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size) = 0; + +}; \ No newline at end of file From 00cfedfd200c2996c25ae9a33ed4d0a5b794f636 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 24 Dec 2023 00:09:47 +0100 Subject: [PATCH 10/37] More docs on communications code --- src/comms/README.md | 29 +++++++++++++++++++++++++++++ src/comms/streams/README.md | 5 ++++- 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/comms/README.md b/src/comms/README.md index 4469af6..30fb84d 100644 --- a/src/comms/README.md +++ b/src/comms/README.md @@ -3,3 +3,32 @@ This folder contains classes to support you communicating between MCUs running SimpleFOC, and other systems. +## Telemetry + +The [telemetry](telemetry/) subfolder contains classes which can log telemetry information for any of the SimpleFOC registers. Telemetry also supports multiple motors, and multiple telemetry objects can be used at the same time to log different values and at different frequencies. + +Since registers are defined for most of SimpleFOC's settings and variables, this goes far beyond the standard monitoring capabilities of the motor class. + +A telemetry implementation is provided that uses packet based IO, and is therefore compatible with PacketCommander, and can use either ASCII or Binary protocols (see below). + +## PacketCommander + +The [streams](streams/) subfolder contains PacketCommander, which allows reading or writing the SimpleFOC registers to control or query the motor. PacketCommander is based on packet IO, and can run in either ASCII or Binary mode. + +## I2CCommander + +Again based on the registers abstraction, [I2CCommander](i2c/) is a class which enables reading and writing of SimpleFOC registers via I2C bus. When running I2CCommander your MCU is in I2C target mode, and can then be controlled from another MCU or Raspberry Pi. + +## Packet IO for Arduino Streams + +[Packet based IO](streams/) is implemented for Arduino Stream objects - meaning they can be used with Serial, but potentially also with WiFi or BLE connections supporting the Streams API. + +Implementations are available for either ASCII based protocol (TextIO) or binary protocol (BinaryIO). + +## Registers abstraction + +[SimpleFOCRegisters.h](./SimpleFOCRegisters.h) contains a list of registers known to SimpleFOC. These registers can be read and/or written and code is provided to serialize/deserialize them. + +The SimpleFOC packet based IO (PacketCommander, Telemetry), I2CCommander and SettingsStorage as well as our Python API [PySimpleFOC](https://github.com/simplefoc/pysimplefoc) are all based on this register abstraction, and therefore share the same register names/ids. + +If implementing your own communications protocol, we encourage you to base it on the Register abstraction if appropriate. This will provide you with ready to use code to access/write the register values, and will make your solution easier to use due to the shared common conventions. \ No newline at end of file diff --git a/src/comms/streams/README.md b/src/comms/streams/README.md index 2234aad..3d04bbe 100644 --- a/src/comms/streams/README.md +++ b/src/comms/streams/README.md @@ -1,6 +1,9 @@ -# PacketCommander & Streams IO +# PacketCommander & Packet IO +Our packet based IO classes based on the SimpleFOC registers are a more flexible and powerful way to interact with your SimpleFOC code from the outside. + +They are supported by our Python API [PySimpleFOC](https://github.com/simplefoc/pysimplefoc). ### PacketCommander From 9b7a755e5d0185bcfb34ee1080b5ca379ae43971 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 27 Dec 2023 23:52:17 +0100 Subject: [PATCH 11/37] fixing bugs in new IO code --- src/comms/SimpleFOCRegisters.cpp | 28 +++++++++++++++++++++++++-- src/comms/SimpleFOCRegisters.h | 3 +++ src/comms/streams/BinaryIO.cpp | 6 ++++-- src/comms/streams/PacketCommander.cpp | 2 +- src/comms/streams/TextIO.cpp | 2 +- 5 files changed, 35 insertions(+), 6 deletions(-) diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index df6ac4c..e515b98 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -34,7 +34,7 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto break; case SimpleFOCRegister::REG_POSITION: if (motor->sensor) { - comms << (uint32_t)motor->sensor->getFullRotations(); + comms << (uint32_t)motor->sensor->getFullRotations(); // TODO fix me! comms << motor->sensor->getMechanicalAngle(); } else { @@ -51,6 +51,24 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto else comms << motor->shaft_angle; break; + case SimpleFOCRegister::REG_SENSOR_MECHANICAL_ANGLE: + if (motor->sensor) + comms << motor->sensor->getMechanicalAngle(); // stored angle + else + comms << motor->shaft_angle; + break; + case SimpleFOCRegister::REG_SENSOR_VELOCITY: + if (motor->sensor) + comms << motor->sensor->getVelocity(); // stored angle + else + comms << motor->shaft_velocity; + break; + case SimpleFOCRegister::REG_SENSOR_TIMESTAMP: + if (motor->sensor) + comms << (uint32_t)motor->sensor->angle_prev_ts; // stored angle + else + comms << (uint32_t)0; + break; case SimpleFOCRegister::REG_TELEMETRY_REG: if (Telemetry::num_telemetry > 0){ @@ -405,6 +423,9 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_POSITION: case SimpleFOCRegister::REG_VELOCITY: case SimpleFOCRegister::REG_SENSOR_ANGLE: + case SimpleFOCRegister::REG_SENSOR_MECHANICAL_ANGLE: + case SimpleFOCRegister::REG_SENSOR_VELOCITY: + case SimpleFOCRegister::REG_SENSOR_TIMESTAMP: case SimpleFOCRegister::REG_VOLTAGE_Q: case SimpleFOCRegister::REG_VOLTAGE_D: case SimpleFOCRegister::REG_CURRENT_Q: @@ -433,6 +454,9 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ case SimpleFOCRegister::REG_ANGLE: case SimpleFOCRegister::REG_VELOCITY: case SimpleFOCRegister::REG_SENSOR_ANGLE: + case SimpleFOCRegister::REG_SENSOR_MECHANICAL_ANGLE: + case SimpleFOCRegister::REG_SENSOR_VELOCITY: + case SimpleFOCRegister::REG_SENSOR_TIMESTAMP: case SimpleFOCRegister::REG_VOLTAGE_Q: case SimpleFOCRegister::REG_VOLTAGE_D: case SimpleFOCRegister::REG_CURRENT_Q: @@ -494,7 +518,7 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ return 2*telemetry->numRegisters + 1; } else - return 0; + return 1; case SimpleFOCRegister::REG_DRIVER_ENABLE: case SimpleFOCRegister::REG_REPORT: // size can vary, handled in Commander if used - may discontinue this feature case SimpleFOCRegister::REG_ENABLE_ALL: // write-only diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index 0f59edb..3e52e2a 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -27,6 +27,9 @@ typedef enum : uint8_t { 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 + REG_SENSOR_MECHANICAL_ANGLE = 0x13, // RO - float + REG_SENSOR_VELOCITY = 0x14, // RO - float + REG_SENSOR_TIMESTAMP = 0x15,// RO - uint32_t REG_PHASE_VOLTAGE = 0x16, // R/W - 3xfloat = 12 bytes REG_PHASE_STATE = 0x17, // R/W - 3 bytes (1 byte per phase) diff --git a/src/comms/streams/BinaryIO.cpp b/src/comms/streams/BinaryIO.cpp index ed8a681..b820075 100644 --- a/src/comms/streams/BinaryIO.cpp +++ b/src/comms/streams/BinaryIO.cpp @@ -114,11 +114,13 @@ BinaryIO& BinaryIO::operator>>(uint8_t &value) { PacketIO& BinaryIO::operator>>(Packet& value) { while (!in_sync && _io.available() > 0) { - if (_io.read() == MARKER_BYTE) + if (_io.peek() == MARKER_BYTE) in_sync = true; + else + _io.read(); } if (_io.peek() == MARKER_BYTE) { - _io.read(); // discard the \n + _io.read(); // discard the marker } if (!in_sync || _io.available() < 3) { // size, frame type, payload = 3 bytes minimum frame size value.type = 0x00; diff --git a/src/comms/streams/PacketCommander.cpp b/src/comms/streams/PacketCommander.cpp index 5fa2cba..6c2a0d7 100644 --- a/src/comms/streams/PacketCommander.cpp +++ b/src/comms/streams/PacketCommander.cpp @@ -76,7 +76,7 @@ void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) { bool PacketCommander::commsToRegister(uint8_t reg){ switch (reg) { - case REG_MOTOR_ADDRESS: + case SimpleFOCRegister::REG_MOTOR_ADDRESS: uint8_t val; *_io >> val; if (val >= numMotors) diff --git a/src/comms/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp index f692292..16fc36c 100644 --- a/src/comms/streams/TextIO.cpp +++ b/src/comms/streams/TextIO.cpp @@ -79,7 +79,7 @@ TextIO& TextIO::operator>>(float &value) { if (in_sep) { _io.read(); // discard the separator } - value = _io.parseFloat(LookaheadMode::SKIP_NONE); + value = _io.parseFloat(LookaheadMode::SKIP_NONE); // TODO LookaheadMode is not defined on ESP32 in_sep = true; return *this; }; From 7d25dfc62a935c1234a17c703e1b943abf5b8467 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 28 Dec 2023 23:43:01 +0100 Subject: [PATCH 12/37] fix TextIO compile problems on ESP32 --- src/comms/streams/TextIO.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/comms/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp index 16fc36c..55fa2b4 100644 --- a/src/comms/streams/TextIO.cpp +++ b/src/comms/streams/TextIO.cpp @@ -70,7 +70,11 @@ TextIO& TextIO::operator<<(Separator value) { return *this; }; - +#ifndef ESP32 +#define LOOKAHEADARGS LookaheadMode::SKIP_NONE +#else +#define LOOKAHEADARGS +#endif TextIO& TextIO::operator>>(float &value) { if (_io.peek() == '\n') { @@ -79,7 +83,7 @@ TextIO& TextIO::operator>>(float &value) { if (in_sep) { _io.read(); // discard the separator } - value = _io.parseFloat(LookaheadMode::SKIP_NONE); // TODO LookaheadMode is not defined on ESP32 + value = _io.parseFloat(LOOKAHEADARGS); // TODO LookaheadMode is not defined on ESP32 in_sep = true; return *this; }; @@ -91,7 +95,7 @@ TextIO& TextIO::operator>>(uint32_t &value) { if (in_sep) { _io.read(); // discard the separator } - value = (uint32_t)_io.parseInt(LookaheadMode::SKIP_NONE); + value = (uint32_t)_io.parseInt(LOOKAHEADARGS); in_sep = true; return *this; }; @@ -105,7 +109,7 @@ TextIO& TextIO::operator>>(uint8_t &value) { if (in_sep) { _io.read(); // discard the separator } - value = (uint8_t)_io.parseInt(LookaheadMode::SKIP_NONE); + value = (uint8_t)_io.parseInt(LOOKAHEADARGS); in_sep = true; return *this; }; From 5069e261a13b58a4f9bd8298498b97d384eda7a7 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 28 Dec 2023 23:45:43 +0100 Subject: [PATCH 13/37] Add SimpleFOCNano driver --- src/drivers/simplefocnano/README.md | 65 ++++++++++++++++++ .../simplefocnano/SimpleFOCNanoDriver.cpp | 68 +++++++++++++++++++ .../simplefocnano/SimpleFOCNanoDriver.h | 56 +++++++++++++++ 3 files changed, 189 insertions(+) create mode 100644 src/drivers/simplefocnano/README.md create mode 100644 src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp create mode 100644 src/drivers/simplefocnano/SimpleFOCNanoDriver.h diff --git a/src/drivers/simplefocnano/README.md b/src/drivers/simplefocnano/README.md new file mode 100644 index 0000000..e481a5f --- /dev/null +++ b/src/drivers/simplefocnano/README.md @@ -0,0 +1,65 @@ + +# SimpleFOCNano Driver Class + +The `SimpleFOCNanoDriver` is a wrapper class around BLDCDriver3PWM to make using the [SimpleFOCNano shield](link) super-simple. + +If you use this driver you don't need to bother with any pin-numbers, they are all set correctly for you based on the SimpleFOCNano's pinout. Of course, this only works if you actually plug the shield into the Nano. If you use jumper wires, either make exactly the same connections as plugging in the shield would, or don't use this driver. + +## Usage + +Basic usage is very simple: + +```c++ +#include +#include +#include + +SimpleFOCNanoDriver driver = SimpleFOCNanoDriver(); +BLDCMotor motor = BLDCMotor(7); + +void setup() { + driver.voltage_power_supply = driver.getBusVoltage(); + driver.init(); + motor.linkDriver(driver); + motor.voltage_limit = driver.voltage_limit / 2.0f; + motor.controller = MotionControlMode::velocity_openloop; + motor.init(); +} + +void loop(){ + motor.move(2.0f); // 2 rads per second, open-loop +} +``` + +There are some extra features, you can check for faults, and clear the fault state: + +```c++ +if (driver.isFault()) { + motor.disable(); + driver.clearFault(); +} +``` + +Or you can activate sleep mode to save power: + +```c++ +driver.sleep(); + +// sometime later +if (driver.isSleeping()) + driver.wake(); +``` + +As shown in the example you can read the bus voltage: + +:warning: *this is a slow function. Do not call it while motor is running!* + +```c++ +float val = driver.getBusVoltage(); // get the bus voltage, in Volts +``` + +The driver's nCS pin is 10, and the constant PIN_nCS can be used: + +```c++ +MagneticSensorAS5048A sensor = MagneticSensorAS5048A(PIN_nCS); +``` \ No newline at end of file diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp new file mode 100644 index 0000000..9f58f48 --- /dev/null +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp @@ -0,0 +1,68 @@ + +#include "./SimpleFOCNanoDriver.h" + +SimpleFOCNanoDriver::SimpleFOCNanoDriver() : BLDCDriver3PWM(PIN_INU, PIN_INV, PIN_INW, PIN_ENU, PIN_ENV, PIN_ENW) { + // nothing to do here +}; + + +SimpleFOCNanoDriver::~SimpleFOCNanoDriver() { + // nothing to do here +}; + +int SimpleFOCNanoDriver::init() { + pinMode(PIN_nSLEEP, OUTPUT); + pinMode(PIN_nRST, OUTPUT); + pinMode(PIN_nFAULT, INPUT_PULLUP); + pinMode(PIN_VBUS, INPUT); + this->sleep(false); + digitalWrite(PIN_nRST, HIGH); + return BLDCDriver3PWM::init(); +}; + +void SimpleFOCNanoDriver::sleep(bool sleep) { + digitalWrite(PIN_nSLEEP, !sleep); +}; + + +void SimpleFOCNanoDriver::wake() { + this->sleep(false); +}; + + +bool SimpleFOCNanoDriver::isSleeping() { + return digitalRead(PIN_nSLEEP)==LOW; +}; + + + +bool SimpleFOCNanoDriver::isFault() { + return !digitalRead(PIN_nFAULT); +}; + + +void SimpleFOCNanoDriver::clearFault() { + if (!this->isFault()) return; + disable(); + digitalWrite(PIN_nRST, LOW); + delayMicroseconds(100); + digitalWrite(PIN_nRST, HIGH); + enable(); +}; + + +void SimpleFOCNanoDriver::attachFaultInterrupt(void (*callback)()) { + attachInterrupt(digitalPinToInterrupt(PIN_nFAULT), callback, FALLING); +}; + + + +float SimpleFOCNanoDriver::getBusVoltage(float vdd_voltage, int adc_resolution) { + float sum = 0.0; + for(int i = 0; i < 500; i++) { + sum += analogRead(PIN_VBUS); + } + + return sum / 500.0 * VBUS_CONV_FACTOR * (vdd_voltage / adc_resolution); +}; + diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h new file mode 100644 index 0000000..6c17f89 --- /dev/null +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h @@ -0,0 +1,56 @@ + +#pragma once + +#include + +/* + * Default pins for the SimpleFOC Nano board + * + * These are the correct pins if you plug the Nano board into the SimpleFOC shield. + * If you use jumper wires to connect, you can make your own pin choices. In this case, don't use this driver, + * and rather use the BLDCDriver3PWM class directly. + * + */ + + +#define PIN_INU 3 +#define PIN_INV 6 +#define PIN_INW 9 +#define PIN_ENU 4 +#define PIN_ENV 7 +#define PIN_ENW 8 +#ifdef ARDUINO_NANO_RP2040_CONNECT +#define PIN_nSLEEP 17 +#define PIN_nFAULT 20 +#define PIN_nRST 21 +#else +#define PIN_nSLEEP A3 +#define PIN_nFAULT A6 +#define PIN_nRST A7 +#endif +#define PIN_VBUS A0 +#define PIN_nCS 10 + +#define VBUS_CONV_FACTOR (22.0f/2.2f) + + + + +class SimpleFOCNanoDriver : public BLDCDriver3PWM { +public: + SimpleFOCNanoDriver(); + ~SimpleFOCNanoDriver(); + + int init() override; + + void sleep(bool sleep=true); + void wake(); + bool isSleeping(); + + bool isFault(); + void clearFault(); + void attachFaultInterrupt(void (*callback)()); + + float getBusVoltage(float vdd_voltage, int adc_resolution); +}; + From 6c0db29e256d8a56bcbf266415764ca685d5c0f3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 28 Dec 2023 23:47:45 +0100 Subject: [PATCH 14/37] fix compile errors due to non-public access --- src/comms/SimpleFOCRegisters.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index e515b98..bcb884a 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -65,7 +65,8 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto break; case SimpleFOCRegister::REG_SENSOR_TIMESTAMP: if (motor->sensor) - comms << (uint32_t)motor->sensor->angle_prev_ts; // stored angle + //comms << (uint32_t)motor->sensor->angle_prev_ts; // TODO stored angle, make it a public or make this friend class + comms << (uint32_t)0; else comms << (uint32_t)0; break; From c0f3e2fc9a668b1e61ab50f23c98828c5cae8226 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 10 Jan 2024 13:25:19 +0100 Subject: [PATCH 15/37] Buffer TextIO input to make non-blocking --- src/comms/streams/TextIO.cpp | 123 +++++++++++++++++++++++++---------- src/comms/streams/TextIO.h | 11 ++++ 2 files changed, 100 insertions(+), 34 deletions(-) diff --git a/src/comms/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp index 55fa2b4..22cb644 100644 --- a/src/comms/streams/TextIO.cpp +++ b/src/comms/streams/TextIO.cpp @@ -70,32 +70,70 @@ TextIO& TextIO::operator<<(Separator value) { return *this; }; -#ifndef ESP32 -#define LOOKAHEADARGS LookaheadMode::SKIP_NONE -#else -#define LOOKAHEADARGS -#endif -TextIO& TextIO::operator>>(float &value) { - if (_io.peek() == '\n') { - return *this; // TODO flag this error + +uint32_t TextIO::intFromBuffer() { + if (in_sep) { + buffer_index++; // discard the separator } + if (buffer_index >= buffer_len) + in_sync = false; + uint32_t value = 0; + while (buffer_index < buffer_len) { + char c = buffer[buffer_index]; + if (c >= '0' && c <= '9') { + value = value * 10 + (c - '0'); + buffer_index++; + } + else { + break; + } + } + return value; +} + + +TextIO& TextIO::operator>>(float &value) { if (in_sep) { - _io.read(); // discard the separator + buffer_index++; // discard the separator + in_sep = false; + } + if (buffer_index >= buffer_len) { + in_sync = false; + return *this; + } + char c = buffer[buffer_index]; + int8_t sign = 1; + if (c == '-') { + buffer_index++; + sign = -1; + } + uint32_t val = 0; + if (c != '.') + val = intFromBuffer(); + if (buffer_index < buffer_len) { + c = buffer[buffer_index]; + if (c == '.') { + uint8_t pos = ++buffer_index; + uint32_t frac = intFromBuffer(); + if (pos < buffer_index) { + value = ((float)val + (float)frac / pow(10, buffer_index - pos)) * sign; + in_sep = true; + } + else + in_sync = false; + return *this; + } } - value = _io.parseFloat(LOOKAHEADARGS); // TODO LookaheadMode is not defined on ESP32 + value = (float)val * sign; in_sep = true; return *this; }; + + TextIO& TextIO::operator>>(uint32_t &value) { - if (_io.peek() == '\n') { - return *this; // TODO flag this error - } - if (in_sep) { - _io.read(); // discard the separator - } - value = (uint32_t)_io.parseInt(LOOKAHEADARGS); + value = intFromBuffer(); in_sep = true; return *this; }; @@ -103,13 +141,7 @@ TextIO& TextIO::operator>>(uint32_t &value) { TextIO& TextIO::operator>>(uint8_t &value) { - if (_io.peek() == '\n') { - return *this; // TODO flag this error - } - if (in_sep) { - _io.read(); // discard the separator - } - value = (uint8_t)_io.parseInt(LOOKAHEADARGS); + value = (uint8_t)intFromBuffer(); in_sep = true; return *this; }; @@ -118,25 +150,48 @@ TextIO& TextIO::operator>>(uint8_t &value) { TextIO& TextIO::operator>>(Packet &value) { while (!in_sync && _io.available() > 0) { - if (_io.read() == '\n') + if (_io.read() == '\n') { in_sync = true; + buffer_len = 0; + } } - if (_io.peek() == '\n') { - _io.read(); // discard the \n + if (buffer_index >= buffer_len) { + buffer_len = 0; + buffer_index = 0; } - if (!in_sync || _io.available() < 3) { // frame type, register id, \n to end frame = 3 bytes minimum frame size - value.type = 0x00; - value.payload_size = 0; - return *this; + while (in_sync && _io.available()>0) { + uint8_t peek = _io.peek(); + if (peek == '\n' || peek == '\r') { + // skip newlines and carriage returns + while (_io.available()>0 && (peek == '\n' || peek == '\r')) { + _io.read(); // discard the \n + peek = _io.peek(); + } + if (buffer_len>1) { + value.type = buffer[0]; + value.payload_size = 0; + in_sep = false; + buffer_index = 1; + return *this; + } + } + else { + if (buffer_len < SIMPLEFOC_TEXTIO_BUFFER_SIZE) + buffer[buffer_len++] = _io.read(); + else { + buffer_len = 0; + in_sync = false; + } + } } - value.type = _io.read(); + + value.type = 0x00; value.payload_size = 0; - in_sep = false; return *this; }; bool TextIO::is_complete() { - return _io.peek() == '\n'; + return buffer_index >= buffer_len; }; \ No newline at end of file diff --git a/src/comms/streams/TextIO.h b/src/comms/streams/TextIO.h index 6651a3a..9fba1be 100644 --- a/src/comms/streams/TextIO.h +++ b/src/comms/streams/TextIO.h @@ -6,6 +6,12 @@ #include +#ifndef SIMPLEFOC_TEXTIO_BUFFER_SIZE +#define SIMPLEFOC_TEXTIO_BUFFER_SIZE 64 +#endif + + + class TextIO : public PacketIO { public: TextIO(Stream& io); @@ -24,9 +30,14 @@ class TextIO : public PacketIO { uint8_t precision = 4; protected: + uint32_t intFromBuffer(); + Stream& _io; bool sep = false; bool in_sep = false; + uint8_t buffer_index = 0; + uint8_t buffer_len = 0; + char buffer[SIMPLEFOC_TEXTIO_BUFFER_SIZE]; }; From d5748ecb3e8028590a7bbc3bb809ccd83f9604e8 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 10 Jan 2024 13:31:42 +0100 Subject: [PATCH 16/37] update build flags to detect nano boards --- src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp | 3 +++ src/drivers/simplefocnano/SimpleFOCNanoDriver.h | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp index 9f58f48..e438b0b 100644 --- a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp @@ -1,6 +1,8 @@ #include "./SimpleFOCNanoDriver.h" +#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) + SimpleFOCNanoDriver::SimpleFOCNanoDriver() : BLDCDriver3PWM(PIN_INU, PIN_INV, PIN_INW, PIN_ENU, PIN_ENV, PIN_ENW) { // nothing to do here }; @@ -66,3 +68,4 @@ float SimpleFOCNanoDriver::getBusVoltage(float vdd_voltage, int adc_resolution) return sum / 500.0 * VBUS_CONV_FACTOR * (vdd_voltage / adc_resolution); }; +#endif diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h index 6c17f89..f0917c7 100644 --- a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h @@ -1,8 +1,11 @@ #pragma once + #include +#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) + /* * Default pins for the SimpleFOC Nano board * @@ -54,3 +57,4 @@ class SimpleFOCNanoDriver : public BLDCDriver3PWM { float getBusVoltage(float vdd_voltage, int adc_resolution); }; +#endif \ No newline at end of file From 76f6514b08d2f3aa3d798173ac57ef7b141b3724 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 16 Jan 2024 01:55:39 +0100 Subject: [PATCH 17/37] making TextIO more robust to bad input --- src/comms/streams/PacketCommander.cpp | 25 +++++++++++++++---------- src/comms/streams/TextIO.cpp | 20 ++++++++++++++------ 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/comms/streams/PacketCommander.cpp b/src/comms/streams/PacketCommander.cpp index 6c2a0d7..a8e1606 100644 --- a/src/comms/streams/PacketCommander.cpp +++ b/src/comms/streams/PacketCommander.cpp @@ -39,6 +39,10 @@ void PacketCommander::run() { handleRegisterPacket(!_io->is_complete(), curRegister); lastcommanderror = commanderror; lastcommandregister = curRegister; + if (commanderror) { + _io->in_sync = false; + //*_io << START_PACKET(PacketType::ALERT, 1) << '?' << END_PACKET; + } } else if (curr_packet.type == PacketType::SYNC) { *_io << START_PACKET(PacketType::SYNC, 1); @@ -46,8 +50,9 @@ void PacketCommander::run() { *_io << END_PACKET; // TODO flush packet } - else - _io->in_sync = false; // TODO flag in another way? + else { + _io->in_sync = false; // TODO it would be better just to reset the buffer, since we just saw a newline + } if (! _io->is_complete()) _io->in_sync = false; @@ -55,10 +60,11 @@ void PacketCommander::run() { }; + void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) { if (write) { bool ok = commsToRegister(reg); - commanderror = commanderror && !ok; + commanderror = commanderror || !ok; } if (!write || echo) { uint8_t size = SimpleFOCRegisters::regs->sizeOfRegister(reg); @@ -70,7 +76,7 @@ void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) { // TODO flush packet } } -} +}; @@ -87,23 +93,22 @@ bool PacketCommander::commsToRegister(uint8_t reg){ default: return SimpleFOCRegisters::regs->commsToRegister(*_io, reg, motors[curMotor]); } -} +}; bool PacketCommander::registerToComms(uint8_t reg){ switch (reg) { - case REG_STATUS: + case SimpleFOCRegister::REG_STATUS: // TODO implement status register return true; - case REG_MOTOR_ADDRESS: + case SimpleFOCRegister::REG_MOTOR_ADDRESS: *_io << curMotor; return true; - case REG_NUM_MOTORS: + case SimpleFOCRegister::REG_NUM_MOTORS: *_io << numMotors; return true; default: return SimpleFOCRegisters::regs->registerToComms(*_io, reg, motors[curMotor]); } -} - +}; diff --git a/src/comms/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp index 22cb644..afd3bee 100644 --- a/src/comms/streams/TextIO.cpp +++ b/src/comms/streams/TextIO.cpp @@ -90,7 +90,7 @@ uint32_t TextIO::intFromBuffer() { } } return value; -} +}; TextIO& TextIO::operator>>(float &value) { @@ -150,7 +150,8 @@ TextIO& TextIO::operator>>(uint8_t &value) { TextIO& TextIO::operator>>(Packet &value) { while (!in_sync && _io.available() > 0) { - if (_io.read() == '\n') { + char skip = _io.read(); + if (skip == '\n' || skip == '\r') { in_sync = true; buffer_len = 0; } @@ -159,13 +160,16 @@ TextIO& TextIO::operator>>(Packet &value) { buffer_len = 0; buffer_index = 0; } - while (in_sync && _io.available()>0) { + int avail = _io.available(); + while (in_sync && avail>0) { uint8_t peek = _io.peek(); if (peek == '\n' || peek == '\r') { // skip newlines and carriage returns - while (_io.available()>0 && (peek == '\n' || peek == '\r')) { + while (avail>0 && (peek == '\n' || peek == '\r')) { _io.read(); // discard the \n - peek = _io.peek(); + avail = _io.available(); + if (avail>0) + peek = _io.peek(); } if (buffer_len>1) { value.type = buffer[0]; @@ -174,10 +178,14 @@ TextIO& TextIO::operator>>(Packet &value) { buffer_index = 1; return *this; } + else + buffer_len = 0; } else { - if (buffer_len < SIMPLEFOC_TEXTIO_BUFFER_SIZE) + if (buffer_len < SIMPLEFOC_TEXTIO_BUFFER_SIZE) { buffer[buffer_len++] = _io.read(); + avail = _io.available(); + } else { buffer_len = 0; in_sync = false; From 4d08d14588942f628d8e07b18fb755a85f93546a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 16 Jan 2024 01:56:13 +0100 Subject: [PATCH 18/37] fix RP2040 compile problems --- src/settings/rp2040/RP2040FlashSettingsStorage.cpp | 8 ++++---- src/settings/rp2040/RP2040FlashSettingsStorage.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/settings/rp2040/RP2040FlashSettingsStorage.cpp b/src/settings/rp2040/RP2040FlashSettingsStorage.cpp index 108b97f..8bd025e 100644 --- a/src/settings/rp2040/RP2040FlashSettingsStorage.cpp +++ b/src/settings/rp2040/RP2040FlashSettingsStorage.cpp @@ -6,7 +6,7 @@ #include "communication/SimpleFOCDebug.h" -RP2040FlashSettingsStorage::RP2040FlashSettingsStorage(uint32_t offset = 0x0) { +RP2040FlashSettingsStorage::RP2040FlashSettingsStorage(uint32_t offset) { _offset = offset; }; @@ -60,7 +60,7 @@ RegisterIO& RP2040FlashSettingsStorage::operator>>(uint8_t& value) { uint8_t num = readBytes(&val, 1); if (num==1) value = val; - return num; + return *this; }; RegisterIO& RP2040FlashSettingsStorage::operator>>(float& value) { @@ -68,7 +68,7 @@ RegisterIO& RP2040FlashSettingsStorage::operator>>(float& value) { uint8_t num = readBytes(&val, 4); if (num==4) value = val; - return num; + return *this; }; RegisterIO& RP2040FlashSettingsStorage::operator>>(uint32_t& value) { @@ -76,7 +76,7 @@ RegisterIO& RP2040FlashSettingsStorage::operator>>(uint32_t& value) { uint8_t num = readBytes(&val, 4); if (num==4) value = val; - return num; + return *this; }; diff --git a/src/settings/rp2040/RP2040FlashSettingsStorage.h b/src/settings/rp2040/RP2040FlashSettingsStorage.h index 3cb4543..07a455e 100644 --- a/src/settings/rp2040/RP2040FlashSettingsStorage.h +++ b/src/settings/rp2040/RP2040FlashSettingsStorage.h @@ -27,6 +27,7 @@ class RP2040FlashSettingsStorage : public SettingsStorage, public RegisterIO { int readBytes(void* valueToSet, int numBytes); void reset(); + uint32_t _offset; uint8_t* _currptr; }; From e6913d80162388979c2999fca7cbc9c327d087e9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 16 Jan 2024 10:00:36 +0100 Subject: [PATCH 19/37] Change order of SPI transaction --- src/encoders/sc60228/SC60228.cpp | 7 ++++--- src/encoders/sc60228/SC60228.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/encoders/sc60228/SC60228.cpp b/src/encoders/sc60228/SC60228.cpp index 4c63920..5157ae0 100644 --- a/src/encoders/sc60228/SC60228.cpp +++ b/src/encoders/sc60228/SC60228.cpp @@ -1,6 +1,6 @@ #include "./SC60228.h" - +#include "Arduino.h" SC60228::SC60228(SPISettings settings, int nCS) : settings(settings), nCS(nCS) { // nix @@ -13,6 +13,7 @@ void SC60228::init(SPIClass* _spi) { if (nCS>=0) pinMode(nCS, OUTPUT); digitalWrite(nCS, HIGH); + delay(1); spi->begin(); readRawAngle(); }; @@ -41,15 +42,15 @@ bool SC60228::isError() { uint16_t SC60228::spi_transfer16(uint16_t outdata){ uint16_t result; + spi->beginTransaction(settings); if (nCS>=0) digitalWrite(nCS, LOW); // min delay here: 250ns - spi->beginTransaction(settings); result = spi->transfer16(outdata); // min delay here: clock period / 2 - spi->endTransaction(); if (nCS>=0) digitalWrite(nCS, HIGH); + spi->endTransaction(); // min delay until next read: 250ns return result; }; diff --git a/src/encoders/sc60228/SC60228.h b/src/encoders/sc60228/SC60228.h index 03b4e50..f476102 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_MODE0); // @suppress("Invalid arguments") +static SPISettings SC60228SPISettings(8000000, SC60228_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") From 38413a5ffe59f772cf17095ade65b9abfb063d64 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 22 Jan 2024 00:26:54 +0100 Subject: [PATCH 20/37] change order of SPI operations on MA330, MA730 --- src/encoders/ma330/MA330.cpp | 4 ++-- src/encoders/ma730/MA730.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/encoders/ma330/MA330.cpp b/src/encoders/ma330/MA330.cpp index 6e5eac0..c40700d 100644 --- a/src/encoders/ma330/MA330.cpp +++ b/src/encoders/ma330/MA330.cpp @@ -121,13 +121,13 @@ void MA330::setFieldStrengthThresholds(uint8_t high, uint8_t low) { uint16_t MA330::transfer16(uint16_t outValue) { + spi->beginTransaction(settings); if (nCS >= 0) digitalWrite(nCS, LOW); - spi->beginTransaction(settings); uint16_t value = spi->transfer16(outValue); - spi->endTransaction(); if (nCS >= 0) digitalWrite(nCS, HIGH); + spi->endTransaction(); return value; }; uint8_t MA330::readRegister(uint8_t reg) { diff --git a/src/encoders/ma730/MA730.cpp b/src/encoders/ma730/MA730.cpp index 7a1b7b8..68db9b1 100644 --- a/src/encoders/ma730/MA730.cpp +++ b/src/encoders/ma730/MA730.cpp @@ -100,13 +100,13 @@ void MA730::setFieldStrengthThresholds(uint8_t high, uint8_t low) { uint16_t MA730::transfer16(uint16_t outValue) { + spi->beginTransaction(settings); if (nCS >= 0) digitalWrite(nCS, LOW); - spi->beginTransaction(settings); uint16_t value = spi->transfer16(outValue); - spi->endTransaction(); if (nCS >= 0) digitalWrite(nCS, HIGH); + spi->endTransaction(); return value; }; uint8_t MA730::readRegister(uint8_t reg) { From 948a8f5841b51f7a20d8b4a0c348502f0bc2780e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 26 Jan 2024 23:36:26 +0100 Subject: [PATCH 21/37] add example and update test cases for SimpleFOCNano --- .github/workflows/ccpp.yml | 18 ++--- .../simplefocnano_torque_voltage.ino | 71 +++++++++++++++++++ src/drivers/simplefocnano/README.md | 2 +- 3 files changed, 81 insertions(+), 10 deletions(-) create mode 100644 examples/drivers/simplefocnano/simplefocnano_torque_voltage/simplefocnano_torque_voltage.ino diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c2aae10..d165199 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -15,7 +15,7 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - arduino:avr:uno # arudino uno + - arduino:avr:nano # arudino nano - arduino:sam:arduino_due_x # arduino due - arduino:samd:nano_33_iot # samd21 - adafruit:samd:adafruit_metro_m4 # samd51 @@ -25,22 +25,22 @@ jobs: - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - arduino:mbed_rp2040:pico # rpi pico include: - - arduino-boards-fqbn: arduino:avr:uno + - arduino-boards-fqbn: arduino:avr:nano 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 smoothing + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage - arduino-boards-fqbn: arduino:samd:nano_33_iot required-libraries: Simple FOC sketches-exclude: calibrated smoothing - arduino-boards-fqbn: arduino:mbed_rp2040:pico required-libraries: Simple FOC - sketches-exclude: calibrated smoothing + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage - 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 smoothing + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # platform-url: https://dl.espressif.com/dl/package_esp32_index.json # required-libraries: Simple FOC @@ -48,19 +48,19 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC - sketches-exclude: calibrated smoothing + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC - sketches-exclude: calibrated smoothing + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage - 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 smoothing + sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - sketches-exclude: smoothing + sketches-exclude: smoothing simplefocnano_torque_voltage # Do not cancel all jobs / architectures if one job fails fail-fast: false steps: diff --git a/examples/drivers/simplefocnano/simplefocnano_torque_voltage/simplefocnano_torque_voltage.ino b/examples/drivers/simplefocnano/simplefocnano_torque_voltage/simplefocnano_torque_voltage.ino new file mode 100644 index 0000000..5e11274 --- /dev/null +++ b/examples/drivers/simplefocnano/simplefocnano_torque_voltage/simplefocnano_torque_voltage.ino @@ -0,0 +1,71 @@ +#include +#include +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "drivers/simplefocnano/SimpleFOCNanoDriver.h" +#include "encoders/as5048a/MagneticSensorAS5048A.h" + + +MagneticSensorAS5048A sensor = MagneticSensorAS5048A(PIN_nCS); +SimpleFOCNanoDriver driver = SimpleFOCNanoDriver(); +BLDCMotor motor = BLDCMotor(11); // 11 pole pairs + +Commander commander = Commander(Serial); +void doMotor(char* cmd) { commander.motor(&motor, cmd); } + +long loopts = 0; +int iterations = 0; +float volts = 0.0f; + +void setup() { + Serial.begin(115200); // enable serial port + delay(5000); + SimpleFOCDebug::enable(); // enable debug messages to Serial + + sensor.init(); // init sensor on default SPI pins + + // read voltage + SimpleFOCDebug::print("Bus voltage: "); + volts = driver.getBusVoltage(3.3, 4096); + SimpleFOCDebug::println(volts); + driver.voltage_power_supply = volts; // set driver voltage to measured value + driver.voltage_limit = 10.0f; // limit voltage to 10V + driver.pwm_frequency = 30000; // set pwm frequency to 30kHz + driver.init(); // init driver + + motor.linkSensor(&sensor); // link the motor to the sensor + motor.linkDriver(&driver); // link the motor to the driver + + motor.controller = MotionControlType::torque; // torque control + motor.torque_controller = TorqueControlType::voltage; // use voltage torque control + motor.voltage_limit = driver.voltage_limit / 2.0f; // limit voltage to 1/2 of driver limit + motor.voltage_sensor_align = 4.0f; // align voltage sensor to 4V + + motor.init(); // init motor + delay(100); // wait for driver to power up + motor.initFOC(); // init FOC and calibrate motor + + commander.add('M', doMotor); // add motor command + + Serial.println("Motor ready."); + loopts = millis(); +} + + +void loop() { + motor.move(); + motor.loopFOC(); + commander.run(); + long now = millis(); + iterations++; + if (now - loopts > 1000) { + Serial.print("Iterations/s: "); + Serial.println(iterations); + Serial.print("Angle: "); + Serial.println(sensor.getAngle()); + loopts = now; + iterations = 0; + } + if (now - loopts < 0) + loopts = 0; +} diff --git a/src/drivers/simplefocnano/README.md b/src/drivers/simplefocnano/README.md index e481a5f..70d2283 100644 --- a/src/drivers/simplefocnano/README.md +++ b/src/drivers/simplefocnano/README.md @@ -1,7 +1,7 @@ # SimpleFOCNano Driver Class -The `SimpleFOCNanoDriver` is a wrapper class around BLDCDriver3PWM to make using the [SimpleFOCNano shield](link) super-simple. +The `SimpleFOCNanoDriver` is a wrapper class around BLDCDriver3PWM to make using the [SimpleFOCNano shield](https://github.com/simplefoc/SimpleFOCNano) super-simple. If you use this driver you don't need to bother with any pin-numbers, they are all set correctly for you based on the SimpleFOCNano's pinout. Of course, this only works if you actually plug the shield into the Nano. If you use jumper wires, either make exactly the same connections as plugging in the shield would, or don't use this driver. From 5c73b2e63412317759b7b59473a3ac4a6b3dcab2 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 10 Feb 2024 03:10:56 +0100 Subject: [PATCH 22/37] refactoring the SimpleFOCRegisters --- src/comms/SimpleFOCRegisters.cpp | 179 ++++++++++++++++++++++++-- src/comms/SimpleFOCRegisters.h | 43 +++++-- src/comms/i2c/I2CCommander.cpp | 4 +- src/comms/i2c/I2CCommander.h | 3 + src/comms/i2c/README.md | 3 - src/comms/streams/PacketCommander.cpp | 53 ++++---- src/comms/streams/PacketCommander.h | 11 +- 7 files changed, 237 insertions(+), 59 deletions(-) diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index bcb884a..acc97f4 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -165,17 +165,32 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_VEL_PID_D: comms << motor->PID_velocity.D; break; + case SimpleFOCRegister::REG_VEL_PID_LIM: + comms << motor->PID_velocity.limit; + break; + case SimpleFOCRegister::REG_VEL_PID_RAMP: + comms << motor->PID_velocity.output_ramp; + break; case SimpleFOCRegister::REG_VEL_LPF_T: comms << motor->LPF_velocity.Tf; break; case SimpleFOCRegister::REG_ANG_PID_P: comms << motor->P_angle.P; break; - case SimpleFOCRegister::REG_VEL_LIMIT: - comms << motor->velocity_limit; + case SimpleFOCRegister::REG_ANG_PID_I: + comms << motor->P_angle.I; break; - case SimpleFOCRegister::REG_VEL_MAX_RAMP: - comms << motor->PID_velocity.output_ramp; + case SimpleFOCRegister::REG_ANG_PID_D: + comms << motor->P_angle.D; + break; + case SimpleFOCRegister::REG_ANG_PID_LIM: + comms << motor->P_angle.limit; + break; + case SimpleFOCRegister::REG_ANG_PID_RAMP: + comms << motor->P_angle.output_ramp; + break; + case SimpleFOCRegister::REG_ANG_LPF_T: + comms << motor->LPF_angle.Tf; break; case SimpleFOCRegister::REG_CURQ_PID_P: @@ -187,6 +202,12 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_CURQ_PID_D: comms << motor->PID_current_q.D; break; + case SimpleFOCRegister::REG_CURQ_PID_LIM: + comms << motor->PID_current_q.limit; + break; + case SimpleFOCRegister::REG_CURQ_PID_RAMP: + comms << motor->PID_current_q.output_ramp; + break; case SimpleFOCRegister::REG_CURQ_LPF_T: comms << motor->LPF_current_q.Tf; break; @@ -199,6 +220,12 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_CURD_PID_D: comms << motor->PID_current_d.D; break; + case SimpleFOCRegister::REG_CURD_PID_LIM: + comms << motor->PID_current_d.limit; + break; + case SimpleFOCRegister::REG_CURD_PID_RAMP: + comms << motor->PID_current_d.output_ramp; + break; case SimpleFOCRegister::REG_CURD_LPF_T: comms << motor->LPF_current_d.Tf; break; @@ -209,6 +236,9 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_CURRENT_LIMIT: comms << motor->current_limit; break; + case SimpleFOCRegister::REG_VELOCITY_LIMIT: + comms << motor->velocity_limit; + break; case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE: comms << (uint8_t)motor->motion_downsample; break; @@ -241,13 +271,49 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto comms << (uint8_t)motor->pole_pairs; break; + // case SimpleFOCRegister::REG_CURA_GAIN: + // if (motor->current_sense) + // comms << motor->current_sense->gain_a; + // else + // comms << 0.0f; + // break; + // case SimpleFOCRegister::REG_CURB_GAIN: + // if (motor->current_sense) + // comms << motor->current_sense->gain_b; + // else + // comms << 0.0f; + // break; + // case SimpleFOCRegister::REG_CURC_GAIN: + // if (motor->current_sense) + // comms << motor->current_sense->gain_c; + // else + // comms << 0.0f; + // break; + // case SimpleFOCRegister::REG_CURA_OFFSET: + // if (motor->current_sense) + // comms << motor->current_sense->offset_a; + // else + // comms << 0.0f; + // break; + // case SimpleFOCRegister::REG_CURB_OFFSET: + // if (motor->current_sense) + // comms << motor->current_sense->offset_b; + // else + // comms << 0.0f; + // break; + // case SimpleFOCRegister::REG_CURC_OFFSET: + // if (motor->current_sense) + // comms << motor->current_sense->offset_c; + // else + // comms << 0.0f; + // break; + case SimpleFOCRegister::REG_SYS_TIME: // TODO how big is millis()? Same on all platforms? comms << (uint32_t)(int)millis(); break; // unknown register or write only register (no read) or can't handle in superclass case SimpleFOCRegister::REG_NUM_MOTORS: - case SimpleFOCRegister::REG_REPORT: case SimpleFOCRegister::REG_MOTOR_ADDRESS: case SimpleFOCRegister::REG_ENABLE_ALL: default: @@ -338,17 +404,32 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_VEL_PID_D: comms >> (motor->PID_velocity.D); return true; + case SimpleFOCRegister::REG_VEL_PID_LIM: + comms >> (motor->PID_velocity.limit); + return true; + case SimpleFOCRegister::REG_VEL_PID_RAMP: + comms >> (motor->PID_velocity.output_ramp); + return true; case SimpleFOCRegister::REG_VEL_LPF_T: comms >> (motor->LPF_velocity.Tf); return true; case SimpleFOCRegister::REG_ANG_PID_P: comms >> (motor->P_angle.P); return true; - case SimpleFOCRegister::REG_VEL_LIMIT: - comms >> (motor->velocity_limit); + case SimpleFOCRegister::REG_ANG_PID_I: + comms >> (motor->P_angle.I); return true; - case SimpleFOCRegister::REG_VEL_MAX_RAMP: - comms >> (motor->PID_velocity.output_ramp); + case SimpleFOCRegister::REG_ANG_PID_D: + comms >> (motor->P_angle.D); + return true; + case SimpleFOCRegister::REG_ANG_PID_LIM: + comms >> (motor->P_angle.limit); + return true; + case SimpleFOCRegister::REG_ANG_PID_RAMP: + comms >> (motor->P_angle.output_ramp); + return true; + case SimpleFOCRegister::REG_ANG_LPF_T: + comms >> (motor->LPF_angle.Tf); return true; case SimpleFOCRegister::REG_CURQ_PID_P: @@ -360,6 +441,12 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_CURQ_PID_D: comms >> (motor->PID_current_q.D); return true; + case SimpleFOCRegister::REG_CURQ_PID_LIM: + comms >> (motor->PID_current_q.limit); + return true; + case SimpleFOCRegister::REG_CURQ_PID_RAMP: + comms >> (motor->PID_current_q.output_ramp); + return true; case SimpleFOCRegister::REG_CURQ_LPF_T: comms >> (motor->LPF_current_q.Tf); return true; @@ -372,15 +459,36 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_CURD_PID_D: comms >> (motor->PID_current_d.D); return true; + case SimpleFOCRegister::REG_CURD_PID_LIM: + comms >> (motor->PID_current_d.limit); + return true; + case SimpleFOCRegister::REG_CURD_PID_RAMP: + comms >> (motor->PID_current_d.output_ramp); + return true; case SimpleFOCRegister::REG_CURD_LPF_T: comms >> (motor->LPF_current_d.Tf); return true; case SimpleFOCRegister::REG_VOLTAGE_LIMIT: comms >> (motor->voltage_limit); + if (motor->phase_resistance==NOT_SET){ + motor->PID_velocity.limit = motor->voltage_limit; + if (motor->controller==MotionControlType::angle_nocascade) + motor->P_angle.limit = motor->voltage_limit; + } return true; case SimpleFOCRegister::REG_CURRENT_LIMIT: comms >> (motor->current_limit); + if (motor->phase_resistance!=NOT_SET) { + motor->PID_velocity.limit = motor->current_limit; + if (motor->controller==MotionControlType::angle_nocascade) + motor->P_angle.limit = motor->current_limit; + } + return true; + case SimpleFOCRegister::REG_VELOCITY_LIMIT: + comms >> (motor->velocity_limit); + if (motor->controller!=MotionControlType::angle_nocascade) + motor->P_angle.limit = motor->velocity_limit; return true; case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE: comms >> val8; @@ -417,6 +525,38 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto comms >> val8; motor->pole_pairs = val8; return true; + + // case SimpleFOCRegister::REG_CURA_GAIN: + // comms >> va; + // if (motor->current_sense) + // motor->current_sense->gain_a = va; + // return true; + // case SimpleFOCRegister::REG_CURB_GAIN: + // comms >> vb; + // if (motor->current_sense) + // motor->current_sense->gain_b = vb; + // return true; + // case SimpleFOCRegister::REG_CURC_GAIN: + // comms >> vc; + // if (motor->current_sense) + // motor->current_sense->gain_c = vc; + // return true; + // case SimpleFOCRegister::REG_CURA_OFFSET: + // comms >> va; + // if (motor->current_sense) + // motor->current_sense->offset_a = va; + // return true; + // case SimpleFOCRegister::REG_CURB_OFFSET: + // comms >> vb; + // if (motor->current_sense) + // motor->current_sense->offset_b = vb; + // return true; + // case SimpleFOCRegister::REG_CURC_OFFSET: + // comms >> vc; + // if (motor->current_sense) + // motor->current_sense->offset_c = vc; + // return true; + // unknown register or read-only register (no write) or can't handle in superclass case SimpleFOCRegister::REG_ITERATIONS_SEC: case SimpleFOCRegister::REG_STATUS: @@ -439,7 +579,6 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_NUM_MOTORS: case SimpleFOCRegister::REG_MOTOR_ADDRESS: case SimpleFOCRegister::REG_ENABLE_ALL: - case SimpleFOCRegister::REG_REPORT: default: return false; } @@ -468,17 +607,26 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ case SimpleFOCRegister::REG_VEL_PID_P: case SimpleFOCRegister::REG_VEL_PID_I: case SimpleFOCRegister::REG_VEL_PID_D: + case SimpleFOCRegister::REG_VEL_PID_LIM: + case SimpleFOCRegister::REG_VEL_PID_RAMP: case SimpleFOCRegister::REG_VEL_LPF_T: case SimpleFOCRegister::REG_ANG_PID_P: - case SimpleFOCRegister::REG_VEL_LIMIT: - case SimpleFOCRegister::REG_VEL_MAX_RAMP: + case SimpleFOCRegister::REG_ANG_PID_I: + case SimpleFOCRegister::REG_ANG_PID_D: + case SimpleFOCRegister::REG_ANG_PID_LIM: + case SimpleFOCRegister::REG_ANG_PID_RAMP: + case SimpleFOCRegister::REG_ANG_LPF_T: case SimpleFOCRegister::REG_CURQ_PID_P: case SimpleFOCRegister::REG_CURQ_PID_I: case SimpleFOCRegister::REG_CURQ_PID_D: + case SimpleFOCRegister::REG_CURQ_PID_LIM: + case SimpleFOCRegister::REG_CURQ_PID_RAMP: case SimpleFOCRegister::REG_CURQ_LPF_T: case SimpleFOCRegister::REG_CURD_PID_P: case SimpleFOCRegister::REG_CURD_PID_I: case SimpleFOCRegister::REG_CURD_PID_D: + case SimpleFOCRegister::REG_CURD_PID_LIM: + case SimpleFOCRegister::REG_CURD_PID_RAMP: case SimpleFOCRegister::REG_CURD_LPF_T: case SimpleFOCRegister::REG_VOLTAGE_LIMIT: case SimpleFOCRegister::REG_CURRENT_LIMIT: @@ -491,6 +639,12 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ case SimpleFOCRegister::REG_INDUCTANCE: case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE: case SimpleFOCRegister::REG_ITERATIONS_SEC: + case SimpleFOCRegister::REG_CURA_GAIN: + case SimpleFOCRegister::REG_CURB_GAIN: + case SimpleFOCRegister::REG_CURC_GAIN: + case SimpleFOCRegister::REG_CURA_OFFSET: + case SimpleFOCRegister::REG_CURB_OFFSET: + case SimpleFOCRegister::REG_CURC_OFFSET: return 4; case SimpleFOCRegister::REG_SYS_TIME: return 4; // TODO how big is millis()? Same on all platforms? @@ -521,7 +675,6 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ else return 1; case SimpleFOCRegister::REG_DRIVER_ENABLE: - case SimpleFOCRegister::REG_REPORT: // size can vary, handled in Commander if used - may discontinue this feature case SimpleFOCRegister::REG_ENABLE_ALL: // write-only default: // unknown register or write only register (no output) or can't handle in superclass return 0; diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index 3e52e2a..6c98900 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -14,15 +14,13 @@ typedef enum : uint8_t { 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_TARGET = 0x01, // R/W - float REG_ENABLE_ALL = 0x03, // WO - 1 byte REG_ENABLE = 0x04, // R/W - 1 byte REG_CONTROL_MODE = 0x05, // R/W - 1 byte REG_TORQUE_MODE = 0x06, // R/W - 1 byte REG_MODULATION_MODE = 0x07, // R/W - 1 byte - REG_TARGET = 0x08, // R/W - float REG_ANGLE = 0x09, // RO - float REG_POSITION = 0x10, // RO - int32_t full rotations + float position (0-2PI, in radians) (4 bytes + 4 bytes) REG_VELOCITY = 0x11, // RO - float @@ -53,25 +51,36 @@ typedef enum : uint8_t { REG_VEL_PID_P = 0x30, // R/W - float REG_VEL_PID_I = 0x31, // R/W - float REG_VEL_PID_D = 0x32, // R/W - float - REG_VEL_LPF_T = 0x33, // R/W - float - REG_ANG_PID_P = 0x34, // R/W - float - REG_VEL_LIMIT = 0x35, // R/W - float - REG_VEL_MAX_RAMP = 0x36, // R/W - float + REG_VEL_PID_LIM = 0x33, // R/W - float + REG_VEL_PID_RAMP = 0x34, // R/W - float + REG_VEL_LPF_T = 0x35, // R/W - float + REG_ANG_PID_P = 0x36, // R/W - float + REG_ANG_PID_I = 0x37, // R/W - float + REG_ANG_PID_D = 0x38, // R/W - float + REG_ANG_PID_LIM = 0x39, // R/W - float + REG_ANG_PID_RAMP = 0x3A, // R/W - float + REG_ANG_LPF_T = 0x3B, // R/W - float REG_CURQ_PID_P = 0x40, // R/W - float REG_CURQ_PID_I = 0x41, // R/W - float REG_CURQ_PID_D = 0x42, // R/W - float - REG_CURQ_LPF_T = 0x43, // R/W - float - REG_CURD_PID_P = 0x44, // R/W - float - REG_CURD_PID_I = 0x45, // R/W - float - REG_CURD_PID_D = 0x46, // R/W - float - REG_CURD_LPF_T = 0x47, // R/W - float + REG_CURQ_PID_LIM = 0x43, // R/W - float + REG_CURQ_PID_RAMP = 0x44, // R/W - float + REG_CURQ_LPF_T = 0x45, // R/W - float + REG_CURD_PID_P = 0x46, // R/W - float + REG_CURD_PID_I = 0x47, // R/W - float + REG_CURD_PID_D = 0x48, // R/W - float + REG_CURD_PID_LIM = 0x49, // R/W - float + REG_CURD_PID_RAMP = 0x4A, // R/W - float + REG_CURD_LPF_T = 0x4B, // 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_VELOCITY_LIMIT = 0x52, // R/W - float REG_DRIVER_VOLTAGE_LIMIT = 0x53,// R/W - float REG_PWM_FREQUENCY = 0x54, // R/W - uint32_t + REG_DRIVER_VOLTAGE_PSU = 0x55, // R/W - float + REG_MOTION_DOWNSAMPLE = 0x5F, // R/W - uint32_t REG_ZERO_ELECTRIC_ANGLE = 0x60, // RO - float REG_SENSOR_DIRECTION = 0x61, // RO - 1 byte @@ -80,9 +89,17 @@ typedef enum : uint8_t { REG_PHASE_RESISTANCE = 0x64, // R/W - float REG_KV = 0x65, // R/W - float REG_INDUCTANCE = 0x66, // R/W - float + REG_CURA_GAIN = 0x67, // R/W - float + REG_CURB_GAIN = 0x68, // R/W - float + REG_CURC_GAIN = 0x69, // R/W - float + REG_CURA_OFFSET = 0x6A, // R/W - float + REG_CURB_OFFSET = 0x6B, // R/W - float + REG_CURC_OFFSET = 0x6C, // R/W - float REG_NUM_MOTORS = 0x70, // RO - 1 byte REG_SYS_TIME = 0x71, // RO - uint32_t + REG_MOTOR_ADDRESS = 0x7F, // R/W - float + } SimpleFOCRegister; diff --git a/src/comms/i2c/I2CCommander.cpp b/src/comms/i2c/I2CCommander.cpp index 2aa8b04..c608e8e 100644 --- a/src/comms/i2c/I2CCommander.cpp +++ b/src/comms/i2c/I2CCommander.cpp @@ -110,7 +110,7 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu else commanderror = true; break; - case REG_REPORT: + case I2CCOMMANDER_REG_REPORT: if (numBytes>=3 && (numBytes&0x01)==1) { // numBytes must be odd, since we have register and n pairs of motor/register numbers val = (numBytes-1)/2; if (val>I2CCOMMANDER_MAX_REPORT_REGISTERS) @@ -169,7 +169,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { case REG_MOTOR_ADDRESS: _wire->write(curMotor); break; - case REG_REPORT: + case I2CCOMMANDER_REG_REPORT: for (int i=0;iregisterToComms(*this, reportRegisters[i], motors[reportMotors[i]]); // send any normal register break; diff --git a/src/comms/i2c/I2CCommander.h b/src/comms/i2c/I2CCommander.h index b978a65..7abdb64 100644 --- a/src/comms/i2c/I2CCommander.h +++ b/src/comms/i2c/I2CCommander.h @@ -14,6 +14,9 @@ #define I2CCOMMANDER_MAX_REPORT_REGISTERS 8 +#define I2CCOMMANDER_REG_REPORT 0x80 + + class I2CCommander : public RegisterIO { public: I2CCommander(TwoWire* wire = &Wire); diff --git a/src/comms/i2c/README.md b/src/comms/i2c/README.md index eac9e34..ebe4311 100644 --- a/src/comms/i2c/README.md +++ b/src/comms/i2c/README.md @@ -83,9 +83,6 @@ void setup() { //commander.addI2CMotors(TARGET_I2C_ADDRESS2, 1); // you could add another target device on the same bus //commander.addI2CMotors(TARGET_I2C_ADDRESS, 1, &wire2); // or on a different i2c bus commander.init(); // init commander - Wire.onReceive(onReceive); // connect the interrupt handlers - Wire.onRequest(onRequest); - } ``` diff --git a/src/comms/streams/PacketCommander.cpp b/src/comms/streams/PacketCommander.cpp index a8e1606..3e521de 100644 --- a/src/comms/streams/PacketCommander.cpp +++ b/src/comms/streams/PacketCommander.cpp @@ -32,31 +32,38 @@ void PacketCommander::run() { // is a packet available? *_io >> curr_packet; if (curr_packet.type != 0x00) { - // new packet arrived - the only types of packets we expect to receive are REGISTER packets - if (curr_packet.type == PacketType::REGISTER) { - commanderror = false; - *_io >> curRegister; - handleRegisterPacket(!_io->is_complete(), curRegister); - lastcommanderror = commanderror; - lastcommandregister = curRegister; - if (commanderror) { - _io->in_sync = false; - //*_io << START_PACKET(PacketType::ALERT, 1) << '?' << END_PACKET; - } - } - else if (curr_packet.type == PacketType::SYNC) { - *_io << START_PACKET(PacketType::SYNC, 1); - *_io << (uint8_t)0x01; - *_io << END_PACKET; - // TODO flush packet - } - else { - _io->in_sync = false; // TODO it would be better just to reset the buffer, since we just saw a newline - } - + // new packet arrived - handke ut + commanderror = false; + if (!handlePacket(curr_packet)) { + // TODO error handling + }; + lastcommanderror = commanderror; + lastcommandregister = curRegister; + if (commanderror) { + _io->in_sync = false; // TODO flush input buffer, don't set out of sync + //*_io << START_PACKET(PacketType::ALERT, 1) << '?' << END_PACKET; + } if (! _io->is_complete()) - _io->in_sync = false; + _io->in_sync = false; // TODO flush input buffer + } +}; + + + +bool PacketCommander::handlePacket(Packet& packet) { + if (packet.type == PacketType::REGISTER) { + *_io >> curRegister; + handleRegisterPacket(!_io->is_complete(), curRegister); + return true; + } + else if (packet.type == PacketType::SYNC) { + *_io << START_PACKET(PacketType::SYNC, 1); + *_io << (uint8_t)0x01; + *_io << END_PACKET; + // TODO flush output packet + return true; } + return false; }; diff --git a/src/comms/streams/PacketCommander.h b/src/comms/streams/PacketCommander.h index 95d81ab..1552e7a 100644 --- a/src/comms/streams/PacketCommander.h +++ b/src/comms/streams/PacketCommander.h @@ -19,15 +19,16 @@ class PacketCommander { void addMotor(FOCMotor* motor); - void init(PacketIO& io); - void run(); + virtual void init(PacketIO& io); + virtual void run(); bool echo = true; protected: - bool commsToRegister(uint8_t reg); - bool registerToComms(uint8_t reg); - void handleRegisterPacket(bool write, uint8_t reg); + virtual bool commsToRegister(uint8_t reg); + virtual bool registerToComms(uint8_t reg); + virtual void handleRegisterPacket(bool write, uint8_t reg); + virtual bool handlePacket(Packet& packet); FOCMotor* motors[PACKETCOMMANDER_MAX_MOTORS]; uint8_t numMotors = 0; From 417875966dddde66c1298b8b8e8a16992578f3b5 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 10 Feb 2024 03:12:16 +0100 Subject: [PATCH 23/37] bugfix in AS5600 driver --- src/encoders/as5600/AS5600.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/encoders/as5600/AS5600.cpp b/src/encoders/as5600/AS5600.cpp index 84e66e2..fa3449f 100644 --- a/src/encoders/as5600/AS5600.cpp +++ b/src/encoders/as5600/AS5600.cpp @@ -27,7 +27,7 @@ void AS5600::setAngleRegister() { uint16_t AS5600::angle() { uint16_t result = 0; - if (!closeTransactions) { + if (closeTransactions) { setAngleRegister(); } _wire->requestFrom(_address, (uint8_t)2, (uint8_t)closeTransactions); @@ -146,14 +146,14 @@ uint16_t AS5600::readRegister(uint8_t reg, uint8_t len){ _wire->write(reg); _wire->endTransmission(false); _wire->requestFrom(_address, len, (uint8_t)closeTransactions); - if (!closeTransactions) { - setAngleRegister(); - } result = _wire->read(); if (len == 2) { result <<= 8; result |= _wire->read(); } + if (!closeTransactions) { + setAngleRegister(); + } return result; }; From 6468e522970d4dcb577699242e95375ab054478f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 12 Feb 2024 15:30:18 +0100 Subject: [PATCH 24/37] change SPI operations order on MT6701SSI driver --- src/encoders/mt6701/MagneticSensorMT6701SSI.cpp | 4 ++-- src/encoders/mt6701/MagneticSensorMT6701SSI.h | 4 ---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp index 01e8d97..3b03ca5 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp @@ -31,12 +31,12 @@ float MagneticSensorMT6701SSI::getSensorAngle() { uint16_t MagneticSensorMT6701SSI::readRawAngleSSI() { + spi->beginTransaction(settings); if (nCS >= 0) digitalWrite(nCS, LOW); - spi->beginTransaction(settings); uint16_t value = spi->transfer16(0x0000); - spi->endTransaction(); if (nCS >= 0) digitalWrite(nCS, HIGH); + spi->endTransaction(); return (value>>MT6701_DATA_POS)&0x3FFF; }; diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h index 32abd1d..00d8f8f 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.h +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -10,11 +10,7 @@ #define MT6701_BITORDER MSBFIRST -#if defined(TARGET_RP2040)||defined(ESP_H)||defined(CORE_TEENSY) #define MT6701_DATA_POS 1 -#else -#define MT6701_DATA_POS 2 -#endif // Use SPI mode 2, capture on falling edge. First bit is not valid data, so have to read 25 bits to get a full SSI frame. // SSI frame is 1 bit ignore, 14 bits angle, 4 bit status and 6 bit CRC. From d4c0ce5b2fa15875c15cc7afc4944593e5ab8946 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:40:39 -0600 Subject: [PATCH 25/37] Improvements to LinearHall Changes compared to the original pull request simplefoc/Arduino-FOC-drivers#12 1. Added a version of init which turns the motor one revolution to find the center values of the sensors. 2. Moved the calls to analogRead into a weakly bound function ReadLinearHalls so it can be overridden with custom ADC code on platforms with poor analogRead performance. 3. Commented out the pinMode calls in init, which makes it possible to pass in ADC channel numbers for custom ReadLinearHalls to use without having to remap them every update. 4. Changed to use the much faster _atan2 function that was added to foc_utils recently. --- src/encoders/linearhall/LinearHall.cpp | 110 +++++++++++++++++++++++++ src/encoders/linearhall/LinearHall.h | 45 ++++++++++ 2 files changed, 155 insertions(+) create mode 100644 src/encoders/linearhall/LinearHall.cpp create mode 100644 src/encoders/linearhall/LinearHall.h diff --git a/src/encoders/linearhall/LinearHall.cpp b/src/encoders/linearhall/LinearHall.cpp new file mode 100644 index 0000000..96cfae3 --- /dev/null +++ b/src/encoders/linearhall/LinearHall.cpp @@ -0,0 +1,110 @@ +#include "LinearHall.h" + +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. +__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) +{ + *a = analogRead(hallA); + *b = analogRead(hallB); +} + +LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ + centerA = 512; + centerB = 512; + pinA = _hallA; + pinB = _hallB; + pp = _pp; +} + +float LinearHall::getSensorAngle() { + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + //offset readings using center values, then compute angle + float reading = _atan2(lastA - centerA, lastB - centerB); + + //handle rollover logic between each electrical revolution of the motor + if (reading > prev_reading) { + if (reading - prev_reading >= PI) { + if (electrical_rev - 1 < 0) { + electrical_rev = pp - 1; + } else { + electrical_rev = electrical_rev - 1; + } + } + } else if (reading < prev_reading) { + if (prev_reading - reading >= PI) { + if (electrical_rev + 1 >= pp) { + electrical_rev = 0; + } else { + electrical_rev = electrical_rev + 1; + } + } + } + + //convert result from electrical angle and electrical revolution count to shaft angle in radians + float result = (reading + PI) / _2PI; + result = _2PI * (result + electrical_rev) / pp; + + //update previous reading for rollover handling + prev_reading = reading; + return result; +} + +void LinearHall::init(int _centerA, int _centerB) { + // Skip configuring the pins here because they normally default to input anyway, and + // this makes it possible to use ADC channel numbers instead of pin numbers when using + // custom implementation of ReadLinearHalls, to avoid having to remap them every update. + // If pins do need to be configured, it can be done by user code before calling init. + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + centerA = _centerA; + centerB = _centerB; + + //establish initial reading for rollover handling + electrical_rev = 0; + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} + +void LinearHall::init(FOCMotor *motor) { + if (!motor->enabled) { + SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); + return; + } + + // See comment in other version of init for why these are commented out + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + int minA, maxA, minB, maxB; + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + minA = maxA = centerA = lastA; + minB = maxB = centerB = lastB; + + // move one mechanical revolution forward + for (int i = 0; i <= 2000; i++) + { + float angle = _3PI_2 + _2PI * i * pp / 2000.0f; + motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + + if (lastA < minA) + minA = lastA; + if (lastA > maxA) + maxA = lastA; + centerA = (minA + maxA) / 2; + + if (lastB < minB) + minB = lastB; + if (lastB > maxB) + maxB = lastB; + centerB = (minB + maxB) / 2; + + _delay(2); + } + + //establish initial reading for rollover handling + electrical_rev = 0; + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} diff --git a/src/encoders/linearhall/LinearHall.h b/src/encoders/linearhall/LinearHall.h new file mode 100644 index 0000000..d17035e --- /dev/null +++ b/src/encoders/linearhall/LinearHall.h @@ -0,0 +1,45 @@ +#ifndef LINEAR_HALL_SENSOR_LIB_H +#define LINEAR_HALL_SENSOR_LIB_H + +#include + +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. +void ReadLinearHalls(int hallA, int hallB, int *a, int *b); + +/** + * This sensor class is for two linear hall effect sensors such as 49E, which are + * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, + * the other is half way between rotor magnets). + * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). + * + * For more information, see this forum thread and PDF + * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 + * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 + */ +class LinearHall: public Sensor{ + public: + LinearHall(int hallA, int hallB, int pp); + + void init(int centerA, int centerB); // Initialize without moving motor + void init(class FOCMotor *motor); // Move motor to find center values + + // Get current shaft angle from the sensor hardware, and + // return it as a float in radians, in the range 0 to 2PI. + // - This method is pure virtual and must be implemented in subclasses. + // Calling this method directly does not update the base-class internal fields. + // Use update() when calling from outside code. + float getSensorAngle() override; + + int centerA; + int centerB; + int lastA, lastB; + + private: + int pinA; + int pinB; + int pp; + int electrical_rev; + float prev_reading; +}; + +#endif From a765332382d34c9448956b32a4e32978f1fa68e5 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:49:46 -0600 Subject: [PATCH 26/37] Example for LinearHall --- examples/encoders/linearhall/linearhall.ino | 110 ++++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 examples/encoders/linearhall/linearhall.ino diff --git a/examples/encoders/linearhall/linearhall.ino b/examples/encoders/linearhall/linearhall.ino new file mode 100644 index 0000000..9ce5470 --- /dev/null +++ b/examples/encoders/linearhall/linearhall.ino @@ -0,0 +1,110 @@ +/** + * + * Velocity motion control example + * Steps: + * 1) Configure the motor and sensor + * 2) Run the code + * 3) Set the target velocity (in radians per second) from serial terminal + */ +#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 +LinearHall sensor = LinearHall(A0, A1, 11); + +// 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 setup() { + + // 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(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + 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 From 3da319d728fde6754e2caa740f4288b7dc024d08 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:59:03 -0600 Subject: [PATCH 27/37] Fix compile error --- examples/encoders/linearhall/linearhall.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/encoders/linearhall/linearhall.ino b/examples/encoders/linearhall/linearhall.ino index 9ce5470..8103e2c 100644 --- a/examples/encoders/linearhall/linearhall.ino +++ b/examples/encoders/linearhall/linearhall.ino @@ -7,6 +7,7 @@ * 3) Set the target velocity (in radians per second) from serial terminal */ #include +#include #include // BLDC motor & driver instance From 9cc5ab85b3b1d251ca3d5ad4af2c04c8648bbebf Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 14 Feb 2024 18:45:23 +0100 Subject: [PATCH 28/37] add atmega example for simplefocnano shield --- .github/workflows/ccpp.yml | 16 ++--- .../simplefocnano_atmega.ino | 68 +++++++++++++++++++ 2 files changed, 76 insertions(+), 8 deletions(-) create mode 100644 examples/drivers/simplefocnano/simplefocnano_atmega/simplefocnano_atmega.ino diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index d165199..b3db265 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:nano - sketches-exclude: calibrated mt6816_spi smoothing + sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - arduino-boards-fqbn: arduino:samd:nano_33_iot required-libraries: Simple FOC sketches-exclude: calibrated smoothing - arduino-boards-fqbn: arduino:mbed_rp2040:pico required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - 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 smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # platform-url: https://dl.espressif.com/dl/package_esp32_index.json # required-libraries: Simple FOC @@ -48,19 +48,19 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega - 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 smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage simplefocnano_atmega - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - sketches-exclude: smoothing simplefocnano_torque_voltage + sketches-exclude: smoothing simplefocnano_torque_voltage simplefocnano_atmega # Do not cancel all jobs / architectures if one job fails fail-fast: false steps: diff --git a/examples/drivers/simplefocnano/simplefocnano_atmega/simplefocnano_atmega.ino b/examples/drivers/simplefocnano/simplefocnano_atmega/simplefocnano_atmega.ino new file mode 100644 index 0000000..5bbe119 --- /dev/null +++ b/examples/drivers/simplefocnano/simplefocnano_atmega/simplefocnano_atmega.ino @@ -0,0 +1,68 @@ +#include +#include +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "drivers/simplefocnano/SimpleFOCNanoDriver.h" +#include "encoders/as5048a/MagneticSensorAS5048A.h" + + +MagneticSensorAS5048A sensor = MagneticSensorAS5048A(PIN_nCS); +SimpleFOCNanoDriver driver = SimpleFOCNanoDriver(); +BLDCMotor motor = BLDCMotor(11); // 11 pole pairs + +long loopts = 0; +int iterations = 0; +float volts = 0.0f; + +void setup() { + Serial.begin(115200); // enable serial port + delay(5000); + SimpleFOCDebug::enable(); // enable debug messages to Serial + + sensor.init(); // init sensor on default SPI pins + + // read voltage + SimpleFOCDebug::print("Bus voltage: "); + volts = driver.getBusVoltage(5.0f, 1024); // 5V reference, 10-bit ADC + SimpleFOCDebug::println(volts); + driver.voltage_power_supply = volts; // set driver voltage to measured value + driver.voltage_limit = 10.0f; // limit voltage to 10V + driver.pwm_frequency = 30000; // set pwm frequency to 30kHz + driver.init(); // init driver + + motor.linkSensor(&sensor); // link the motor to the sensor + motor.linkDriver(&driver); // link the motor to the driver + + motor.controller = MotionControlType::torque; // torque control + motor.torque_controller = TorqueControlType::voltage; // use voltage torque control + motor.voltage_limit = driver.voltage_limit / 2.0f; // limit voltage to 1/2 of driver limit + motor.voltage_sensor_align = 4.0f; // align voltage sensor to 4V + + motor.init(); // init motor + delay(100); // wait for driver to power up + motor.initFOC(); // init FOC and calibrate motor + + Serial.println("Motor ready."); + loopts = millis(); + + // Set the motor target to 2 volts + motor.target = 2.0f; +} + + +void loop() { + motor.move(); + motor.loopFOC(); + long now = millis(); + iterations++; + if (now - loopts > 1000) { + Serial.print("Iterations/s: "); + Serial.println(iterations); + Serial.print("Angle: "); + Serial.println(sensor.getAngle()); + loopts = now; + iterations = 0; + } + if (now - loopts < 0) + loopts = 0; +} From 59e3502d5d968cd2a174ee5d25fb3702b1b11ba1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 14 Feb 2024 22:06:14 +0100 Subject: [PATCH 29/37] exclude linearhall example from ESP32 --- .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 b3db265..3f98490 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -48,7 +48,7 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega + sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega linearhall - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json required-libraries: Simple FOC From 3a4d6714dfc43b661989921fd1228bc3705369f4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 20 Feb 2024 08:55:51 +0100 Subject: [PATCH 30/37] add HysteresisSensor --- src/encoders/hysteresis/HysteresisSensor.cpp | 46 ++++++++++++++++++++ src/encoders/hysteresis/HysteresisSensor.h | 23 ++++++++++ src/encoders/hysteresis/README.md | 36 +++++++++++++++ 3 files changed, 105 insertions(+) create mode 100644 src/encoders/hysteresis/HysteresisSensor.cpp create mode 100644 src/encoders/hysteresis/HysteresisSensor.h create mode 100644 src/encoders/hysteresis/README.md diff --git a/src/encoders/hysteresis/HysteresisSensor.cpp b/src/encoders/hysteresis/HysteresisSensor.cpp new file mode 100644 index 0000000..841b2c0 --- /dev/null +++ b/src/encoders/hysteresis/HysteresisSensor.cpp @@ -0,0 +1,46 @@ + +#include "./HysteresisSensor.h" + + +HysteresisSensor::HysteresisSensor(Sensor& wrapped, float amount) : _wrapped(wrapped), _amount(amount) { + // empty +}; + + +void HysteresisSensor::init() { + _wrapped.update(); + _window = _wrapped.getMechanicalAngle(); + this->Sensor::init(); +}; + +float HysteresisSensor::getSensorAngle() { + _wrapped.update(); + float raw = _wrapped.getMechanicalAngle(); + + float d_angle = raw - _window; + if(abs(d_angle) > (0.8f*_2PI) ) { + if (d_angle > 0) { + if (raw < (_2PI - _amount + _window)) { + _window = _normalizeAngle(raw + _amount); + return raw; + } + } else { + if (raw > (_amount - (_2PI - _window))) { + _window = _normalizeAngle(raw - _amount); + return raw; + } + } + } + else { + if (raw > (_window + _amount)) { + _window = _normalizeAngle(raw - _amount); + return raw; + } + if (raw < (_window - _amount)) { + _window = _normalizeAngle(raw + _amount); + return raw; + } + } + return angle_prev; // no change +}; + diff --git a/src/encoders/hysteresis/HysteresisSensor.h b/src/encoders/hysteresis/HysteresisSensor.h new file mode 100644 index 0000000..82e596e --- /dev/null +++ b/src/encoders/hysteresis/HysteresisSensor.h @@ -0,0 +1,23 @@ + +#pragma once + + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" + +class HysteresisSensor : public Sensor { +public: + HysteresisSensor(Sensor& wrapped, float amount = 0.0125f); + + float getSensorAngle() override; + + void init() override; + + float _amount; +protected: + Sensor& _wrapped; + float _window; + +}; + diff --git a/src/encoders/hysteresis/README.md b/src/encoders/hysteresis/README.md new file mode 100644 index 0000000..894b296 --- /dev/null +++ b/src/encoders/hysteresis/README.md @@ -0,0 +1,36 @@ + +# HysteresisSensor + +A simple wrapper sensor which adds a configurable amount of hysteresis to any other sensor. + +Set the amount of hysteresis, in rads. The hysteresis window will have a width of twice the set +amount. + +## Usage + +```c++ +// for example, using a MA730 as the real sensor +MagneticSensorMA730 sensor = MagneticSensorMA730(PIN_nCS); +// wrapping sensor with hysteresis +HysteresisSensor hysteresisSensor = HysteresisSensor(sensor, 0.01f); + +void setup() { + ... + + // init real sensor + sensor.init(); + hysteresisSensor.init(); + motor.linkSensor(&hysteresisSensor); + + ... + + // it may be better to run the calibration without hysteresis... + hysteresisSensor._amount = 0.0f; + motor.init(); + motor.initFOC(); + // switch it back on afterwards + hysteresisSensor._amount = 0.01f; + + ... +} +``` \ No newline at end of file From e50a40ba4d1cec44c789d9a3ffaf6549af9c388e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 20 Feb 2024 09:36:52 +0100 Subject: [PATCH 31/37] refactoring registers --- src/comms/SimpleFOCRegisters.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index acc97f4..76c3fbd 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -473,22 +473,22 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto comms >> (motor->voltage_limit); if (motor->phase_resistance==NOT_SET){ motor->PID_velocity.limit = motor->voltage_limit; - if (motor->controller==MotionControlType::angle_nocascade) - motor->P_angle.limit = motor->voltage_limit; + //if (motor->controller==MotionControlType::angle_nocascade) + // motor->P_angle.limit = motor->voltage_limit; } return true; case SimpleFOCRegister::REG_CURRENT_LIMIT: comms >> (motor->current_limit); if (motor->phase_resistance!=NOT_SET) { motor->PID_velocity.limit = motor->current_limit; - if (motor->controller==MotionControlType::angle_nocascade) - motor->P_angle.limit = motor->current_limit; + //if (motor->controller==MotionControlType::angle_nocascade) + // motor->P_angle.limit = motor->current_limit; } return true; case SimpleFOCRegister::REG_VELOCITY_LIMIT: comms >> (motor->velocity_limit); - if (motor->controller!=MotionControlType::angle_nocascade) - motor->P_angle.limit = motor->velocity_limit; + //if (motor->controller!=MotionControlType::angle_nocascade) + // motor->P_angle.limit = motor->velocity_limit; return true; case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE: comms >> val8; From 0d149e574dea143b81888b5747c29c9f0f3b1794 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 20 Feb 2024 12:27:01 +0100 Subject: [PATCH 32/37] change order of initializer --- src/encoders/hysteresis/HysteresisSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/hysteresis/HysteresisSensor.cpp b/src/encoders/hysteresis/HysteresisSensor.cpp index 841b2c0..f0058d8 100644 --- a/src/encoders/hysteresis/HysteresisSensor.cpp +++ b/src/encoders/hysteresis/HysteresisSensor.cpp @@ -2,7 +2,7 @@ #include "./HysteresisSensor.h" -HysteresisSensor::HysteresisSensor(Sensor& wrapped, float amount) : _wrapped(wrapped), _amount(amount) { +HysteresisSensor::HysteresisSensor(Sensor& wrapped, float amount) : _amount(amount), _wrapped(wrapped) { // empty }; From 3a11d581d73018d3836614d674210e017da43661 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 20 Feb 2024 14:06:55 +0100 Subject: [PATCH 33/37] add some missing registers --- src/comms/SimpleFOCRegisters.cpp | 14 ++++++++++++++ src/comms/SimpleFOCRegisters.h | 1 + 2 files changed, 15 insertions(+) diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index 76c3fbd..d83523e 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -245,6 +245,12 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_DRIVER_VOLTAGE_LIMIT: comms << ((BLDCMotor*)motor)->driver->voltage_limit; // TODO handle stepper motors break; + case SimpleFOCRegister::REG_DRIVER_VOLTAGE_PSU: + comms << (((BLDCMotor*)motor)->driver->voltage_power_supply); + return true; + case SimpleFOCRegister::REG_VOLTAGE_SENSOR_ALIGN: + comms << (((BLDCMotor*)motor)->voltage_sensor_align); + return true; case SimpleFOCRegister::REG_PWM_FREQUENCY: comms << (uint32_t)((BLDCMotor*)motor)->driver->pwm_frequency; // TODO handle stepper motors break; @@ -497,6 +503,12 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_DRIVER_VOLTAGE_LIMIT: comms >> (((BLDCMotor*)motor)->driver->voltage_limit); return true; + case SimpleFOCRegister::REG_DRIVER_VOLTAGE_PSU: + comms >> (((BLDCMotor*)motor)->driver->voltage_power_supply); + return true; + case SimpleFOCRegister::REG_VOLTAGE_SENSOR_ALIGN: + comms >> (((BLDCMotor*)motor)->voltage_sensor_align); + return true; case SimpleFOCRegister::REG_PWM_FREQUENCY: comms >> val32; ((BLDCMotor*)motor)->driver->pwm_frequency = val32; @@ -631,6 +643,8 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ case SimpleFOCRegister::REG_VOLTAGE_LIMIT: case SimpleFOCRegister::REG_CURRENT_LIMIT: case SimpleFOCRegister::REG_DRIVER_VOLTAGE_LIMIT: + case SimpleFOCRegister::REG_DRIVER_VOLTAGE_PSU: + case SimpleFOCRegister::REG_VOLTAGE_SENSOR_ALIGN: case SimpleFOCRegister::REG_PWM_FREQUENCY: case SimpleFOCRegister::REG_ZERO_ELECTRIC_ANGLE: case SimpleFOCRegister::REG_ZERO_OFFSET: diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index 6c98900..7def8e1 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -80,6 +80,7 @@ typedef enum : uint8_t { REG_DRIVER_VOLTAGE_LIMIT = 0x53,// R/W - float REG_PWM_FREQUENCY = 0x54, // R/W - uint32_t REG_DRIVER_VOLTAGE_PSU = 0x55, // R/W - float + REG_VOLTAGE_SENSOR_ALIGN = 0x56,// R/W - float REG_MOTION_DOWNSAMPLE = 0x5F, // R/W - uint32_t REG_ZERO_ELECTRIC_ANGLE = 0x60, // RO - float From 72ddddc45b53fd782eaad37f6b5473ff9e4609b5 Mon Sep 17 00:00:00 2001 From: Yannik Stradmann Date: Wed, 20 Mar 2024 14:59:55 +0100 Subject: [PATCH 34/37] Fix compiler warning in MT6835.cpp --- src/encoders/mt6835/MT6835.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp index ff0ac97..17774a2 100644 --- a/src/encoders/mt6835/MT6835.cpp +++ b/src/encoders/mt6835/MT6835.cpp @@ -91,7 +91,7 @@ uint8_t MT6835::getStatus(){ uint8_t MT6835::getCalibrationStatus(){ uint8_t data[3] = {0}; data[0] = MT6835_OP_READ << 4 | MT6835_REG_CAL_STATUS >> 8; - data[1] = MT6835_REG_CAL_STATUS; + data[1] = MT6835_REG_CAL_STATUS & 0xFF; spi->beginTransaction(settings); if(nCS >= 0) From a542a71eac2c37e3d080906e47d1520439e6e5be Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 28 Mar 2024 10:53:59 +0100 Subject: [PATCH 35/37] very simple trapezoidal velocity planner --- src/utilities/trapezoids/README.md | 68 ++++++++++++++++++ .../trapezoids/TrapezoidalPlanner.cpp | 52 ++++++++++++++ src/utilities/trapezoids/TrapezoidalPlanner.h | 43 +++++++++++ src/utilities/trapezoids/pos_0to10.png | Bin 0 -> 11035 bytes src/utilities/trapezoids/pos_10to0.png | Bin 0 -> 11656 bytes src/utilities/trapezoids/vel_0to10.png | Bin 0 -> 12471 bytes src/utilities/trapezoids/vel_10to0.png | Bin 0 -> 14326 bytes 7 files changed, 163 insertions(+) create mode 100644 src/utilities/trapezoids/README.md create mode 100644 src/utilities/trapezoids/TrapezoidalPlanner.cpp create mode 100644 src/utilities/trapezoids/TrapezoidalPlanner.h create mode 100644 src/utilities/trapezoids/pos_0to10.png create mode 100644 src/utilities/trapezoids/pos_10to0.png create mode 100644 src/utilities/trapezoids/vel_0to10.png create mode 100644 src/utilities/trapezoids/vel_10to0.png diff --git a/src/utilities/trapezoids/README.md b/src/utilities/trapezoids/README.md new file mode 100644 index 0000000..46fee98 --- /dev/null +++ b/src/utilities/trapezoids/README.md @@ -0,0 +1,68 @@ + +# Trapezoids + +Classes for trapezoidal motion profiles. Intended as a starting point for your own higher level control schemes. + +## TrapezoidalPlanner + +Produces trapzoidal velocity profiles for the motor, used with `MotionControlMode::velocity`. + +The following graphs show a commanded angle change from 0 to 10 radians executed by the trapezoidal planner. The accelleration limit is set +to 1rad/s/s while the decelleration limit is set to 0.25rad/s/s. The maximum velocity is set to 5rad/s, which is not reached, resulting in a triangular shaped +velocity profile. The position profile is nice smooth shape, as expected. + +|Velocity|Position| +|---|---| +| ![Velocity Graph](vel_0to10.png) | ![Angle Graph](pos_0to10.png) | + + +In the following example the angle is commanded back from 10 to 0 radians, but this time the velocity limit on the planner is set to 1.25rad/s. You can see the trapezoidal shape of the velocity profile (negative because we're going backwards). + +|Velocity|Position| +|---|---| +| ![Velocity Graph](vel_10to0.png) | ![Angle Graph](pos_10to0.png) | + + +### Usage + +The following *incomplete* code example shows only the parts relevant to integrating the planner: + +```c++ + +Commander commander = Commander(Serial); +BLDCMotor motor = BLDCMotor(7); // 7 pole pairs +TrapezoidalPlanner trapezoidal = TrapezoidalPlanner(5.0f, 1.0f, 0.25f, 0.2f); + +float target_angle = 0.0f; + +void onTarget(char* cmd){ commander.scalar(&target_angle); trapezoidal.setTarget(target_angle); } + +void setup() { + + ... + motor.controller = MotionControlType::velocity; + trapezoidal.linkMotor(motor); + motor.init(); + ... + commander.add('T', onTarget, "target angle"); + trapezoidal.setTarget(target_angle); +} + + + +int32_t downsample = 50; // depending on your MCU's speed, you might want a value between 5 and 50... +int32_t downsample_cnt = 0; + +void loop() { + + if (downsample > 0 && --downsample_cnt <= 0) { + motor.target = trapezoidal.run(); + downsample_cnt = downsample; + } + motor.move(); + motor.loopFOC(); + commander.run(); + +} + +``` \ No newline at end of file diff --git a/src/utilities/trapezoids/TrapezoidalPlanner.cpp b/src/utilities/trapezoids/TrapezoidalPlanner.cpp new file mode 100644 index 0000000..af15e5a --- /dev/null +++ b/src/utilities/trapezoids/TrapezoidalPlanner.cpp @@ -0,0 +1,52 @@ + + +#include "./TrapezoidalPlanner.h" + + +TrapezoidalPlanner::TrapezoidalPlanner(float max, float accel, float decel, float min){ + this->max_velocity = max; + this->accel = accel; + this->decel = (decel!=NOT_SET)?decel:accel; + this->min_velocity = (min!=NOT_SET)?min:0.2f; +}; + + +TrapezoidalPlanner::~TrapezoidalPlanner(){}; + + + +void TrapezoidalPlanner::linkMotor(FOCMotor& motor){ + this->motor = &motor; +}; + + + +void TrapezoidalPlanner::setTarget(float target){ + this->target = target; + this->start_ang = motor->shaft_angle; + this->last_target = target; +}; + + + +float TrapezoidalPlanner::run(){ + // given the just current position and the target position, calculate the desired velocity + // assume we're within our acceleration capabilities + float d = target - motor->shaft_angle; + float dd = motor->shaft_angle - start_ang; + // if the target is reached, return 0 + if (abs(d) < arrived_distance && abs(motor->shaft_velocity) < arrived_velocity) return 0; + + // how fast can we be going, based on deceleration? + float max_v_d = sqrt(2.0f * decel * fabs(d)); + // how fast can we be going, based on accelleration? + float max_v_dd = sqrt(2.0f * accel * fabs(dd)); + float max_v = min(max_v_d, max_v_dd); + // how fast can we be going, based on max speed? + max_v = min(max_v, max_velocity); + max_v = max(max_v, min_velocity); + // let's go that speed :-) + return max_v * _sign(d); +}; + + diff --git a/src/utilities/trapezoids/TrapezoidalPlanner.h b/src/utilities/trapezoids/TrapezoidalPlanner.h new file mode 100644 index 0000000..c048280 --- /dev/null +++ b/src/utilities/trapezoids/TrapezoidalPlanner.h @@ -0,0 +1,43 @@ + +#pragma once + +#include "common/base_classes/FOCMotor.h" + +/** + * Trapezoidal planner + * + * Very simple class to demonstrate higher level control on top of the FOC library. + * + * This class is used to generate trapezoidal velocity profiles for the motor. + * Configure the motor in MotionControlMode::velocity. Link the motor to the planner, + * and set the target angle. The planner will take care of controlling the motor + * velocity to follow a trapezoidal profile. + * Call the run() method in the loop, and use the return value to update the motor + * target velocity. Call the trapezoidal planner less frequently than the motor.move() + * method, to ensure the motor has time to reach the target velocity. + * + */ +class TrapezoidalPlanner { +public: + TrapezoidalPlanner(float max, float accel, float decel = NOT_SET, float min = NOT_SET); + ~TrapezoidalPlanner(); + + void linkMotor(FOCMotor& motor); + void setTarget(float target); + float getTarget() { return target; }; + + float run(); + + float arrived_distance = 0.02f; + float arrived_velocity = 0.2f; + float max_velocity; + float min_velocity; + float accel; + float decel; + +protected: + FOCMotor* motor; + float target = NOT_SET; + float last_target = NOT_SET; + float start_ang = 0.0f; +}; diff --git a/src/utilities/trapezoids/pos_0to10.png b/src/utilities/trapezoids/pos_0to10.png new file mode 100644 index 0000000000000000000000000000000000000000..3207fa3ac70ef049aa11773bcc2a047cb910fb6c GIT binary patch literal 11035 zcmcI~by!s2xA)92z>reX$k0ejNenPZHv-Zj-QCR~5`utqNeLndA}t{y-3lm;NOyO6 z5BmMx-|uIza)aCJ9lD_aLk5J)~MITce&bCNWC`13j)9rDo{fgf=JF;a0Y zPrf{#C;^lgMgU{=vUGuVhepCu2x$sgh1g`eUZAr>uqRl0a&u#!p;NK#LZasf40b;s zd;QQ`?Qg!ky3A@EGPXAX_3+2aCxO{P@%IPrMqK*xnk_C+d@=>mN${$JweTOxbztM+ zK_UkSg6mEL!7+UT+GE6*%YuM>k6x?x{p3%y$e>nz5mPkGYw6=G8O!>t>gaa>&F^Vm zK1M#_n0QIWyO$9Rd^En+-lJ6zOAh4>THbcZoYoadk>b@GtF+9;yAth)Snpg1$F8ef z3RTjxN=uRYtsaV%)iddVk3#H*DeQV%of)RFK7!?pVu~wj%T<;a=*6!d@MWFOPH*!3TKY%c%2Mn?GCX&?f#MlI&4TvC(mi^9zW#? z!8+{`zpTy>0W>?$3-@q*aM9J>SVa`cEKz3pP3m;-e6)BKWAvlgWRu^NJjQz`sD-&9 zsQAJ!>LRQwzt{$23bGw~A}5zuzvghHcqHq=)POM?(;V6S@*XlSzs0nNTqQ=hi=w@& z!%TzcJdIvicm@6z%N=Vpn&L}Sw^@Zp>4P7Hn{hAO-a=}I6EP^MUEe# z*`B^==t?Jq&qbyaHXS7zBO1NghWAT+VR=u)hpLHN{2q5ld1vb4=AQMQ*qQa2Ybc~G zeqI?yw+BwIX@-l#FstymVDf}e2ME*0#H|JSC6yV zobL~(toD{ClqWc+?7j+?p;O1b&(TM(2rkTW%W{i8&_LxrEtoHCeXLuoUg)BcXJV@S zs>G&@JU^}Y)8h)&{gQ=`M5?{=y>d}STJNe&)5^BhAPQyQbAOtDfn zA04q_s%EHGoqAW9Q2E-e`haMeZF#g$L*#{%wDl<8TIPE9THw(zn?4)I-R3@wWQ^o{ z$#+U9OYW5fd^h_p``!5aupO=dZH-4wnH{HH+}O7bs}0kQ^9}zo?&6lQT-#4JE&?wE zVr^%}!vqHT+XYf>cgCCYgx?mHEf+D3Oci@dZWam}O-qTYG%8=loJ8V3=~j`;)qLx& zy(%n~-ezXO!$wX<@unf8;iiFTHn8D!!=hi8-(x@fW6I-u>%_ zS7lViJJTz@&FaLe*y@Q@-GHe(raS7~>2OClO<2`E%)Mmw$RM6Wm(-W^m=rC=qaWU1 zlfs!&zCt1kRBb3O3TU-sx* zPpo?8{^9V!YKtL<;fT?OVfZS=@Zzv2kMq|j!^w*c!%sH4T$DU6Csj5+Y!11Wdj&7# z?Y12Xt?kEH_9>dvSbaXIJ|OWI^bOi`JJJ8;IYhO*y=t^IyZ&a0cv*A(a6)JJVQ1Z( z_t@~9bNWx=;F#dt-`c-}YDqKuKUl_L2w{}MK9S2~$G}Q)1Bfh08S(9K!k?jq)}c#e zISZKxwJ<@$K1$U-M_>!!58_{Q;!=@PYTrHKpAiZ0^^t=Ln7K+DbzI61_YA(eldqi5 z_;~wU(puQs)Y_c~a}Nq1j7-#5u$z^47k6VV=r6kNf7{1g>?S?qm}q{=+9saIn&+Ii z|Ah&soJ+$bsbcNOtj}y{VKFMLBdw#MgFA~rRlu4f>UA}{ZPI4vY+GR4;e7JG?AeDa zjhiZq@uiWaUnJv6j`tagcZ!#bxzUY7727uXe^-CDr5%wPxl^cS(P6=FwZKQrr-q#z zw*DnrZ%R+8vO#7gpZ*p9w6MSb$vYwS03OUfy|mE`C8>-zR7AwRcM?#Dx~7t*xbtu4 z-Knfxgk1F%V-D-#g~QkZI6->h#$84~nhm|rGnUcVb=gnK!nB#o983yZN?g8wwQKn< zU>|8eGCpHyUsLO`-RSvXa|*i#TZat)X8u$v@KwO8Pw#?1_g{RL$PXsDny7E{Y9GS; z`NFqzT(MNKPpR2q$*Jii@mTGygo=aW+*tjj>0vhK1m_pdK!e%3A4zN}q)GTrICDP? zyI9$|{ObbuPu?$kE&F>Dc*}dqvxl+F3d$TR&Ok8Z@Hv3Y-|6o~`OO zS8F#e`^@Ao@tfo9d`G+`a1c1y%a;|GhbIzZ1cAAWWGmb?nBVWDP+$4U`^f7P8@pHh zsHm+~t#Wb~GvqF&70z<`v>AE&>IX)2c;L&Uu7o%9@0QjMO^AcHMM-^1uZ0eM+OgV+ z8#CUibPlQ?j;wr386&l|H?%+P4N2T~Y>~}zUsNe4yu{ro~ z%b9<+x@B@~{p(~v|Gu_;s-}*?Y10m$*pAq$v;WWLfQZ0Vzul|3o#uDVjn`P$BaKas z?WepupPO^MgRj~)BGMu{<4Eo$OZ*5hyl!0IeW`)Dz7F0+QK`{qBgmKVo6i_l*c<9Bk;cz&@&B97VLt6G9cHl~k-p0ejMTDE%+uNJVn~%%c&6@iG z5U;s;c)59bIROezcV8zDQy)$zcZR={{A(U*OLucOTNe*oXD9gWyryQ(o*rWK^tXck z`{%EomOi%smgMCA&$568a^F7Te!#`U{lBq!*joKx*lwTvW&5+Pzv2+LlZmL?`dB*X zN!vOCRs~cO=j9Vb{1Nj%p8Q+Uzno9pE#0J?9RWrU@qcg2Kg|F6@c(4|BU1O@BKZaR z{!`?CJoyjK+a-vox!D5JOmA%{{s6)KKkofQkKn#F@P8Qm?{5CN3v8!2HiG-VT|*q( z=3L4U1fp?Kke1Z)0dLPvyHR)hw(dI*3q|1|b74dvEDiqq(c$@x1#Z7|BHu4mE03kx)uJkATjp@!QRUlH`r&e{=-fveW z*svR`+}CbA%_Th4PL-`Jm)lc5!b3n17(4{|6y-3_IY|J95+FffC;}YHau@vP0u>K| z4#z)ri2XwcYD*OPheIA5YAw$rC(Ul9Zv zA@dYOF@uDE$lyL2n13V@!0sdWaIv~p03`qk1c+j^zoRe`xKB9L-2JaYuDe) zH_@3NKX4`|bQ7(r?h_Lyx*cy+Uh0{~YgTaoHBSZMTH~_b`zOoX!ROl5&wV|Lwb&|a zPvlcloqJx3(#lup9?a3shL8U>Iip_g?tHOPWh$3ZN@wT$?t_-9D$eDDYGqYb1;I?u zy7bi9S(@yxuF6pl|1cRP`qPUQBp@+9_AvMLHd%P@g}?)KBYY9l>Er;eOQd`-C;Gl@6}`VtZwDY z>$a;OqJ#6@{Pz(3%asV<${jqSjI6ArBDIGWetun42oi*zLloed6Zc*8pKcA=jzBEE zFGly~eD=mtR=<2nKm?q%X08^GOQfcj7qulI=mB4*K^aRXv`)||9z_rI^+iNnLolk` ze996t8OSYYrt+>(h913mFA=UR`LS6Xm2bd&+mgJ+v$e+`{K>L7mQN@bIn{nk`@KqQ z#6G9I0*qvU4oj;|*_u3m!Qqn~s4p%9*e&6{3%{-%{>a=HWvl!6*><6;^WoIMLFXWa z>*^bRLbG@|FATcZNFNd+263;C3E+LmymQC4@0U7(e#%KwMQ;`XLdDLF14is%y=_wqx7nP;LOmPzV72oa0g z^aYq;C_V97b5Jo$zJVaG)B8Khd({m-z0?rc8H7?uf^Bu#Db&ezwdi%uJ~7;h%AuBu*nu6Bj6SiSKtf^=*bxh|b!+(L=K%m|28G;6pakUkAsDI{zi|n(G+z_+ zWb3wL--1{dBmy<6Fz_tTd_ZC3f+Tl&l7+gFE~Ir2x|UCqEntn!44~FZhc*n(V*!S& z#J33>u>yF=$|WcZLYcy-y#@u>GH5V_R$-g~p-LA%3{LPI#5u^SC2XYy0O~snVJ-+P ziyhgDNR7lZV?*x6*tW%UKpn_(?SGsQ)rtl~)f)R#XXpq-!7VktpRLSw_!(Kl0BD7h zX3~eWK7~{ZVji0Bp24KO62r9LGYwkti3>9^0s3g5+FN@E@nrQ0vSuPf_EU-$`Iq(= z%Z{>41>TJl|r1RERxBJ@tA=#A|8CBOx^uT)LT4WnVG@f zKF%^QPzAsWs1l(8T?ldkeX49AH1*EugA9Zm-#(KfMgw{b=CJG=y2M1yguz<>u%fqb zE!+?FPJMmjKoC;wuYJ`Y)q6*ZAzjZ_x3;oMKLqWftKsDiXN$e+PbE_o6J{qww&H_Z z3KiH#V-Fa~R&mJhWSv z6!n!I4MCJ9t$VMWhS9m07TXSgwLeKuOH0ap=fJ1Uq!L7 zf|#-1a8Ewi%vxKT3wbWBn$<%*u*`d)=B)POQcC=%_fqhC&QlG%mi{P?fk@)l@B9eO zyb8YoFj_seF*6gug@eOXUU+_WuDN@`qWOgWLmu@#6dm?ewPE~sKh2=al1E>u^F@nZ z#DxGnlluA%%yIY1`o>Ly*Ox1`*m_@5J^XB+v)?OtUlN20LJty?__Z^Y5d0Idc1o}n z0qjT&rWy_P(N3ep-VRs6>?e;)rsmjqR1(hFs2Xp23QJzgc*{(w3uZ;qcyzV(PoKRb z$!p*28~-5s5TqOQC}V}i-F!!2FlM^T!8P&O5(&+Hngpapo4fq_DnH<|DCA{^m6G`E z2Wb}%{3zrBdYWj$6)z(ujQsA|zO=E^-6}Wla;KwD)VSbGsEzNGR>l%K3Jh)7B{~|x zdf*fsa#+a@Q2-)2bH$iEA0!-o3dOfuD@Zl7)%uY$qDnJl-qyN4fgK9LI%BY{@~c9F zSi|`QA0dHQK86O>U_;vT6{4%vzJ&2S?SV01O2YMS&t+bwpq`AJojE~y5xx*91JigI!!@I$P|=AK%B!+ z5+ViZrv#{ASLdbdWm5lV1eM$n7AP#6MP3&L0N~WZSnGN8>WtNJwgwZ6D=>Fg9*H~} z1Oxp_Qu2Z%DdsX~=%R;t?LK6+RTSMqJqtkHT%FOe) z@UpUUTB?f6u^{{1Jx;PHzzmR}hn#g2d|!Gc31Ex>NOW|_!a6jOwEEYNg}GaPI`_~T zyF2n_7fP2nkmOGP=+aT}xiQ);l1_`S|N1~y;4)!A`w8TB39If09^L8r&cR?9B!k0#3;aS&w68SbyK zAGb6!82^2U`0CntY7K}EpPNvHXjLZAS#*#R=j1vvHtifb@NVp$(`|Y)b zlkyDh0mZn`8_ZQ)*b&`RexD5iRT{>EaYVUE@HiDmbwvf&`}e1_PBR8Y%)_o(n&VY& z)sWcsSv%P$kl#XlD|2V9uYNYaI5qD$o27Yzr_kk_s3#Hn@F2$caKLIhqR}O9t5)cz zTccCAg{kRde0*bnNdXEORW?HM*R3r^=}FU_Spm)cFYn0eU}sUF4$)(35yu{sJaMt5 z$K{?UlswYjJLk{&`#4#1eyv@)3mrSO%#4v*%T`X#2czF%NiND^y1$MPJ1%{x4}X08 zhQhGjXD=%=GtqiDTd^Lot)l6BaTN8?C-OYI4d*%MC`0S<;8Bu*aE($cK$q@oVB8`e z?|<5SQt+ZPpM$%_K9#awvNb+LRqAilF3DqXBM0JNc*y>I@oaT;w36HT!QEYEtZJc% z$1zz9Q4opjp8hg3?Hlv=*&R!XR!|L2AVyqd_?5+MS~g=y5DIbSjtd35(Q$j9Qv0rl zefsKl#nU7OLJ?v~lOp7jXjP+;Tj)WawlkUS=fdtn<|f*V1Sn&`Esc3g<=_yoqwB`t zouGZhoVyT1k4S`cD;qep=t-x7)dJhRnoMs4O4A^V0|t+G03vH;f81ENVMsCS18#Ui z1TqJhMf}$U9tsXomch#J5QH(RwXSWSC!_{y#2`H&H(E)J{>m~@s_6_2Je1(jRht>% z_salnMN>$x@dkh|#pMu?G~FKGsc@D^#9=fV2Byrf{s)BQHJ~>4F{ak@GUBh=MX@DJ zF8Vsv7BEyS2rXQMqzRcD3=U>fWFB?PiAXRNQT&~>6@{b+AR~&EnTWy~jF&~5e5~z{ zr=RJ{#X1rmA_;DA#4~eycZ5Zaq58r3ZTV5JYX%_QmMic;JRXE{hl_j3!oLBlktU;! zqAdi@gM#_w{F&7omkkW zR>L0IMJQhdBnN|qYIB!F^R@ng6&1t~Z7iNAoh1v5e2=lR1mg}1M`JcTsL8-kGcF+A z@CCnW`i4UtTVi={bxr9R37p^#pzGpe`~v#7w>asW^?;K;;d!e(us8)VrW=%{1f0(1EUN`yycx6GNed!{5;4wu zj|c6{m8YQJrtPEv&^S4XyR*D%9JV|lUHRhKekk4y37TK>&D!T=&U{8J1(sKC-oY8J8a3x1m*gpnI2rsY!AYqsGfT;1`rSv*+s z$Y6q=gd0k!-)3)2^3mp|J+DNtOy#6JWetk2D;x&B`?b~AyIl4wE_Rm}3=WrlX8q=I zs$C6z9IeFB&3W_bP9Veh2cuLRvc8*=p^7T)S1?wDIL|_iRr~Y>i>p(Xj>J96ofrzG&oq@L*DJ7zerl&z-ID zBM#$cHx!AK@{PxUCNSP&*jSQT#aM5P*W!i-2kCm(_PafLd`QHzXP-p ztq2Tpe5-Yv`Xk5a0O6t5BZ@Ic;Y53l_P||M!P|`39dM#m(7vl1`c-T1o6ZD-@B(Vo zC!msF)aNa*2LkgdGGHqP1X+bi`z40C4-Iz1$jQ?BEteAOAGikhSo_b2^$77`0!57) z&}oqQs5oD=99sIr;9%YBdr8163MfHjDrmNLpY4tYUr1{l%YobbG*4P?ZQ6O(trtKU zmc@QNqInpi0{Z0%F&DguF@)WMFxVICho*nad5V z!^B~WgasUI!Rao%sKr)*4sHaD;FVf_Tz`nh;x1gqZO0plEQOLgQoUb*KafP{x7oP1 zJFfMgTDV%sCG^m#l>#a*3{toczH$#ZoHN&5__nw?y~I0jw+~(Ucr8o|fkl3GRN|D+ z3K?1sP$!01FDS-B8J@-gmd&nImQhXXPk&*?oCxbM2AU_A^z^t66ot!F^N&I5qu z>boUjXB+T}TR5{hCSU`3fQpbKLV$fB0yZao0rpJpP>@#v!tX~ zwP0|Uw@3@kC;DY6{FHe2NwBS%L+%i+F|{ss`t^Ri2ZqXW5-98e*();@)h3YGQ`O%3 zcX4r@>}#E&93JpJ?v#%ZhI&|me5+5LW1+@AWVOs-w-@Lr^b{%`9;|({dm-Ojf(3qC zkfy#w4x|#ve_~8cbhJ*P7UkgxLcp_av)NJ5S6NHzxy`g|P2&j**%AIDgz-RaC6fef zg8pgKv{yiY;ZaV@)>czOR8+4j*@u?w93WAd$A9$MLKi46TnTyZA>ynYoSk*D#RKUd zI?stTTuj(^yT*QARY5?ht@%%5Ub|dA3R3Usb2iorn|t9i_T0F5-n%Xpck>B6{g{F5 z(;hue_IEoOZJpizkr9D9CJ{m*NryCJ@au$weGi2h%)%a}2&bE)A5gbg zH@6_T#Z9r5EC@u{n~RZ8S0B?78~wLGTvt=Xn*gQz2cRmncDpZs&Vydv>Dw}wpjP+3 z9l^Q!2yoXJ*w+L8FFlf9U~7g$2Av$7Lp(MP(I7|#QSLIu`-P7Z4@4?YtjT6rwyV?n zZG&wSrJ!4P;XYFawH1BowER^&4orc$$7Gmq>YqI>RhP#FN=)sL`Sgi=CDku?R$Yns z6E*D$NWjn`qWO$fJ&^#xkFBQj7WY|mtPwGdY-+Cuwrbv*VMid?r!UWU^O4x3?lS0%g{Q^U#>F)P{6Pp7B z?I25{ev<2rkx%}kXtJYSl`irmK$tfInkgO!ORv=pGKEi_BwKupRg4!D9sqqk0u;am z(K5gE6Rci>hw4+2su$%^zW6hrypJ!Yk(| z?P@w)se4x$M4#VBi#?P@mfPE%me%tAwr-h~$sAnS18Nx{MiWIcRyqPX`OB9Awt|^J zJi4z_&ZU&6z9u5zyhuM^Cpi0Q-FUpTgGsLd4WN%K15USIv6hA7_N$psu9{ulEHmSA zpHKVyADTtS{|GymOgzu9Edwu6fuR?70l)S3sx36kPU-Ev&#jWmHQ4{z-^AbOQ$4j& z$TewF@%7bx7C(g3f|FwMBG8zgx=r@=%h=4#&1rxyMXY`P@ZHWy$)l+%o9r9T8K+X< zl!CRZ$xXt278?9NEyrsZ&_sACJG=QoEQ6eezhF~F6+?|it7F7q-h^AtPgBOv`P(J2 zqh3(0!S?3c?Ia~<9dTP6b-8y~)>w>HLC{?qfqMv?=L?z0NvrRV>t4#X)9{Qrp|YO` z{;)ngkGZ<;aKWHash3hc&0;94VZa(4Zb&tr_HrH{zWg0D8~<|~_xB2PDD!G{<~}bUEPBtuZK6iOK6%>(B>1QYz>HSu7cp zA`D7*+qLfioR#8~h(7R7Te#x3YxhB3p5V`9GwAdbj#Q`0iOq-Tp>s_OR}2-;%OGQf z21-Aan;pt&s}X}^#IZon5J>dtfk|IrXO`Y}Lt$xzaGwm2|y zp#bUxitaIL8Cve{Th1dQ8^O%Ez(M~&r+#nP8%O_Sm%bm)&MgN{E5ZEJhli=l3!O@H zB|uiVyuTPR$FOp=?}me;V`*o%v9{7wYdGH|;SfYfD$xV$GY{zz71w=;NZ{!NMB>AyH8lN}g+`Ovm8$b&~|3Z{wFUJZN+j zBz#ZI4_@bVt?ob@yJN7Y*@P)}RKl?F26cGiSVkOS!VVoe8l%R`5EY$1nDh5;3OT<% z4Sq$&Ehy8%=WvCeH!UdUd8w;f!)xF9!}`OG{xM$DMcB3_e zv5xxHb28pY9CIs>`lEjtBb?PveUvv*EOBLO={);2%X2(#dpq@DNc3Gb0J zd(Jy;UAyt^5`^1a)^6iW>SfxNf!7r=FJJa9EhUX@;<;wM2sj)}o#U1PDon4z+D1dW znv%xZPh>pDJd{Gm!etHFoyI60PicRyqd2a>*z#KO>Sn{{fX?ly;C|!Wq}9^KAT74L zr@X_ua)nxKQ5Y&lw<}tnyuab`nVgo^yK!Rf6jHP@<4S3~d2n&;E%V#<{XuV{=N*|A z@?-AaH_^3h{PUF)>;!LPZLYti^cXlB?KL<7{yx+4Tc<8|Y#UwCcc@`g?$V!r{PBL{ zZjJH88I9W&RYjtsae*YV(e`?|FByGTKe_gvM|w^RJr$L`)23!(pSn>WgJ3K!lm zPPLW-%^P=X>+Z6Fh%#u|UYT~k<~Ngg#pPCey)|s3V3$4pYJ+elF^KhdLK=?hmKJZ^ zMH?EC6R>kB$yX(0(8Dq-^%lPBOJg#s7AaV^@wd0T!Lf@zoJuV+#ceL;=Ce5Q7n6sp zYO|Y9$Pna?rW;5Cqaq`o%*^4Y&1~S+&iXT@$Fc2Wgf!`8n&~GeQ(&!ff4gxB&yG3l zQ6mWtyS^%GwWiI?W52<`Yb`4)tFmjrkABQG1X>MMC)d}X8}=HwMHQAZH?9%-9w1nZ%vtnPAv8wXGw9$E}pZ>Q}Vu)9Z zU7>h`jgpqmIoV}g(A#oPP$Y&LaJ0h87#cpC$Te(gi*$@6_m6Hw)XICV3q0v9r&UPJ zUrs+FqK01=1=f4Um28V6P*nyUJEx|bkxpv%qdOw5{N6?1DkyGI0t9IWf??`B%ijwykO+YDCU61t0|3%w{x^8AP!b{>6!GU>y@HIY K^oK`JL;nlwm%w!Z literal 0 HcmV?d00001 diff --git a/src/utilities/trapezoids/pos_10to0.png b/src/utilities/trapezoids/pos_10to0.png new file mode 100644 index 0000000000000000000000000000000000000000..a3dc2afc2f9a6e2e387cb424cbb645c535477506 GIT binary patch literal 11656 zcma)i1yoeu*Y+?DGNiO12ty+^lt?plhk!_TN_VFSDBYck3W9WZOAAP+ba&@>(ceGT z_kQnMA1vmcxo7XQ&pl`FeV+YX-YY3cVxm1k1A#!8(o$l|AP`bN&|ZK(1U?Nw0>U5= zT9JjQsFJj(C``%0*3`nv1O$?LAD@V#sxnC!G}y2~L4goi$M(Scgolt>&z366!HofB z2Vz6%T}&A`1e~Y2^xO>Moga`H!Dth7KeDnS-XfDQ?14iU`n2{MPF%(`)_NPS zuCLPS26Qd;KtDJmq~eemK~c~82!gNN*$tN#iK`7j6hiDuNUB&bB-_z2F~K4IeZEy^ z-cn44Rm`*%w>KBQ&&A1vg3&?vAM9}9!Kv@}QH|>tKeX3?van!2H`m%j)Ox~6v=Uq) z`)$$+$*V)s3Mq!19t^bXGw24dK>O5Kl4)R2H|T)lu=-gDRyTm#HU?6!`Nbg6=Xy5zzo$#)+)8z2?F&&`} z0wy(L#Mz~8K0%p%nX!*4v{FO0e@s?nm%1i(qC(RRup!r=2j>rLu_OA}U{)W5S$w;f zpCapO0c);bAPe^f@tf5D!EZTP_m2Q=O zLKBblQyJp7d83ipLI$FL2jZ-9q*<)Dm^bD@!hnI%vXGuX_FlPzOTu8Mwx zmL51D`a0WYos1I{j;$1Nh`8ZtBfV}5{%v~B z@AY)|UG6@*8wRqH13jI*|5ft#&9x3WKNU84;*M97;N^dvbp0}e!S>l;!diDx zOi_$&!d|qmIE50%Go~I&8Km4ahct)KLuLOg{Z9+I%`esSlydEqv-J$*qVvrQiE@(i zs$Z5U9ON$+<0^DZbxXegqWZO5ZzEtBMj$@=#p7koFXvwme#!o#c#>c$XEZWwPE$@* zt}ykrG^RA&q5Kecg<)l+N16A7jhNX8$9n2U*Sh!dAcH0Y6G3Co!}y2sWbu#kpX8I} zdyN{7N{s4`4!*&7PF~?uQTT@WP2{hkP18+-&5KRXU+}!9Us)E_=626NJdd!L84rBk z&)N1o(PDSJKAY!rZsE!on&GKDXW^|}E}dynKKVMitFY4$tXEy~l36OB9o5!&M3Y+# zjaeCp9ut48O{u-D#hvx8O|M<@==6B$VR`c8glq%vB>A%Y(&TdI^5a9^hb<3#F*h-H zNpLZegYts-F!(Tph=@r#hzei4VSB=*_S}@@G0`}Q5GO@yb*gcyYU&JIM+&?Ah`ej6 zOLB|pscD|+E7Phz14k4`{|lR=U7jQ!1;;?g{Iz4PC?<76cfu3G2MJESu-=LU=7gfv zCw<|WbT6*gr)Mn6?vsKMzWPB6*?x zx^}+{j0#TpGGC})YK%ZE0cxM$`^{bHc~p_Ar{Q_n_h*;~i& zM{H|N+DzKRI-A--Ys7;~gM6&EKVJ>TFVzmd+U&HGb-J3A-~7HcU|;0oyO_P#a>TuU z5N6UNWAxOt;jsJ=-;>MTXW!ve^SAQ=$;!@}&i3ra$7Q?~m5rl`*Ml!Qs^(mO4bIsn z&-3_(`DWdz-T71!ruKd}iG0ZYumD<3B!w0REx_=?H72CSdV?PH_CY`uvQU~Ww;p#B z4J5EwwDKK4+Hg< ztQS|Zer(udIIy_%KB+yay|x{mhOO}2jOl%PIip3~R>y3McgxX2{DH*z_iN?bGUM^( z;pN}><8ju{sPcC6R`TG;x&bmRTby_04Ho3XqQj4J6^+}CIZYQi@HiCF;sZBYLp7!} zWJ_zsS92(%Ij4C%Jx{-KD|xY^^k^iFq{xbwXmD&tFxaUmtlfbkUVa&w|5J zQ}WAdBPe$e%?sT}BS^PXXRJ}%^&(~E0i!zOX{ zrbo8XYT2g#H1P1h&eGo?qeCT~n^y>eA@b#y8ufbE^N^mr9sfin6if%Mu#%1ufWPuH#G8}1{wHqdCHzX&ZXwJp!O{bF~lS*+!oG;Uqi<974%M|!@z$fU@4#?N72xv)LKGgOnfrZVuT z5^m#bM{(Mk9%}HI57^IIhsm?UcUWp&lZ@YAE9f{kdK^AE((H8(-s*p`ZOb`Z-Zc4Z zirtF6nH&7y zo>meIK40V;KJjWf%6^>lcnKbQt9tD?&KUce?)LC}tJ8dY{io%@*83ME^Z5+|#{;`M zSL*8%OJ~V@T|)T2s#oeq?=SL}9Q)SHmM-+{^cWgbe5|hs&v$-{t+oe-P_4>Ufa#5! zySqX4JP;5M7U<1mP&@L7z)76&$UKq?cEa;ZkC&1SU01;c zL33d~k3rK@J)LrQe52nKQ@9Qbql&<+&lC4h%9@3&Ya{P$=I_#4KIK0>U%Q4kM4@Ev z=QJAu2-eUf5&p&&34F6aT!LIQq4!4@FMpEd|Y3tT~<2Qfh)6yS;r{3&ID z|J94spY`BhZO{VH2NG5hm6issDncDVWkvAgTY|@4#uXu%3>1#r~}^wD9xRm?0DgDS65dSR}L0i2QxSua9+b%+2QQ$ z%zy;5qq~ihfg7`pBh}wQ{xy!6iKCH&g`JaytqtscTmwT}XD0zl%6miq{`osk6E}^|84TWdj3oE z{t8D5ns^t0>s_pwC3 z##5}kK(`v?HG-RM{i;^hp#EUoZKCx^8UL9~&Uw4zCFdwL0&M)kj>OycY@8(pakk?* zcr)KK3QpyD7n@xY2D*!Q*;$dJTF1TqcS{tQSh1{#us@^^^b&(F^jf-QgM z71Z%ZFbW$%m$5xxX+qLKj?&Wje88rGeuT}V2&sdOrw+I*Sv_b3HkE=Oy z%)P;FC)cw94!TUiZ@rmZ7N}>d9G7JVOPk( zVY`kmiahSe=Rc*{3&6(s8;Incy|Gs~4ZJ$WGmfy783^FQi1wIMlJ7*wNCYp_kFJKS z#OIDosU80W8G;jB;xs71@q7K3(;!(tz08LO8!dxQYkWJ3k>L|UT-Bgh9I9Pj@}aPG zzQj(>$owaZ?)cd)-;WTaVOj~hloB&1Tsg!;t`@lcS(~1Vd3ZrdO!#)h$1fhW3wN8W z2?F!e<0xMFz89u3$73Lc9Cs!Vo=rmqMZipeIEv$On%Hz|+{<$>QEDv~t04ztOW^^b zCqKujnyB#mml|E~5iwYn3y9An2+>H`dMjsy1z8Y`XwS;MEuEanhAbh@5)Nh66{jZ^ z_aox@5Y;elVKKEu!pMZ)$tRSNTHZ<)b{_#z)JTLcq+%LHP41k(b>~2_F3?pdK>lvf zp8_P&mSeSMdpkYLsky1O% zn1_IE@%tI^h|{>Mnig4K!K(w~Z*1Rm-q`~Q4+0J`g{V`9D`Yu#;WwlfcII#_1CiKF z1}+J+sBCx-D^%;v&@`0Jfv9o|otaKF?#bg`&Ta>$(J$jx1h-NE*#bkRc|FOrY%=QV zY_EHBlVd(Z#zbTwoe!R??5+gRbRTNc(Ls8w` z&$`!+VvaO}Ra8`l&3_rOr(5Yz7Y|!?pR6!nWIjfxyk6V*5IWjy3b^7&Kxy@0iO$5t zM5Dap;{2eLiHScdmQHVgUNhgD2&e18^{q8nHgc_Em^vW9RFC#Pi>sxsaiyrClC{JNQE{>VyT1Nnbz0YT zr$xeA9f`riL!-jD(?s7%|NH7XC8XrHK+^Tgso_d82QXSt9o`9tj3MFI@4PpEg0I$%m#ZKJm0Ih}_GEsMMd zZB5>|GV44lxnMpawym5xv@WC(I--S|xTlu<;!8mjX=VdU(1AN%xvD`vgci(RkinN% zUUSe)CJPj%YObYCu)XUxbm@x69K)gFax-8h39IBp;qb46{Y*s^V~g0uKUD$aUI&37 zF_eDhwBi{KRCqs26a*&TFtPL60P%+b00B&Y>eL}@1h-^cvN81bRn)v4(QE+n*Y1>J=IG=CZ{fgaUF_lzP>w}32P@4JBm#vSg8(H?n@@nP zhjf1CSOXD!%oIN-H&tV`^?U|#3z7ibbPUtCB5ojacSHkIQDwDW@OU~!guOr(hJAym z&%kolvZM) z7q5x7wjYFy0AQ>cn6>|q{l^^$(N_7a=mZMV85EYXdw~&31w0y6GvzY~%#IpK>_|$C z+AbZ`x>1W#LJmH~MsV&NASbj-fqsYKuxNoE_KY| z5~3eE3_I%T@P|Y;nfo^p(C!ZN4|8i_r+7#ZPyjWi+xueC7@oQ-WI{MM! ztn0knY=!OdGQsiIS^b1P``*cRWqZW#Y%!N>(qLB{_PImDGrjNQ^)%<5%Ru@fuevk* z=#%BrZ@SdXqkzbY1+fiFw?zyZkhpe=ab(UX=Wz(=oMrOZ8DV?M+sfeCk}7sA66jhQUAr44l!BtVbH3h92`MWbkh^ z*pyONX!h^bYHE9}E21rW_fs^q@2KJav{y)&FB^VNgzP^7C9<{{=u3+V50|fT-U_m{ z%TtX(f2gR>v7nTt0Fz4HOmtH}m5-1p(=pLV9Fb(;5Ty8#iay@{HM4*%|{k=6= zukc90EFn!%heH@B2Kpd845SEyg@HZ9d_ zu>;7UUv323M4*8bK%JB=_5_N7$Z{m0Xn6XY6hv8tXl4gAOt4zgoN(6hfyb0hw9S*} zLM57i!h{uOg-M9=>M$%6(X_L4TKNR;;+Qx#aB*=@q_M>zO)kl)MQfI8czAoY7Yd5xUH#iyOmUi zA^2@4w9Hb46#pu}b9>SoL3$}8J3?V*@TL~k{F{oeDu0o}<78NzjjIZuBr4IY;>gQ~ zNtj6ApkM7)`9lR$3V|eW`@N!vhbYz^d>z{71qC%rfX5T7t_kV(9H-WwTJ^94NV0eC z%dQ$8los`7M(a3TT12f5ZHZmX?>z!1U0|I$GNdDF#sF;JF2ySSb(HWz-+sh^H|aYh z3A$`U`g0gSL+Tk6;1=ra$TeCHRm*%dq$?-@=>#+NaUhqt=hH5+-NN1*c1Yj@JOEe; z8Oiwhv4JxSM{f@L^#h_z`y!ro?#JLjB7g#y?0pCXyEqu}Q=UYnS>s*k8XCy+je z>^|c|+eKhW1|;SDlWa7Eea2}1I^d917BoRbvpg93CDo_x%?s3a%9I@!t;XBSN-GFu~=%N%{YJzRYa@Y+PzF~ZzOf%$h+p_eO;y{Ai@mk{Omd-lE5%nryft< zXeCh8AOm> z$=t9z5B1cgsz6*T8hzQYYkOy{+mq$$>+y{bXM2*%v0tVSJ0xb+tddK=OW9jfkJjKl$4(V%6|8ijbDwf7WSzJpI;z`PVc~ob ztB7bl52JLgIr3^>H)Tf@b2Mg5531Hm8|9GZNd{s(d25S&yNAJNuNp5m_Z$uuE!o-F zcvQFWhA_S!P9uZ4LRSw&bbXxbmj^-D=NdZd7!=7NBAl_mZE{qNX3H)=Nav}}RY;4g zKJu1~-JQ~5^72FEK+1bhFL6n<={6x~t|@+OLoQ40a+p@c18K3HM%jhwIoWYPC;63Bm@OH%PnFpZ z)7XwctaLqHCTce=p9v*1$V^`qjSh69gr>HcCU;+Hc#i2*jr<8cEdOXcsOh*`eairV z347m*L|Au9>>{Jd#eneqV?HTPV%x;h^_ zoWmNfrL<=!_G8~VJX3I}VEuaw;$p%8_6mu9%E~)%VYHVHU`aNUVCj9|xe<&%L{->2 z2U!S?zX>Z3+whJP!Cf`WTjl}wQh1hmcelC-@op`-< z6|{TKyz4@8yR?4hE=LO_5EwR||3bq+q{3@M27fCGY#|`?`Eb({G9p1BEyhg!i#%wc2QxQUq~j#5v2B>63JUx{3-KfQyQIF?Ze= z*SnMht}Q>7^4Wh|6};FtrZ4(?X%z=g|J2H9LBmtv-DKI2QF{U(0KO}+7FV8X6Fw&I*(z!pa6wn@vv-H42f z)WEv3Ma@bo2*k!tdIld)JzVjSzqQDrfBi+)!Llr&78xg{ztb+NdYaoGD9RWH)pdC$ z=jLMe8m}aP6t`EwuTjktM<)dkD^57)h96nx-O(wv1=MzV2OmSJrJL8qV1*%#ZyeSJ zn0f3MKc(?HPf5)tL5(3)$zdUYi+00GL@MF?@vwtN^s$WH|fr>zD9`fAUEI` zXuUv9mM#$h@SS}6zT?`!>p3D*g@Meba;=$(iKJzRx2{E$DRuN03m{;R$>=ulmgf~B zwx*vAL^@NZ9JK$ScX-Z_GMFI{4;Pct}(K16=0sI<;Jzd^oivZ!X{RZ8(6ev`&A;Bt0+Et zx^&^O+}`s}&;J!6hE*eHVw^SNxY+V_M$4>Q*!^wm$JLz=N>K0>oc~aa>Q^+_g9+oH z8-07nPcD=xR2|mk#p;AsFWyij1LO;Kh`(Qir>@=5*a2Q^EmDRb zN8Z&4a?g$ZYmSSJBTIyeVk5Ie*?99n)xvWhn@EJxA*fbprQ)0_3u5^|RJMy|c}557 zM)oexUxa;GU7m_W6w$9ICLGa; z1W^{rDx=ANCBm(;I3b$vZ%zgdmc3Vup#1qP4ZyBaxxJ_KrYw49r&eNnY#i*A#Tc}7 z671{85e&h}?!DWSSo#s(Vw_aGVJL=wm)`j2hCqHjT?WeI50adoAjf5VfYW=)k4^Pw zL8#vA8Voofm`T3p0o%U~guEIfav#^)I{h+YayRgj zY4-7uSj-s!TThWj_BbE4`xtYRW5XnXB70Xdi$j7}D@%@YQ?f7Ps3gR6ddu^t;8h75 zN33HgKnH1+lFOPEPcKO@mAnVYjMx|3<+*xcPU!1C)5-C5nNqm(;i0QNJw_VpZVS#v zmBxmK$^+?ju#Uxn-ln%|*h8doq-Qk3S@RFrP_^hs6u@uWe`I7un9>pQb<^0!zZ{yG znu?jOvhB&0Et1V;r+7p*bEcVw2@vSRO3%ePgIl}$sc2b$Jg{%4Rn~5W1ig{$nyio4T5WqbKl4^+N`--r>x!5-_zwl zq9_hWFy&Zi(UhS4wV@vXV6Om5_rIJuEkKqe+i}DLu?QSgc)*(@JpAqle4VWLwSN=>@WI%CSw;4%GpTiDjwGa15~S?U}>g*tvs1zB`PTa)#T?TiJfEtl2*l9 z_&Pwv@dNCI*TG#qgw7|ocg5cJ zV}LBK)%&WFl3kr9#@bgI8GMebX>#)NCMej?<3Ab(n!V=}4lpPC%}CH^+jucGUtx0R zQOl5&2){$1y%Pw7uHqn6+}uq}I`G<=-M0!E=g8&UeG@Y!BguMZW_W~bc4w~!rhhDa zO5=BzQBxzk=l*&~Y|Uqi=`7LOvd!YLLU9`W&7>8r)G|JKRt9U8qput+nfNLy5%kOC zQWiCw3J{Y_PgjVKtiNPRxV{lNLHZ$DjM9gnu^ASbPDJ0cy-f_qLi2&Q$-?EY3S}B9 zYic%JC-e0;$yIC7qi)B>=p;Q*#ops&(Aade=;C!V*BMo))eWt#?e(6YSIa3VbTbyE z@hD|PC~mAy)w-?&kDs$B1>? zSu?dk$4`lfIHKPgi)uizxcwV$t)bg^{TZGlz;g8^r9Wu?3&rO@Z6-Y$R$V*ZIG^})bB*0p<(*3X+LA+)k>)-wkhmXk7%#hm4yRx6J1VXH zaSapir0Ba>b<;ArrS;c`1@k#s(}ma+W^uqR;>={AW{baXqxKc}t_ZJZ8*&G&vnMZn5>RQt{3Qxy> z`Qqc@#inf$h4t+(&vyM1PDvdNd$P-`6E zd7h31m{>YkqG>V&n5#K*t)sg5dWV&#da#pGJn#PHp^xHCT53yTOYf)}AUW;OLENfE z*xF6oXQ7hd!EzGjq=Sj2!`)}{UkcHlengA)Ig=3NmWNq+IjOOS#11%mDZu@|vbC1A zhpT7hPIf68AS;aw)owpv>-f^vDM<&Wy>l9tx?B4zl9Uzs15?O==1+c6M*zhXP_g~Z zCN=<&n%&1!`}oZ5hndJp{25d~u=(zR-q`YmuVGQq{r`nXiz|qI I7ts&+KjT6jod5s; literal 0 HcmV?d00001 diff --git a/src/utilities/trapezoids/vel_0to10.png b/src/utilities/trapezoids/vel_0to10.png new file mode 100644 index 0000000000000000000000000000000000000000..f6b2edd2eca2dd7ceebcd4a17955da62778e517c GIT binary patch literal 12471 zcmdsdWmH_v((cSKxI==35Zv8^5AG5m5Zqk`3+_qq5G1&hU;#pKAKWd$28ZD8u6L66 zeBU|i%AdQ|{d0#k?4I3KUDegQpQ^6f5$dXP*ci_+0002Cg1oc_003-7w71bv5dW*I z1X%$9jB;BkDRl)YDN1!$sFkgQB>*5Fk(i9Cr8z?sI@+>LMFo-E!t){gN(fQh%9StA zBZvp&zQ;pj^ssaW_lAU{CE=6jGrnY&>4`*U0bxut^yTElyhA2qJ_JQA59uGaoO?{^ zZVtk3?`|`iMvUx@0e!qN@}Ge$fLNxXr(w6=+-9pQr1ho%DsgUgpcamRTo(p5HYj{} zD6sC*U!K*hj*Y(N;r=?1Pv(Vq7$)FJq%#3WSbD?}x<&I!WLEc%ex)ig6+A7*;)c}&ySfFoKQxeO4XA8^cbQqL5Q(@$o~LZwD2O>&Jp z&&^Zig+}8Yj7~r!>7|jKdX}j=u*92Sk@j3QGP-b-+tLdv5ySYd>g>^v{Be$(hn7b$ zI#xpQ>1`Md54pFs=Xe#!l2tj4g1Zy6sCD zN=u=Jy*|>7%`54lA1_%9l2~;2y3;Y0rXSP|6`Tf2TQQf5K8WU+q4PDA- zI%J;HB?5nHfP;4Kb+g-F02n_2c>EChjAlpu^8AYTslSupw!0Dl{>9)P=y z1cIQ2gI!GUmqWB-(Yq)qSW)dE_>4gPHV<|5i;(*ql;^-+2(4yF9Uy(>9H)657-p^A~Frs0Et>j2EA`?gw*a%kRFb6B>U4(v zwz?AWBR}|%e}w6Ug{DmH&COiS#M;Dl**rmh;e zP`$)qC)RmC7a<%i9JSky{ab8h{Yc1*teHdX1xHtTck=4)k@b=2mGzZN2&g@FSs9J$ z2u%q+9UT?T@xA_gs*abh;%t=N>0}AU!|lVLcA>1$tw^ot3knK~3Nkcs3I@;HHDEM+ zan)HvSrd!P@kFzOzQIv8i(0H=8n& zyp-blR~4$q-&TGQsP@bE%S9Aw6;&H=hm27^m6WaX@crml9jUg_?Qh{ zHBGhZY*A%=Wu|NO3Bfw^`uKo`P^6Qz^*GN~`gZS@|Jf+B9y9Ax*Z@i*O5%$|+;7jm zz4+!gX*MZ4X*4-%hs8%x<6cu{$7c6w;@6JVj_JOh`ob- zKt_O-8d@AGj3taEPC`o7O;RRc$MuX~o6m}jm;_EH&P$bEpKg(^l|IkaoyM&)uHu>Q zk=kx`VO4DP+Ny5I)D6`w_}c08Krls6)$P68x6L#CSk^a0-bCj_NJ;L4l!G-%Y)R!C z&xSr`H@$W0orh{)Xp?HAmcDqyqKo>byvC_2z}(!bv!=MR!Ca~KyvERS&hB=0WIhI3 z*L(0?bW&_OpvAxGUI$eUwL zE(r!k2j)C#KL*qir4N3${Dkrnr4+56L>?m=trW|Tz=DVt#||^}9a2agvUmpcrSZ#F zI`I1+QnepMF!*qWaqii$$cUb4KfU0c7xMG=k^}RZxkwv!-O7*l4aec;Dd*9?+W+-= z>;2a37B1H!S3cL+bVCJ;S$S`9FZzn!s>|`OW7O4NqAS*Em)|uIW*~rRjL`qjmS7R4P2A(RPHETp>HH+=%-R^~U|Mt`6#ADg3?{^vxRTl8I zv9;e%;GZ3tXo?St*NZujjY1ULcX=PHTWl%Dq{eXb)hxO!c&%1=2zk^n65nrkMCs1z zDpfYhY~)eL@y-eQ`d$>hRQKaV9neh~Pg9ag`$9%Q*pC|@EK=9}t@+dSuj}4qMs|Gm zhKdP??a=&D3_r{O-B6<*!zq}7=XKgT63ZKwi?a9H^kojl`K{laCx61O8Lk6#nl%DwhZ31i01`9xtnfi_vjqKUW)YYhAWmT z4k*DK)|{Fz63*40imNy%E>1Men4V^_O|x~d`Rgy#O?_rgBKnNugt<6n(8I{W?px=7 zd{MIQvF_`M=PBfntcA{3 z{E_wQ`arDJe;Bc`*KaFsPcOtp@%(dEi8nZEP$zMdp5OV%d&%n&8o5Oc=N?OypI1|MJwU6TOqLDXmDQdszK+ zY~xqb1d*-1f&FEFP{O_=yQ500?t{nH=Y!&J;{$~P*;H9fB0Mg&%X`y;zZ#M^HAir3 zIh+EWsV+J)qfCi~5$oCLI&*pQ0Y|%chUL4Cs^MFh&&jh>y}`F(yTbx|P~L^=)|rXz zpEG`g$J+MEnmYQI%?CW92cnx$-zAt|nE$5F;oaf^tO(Y0kA6Sa)ZEm0$$iiQ%k~Vs zYu^b=3G4p!f)2yO$<=O;&ED2e`{UgR0kWlUEuv>52ZpzA zwx(AvQxAK^p9E^%zB!G!E?#vT+O%H1Hg+~CuOnaAWpvIL)5A^amU6_k4{UE8SP)ckzZ}zakRvuW*_CX znIj@tGs`y$Rw^n0W<(ne00I&KkPs~(;tvpJ3jqIZ0|4}hD*%8L9|}N4TnP|g^&HTD z^#WUSkp8O;*#1K(p(&-HfVgU!yINX0x!FM7QJl-u0RWJwt=1d&H!8|P=1@m=Qwyk> zCA*iS^B)#~h?fwe>1gS0O6le3;N&LcB})C5LI~0RbDM*j@-KQfo|ay= z|CZ$B_OEFnCdl!phl7iqljDEK=5A~Ce_;F5^AFqKdHo|!b72%4!Y8|jtHwF zR1@Ro;Su>O=6~t=x1#@WzIL;8m4Z4V7~RGGy)6G?{?E?;neng4H~$vN%Pa7oBLAc3 zKREwPK}gNj79q{_j}67RL^%GJd;g*r;rL_V|1kJJtNHgXVmZYyL^%HY(GbHZ5MG)9 z0BEojq$RYxfcu%c4hC@V-d5e0ow%*Qk+2#IKrHbGE$wi|n4+g}}muJDomHkO|QyG8|* zHBJ5q#Dm<0{}qVm90OR{%ot?i{G%>HOxHiEzLWzTyN#?rA^EEB~L^2bD)td+*SV20b(f+H+|9b`guj~e4T!B0A=j)Y8691hPr3qcI-%st~ zVZ&s5xLA^dhoKII-;pUP`2v|c|2*zLG(QD`A@%|5t}}$N1O|#YISPLRoaZkYlR1p{ z=3cVD|HoMGB`DR8$2Uf3pTLwEutX9cboJ6D%%VP&NkLm@un<288e(*j%Pq1dBTFNg1O zf>}!GAklAjpRr&mN&10j_qH|5wFX>|SC9pL&LlJmpC99%5`%;Z>IIVH4#^j3)DB5! zk40dAwB%$AZnF=V?e+ZK5mp)a2qdM*p8f?hV6R)f^dsd6F1kOxvyfmOj=t;mdS#bd z+T5Us&AW&;EqYmGZs8gAvXdUf-z%51Qi!B4cC&@lzj-QqMK7>%v%RTIZ=Rs`qnjHf z7gp(78JnmBE$h$qlg*;gFTbY>N)p6yql_g01xGH8jpf-S6c)H`E-pa6-UsjoIxR1& zpn{BN(vzd?so~$SPxZtlkkyUL*|dt0JxqG2&>%U1pw#igY@3TfL7jrQAaOjVZ}gaD z=l7kW$eOz)J}dBnvrmeG*C$&(YIi z%!9gBzbp!-1-8X-EbO#KjBN}GH*Q_4XU7Aj`}+%*BbR`R(1BQBVS(VF(?9!4iqZ%m zJb5$AX8#m&1De}DA_d`XeE_V$G1*42KL?HM3hVZH(}*;+xcKsHoti)EAVukR$BaEi zY@<(zElSxXyUf!msB6o6_-t+LqXZ0Xr(u@jY92-<``!a7#_bOogpsAWjR>@Wchw

F}VF1gzH7M#iic_1<>JT@Ro zv-Dv4JXY{{cSeRW=Jy6ShNAR$rlF*GRfLSNcR}XhqHDnoXR^rK`qY)9FWs3s?&xEx zcG85q{>WBVwJb6lB7e}zF(zkwQzeb|X-8%cJC%-?=y@nQ|X@^zx z(2;jBr1@Qy}IeR?AdQ@uCe=cb)EIAozo$0cYOLDO2Sp?`#mG>2}mIv zw~K@6*OS~#3DkoZgL?STQ;_>BXxSof?DWoZ8~IwtSX2M8{B>vR`LRCpmrS+qcQGuYBvq6MB|$`ki6 zKn|uwpmY3kK6tK(Z0dloJjVkXPVhlB<`EsqEKge12roPuFq{>a>%l`4*f~q-2@Q|8 zu;(cn!L-=CV%$z326rHQ%ZJ`1VKR_{!^z&-OJnf4F!>XyKobVV=kFk{Xg~K-pBvMS zjO7TF)*uIAm?rff*VZEmQ~o3dc9nBD7))RXbMlw&(=RcOn7aBs`P`2L`i;L;JX1B} zFfcz~s;m~sD)&i@adnkYpS480#PwjlNk8Ue%t2xpg;?!63B{Ni1hp}Pb!}}@-@cf& zbO~!f6~EY>H^o^vdYF&0fWj3pX75(|55=5xde#kb|!@Khs+ zUSI3Zv_1yG*BY9eSvN;pH+4(z18_yVp0^&&TQ6lMG{?Su!uz(up`7TYcI@WR?p)PI zYqrm3QXJzR6)sx6B4zBb!(1WXmH~u(sB*y%qGQUCKmP#v8j!{cvq%$ZwJ};sFQ?6~ zub8=p%idM1ouLXO)Zw*JauJX@bH_v@A{6swE4BO?v23;58$4Lw%FwrHo0Gjoa_Ic` zex@pnkxSg9X7AtyYAQxbA?ja7L?36FYPvb>2}Q%B z6u6aJ56_Txr1n*AOOJe|Xz+OpfxM*O&rNuVj$j~WaPv&{>UBl)oqKWUks}q)6>1qe z%%VWj;M>&tjDM!52B`?~0RSj~P8r~IBl<&wf?`0SzjW2={X3RLP7NDiQCC% zTRnOS0H_^-J|Vxi_6%dv17J}FR+1Q>f?_ZxGpVz^W5?&>kR+&0}VH(_! ziOhnO0Jyotd>+%O=9&DNJN`}aD>XO-A*+JQ!qHM?}N{W zxU34{PLgFf$e43DN9a()GsZfCmrUdxb}a$q&fz?I-6N6P%A-kPkwQ?p|?Q_KAB zt>n4reXwFMIJgWkl`@g53A$iw73|f#nNX+12N-*VjPLAJ8YQNsoKp6DJOtrALy}pm zUU}M!+g{}Qo@<}*o7P}i7HwSdN!SJFnr&>L(*<48O}MzE-KYM}5EwYstHCcMU0a|7 zwi5MKhqDK}q!A(l4u<*X4pgM;q^zTi2UE;J%ptRS_vfjTjFt4bXm1pPYDCN2qgrHb z6E=-beF~M!MvXY$)+!?(tNaPl4bJeLQT`qKo;o!ibZJM_#8V890k|eZA!N#LBoKtuE!fhqze*IanLtZ)A_G zuj@*yK2dTH5(MNBZu?#G79`XslRR|O?P|EFR~Agy4b}pmTFqn~K7StP>#l``4q~Fi zOS@uYV;h*MvR>PnWMPuoRIPC5OJ?H9z8`-2eiXUVfnLdARRM~82t*J4{n(x$ z4-cE08$4f&nbe8(E@f?*luP`RY8tk^al_hYP@ml0Lxz(V+l+pL3)t^wb7Q`-R#t(9l;^z->2A39 z%B40=f1VU5E<*k2U&7p~m#Z*u=Gkqgx64CIgw)u_t9`uNA*LFU+P2#1E!eAJ#pT=n zeXmC0%(x(V?UP*S+6R|?I5GGqJF%$>G1%;BtHFo^vT9{g(9I_LY0&L2v)7fnewJll z%((aV6{SuFUwgac*{skE&bOu9nYh*Ri?2#;pH|I~PEUyjcV3s;9yttjr-{#&-`Wd~ zV}oXKuM!Zk)7zB8=Oiv)etzbDir5%h^;{W}cg4EDWIXMSEwqeZ25GwLJcW>xe)I2K z5`9Bph#d|F&Y{15mM^rE@L3sTJkQg02eW8oCn|Y6);Z|5_XHo)k9$7~QnUh3mN9rD zjBvSoq&&$=$voz}dD9DVj7Zf)Mg7LKzW#BT^t}uN5iulDSAr{BMv>PUq;1YFW$mt3 zEs)iDY!Gu0BD$8iz@CS6l0_lTutb8LD4Onbc?nidOMyeXJ1>SD>e?9;>e`7rHiw^8 zq)l^CZ@*9pyAL-Ox+9Tix^s*QXs+Mj^W<)!!YiHc!h-(KN)Oyp>`<@iU7tRsg zyBGJ=%5Ys%?$1yM*Stn)@V7(3i8w! zg#gb50W&LKp|2KuhihesheNt)(cY^-?1MNetWBm<8XnmZhBO2;oSa9PsTO=IU0aE4 zb!03&AoUu+`hEjTB^h1wA`@9j!alohSU+@B^6i+o3+Ugun>+a(wT$lNi;@;`oOZX3 zCq#W#^MRR0@?GyR`5(m{AqICv?+gcqLk)5^HuG-F7_0a{>|o+ocaW=+1YZgM-pIt% z<+0b?cG^FuZ>l*|j)Zp7|6~tIqc{b1794Ecz<`ord3K51Q z#=S`y`{^EEsu?s)pLgT>!yx>oiGqA!z^LlOuT*ZgyHZag6d!OvjRp@UH}Bev|1PHG zJ7my3!g<}~Cgs_~Ds?>Vp^+mE)Do0T;6+aRpJHZ2R$c?uda;y0aS?GYg&k9ati3llXIQcA#g z!JH42Bn8rV#4QMMKDmi`ZijsLevP*KC3RC8TFj9v)WJJ;|M5zbshK1@7_Gh^A4 z%-cWc-x9269Ek`LU-{u{VWrndy#IOpW4iV#e5uxE>>hcXY;4~ct^{X(pUwBqEalm6 zAL)6+0F>tWVcK)K>VC~af#%M$(23hLP$VZ`2;-JC!cR)mt@qy1kESa_6ZTIEN;CZW$3WmElS!EW)o3si4Mo$N3v5Rm)>56^}e_-@m%(MH3fDx%$p-E zb3XF2a+d?0xD<1)(YVL>N1GSF)=0B)`ve0(FuWV;BBBa)kGp_~v5@awYBEq47l{|% zYp>Lf5D5hTKltl=*aD3ztI<=5?9O{G?BTlV)dX?!YftY(7jcwr*^Xo{pD&Wl5+Idq zlLaw{JV?$QrZ~2^dlJTzfg9~w4c^o<-Ck0B&dsGlWFr=D_3NDGWr~c-6>`;M4cHlG zd3PRLcBOH;B$r1oa8l`VcO=U=i0&-xn1JNijdninV&d^t6Ouz1?;tuWI_$|RADz5t z0?QW4zJGV}yI8UrF4ry@ygkfL=CKaoAZGWYoMdJRzw%po@SRn52(+&usv}*)1@5BV z6&NIpru(xg9N=vY%rVb>O?J`1sZTQ>|N72Z`@u8SDOh6lwLZ{M z%`hUH-}Bp#>tgK^pFB9T-_1f-*Zb&CYF;W|Zmy4h)3{#jkL|cVe$d_D&IeJ zN95f(MCf^)o%M5}gSAzRmv)t&UY}Ovc|X-t9_hn{z8;eH23H^}GB0}21$1_H8u1jy z%ui^smk%5{Z?gkfSXiQNRCvb?5a8Cg$d;Gn$le?Mw_z&ptLhzuuk*-GvugI+m^Q z*&DTrncI0M7)iWFD=aMcKCJQZS|6ZuTTClj9v&I-;u9kWOGSPHB+RawtBv&2=X{Z9 z71by5SFePcRQk_2`RnkBI}R8a86}-=j{3k$NK{o9S6kr09H*yMR^TmQdB59z^JVj4 zb;DV1SWL}k|0Wvx))|Un?GE90hDcX}k_%kDu(RIaI;e`T9IY08>cPqDc5_CU zIY%pOu;%Gnr(;=@1Q*IyL+#jVU=H|tg|-EnT52w5Y<=^6Fx~2Dp53jMbrkkimyaGEo{s*8 zo`JK-$>O7|qLny0h+7iZ(1I=SP2Q$hT-yuVH|?Cx9hk+;DYY37ubPFHQ9ZsR&tel-!M&=;c#sP)Q|6Moo-C{z{{ zBy&CbW?GHB*me;DchDX6XsE#Wbz=*Ms0kn8ozLf-%T^Bl1moJz(7jrJb=f-jd)LMT zqPXxu2l(}`peeA+ZzAS)fj4H{&Hh|J$1ylAQV(NfU|W_v1U@RG>yyIs5k>8aiqY<) zP&XNp)<=#6A1>t%432Q)ORnd0sp`RExn24~5(k*fvTgq`aEPibqctUm<2+ zf*pPz&TJGbbdqx)kA6;rIE!u5FGuAhY1qGO)JlWlwL9O%Tz%+#yWCVs{;LhG0SG2g zY9sC#P4f<64k{%gY-X7mWArLOTKWY&un^7*e&vdUMw=G&oye_X_DDNQ!CiutH!Ce$ zM%6ZZP|mGTW~BWIT9@>)wN*_@D<9I*MVbOk2|Mt+iE__ZM*Xf)0&Ay{b@Pv^N(LdM zct1kgO1Dob`YqO+#aK3n&1SxuAmWU6=e=9@{TfjZi!CxbVoAyxaKX2?f~G`+vUeeF zTr0%S2eS^P9m}mP3;;j|bmi25R>JO#a9xhiPp}aAIg%LBg%7hvYV|KkK_i4I-}u*~ zXsk?)HU69++tfWgY3zI#Tvk}oASMZIpSO4Q9dY_&e~Ev2_Z5H;4TS7%5bd?XvFS#F z0bn6~^>$$sB3L~`a^m*0C?j{XiQNkIuJRd&$nX0}qhYSMMuQ}~*8zlu{BM3d8kK&h zbVHb?t(0`jn=_khC3(y2jk(|Qj{9m)vq6)`>u17T1XvD z+lpe$uK|G!Xj`Ad?yVZ%$=?(TW2Kt~?84&x+^k+tNuD_6{Bb7hJClPa5o*tqHEg?@ zTUY!8wsO_pTLj$Iou~8Yv-PLQ*c!JE`?YA(H-`bhC^VeVG~t@<@egO}Vhn{tTT@|} z4L=J!!OQ_sQJCEP{OrCL(LN7?&dWI&;*VKyaN1z1X(N;JC)~}oZk9nNR#KS|0N8;# zXE>{O$UvNU%tdoVkmka4#1ZTB+N_>TXjqtlpC;pz=1Y5f)7J(9l`GhYQctiJVG0l8 z95WO|OdLOAJHe;Y((sY$>C2yK#%0>)X~O!BlM)<0245$9rhN~gUS4owbm3*_UMgo* z9iISOz)WqB9~URk%);co=#oj}*ysCIU%+IofsaM%Gdc#;=y5+!&!N>7V@9T8*s_g5`je}$rhAn~1@+jM%nnqcS>+AcI zBQfKLv*U-u4J=Ih4z&S=B7!_7Vc=j@aCn@mR)?s^FD&W|{+isw=d*-PvcdBUfgWdz za>Y8V@O0U`Zzm0LRM+2`$^+{l%1v$wP1zer^DXL(11BeAJ><-Zk(Z0mhxx0Ok#wh9 z;An~h`=fiyp5b{5BJXbH_r_Hg{n5pz=054zZ{%xj!1jlh(Sc0*WJxW z;n8+4*F!O#R?D0@YrkG$>q-JEZzM;^3o^2V{Cv7oJzh=n9c~^sm7ZP(+H`F8bn(x- zC(=wTH_70Csy$m_5a(|euJE*_`%8*MrR0LO1<$28L|heyLw?Zv-Ld>yDxY0$m~+~5 z$e7@7We$=<0Se~#{RT`(xa{ldTFZpNmKo}@Sp?RQszw$XFqH1jR-L~iwgRHC5ZBhN zmTNWC6J=VqQ|W;6%dZp5iwXfJf9ilYba*yZ)~&M6e6H@t{M%bf8FZ!l z8zuU0*)f}Cy|pP1}_T#GuY7R zXoMJ#gMTQZ6!8@evrija7JEqc%fUhgBDm!fdRpbczZ;=PU-jZyNl8f_t{AqNe~Jns zy1Sn=cKv!rutg_(W}qS^_2J{y@32=s=2A*iFDNiE1uP#Q-qE9k(q@k$Fl(Xlx5STg z(#mTdwgp(8I~%zA`egM}dEe2|kG&Zl=1Du1o%6~JQQscSor(&V@#=;}i;7xpuFXXL zG!89*Qa=8vQtOzj++x8;2nSc5c+8nx0)OUidEm!nsW6=Mg5wt3PD$?rJeyCSTpJu3 z6+#8BocBa#ULC@3z_KCp%NKuBqx6m|dfAbsxBbdZuyj&V89F)C(Cbv5pF`Srtw*mc# zsh>@ypKTW-JdwY$+xAqPT+FW(uJfgJk&hfv68Ub}WEaq~bjW9Jb5u)zUs@xs(TMI& zM$^6W@keLPTL};Jw({Wr~ zUvCrQa;npa;!rhnR%8>DJXo^G;VYWctD~bMoz5qJfH%z~>;7Z#>*xa3iOV z+$rC^7^jbJXxKG+TZcM^?YRuinleQGoue?hR9QtPk*? z-CZith7aA6rlzD+-;hdHS^KQI&BfcYL7kkmdwS@bA4D=&IQia6Ec##KF7gSX$)d8_ zi~-r@Vnj>|gq6L4Whx|y64n-L*Nr&tsQ# z7j80Pa3B_Q0IdMp*)tj+7zNFK#X1UCo#G6WShZ}CZBsFOfZsGEGA$e1D7-yX6}Rx> zRp!1GmEXSf*nI<>ypQ7Ao&>g@X3=r_k-$bKn>C@S?gVjr=VE=iIN6Z!dbaPZKeZ_Smr zKMPA^ z-+d%wC3Ju1I#B^Odn23tnxzB0ta|?EF0 zpcj8@`&xAHI~Sd9Je}4-SF%xr8%HTQzx0;EW`)`9)BGrA)>NwCbioepT@3#UW_^|x zO-zObL4%!b?vxn~m&?J_<6%0(f$6PW@!Nsn;iM%&gWlw+!H~GaPd!(cNuNTZyTTm# z&FVz)zm&WAe8?Egh`}Y%NeR>KovZp%?wZ(*1W7YOhrb6sxqf1Y9x=#FY!fQX;M=!B z_*7pjE5}$^+KM6Yy()f28;&FWHj_<}%>gK}Xl|G8l*6k4hvYU%%K%4NWbjHd%_i-F zA`Wg;8QyR2K`Wz;2t@rJ#8%@(vDRq0V#*Eoj2MiZ3CEuK{HH(CcMwM_TrCJ~^$A!A z5)AKPh_M==`WdN{`1Kn^YatA3IGt7(MWoAshfIW5a6LjOTu+As2UdA@z)h=gp94rv zz~KQ`hOYvMUx|MB_W9-4C-`r-K0ogaS|ol=4MG06mMKd86!R^n1$QMXt-tgaX^YpY zC=bYKLBk>%U!XfgY@kmx*C@e=)7aI7pltj5(yZ_S9_mMxj-+*6 z2NEt+23(Dxg%I9w-mv|)=RXA2Hcxom2pSm$h!{IdyAsy-Pu`vIU%$I{2mrTzUX_KA zoIqrdQjicKwm~{UB<);EQ5Lc=3USPdVC!J)PJ}gzHIX$PZfh zm6%_vR3?8~u=X8OzE84GA|y}sTcyEnz&J6s*i@c}Qr)EU#s-^!!Q(;O?$FgIy8C;F9nLZPDXCRle;c6)Zbe~!><)4suO?nj72h$D(aD|lHz zRNysbJS9G*KQ&_YjQw>Ltg6_G-YRl(Y|nhpXzymvbCNN?Wir#U*211Wls&?7aVCg; zh^>P?!SZOP@e6luPVr_Q#rS-_v+#Znhu(q+uUv!dUHD}%s#=enM5aovllnHdNK%`z zDHAOoE`D}>a{XgH=8|`PTK&35w}+C4^~K8zqFtyD=4kP9^8e z+bz0pbjS7fbU$q4kF1aIGTDu)jl`|jkErc++snZ2=H&KD_JsSyA!lC zzk|lS%$&nKK3i8tXI$Eo--EQKz3y;2c8a*(gLD06w%L%nP2dal7rQT~?Gz}b49W(v zWjjBO`;CX!)5v#4ub zRSi3AaAw}0N3KHFz(svry%O<`@`|ec=G!!I+a#Ff`{I7KuFa)m`1wkxd)JI~k#xUI zv&{yy@iOK@0b5YcMtXU&ZqDdDoqm?So!(n#sb)HsHXbJy6^gPvt=mma$KYAxeR^5A z>9XnRitZ}uB1soOtL^&oV6psM#>~phF7BP$I>V42fs6O-(85whkBLHuesL3pr{J$Q z*EdH3E#5-_V{hJ--JM?w@}qlauH$YoRv}KI#lO1ukaUyO#?p5xn=Y%amal+1@#`|? zzvfQ0uiX#6ike0U`{4cQXLoe=>bH%Za|0~jLtY%Wq6e;Xw+^HZtcK)oa$Q4;=i^&r z@sl`~*1FbLeSR^AwhXp%En1H*JF!Rk1rz;woEaqPG<>WM)vE`y++%eK+bY9o)r?SI zdy>ocv@j!FUf@3K9p|pj-lMAb%+Zx-$m=;bdz`&I*B)^GxIe^sV8^yp*)lh|J38kz zaH?*dprWC3)p*3pf5g9S=eg4C_0fCV2}MSLjn1$k^p8B&^04 zU|-`Mzwl~1|AL!^yUrN)sCw@-Ll>h#^>}u@-)(WQGirUhAHqqnQqaWzbNEQ_PIG5= z{VM6WNAQKO>Ye6!$W8vb)8O{I^&10w1KQ?fAKN>e>%$+STb)6{WLvUTU~1FWzCKVR zH#~?N6=a1A>U=uEe-SG@u>z+8|A?hQ!?66);v{9lc2qDykQ`zk zT+qUNf4A%}-l;-`WRA1q&!u1{_Jm`^idI3}`p936;!JD5C~s51xz=C@SUglWpj@|f7!G8mcK8JjV< z+1mdG0r9!<08Lvnm=Up?tqs(P$Bm!#&j=o%{rfc|De<2nFl&BNO*utk5j#gSVh#o- z1}0JgWMX1sK1Wk?9%WJSf53rX{G=8zm^}|8qpPbcgDWe8o#Q)3Wn}RK-;?nuTDqCpXo*_d0;dWrO@M`s zgYOTT|IzdBh5m(9b24)jv9kpLVFLfYmVbc%v-5ui{DV^S-zeEQIsX&ofAstZkzcfpBr9H$Y0Qu@=bVn(%(9-R+EK(YwtI{~eQ6UCeATT=c@(nnc0UPd5 zlXw*aPIQKBl>EuxULjcilix!CxJU?bJ&`{p{y$JzGKBxk=OzjoyMVCu{WBZ7-rm3P zkb#3sBr>HC{!Z<;qm1|mKL|{OAq3+G%dz}3wP*s*?{xvIISB@3?PFL(`~RJSxPJ1V zRr*Q5qw7Tq#XS4F1<+Uy>Oc7SNd0y^;FC@NnL3!k`R}g&-?q}KF;+ZfrOc4PPfPo+ z9oS|gtvK~`IL&fO6uck8O7zwziRpDVg((tI;La{Bb|Iij4xwAO?@OC2>2x_|*l*6?r!__L@;oYRBW%ohiJK2- zi_N1=eO0GtHklsw!LG_6b^Y)OyUB5l7_ZboGNb4Nt?B(isrcBJtPdl?wd1EI2mvFb?-;5nLyH2|!cty0~j!m&d zej-d58)LQ>Dg=yQA^cVUi z6-xu=t}3W96e2_%0RLfdA67MD0}om?k~ZXQI*};IL{3g(@Pf{fJ0<$pNiDSk?dc(u z8y%gB1mrJ?iN_J4>SUfPrTZkyGiXx{*_W-wyHoFu@ilwP5!&E=r7v4*6!yfBqX{*= zUGevqGd`iI06%4k8wxcgnDFI>?cAzn`EaUW!uE2nvi*t?ZZ( zzB}%X!GtFswFa;B=56Sgz1*h}8JswDF5YwS;@P^(>-M~DNsd2yopNLj1~|c=3Dd0$ z0{l#Rd0f=hFoQbwTR~TKobg0j)uO4Qw^2qc0G`fJP=M$6I*g^jx}dcp4-SQ7 z!p}ZlapPo{*7TwRf>oi7YmAiFuGz{6LV~tx#s<( ztX+7+$1@{yF&J7lR-(*j9-8Y{{oR)fwZ z<<}9Lq$;d+cDX0-@L1segE56{8aUr%E$}zA^3_?o^xfXmDVSewJ`dOGg}{@Fwc^Yt z&wa>Imw_zxE+yfP9Y*M^rhE8izY*k1j~P>qc5m>nK*bLbazmOmE~Hx{sT@8G-r;$N z7mSxHqo&2FH!yQ1wXTT0@;vK_(K8k}e+ase%@VWmOqNc5EJc#6)LQV(m(B9!gzrbr zd5}&ktk$qO$~G#19_WFE>o^|r&y?*H%r3Y1YH)CJe)>$gIhoJOBH&|eV3niXw|JHF z0{LmadLavoqwH`-iv@r7jKJMNOzt}b#E%=}IRkRrTsz^dCAd!FFk5py+UVc%I1>wO zk{`s5wYF{kgYW@EVTa#8LhNcR8(%7C{I`^U2k$I#w0NU?dxtpK@|e3gG(0S6u;Rye zcc3;leU!$lQeIzAnw@Pj?ZA)_Gmua^nZaGqrP~t4NRMQbq`!GJ_&Thl@BWAU`&|)w zDF;9ga|O3Lv(C5_D2mym1x#c+5f)3Tu{sH9sHZ1RA6gc6&Ckr(iTkyi2fG0c;=903d>$#C_&OAxRh>l+GW!Xc%-tgSPk@x(#o`FG(zrXN)IZ?s@ zcAdAUG<5l1@o2e;%w_TN+{;BlsNsEc(s3{xgU!R}E8}W=KaE7B}Y0 z^Yd&@W$EoR@r9u|A4J#k)vf8&dn|OGep>(0amT{O_J#*qD7>x|)nsq*9JI8vFNf3l8Ec?Rm22e_-&O__fRkgK8bL9;IZ{-A9EeGa$k+y` zftUzh)uu%-wQ5?{$J5#Mnvsa&sx+2f?5aNw_j0ozdUJGOqhnI((Q$bh&*s9(DNtEi zX;>epLI06P=j+Ao^mIJM{O(VvYlZhvL<=+O!_m)#uI1}SzDv5}t~$qckZESS=RU%$8R;0eu-71#B#wzL_kne zA6T_u;^fqECP{>kUzM(}naCPYZ|QBIqZ2nbCo#=N!vBTdxWQ9fu!=(O4Nft`L06Z1 zA-1<>*j#AruPQkqorb->dp%b94*dpOlXY=O=Lb-F|6^jknM{qEi+qNFNw+~utb*Xf ztW|M3zo-0UlC)0#z<|t_Zok#j{aBLcuE#z~k$CHzJ)9i=J>8Yj> zP(szn=5JKz*5a_Km?Ya@9`JD%#OQEf6Z5*+5Lx9%s}KgW(xIeUm&47xWo5#z`V0w0 zH}#*+t<~HJA|pfPdp=Zkz3Ot%U4UypK6-f}Ioag$>w(3jL1Idxv{a+80W|;$BlOV2 z8F%{Vr{lbFoVY7&!0EaAxFEKSa-><}Ttw*1$SEEp|LzjWhAN=YVOJCS;%QA(%*u*$ zT!GJemn?3@N<6VQ1pZ@d&0v9Zi}4hX!L#wIo!e*~%)>;#66FhJhM_%G!kBe9h-i!- zLqL?mebOF+!CfjI}~`|V2OH)<9t9Usb{XYJ#gT+(vSFV*A_=&687jsXxP6a4L<&(?hVgI%u7jl}ta zf1idfYrsx-=b23aI+lovEaZUX#CD=zk6mujpp0dSh~@DWY5k-972i|~hY8n1_q@oK z1|uMMg3;h|5uXlB2SdLf{LqG~f6B}1Km3ZnKml0Vr^K0b3KvO&H!VwT4z$!b+s|LU zka7DQ89DfR86~$c1Q3*+fvqxiUyG7THhau~dAinup^9&Q2nz_XoI97AW9XqN#=yga zh5br4g%ZEeUgpmSR#whJMiuDHAZwU1U{8`wt2&kQEQ<4I9 zjd)BR(LY)q)T1QNlAwdYrO89`xyWi-xz^mEX}9F3qE|FwnqE2us%Fp7wlrsx-*zYX zRXb9ZBf0xFy)uBn(Q{`JD0bAw6^os7x-H%dV*+-;4(4~AI##oabtDo+c(*%*{#Qru z%*QHW5|+F5Xvr^#SFzxh#3XgGc`)dl*ymT>(@rHQp5^%H*9YP27cyf6o$ka>^q=eUGT5)u2 z=N@5IlKULwYH(xEL%w*#$=U7>Cp+j7PMLM)X{d%Ef|oJfO!({`9!RT~9goGVN`j$P z_4~Q$vg46+C}bBbs8!)m3qcFM#F+~!7T~@i*#2d)H+`w!`dYN8dvSa_zLck#S8r$W zvYsmv3F=o10dkRYd&J;#XSx}pOir}d3HzI^AzO}(6yj~&LU?^s- zKw_DC@>~0*uLt^z{MdTf3sqHW?vFRP^j{v%1{TU28_Cz#sgk7?84@TG2KqJ!H1@Wy zkewdD1|IT;LkDMF&Gq~}(n092dik1JDCRE}pGmVKry(eio4 z$8>a&S7uGhG%_41ajLWw^G|Jkj~b$|O3*dghgE}1=qy5BfJ-n9b7(Y9JuuxBbn1C| zc^%h8o6{X^hoE+LGm(dk^W*dhbdGnIu?Gh>1FNfDy(xZ{G4tJam24rx9X)piLlwcTywi-X#)+_ z2uvZfEaG0tS6qN~;U_=!Xw@RW(zb%tZ#5RT1GOm{xWBjIUjn=nZs)ylicH#XF4PG! z@v|JrCHe$}gmET4;>8RW`~serjf?fp6A*{KP>~UgMY8W!>Nz}?e2GYaMB;*Y^+1k9 zIcR*#zGFu`iy4TAugBEG)_SPn+&FhH%NXX3e{v#BjIH?>B) zc28H#J_EEoaKuXa6!J5`5BccRk0OfNqP#_vtjC@)H9!+pR}Hv1IjQ94LbYnXK>(Mn zrnyjL>g+7*)Iz|_nf`eBD&JYIDUFJXu_iSBoS!mtX+Y#&`-oXBD^SP|X{SI%ZG!q{ zS6cWn*WRAd&`yP%gAwvm6~^ANJ)D*>kvF30Zg-$Pb!35U<|$CZknC`Iz^xt?6YEz}Sj2J{I48G1*6nJU=iuP`9r9BbR^?Vq8%$rz0VW)%>Tx=B z@~T%5(y0#&Nso9_n_;Q!C-e-$1~qPl3XieX(q$3IsgBW1Ra@=~Fim_YH+LU4M%k{~ ziH(mIBEE)y*%dGMEWnPDidz(xHj1)aQNNIT!~&a=6~3-*)m{3W9AzUBv#K=_UpzTqMQuz%PPZ>axyhmj{^sDE)FBMuYfQI1E!lZ zldbIpr^@nD;9}R;U>;(4^d&#M_I;ZdLta4%j+Qh(BsbYT#smGtR(T&RlX}TF^jPC% z0KFp0DNvpJ6(B-CNTa~#YV~Ao4wY-jDT&`sch7X}3qTe@;L=D<&5w{|co8~@iOipj zU7)*+M%P=W<~NUhWK|nIc z4YG`03w3#xOskp{v;X|4HK%=n!=Cb#LF9E}$hAstm1l?S;BUEngoVc{Xiqh$KP`*l znBlCbA_NPx!jC+=cod)WKuR@I@r(?PkxfNvjFst6c1Sf2RrIok%OWsX(4oj+P}_8x znD?HNH6Z>y6XvzLfwoW8&|ADCWmJ~~KzSanOC42K@mfwB`HOoPvxazKL2$d z4869Ut-!o*A6$5tp*mx+xV&04w5(5epvbz$uy~=vkrSEmfJVoh<%yS;sF%X|HidIx zA%g?;wJR-`XXoJkiy0C)Jai?h6;2k^l&S{d-V1bAo@yYK?IQ!K}Z()50UXFRA zw$Jzw{oOFXb7V8BZwLw)xlyLGXErIXOHL8x%`Q4G49=yQehHtQO$oUC;8!&so8;0X zt$%1aDUXh8~A-aIzjvWCOgWWxrmtQfO1*(PnY6TvECZ@%y*J zKnN_A*HsAt5izB9vEeGGsI-(lZ6eTmMn7ITpFG3+Qfi{At&OA(w(B^-joL_cVwO*; zV{YO((P6_;dnMql93+H?v?HzZ%QH+bu-$^U^n9^wN4-$Z{raRi{L`m_>uV(Ud$+k) zD=~B$`kz(zEKzpXx}@@x5^4zJ6B71Fi|99W8K@E%4ayj0NR5`y;8&1deCmrx5u587 zYMFDKy1M@i2Z2xX8*Uq-vO3$RPelua77}lit{-h49@;96skW(oc}2hr^|sotVJ44} zk_l1^j_FRbb9YygPvCHkt9X}jc0)3}8N9PR=dB$;JjjWp6-R-T|NDZ<=I$sowhfGAIW160Je$!D5x{@B9 zmS$UMeS3cs281eg#M<1^koZ^zCet_=u>hj6=YktBNosOH``9@}38oj*Xo#w*soCSS zmQn3ed&-&y^^w&n${BBySDWu@{LV!6ZaB5DC&wXHTCC3As2yS}zO}u*vE0;fw& zf5qmLA7nrLt$M5DWp0>RU96l`m9%S83db{(CYKnA#5Es^jCjCniYa$*dDvX5R@K#( zr5rqtVw$UnY2-l7b~#J+wyW@%e>apDnia7$Pm(g!$2!7i9r88Q++@@Qdn2%YZ!dI; zmWYTb-+<_^!rNc-zC3WRbKcM0G|VBDJxOS-JT-jm@Htp5#g^p%1`qQX1+>S#O%?HL zn>9XcRo`k1|GqaJ_L0C-3L#D+X)d70A){ z*2mpa*8;iWY-l`b_%Q~jdl2_gL{mbgSfNR2SlD^|2?IU7X2AT4pkcixEekKNiL)?W zEMOw-r&e1L1XhPT_bZiYR*LGZkK>de^R)SbBL@pq2e8kxLyh!)eG*0mi}@`DfXyNl z&2xXdWtNi;(rM|i#7+ObhlOC=^v%{6g_rv(r@D6}8&#nf47 zYhz*t$85V6_s@Bhc5-yJ3#z7v9rU+DeiE;8!Y!!W;98rxpqwq>k;n@g;S}!7kLcmn z&GI4?>UACt*G(~4*%BUqBJ1Sa*Q!Y~ZJ($ADM*E|nIX@q@T3qof{~xZkhfs0X%Q>6 zMB;2jd~h_`y~Vw*+HrP;tb~9f2nn{>w5Zt~*jcXEHsx07?4|hXljR8^5YzTs0X!JZ z0r-^BJKGPEUymhWWWs)L0ojVq70`Na&Kj8Pz+{SbOD5!SBMA;ZiVCj%?X0lE5e_s7 z#BvnMGxJjqUu0^SNx(eCqTK+lk;F8d%9l)@s(DLN-~0s)+OAClP4;$H=~hVt`Ey|z zhAO5OAIr~S=(*N@;89OwVY7K9F`Sc6^o4^w$yIbU&Z~cZaP@Es)1)p!^30yhKVo~A zLtyY~2eZ~reUaNvJ;1A#Ngeh{whxs&)7aC5+GHy~tg%Tub_H62aO0oJHHQr+T0>_u z=DX88C*xX^Gk`*MJDnROovCEw8#=2@qYI=g>ft#K#;jzns)P$9W)CGFBPl<#H>3Ev z@V#3Ko+TOz<4wg_E#3<^FwO8Wtb)y*5SJt58ajhVw!@DDPVw)5EV?|rG9X^Xf#Wk< zBqca=+m>e6VjqBQ&0Y|1#FO7iV7QSaL`S;diF1cy&3KA{#txtN^mdgOTU(?k>oM!x zV-Ta8fi)^P{xnXks#e$M5hp3`G3Q_cJY<&uPX4ex$LI+JtDXS zH&!IMTViXbr5X%D4mXnsd)+`yLCD5_C6&Rx^m<@SahbzD3P6V ze!&Ov7eY5AyA?_#xl9s*nHW4pMa-mMs*ok%d4;JVBAj_#C|6N^nuPqmDJxk@l(QiI zP^nHcsX&4_q}vsI*<7nr9|H#m7Qir+ouN80l5)v!Y+zAqarm}ohK*AgH6jk)%fG(@ zEHweB+~d-?{5nOmNUIZYF5A5wajW%~*hLyvXT{C2Uf;D)HMaU}50GY60)5K04XqRp ztVNhWqfM`#@xt@qwk*;PaGu`_pj1ZnE((CFS6OgfU%!;UmTYw$`yFx9se$$n4pv6u zN!~JneQh~Tk2u-Qf``yhJ36aC7((_>OrMpHivp>tw2X`xAjKiYkmsqjwm52<1R^X- zW-xcYPwfoj7E&jnD~4F;0nuNhs(f`nu46hl12K*FmwQvVK=x@=148b@2FD@Dm5kvE z9|-u%Q^HA`gn>qAj$^W|Gye4|dBbemAF^99M~BcJZ-Or+(TUOdz;mK+9}ljf50e5F zuG67y1LWKRek7|QJMq%yxRJ(RQ2Ek^y{2+>1)X;iq$#}iAJu8c&VHuN7f73N(XIC- zzlohgI2=V@*y>`M-scG0y0O&=&&~)vZ1Jf4;XV(g#H+drPAO^(<&AT!K2N5~YA7pX zI6tp&rUU}0Z|Q0x$X*!0bg$0eYBh5v#?`1N?&Be6cTvZTY%8DraH;g!>F)I^qb;Dy zDkLrZRsUSxR|}Q`WlTqLTE2?^)ncc<@i7)TTgPVhlF1|^jNc-loDW%++E}06q=Xt zkZUne+eH}PE)QTp@=)MaY!3~RSU(9`;as=)-RXy&cTcKNFONhFe7aBEIfR10j6G2` zRZm6!%-k#81DNU&QKzlPE-suxTxq$tiUf@mhOTrKYq2$pQpK+D;2)Ax4QuEzxbQ$^ zFAudzAdv&b?E7Lm?%;a*htOgdn*C%TQiY?X!F_~H+PNtK6S3hFtax1Z(0cUZqXM#b zMfez$Lap>*liUcs*U|$cLzTpY^Ta~LgYY}ApH*CV4c?_{`D#5RQ0*9Bjv=2)-rgFD z13?^kPNJ;{FB*)c-jvxUaQRY(r%i}^LaXcT1WS}$Z$oaJjGxd8AS9ynJxjrC^zgC7 zJc(xm>#>)raJ?3YuN4jLOhx*E{Hf7}VA+OZ8T}`xE@B1dB2~{&%&txScPOnR*B_0G z!zWZ>L$3y+N$ShkJnfK+eFkt@q^6CIp356s61Q?epFC?84+UvAVQElRo^DX@u zkynrkpLOav?A4X`I*Edx&?MxfK0H~BpCJ+o73_=M4Y6~$oU-a`X%zE(#5f0CH8h75D&T_M+2cyExh}Gya$W&QTI$@GZWmzFWHEXsCz>{Qw%eq?|8`8C{p)!e7yRq%*;Ea`!L^J9xqK%=le@$?5@C^VY|1N zncMd~#vVDLR z86FX#)l;V{&tTB3?90(6&+d$4ba(bBuI1y#NA+%S3$Y@1O2)5M7#{X2q4*Y+w4;s- zW2eAVOQ(P~u_rRJ#&l{$keEt7^?OO)gyq<}ahV~Z2CZ42c}28zPEma6!yF3>1AKC8 zC5E=`VYB)4Acc@2qrX4}#}&c4dmVFP*=6@b@f=W4?_Z0#BMs~6jv&A|6pWn%oDi}i z?tLQLU8{XJL&NVrx(pf{mxnZU?&l(Po>vjOj=2rHikWaN--?%zCdQ|x8Q{R5;G1o= zYpu->ej%D$p$9BTemHg@x3`!O?J=&Y$#8kS?`C7L^24|R*SOgHl`YY7!j~^bV-7mc zY1?VbaT#PKDPw@VI`@R0w??|>(cW@Tu?Zn3s?O-NS8BVrNp^l(?7fnK^fGPin(EJz zftTGLOL9gnNh5u~=Bc$GUoY{!-hQ`sNrV(wT=i~Ric1xyhNs4Km`OkLF5P`vz-Ip( z$dSpIQc*HY9l!o-vWqpOv8VET>O+_6Ex9%GAz1BFt7&lojqc?JJ~ofc<(`$#FA(!T zEZ?xs7RDZQH>5oA9ihmfG^ee(_XvA!+;F-2%Yn-N+s>Aw`|qG)gZV$nmEuzaTZ#oE zYwLlR1Ax)qkaB2!Dbw3L@(8;fXpiWL9AVmSQqiZ|@_$W#0K3(+Ij+&EFZ)Io5_|e1 zxHz8>mk-CelR3v;^h6cO^YhC4_uPQ_UumrUADNS3jrrmtVL>3Fgx6D$^=*`aVOVEr z5Qta(Qv@R#*9z1E$Rii8)ipMnYJaC0P|?bJW?Acb73RUlQ^L!A6FybdSXW1*qLQIK zQz_%_7|mVptmyTp&;>A9C3PDv9oIphv)>k+xUdN1L}<@<;dD7HYC0406tJIqC-Pt# zEY#>Y?nyR$ZFe#l(XQ9tkcZbeo+;DyB!u3a!^q&6%%mJtl+kmzI5Bh&QrmqW)kqbQJpQUqn zyaE>1#es4e&M;Sf{XNO;!GK42`lU>r;CtcUbwNU=faZ!~rg-}Io-?t4I1`lQ4Dru( zL;>M1@$T3c@PF!h0^~ts&+Fg!8f-vR3)N^62IvQzXRG4T97vSmBMQi(Xl(|O1Fsy> zp6eG*nOa&ZZEX?ty+^@vJed84eFwOA%5QK|ls$HHN@Yl+EC8uli8N`Y1_V&3>F5-S z0sxLlO7#*4A}P*|RaKTHf}h`hF-+riJ<589iYiWkoMTODuS)nsZ#;-D7BLtaIhY&Z zd3A)F)uPCnB&$+HTdPspOq@TdvIv1_s$Qud$y8($Sz1Eye&`^IL?g z*%1jeGMbpI14;L{Ko%kXBieCa9G$jZq7P0OvN5u|CZQ5SqfEO2){ef5p$PWu<|f5? zdr@#G)W9G^xA{Kx>x6=s#_`zaK2spwug$2apM{$Fac8XiwoC_+#qsthw+foFI%Ci1 z==Z6DDeHxW1@(jqd3sDQE00~WG+R<$Utjr zW-7N6)Zms!AkVn4I2mxgo$NB|j*E6VUd_8QQe}x5+CAVA5n0k^y09)>WNNgtJOI)f%NfLzs)|W2&@fh5L#+?DJiGEG)*( zA_>wt>XG&Wr0}SPujh+(X|an-OzBIS6(@GLR$AxDlxeG6YC|Epnz-39JnaqSFV&uo zIa^39AmysD?Y+rvFIsv(XCn{9hit1Xb~tRr-e_;56}+Br*VEPgp8Dd+5M`b z9?yp-Sz;f@sr_B{A;-}clenTYX1oRK9a~%CDzx)UMJBC5*BGO(b@rHY^apM+s{)(d zTwSV*^)dMKE=)W<2Nq*0?0&FP!fQ|cIW$mk$rE06TI{5>w7HqLmfkZd{Ct8uMOvaPj%M>2b8^&Klv_!;@32 zo7Ul}m=OK&EMtuLdyUT(YDQVuOK1KWmxMx5eBfbArz6g(;g^|(np8B*$*2Vk( zxN)R&LW`#ECk3p$ z;BYbZKjnJFi-2WqzYw|jyS@u3odOu1gat?I{U3R34={X(t)Syy`AY)${U3TYO|kzl ze7hWRWmX|w-K!U{yTVVOE^aA2<#irJQw-QpJ?ZGZMQ7Tk= Date: Fri, 29 Mar 2024 18:08:50 +0100 Subject: [PATCH 36/37] update README file for release --- README.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 23125e1..f2453b5 100644 --- a/README.md +++ b/README.md @@ -10,17 +10,19 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.7 - Released ??? 2023, for Simple FOC 2.3.3 or later +v1.0.7 - Released 29 March 2024, for Simple FOC 2.3.2 or later What's changed since 1.0.6? -- Added AS5600 sensor driver +- Improvements to LinearHall driver, thanks to dekutree +- Fix for ESP32 compiler warning, thanks to Yannik Stradmann - Added STM32 LPTIM encoder driver - Refactored communications code - Working telemetry abstraction -- Working streams communications, ASCII or Binary based +- Working streams communications, ASCII based - Refactored settings storage code - Refactored I2CCommander +- New utility class for simple trapezoidal motion profiles - Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.7+) @@ -90,6 +92,7 @@ Other support code not fitting in the above categories. - [STM32 PWM Input driver](src/utilities/stm32pwm/) - PWM Input driver for STM32 MCUs. Accurately measure PWM inputs with zero MCU overhead. - [STM32 CORDIC trig driver](src/utilities/stm32math/) - CORDIC driver to accellerate sine and cosine calculations in SimpleFOC, on STM32 MCUs which have a CORDIC unit. + - [TrapezoidalPlanner](src/utilities/trapezoids/) - Simple trapezoidal motion planner. ## How to use From 447be3f0ed520066cccac2211121c34844c4face Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 29 Mar 2024 18:29:56 +0100 Subject: [PATCH 37/37] change link to include PRs --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f2453b5..8f49c26 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ What's changed since 1.0.6? - Refactored settings storage code - Refactored I2CCommander - New utility class for simple trapezoidal motion profiles -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.7+) +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.7+) ## What is included