diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c2aae10..3f98490 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 - sketches-exclude: calibrated mt6816_spi smoothing + - arduino-boards-fqbn: arduino:avr:nano + 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 + 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 + 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 + 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 + 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 - sketches-exclude: calibrated smoothing + 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 + 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 + 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/README.md b/README.md index 69923ec..8f49c26 100644 --- a/README.md +++ b/README.md @@ -10,14 +10,21 @@ 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 29 March 2024, for Simple FOC 2.3.2 or later + + +What's changed since 1.0.6? +- 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 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=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 @@ -25,12 +32,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. @@ -46,21 +57,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. @@ -73,8 +88,11 @@ 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. + - [TrapezoidalPlanner](src/utilities/trapezoids/) - Simple trapezoidal motion planner. ## How to use @@ -102,6 +120,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/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; +} 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/examples/encoders/linearhall/linearhall.ino b/examples/encoders/linearhall/linearhall.ino new file mode 100644 index 0000000..8103e2c --- /dev/null +++ b/examples/encoders/linearhall/linearhall.ino @@ -0,0 +1,111 @@ +/** + * + * 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 +#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 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. 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 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/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..d83523e --- /dev/null +++ b/src/comms/SimpleFOCRegisters.cpp @@ -0,0 +1,699 @@ + +#include "./SimpleFOCRegisters.h" +#include "BLDCMotor.h" +#include "./telemetry/Telemetry.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(); // TODO fix me! + 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_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; // TODO stored angle, make it a public or make this friend class + comms << (uint32_t)0; + else + comms << (uint32_t)0; + 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_motor[i]; + comms << telemetry->registers[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; + 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_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_ANG_PID_I: + comms << motor->P_angle.I; + break; + 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: + 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_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; + 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_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; + + case SimpleFOCRegister::REG_VOLTAGE_LIMIT: + comms << motor->voltage_limit; + break; + 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; + 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; + + 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_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_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; + 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; + if (val8 < Telemetry::num_telemetry) + Telemetry::telemetry_ctrl = val8; + return true; + case SimpleFOCRegister::REG_TELEMETRY_REG: + 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> motors[i]; + comms >> registers[i]; + } + telemetry->setTelemetryRegisters(numRegisters, registers, motors); + } + 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_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_ANG_PID_I: + comms >> (motor->P_angle.I); + return true; + 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: + 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_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; + 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_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; + 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_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; + 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; + + // 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: + case SimpleFOCRegister::REG_ANGLE: + 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: + 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: + 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_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: + 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_PID_LIM: + case SimpleFOCRegister::REG_VEL_PID_RAMP: + case SimpleFOCRegister::REG_VEL_LPF_T: + case SimpleFOCRegister::REG_ANG_PID_P: + 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: + 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: + case SimpleFOCRegister::REG_PHASE_RESISTANCE: + case SimpleFOCRegister::REG_KV: + 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? + 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_TELEMETRY_REG: + if (Telemetry::num_telemetry > 0) { + Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl]; + return 2*telemetry->numRegisters + 1; + } + else + return 1; + case SimpleFOCRegister::REG_DRIVER_ENABLE: + 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..7def8e1 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 @@ -12,19 +14,29 @@ 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 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) + 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_ITERATIONS_SEC = 0x1D, // RO - uint32_t REG_VOLTAGE_Q = 0x20, // RO - float REG_VOLTAGE_D = 0x21, // RO - float @@ -39,25 +51,37 @@ 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_VOLTAGE_SENSOR_ALIGN = 0x56,// 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 @@ -66,7 +90,27 @@ 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; + + +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..c608e8e 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){ @@ -91,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) @@ -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; } @@ -255,170 +169,19 @@ 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;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..7abdb64 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,10 @@ #define I2CCOMMANDER_MAX_REPORT_REGISTERS 8 -class I2CCommander { +#define I2CCOMMANDER_REG_REPORT 0x80 + + +class I2CCommander : public RegisterIO { public: I2CCommander(TwoWire* wire = &Wire); ~I2CCommander(); @@ -27,11 +29,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 +54,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/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/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..b820075 --- /dev/null +++ b/src/comms/streams/BinaryIO.cpp @@ -0,0 +1,141 @@ + + +#include "./BinaryIO.h" + +BinaryIO::BinaryIO(Stream& io) : _io(io) { + // nothing to do here +}; + +BinaryIO::~BinaryIO(){ + // nothing to do here +}; + + +BinaryIO& BinaryIO::operator<<(float value) { + _buff((uint8_t*)&value, 4); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(uint32_t value) { + _buff((uint8_t*)&value, 4); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(uint8_t value) { + _buff(value); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(char value) { + _buff((uint8_t)value); + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(Packet value) { + if (value.type!=0x00) { + _buff(MARKER_BYTE); + _buff(value.payload_size+1); + _buff(value.type); + } + return *this; +}; + + + +BinaryIO& BinaryIO::operator<<(Separator value) { + // separator is ignored in binary mode + return *this; +}; + + + + + +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) { + remaining -= _io.readBytes((uint8_t*)&value, 4); + return *this; +}; + + + +BinaryIO& BinaryIO::operator>>(uint32_t &value) { + remaining -= _io.readBytes((uint8_t*)&value, 4); + return *this; +}; + + + +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.peek() == MARKER_BYTE) + in_sync = true; + else + _io.read(); + } + if (_io.peek() == MARKER_BYTE) { + _io.read(); // discard the marker + } + 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); +}; + diff --git a/src/comms/streams/BinaryIO.h b/src/comms/streams/BinaryIO.h new file mode 100644 index 0000000..5604b16 --- /dev/null +++ b/src/comms/streams/BinaryIO.h @@ -0,0 +1,46 @@ + + +#pragma once + +#include "../RegisterIO.h" +#include + + +#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 58 +#endif + +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; + 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 new file mode 100644 index 0000000..3e521de --- /dev/null +++ b/src/comms/streams/PacketCommander.cpp @@ -0,0 +1,121 @@ + +#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 - 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; // 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; +}; + + + +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; + // TODO flush packet + } + } +}; + + + +bool PacketCommander::commsToRegister(uint8_t reg){ + switch (reg) { + case SimpleFOCRegister::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 SimpleFOCRegister::REG_STATUS: + // TODO implement status register + return true; + case SimpleFOCRegister::REG_MOTOR_ADDRESS: + *_io << curMotor; + return true; + case SimpleFOCRegister::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..1552e7a --- /dev/null +++ b/src/comms/streams/PacketCommander.h @@ -0,0 +1,45 @@ + + +#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); + + virtual void init(PacketIO& io); + virtual void run(); + + bool echo = true; +protected: + + 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; + + 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..3d04bbe --- /dev/null +++ b/src/comms/streams/README.md @@ -0,0 +1,81 @@ + +# 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 + +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 +``` + +## 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/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp new file mode 100644 index 0000000..afd3bee --- /dev/null +++ b/src/comms/streams/TextIO.cpp @@ -0,0 +1,205 @@ + +#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; +}; + + + +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) { + 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 = (float)val * sign; + in_sep = true; + return *this; +}; + + + +TextIO& TextIO::operator>>(uint32_t &value) { + value = intFromBuffer(); + in_sep = true; + return *this; +}; + + + +TextIO& TextIO::operator>>(uint8_t &value) { + value = (uint8_t)intFromBuffer(); + in_sep = true; + return *this; +}; + + + +TextIO& TextIO::operator>>(Packet &value) { + while (!in_sync && _io.available() > 0) { + char skip = _io.read(); + if (skip == '\n' || skip == '\r') { + in_sync = true; + buffer_len = 0; + } + } + if (buffer_index >= buffer_len) { + buffer_len = 0; + buffer_index = 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 (avail>0 && (peek == '\n' || peek == '\r')) { + _io.read(); // discard the \n + avail = _io.available(); + if (avail>0) + peek = _io.peek(); + } + if (buffer_len>1) { + value.type = buffer[0]; + value.payload_size = 0; + in_sep = false; + buffer_index = 1; + return *this; + } + else + buffer_len = 0; + } + else { + if (buffer_len < SIMPLEFOC_TEXTIO_BUFFER_SIZE) { + buffer[buffer_len++] = _io.read(); + avail = _io.available(); + } + else { + buffer_len = 0; + in_sync = false; + } + } + } + + value.type = 0x00; + value.payload_size = 0; + return *this; +}; + + + +bool TextIO::is_complete() { + 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 new file mode 100644 index 0000000..9fba1be --- /dev/null +++ b/src/comms/streams/TextIO.h @@ -0,0 +1,45 @@ + + +#pragma once + +#include "../RegisterIO.h" +#include + + +#ifndef SIMPLEFOC_TEXTIO_BUFFER_SIZE +#define SIMPLEFOC_TEXTIO_BUFFER_SIZE 64 +#endif + + + +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: + 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]; +}; + + + + diff --git a/src/comms/telemetry/README.md b/src/comms/telemetry/README.md index 384098f..743a66e 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,28 +9,115 @@ 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: +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. -- Serial ASCII telemetry -- Serial Binary telemetry -- and more drivers will be added in the future +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. -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. +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 + +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. + +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: ```c++ +TextIO io = TextIO(Serial); +Telemetry telemetry = Telemetry(); +... -SerialASCIITelemetry telemetry = SerialASCIITelemetry(); +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY }); + telemetry.init(io); + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` + +# 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(); + telemetry.init(io); + telemetry.downsample = 500; ... } @@ -39,4 +126,4 @@ void loop() { motor.loopFOC(); telemetry.run(); } -``` \ No newline at end of file +``` 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 (iregisters_motor[i] = 0; } } + headerSent = false; }; -void Telemetry::init() { +void Telemetry::init(PacketIO& _comms) { + comms = &_comms; + 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(); + } }; @@ -42,17 +52,29 @@ void Telemetry::init() { 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 (downsampleCnt++ < downsample) return; + if (downsample==0) + return; + if (downsample==0 || downsampleCnt++ < downsample) return; downsampleCnt = 0; if (min_elapsed_time > 0) { - 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(); } @@ -65,3 +87,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..c0f4006 100644 --- a/src/comms/telemetry/Telemetry.h +++ b/src/comms/telemetry/Telemetry.h @@ -1,8 +1,13 @@ #pragma once +#include "common/base_classes/FOCMotor.h" #include "../SimpleFOCRegisters.h" -#include "../RegisterSender.h" +#include "../RegisterIO.h" + +#ifndef TELEMETRY_MAX_TELEMETRIES +#define TELEMETRY_MAX_TELEMETRIES 4 +#endif #ifndef TELEMETRY_MAX_REGISTERS #define TELEMETRY_MAX_REGISTERS 8 @@ -17,27 +22,36 @@ 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 { + friend class SimpleFOCRegisters; public: 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(); 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() = 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 +59,13 @@ 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; + unsigned long last_iter_time = 0; uint16_t downsampleCnt = 0; -}; + uint32_t iterations = 0; + PacketIO* comms; + +}; 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 diff --git a/src/drivers/simplefocnano/README.md b/src/drivers/simplefocnano/README.md new file mode 100644 index 0000000..70d2283 --- /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](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. + +## 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..e438b0b --- /dev/null +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp @@ -0,0 +1,71 @@ + +#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 +}; + + +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); +}; + +#endif diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h new file mode 100644 index 0000000..f0917c7 --- /dev/null +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h @@ -0,0 +1,60 @@ + +#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 + * + * 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); +}; + +#endif \ No newline at end of file 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; }; diff --git a/src/encoders/hysteresis/HysteresisSensor.cpp b/src/encoders/hysteresis/HysteresisSensor.cpp new file mode 100644 index 0000000..f0058d8 --- /dev/null +++ b/src/encoders/hysteresis/HysteresisSensor.cpp @@ -0,0 +1,46 @@ + +#include "./HysteresisSensor.h" + + +HysteresisSensor::HysteresisSensor(Sensor& wrapped, float amount) : _amount(amount), _wrapped(wrapped) { + // 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 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 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) { 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. 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) 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") 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); 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..8bd025e --- /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) { + _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 *this; +}; + +RegisterIO& RP2040FlashSettingsStorage::operator>>(float& value) { + float val; + uint8_t num = readBytes(&val, 4); + if (num==4) + value = val; + return *this; +}; + +RegisterIO& RP2040FlashSettingsStorage::operator>>(uint32_t& value) { + uint32_t val; + uint8_t num = readBytes(&val, 4); + if (num==4) + value = val; + return *this; +}; + + +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..07a455e --- /dev/null +++ b/src/settings/rp2040/RP2040FlashSettingsStorage.h @@ -0,0 +1,34 @@ + +#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(); + + uint32_t _offset; + 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; 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 0000000..3207fa3 Binary files /dev/null and b/src/utilities/trapezoids/pos_0to10.png differ diff --git a/src/utilities/trapezoids/pos_10to0.png b/src/utilities/trapezoids/pos_10to0.png new file mode 100644 index 0000000..a3dc2af Binary files /dev/null and b/src/utilities/trapezoids/pos_10to0.png differ diff --git a/src/utilities/trapezoids/vel_0to10.png b/src/utilities/trapezoids/vel_0to10.png new file mode 100644 index 0000000..f6b2edd Binary files /dev/null and b/src/utilities/trapezoids/vel_0to10.png differ diff --git a/src/utilities/trapezoids/vel_10to0.png b/src/utilities/trapezoids/vel_10to0.png new file mode 100644 index 0000000..8c81051 Binary files /dev/null and b/src/utilities/trapezoids/vel_10to0.png differ