diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 162be4d..bd74689 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,21 +26,21 @@ jobs: - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:uno - sketches-exclude: calibrated mt6816_spi + sketches-exclude: calibrated mt6816_spi smoothing required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: arduino:samd:nano_33_iot required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: arduino:mbed_rp2040:pico required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # platform-url: https://dl.espressif.com/dl/package_esp32_index.json # required-libraries: Simple FOC @@ -48,18 +48,19 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC - sketches-exclude: calibrated + sketches-exclude: calibrated smoothing - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - sketches-exclude: calibrated mt6816_spi + sketches-exclude: calibrated mt6816_spi smoothing - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC + sketches-exclude: smoothing # Do not cancel all jobs / architectures if one job fails fail-fast: false steps: diff --git a/README.md b/README.md index 4c952d6..e5f571d 100644 --- a/README.md +++ b/README.md @@ -10,14 +10,17 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.4 - Released June 2023, for Simple FOC 2.3.0 - -What's changed since 1.0.3? -- New Comms/Input: STM32SpeedDirCommander -- New Utility: STM32PWMInput -- Fixed MT6835 driver bugs -- Improved AS5047 driver, fixed bugs -- Improved AS5047U driver, fixed bugs +v1.0.5 - Released July 2023, for Simple FOC 2.3.1 + +What's changed since 1.0.4? +- Added smoothing sensor by [@dekutree64](https://github.com/dekutree64) +- Added TMD6200 SPI driver by [@YaseenTwati](https://github.com/YaseenTwati) +- Added HybridStepperMotor by [@VIPQualityPost](https://github.com/VIPQualityPost) +- New Settings abstraction to load and save SimpleFOC settings and calibration +- New Settings driver: SAMDNVMSettingsStorage +- SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers" +- Updated I2CCommander to use the new registers abstraction +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5+) ## What is included @@ -25,6 +28,7 @@ What is here? See the sections below. Each driver or function should come with i ### Motor/Gate driver ICs + - [TMC6200 driver](src/drivers/tmc6200/) - SPI driver for Trinamics TMC6200 motor driver IC. - [DRV8316 driver](src/drivers/drv8316/) - SPI driver for TI's DRV8316 motor driver IC. ### Encoders @@ -43,11 +47,22 @@ What is here? See the sections below. Each driver or function should come with i - [MT6701 SSI driver](src/encoders/mt6701/) - SSI driver for the MagnTek MT6701 absolute position magnetic rotary encoder IC. - [MT6835 SPI driver](src/encoders/mt6835/) - SPI driver for the MagnTek MT6835 21 bit magnetic rotary encoder IC. - [STM32 PWM sensor driver](src/encoders/stm32pwmsensor/) - STM32 native timer-based driver for PWM angle sensors. + - [SmoothingSensor](src/encoders/smoothing/) - A SimpleFOC Sensor wrapper implementation which adds angle extrapolation. ### Communications - - [I2CCommander I2C driver](src/comms/i2c/) - I2C communications protocol and drivers for both controller and target devices. + - [I2CCommander I2C driver](src/comms/i2c/) - I2C communications protocol and drivers for both controller and target devices, based on register abstraction - [STM32 SpeedDir Input](src/comms/stm32speeddir/) - Control target velocity with PWM speed and direction inputs + - [SerialBinaryCommander](src/comms/serial/) - Serial communications with binary protocol, based on register abstraction + - [Telemetry](src/comms/telemetry/) - Telemetry abstraction, based on registers + - [SerialASCIITelemetry](src/comms/serial/) - Serial communications with ascii protocol, based on register abstraction + +### Settings + +Load and store SimpleFOC motor settings, based on register abstraction. + + - [SAMD NVM storage driver](src/settings/samd/) - Store settings to the NVM flash memory in your SAMD MCU + - [CAT24 I2C EEPROM storage driver](src/settings/i2c/) - Store settings to CAT24 I2C EEPROMs ### Utilities @@ -80,6 +95,13 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi ## Release History +What's changed since 1.0.3? +- New Comms/Input: STM32SpeedDirCommander +- New Utility: STM32PWMInput +- Fixed MT6835 driver bugs +- Improved AS5047 driver, fixed bugs +- Improved AS5047U driver, fixed bugs + What's changed since 1.0.2? - New Sensor: MT6835 - Fixed bugs diff --git a/examples/encoders/smoothing/smoothing.ino b/examples/encoders/smoothing/smoothing.ino new file mode 100644 index 0000000..1ff658d --- /dev/null +++ b/examples/encoders/smoothing/smoothing.ino @@ -0,0 +1,141 @@ +/** + * + * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor + * Steps: + * 1) Configure the motor and sensor + * 2) Run the code + * 3) Set the target velocity (in radians per second) from serial terminal + * 4) Try with and without smoothing to see the difference (send E1 and E0 commands from serial terminal) + * + * + * + * NOTE : + * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor + * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger + * + */ +#include +// software interrupt library +#include +#include +#include +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +HallSensor sensor = HallSensor(2, 3, 4, 11); +// wrapper instance +SmoothingSensor smooth(sensor, motor); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} +// If no available hadware interrupt pins use the software interrupt +PciListenerImp listenerIndex(sensor.pinC, doC); + +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } + +void enableSmoothing(char* cmd) { + float enable; + command.scalar(&enable, cmd); + motor.linkSensor(enable == 0 ? (Sensor*)&sensor : (Sensor*)&smooth); +} + +void setup() { + + // initialize sensor sensor hardware + sensor.init(); + sensor.enableInterrupts(doA, doB); //, doC); + // software interrupts + PciManager.registerListener(&listenerIndex); + // set SmoothingSensor phase correction for hall sensors + smooth.phase_correction = -_PI_6; + // link the SmoothingSensor to the motor + motor.linkSensor(&smooth); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + // add smoothing enable/disable command E (send E0 to use hall sensor alone, or E1 to use smoothing) + command.add('E', enableSmoothing, "enable smoothing"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + _delay(1000); +} + + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_velocity); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/library.properties b/library.properties index d60e428..d3565e4 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.4 +version=1.0.5 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. diff --git a/src/comms/README.md b/src/comms/README.md new file mode 100644 index 0000000..4469af6 --- /dev/null +++ b/src/comms/README.md @@ -0,0 +1,5 @@ + +# SimpleFOC communications support code + +This folder contains classes to support you communicating between MCUs running SimpleFOC, and other systems. + diff --git a/src/comms/RegisterReceiver.cpp b/src/comms/RegisterReceiver.cpp new file mode 100644 index 0000000..44f7c54 --- /dev/null +++ b/src/comms/RegisterReceiver.cpp @@ -0,0 +1,131 @@ + +#include "./RegisterReceiver.h" +#include "BLDCMotor.h" + + +void RegisterReceiver::setRegister(SimpleFOCRegister reg, FOCMotor* motor){ + // write a register value for the given motor + switch(reg) { + case REG_ENABLE: + readByte((uint8_t*)&(motor->enabled)); + break; + case REG_MODULATION_MODE: + readByte((uint8_t*)&(motor->foc_modulation)); + break; + case REG_TORQUE_MODE: + readByte((uint8_t*)&(motor->torque_controller)); + break; + case REG_CONTROL_MODE: + readByte((uint8_t*)&(motor->controller)); + break; + case REG_TARGET: + readFloat(&(motor->target)); + break; + case REG_VEL_PID_P: + readFloat(&(motor->PID_velocity.P)); + break; + case REG_VEL_PID_I: + readFloat(&(motor->PID_velocity.I)); + break; + case REG_VEL_PID_D: + readFloat(&(motor->PID_velocity.D)); + break; + case REG_VEL_LPF_T: + readFloat(&(motor->LPF_velocity.Tf)); + break; + case REG_ANG_PID_P: + readFloat(&(motor->P_angle.P)); + break; + case REG_VEL_LIMIT: + readFloat(&(motor->velocity_limit)); + break; + case REG_VEL_MAX_RAMP: + readFloat(&(motor->PID_velocity.output_ramp)); + break; + + 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 new file mode 100644 index 0000000..fc2af1c --- /dev/null +++ b/src/comms/RegisterReceiver.h @@ -0,0 +1,15 @@ + +#pragma once + +#include "./SimpleFOCRegisters.h" +#include "common/base_classes/FOCMotor.h" + + + +class RegisterReceiver { +protected: + virtual void setRegister(SimpleFOCRegister reg, 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 new file mode 100644 index 0000000..2a163ce --- /dev/null +++ b/src/comms/RegisterSender.cpp @@ -0,0 +1,193 @@ + +#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 new file mode 100644 index 0000000..b2486a1 --- /dev/null +++ b/src/comms/RegisterSender.h @@ -0,0 +1,18 @@ + +#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/i2c/I2CCommanderRegisters.h b/src/comms/SimpleFOCRegisters.h similarity index 64% rename from src/comms/i2c/I2CCommanderRegisters.h rename to src/comms/SimpleFOCRegisters.h index cee8b9b..6fea825 100644 --- a/src/comms/i2c/I2CCommanderRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -1,9 +1,17 @@ -#ifndef __I2CCOMMANDERREGISTERS_H__ -#define __I2CCOMMANDERREGISTERS_H__ + +#pragma once + +#include + + +// this constant is changed each time the registers definition are changed *in an incompatible way*. This means that just adding new registers +// does not change the version, but removing or changing the meaning of existing registers does, or changing the number of an existing register. +#define SIMPLEFOC_REGISTERS_VERSION 0x01 + typedef enum : uint8_t { - REG_STATUS = 0x00, // RO - 3 bytes (currently set motor, last command register, last command error status) + 1 byte / motor (motor stati) + REG_STATUS = 0x00, // RO - 1 byte (motor status) REG_MOTOR_ADDRESS = 0x01, // R/W - 1 byte REG_REPORT = 0x02, // R/W - Write: variable, Read: variable, up to 32 bytes REG_ENABLE_ALL = 0x03, // WO - 1 byte @@ -14,7 +22,7 @@ typedef enum : uint8_t { REG_TARGET = 0x08, // R/W - float REG_ANGLE = 0x09, // RO - float - REG_POSITION = 0x10, // RO - double + REG_POSITION = 0x10, // RO - int32_t full rotations + float position (0-2PI, in radians) (4 bytes + 4 bytes) REG_VELOCITY = 0x11, // RO - float REG_SENSOR_ANGLE = 0x12, // RO - float @@ -26,6 +34,7 @@ typedef enum : uint8_t { REG_CURRENT_B = 0x25, // RO - float REG_CURRENT_C = 0x26, // RO - float REG_CURRENT_ABC = 0x27, // RO - 3xfloat = 12 bytes + REG_CURRENT_DC = 0x28, // RO - float REG_VEL_PID_P = 0x30, // R/W - float REG_VEL_PID_I = 0x31, // R/W - float @@ -44,23 +53,20 @@ typedef enum : uint8_t { REG_CURD_PID_D = 0x46, // R/W - float REG_CURD_LPF_T = 0x47, // R/W - float - REG_MAX_VOLTAGE = 0x50, // R/W - float - REG_MAX_CURRENT = 0x51, // R/W - float + REG_VOLTAGE_LIMIT = 0x50, // R/W - float + REG_CURRENT_LIMIT = 0x51, // R/W - float REG_MOTION_DOWNSAMPLE = 0x52, // R/W - uint32_t + REG_DRIVER_VOLTAGE_LIMIT = 0x53,// R/W - float + REG_PWM_FREQUENCY = 0x54, // R/W - uint32_t REG_ZERO_ELECTRIC_ANGLE = 0x60, // RO - float REG_SENSOR_DIRECTION = 0x61, // RO - 1 byte REG_ZERO_OFFSET = 0x62, // R/W - float REG_POLE_PAIRS = 0x63, // RO - uint32_t - REG_PHASE_RESISTANCE = 0x64, // RO - float - - REG_NUM_MOTORS = 0x70, // RO - 1 byte - REG_SYS_TIME = 0x71, // RO - uint32_t - -} I2CCommander_Register; - - - - + REG_PHASE_RESISTANCE = 0x64, // R/W - float + REG_KV = 0x65, // R/W - float + REG_INDUCTANCE = 0x66, // R/W - float -#endif \ No newline at end of file + REG_NUM_MOTORS = 0x70, // RO - 1 byte + REG_SYS_TIME = 0x71, // RO - uint32_t +} SimpleFOCRegister; diff --git a/src/comms/i2c/I2CCommander.cpp b/src/comms/i2c/I2CCommander.cpp index 6ef5068..737b2ee 100644 --- a/src/comms/i2c/I2CCommander.cpp +++ b/src/comms/i2c/I2CCommander.cpp @@ -49,7 +49,7 @@ void I2CCommander::onReceive(int numBytes){ lastcommandregister = curRegister; commanderror = false; if (numBytes>=1) { // set the current register - curRegister = static_cast(_wire->read()); + curRegister = static_cast(_wire->read()); } if (numBytes>1) { // read from i2c and update value represented by register as well... if (!receiveRegister(curMotor, curRegister, numBytes)) @@ -182,10 +182,10 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu case REG_CURD_LPF_T: readBytes(&(motors[motorNum]->LPF_current_d.Tf), 4); break; - case REG_MAX_VOLTAGE: + case REG_VOLTAGE_LIMIT: readBytes(&(motors[motorNum]->voltage_limit), 4); break; - case REG_MAX_CURRENT: + case REG_CURRENT_LIMIT: readBytes(&(motors[motorNum]->current_limit), 4); break; case REG_MOTION_DOWNSAMPLE: @@ -249,7 +249,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { _wire->write((uint8_t)lastcommandregister); _wire->write((uint8_t)lastcommanderror+1); for (int i=0;(iwrite(motors[motorNum]->motor_status); + _wire->write(motors[i]->motor_status); } break; case REG_MOTOR_ADDRESS: @@ -379,10 +379,10 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) { writeFloat(motors[motorNum]->LPF_current_d.Tf); break; - case REG_MAX_VOLTAGE: + case REG_VOLTAGE_LIMIT: writeFloat(motors[motorNum]->voltage_limit); break; - case REG_MAX_CURRENT: + case REG_CURRENT_LIMIT: writeFloat(motors[motorNum]->current_limit); break; case REG_MOTION_DOWNSAMPLE: diff --git a/src/comms/i2c/I2CCommander.h b/src/comms/i2c/I2CCommander.h index 2cb467a..65cefde 100644 --- a/src/comms/i2c/I2CCommander.h +++ b/src/comms/i2c/I2CCommander.h @@ -5,7 +5,7 @@ #include "Arduino.h" #include "Wire.h" #include "common/base_classes/FOCMotor.h" -#include "I2CCommanderRegisters.h" +#include "../SimpleFOCRegisters.h" #ifndef I2CCOMMANDER_MAX_MOTORS_COMMANDABLE #define I2CCOMMANDER_MAX_MOTORS_COMMANDABLE 4 @@ -36,7 +36,7 @@ class I2CCommander { TwoWire* _wire; uint8_t numMotors = 0; uint8_t curMotor = 0; - I2CCommander_Register curRegister = REG_STATUS; + SimpleFOCRegister curRegister = REG_STATUS; bool commanderror = false; bool lastcommanderror = false; uint8_t lastcommandregister = REG_STATUS; diff --git a/src/comms/i2c/I2CCommanderMaster.cpp b/src/comms/i2c/I2CCommanderMaster.cpp index a63c9df..666e009 100644 --- a/src/comms/i2c/I2CCommanderMaster.cpp +++ b/src/comms/i2c/I2CCommanderMaster.cpp @@ -29,7 +29,7 @@ void I2CCommanderMaster::addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, Tw -int I2CCommanderMaster::writeRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size){ +int I2CCommanderMaster::writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size){ motors[motor].wire->beginTransmission(motors[motor].address); motors[motor].wire->write((uint8_t)registerNum); motors[motor].wire->write((uint8_t*)data, size); @@ -38,7 +38,7 @@ int I2CCommanderMaster::writeRegister(int motor, I2CCommander_Register registerN }; -int I2CCommanderMaster::readRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size){ +int I2CCommanderMaster::readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size){ motors[motor].wire->beginTransmission(motors[motor].address); int numWrite = motors[motor].wire->write((uint8_t)registerNum); // TODO check return value motors[motor].wire->endTransmission(); diff --git a/src/comms/i2c/I2CCommanderMaster.h b/src/comms/i2c/I2CCommanderMaster.h index ffba9e1..9b52ae7 100644 --- a/src/comms/i2c/I2CCommanderMaster.h +++ b/src/comms/i2c/I2CCommanderMaster.h @@ -4,7 +4,7 @@ #include #include -#include "I2CCommanderRegisters.h" +#include "../SimpleFOCRegisters.h" #define I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS 4 @@ -23,8 +23,8 @@ class I2CCommanderMaster { void init(); void addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, TwoWire *wire = &Wire); - int writeRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size); - int readRegister(int motor, I2CCommander_Register registerNum, void* data, uint8_t size); + int writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size); + int readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size); int readLastUsedRegister(int motor, void* data, uint8_t size); // Motor intialization interface for convenience - think about how this will best work diff --git a/src/comms/serial/README.md b/src/comms/serial/README.md new file mode 100644 index 0000000..2e1b733 --- /dev/null +++ b/src/comms/serial/README.md @@ -0,0 +1,49 @@ + +# Serial communications classes + +Serial communications classes for register-based control and telemetry from SimpleFOC. + +## SerialASCIITelemetry + +:warning: unfinished, untested + +Telemetry class that sends telemetry as ASCII on a serial port. Similar to the classic "monitoring" functionality of SimpleFOC, but allows you to configure telemetry based on most of the defined registers. + +Usage: + +```c++ + +SerialASCIITelemetry telemetry = SerialASCIITelemetry(); // number of float digits to display + +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(2, [REG_VELOCITY, REG_VOLTAGE_Q]); + telemetry.init(); + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` + +Some options are supported: + +```c++ + telemetry.floatPrecision = 4; // send floats with 4 decimal places + telemetry.min_elapsed_time = 0; // microseconds between sending telemetry + telemetry.downsample = 100; // send every this many loop iterations +``` + + + +## SerialBinaryCommander + +:warning: unfinished, untested! + +Control SimpleFOC via a binary protocol over the serial port. The standard SimpleFOC registers are used. + +TODO document the protocol \ No newline at end of file diff --git a/src/comms/serial/SerialASCIITelemetry.cpp b/src/comms/serial/SerialASCIITelemetry.cpp new file mode 100644 index 0000000..084b940 --- /dev/null +++ b/src/comms/serial/SerialASCIITelemetry.cpp @@ -0,0 +1,65 @@ + +#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 new file mode 100644 index 0000000..e218f3b --- /dev/null +++ b/src/comms/serial/SerialASCIITelemetry.h @@ -0,0 +1,27 @@ + +#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 new file mode 100644 index 0000000..ffa2406 --- /dev/null +++ b/src/comms/serial/SerialBinaryCommander.cpp @@ -0,0 +1,153 @@ + +#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 new file mode 100644 index 0000000..f80ccc4 --- /dev/null +++ b/src/comms/serial/SerialBinaryCommander.h @@ -0,0 +1,57 @@ + +#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/telemetry/README.md b/src/comms/telemetry/README.md new file mode 100644 index 0000000..384098f --- /dev/null +++ b/src/comms/telemetry/README.md @@ -0,0 +1,42 @@ + +# SimpleFOC Telemetry + +:warning: unfinished, untested + +A flexible abstraction for telemetry (monitoring) of SimpleFOC systems. + +The telemetry implementation is based on the SimpleFOC registers, and allows you to send telemetry for any (readable) register. Telemetry supports multiple motors. + +The concept allows you to choose registers which are then sent by the telemetry automatically, on a regular schedule. + +The method of sending depends on the type of telemetry you add to your program. There are telemetry drivers for: + +- Serial ASCII telemetry +- Serial Binary telemetry +- and more drivers will be added in the future + +Multiple motors can be added to the same telemetry, to monitor several motors at the same time. The registers reported by telemetry can be changed at run-time. Multiple instances of telemetry can be used to monitor different sets of values at different time intervals, or to send to multiple channels at the same time. + +## Usage + +Using telemetry is simple: + +```c++ + +SerialASCIITelemetry telemetry = SerialASCIITelemetry(); +... + +void setup() { + ... + telemetry.addMotor(&motor); + telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY }); + telemetry.init(); + ... +} + +void loop() { + motor.move(); + motor.loopFOC(); + telemetry.run(); +} +``` \ No newline at end of file diff --git a/src/comms/telemetry/Telemetry.cpp b/src/comms/telemetry/Telemetry.cpp new file mode 100644 index 0000000..6a6c446 --- /dev/null +++ b/src/comms/telemetry/Telemetry.cpp @@ -0,0 +1,67 @@ + + +#include "./Telemetry.h" + + + +Telemetry::Telemetry() { + this->numRegisters = 0; +}; + + + +Telemetry::~Telemetry(){ + +}; + + + + + +void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){ + if (numRegisters<=TELEMETRY_MAX_REGISTERS) { + this->numRegisters = numRegisters; + for (uint8_t i=0; iregisters[i] = registers[i]; + if (motors!=NULL) + this->registers_motor[i] = motors[i]; + else + this->registers_motor[i] = 0; + } + } +}; + + + +void Telemetry::init() { + headerSent = false; +}; + + + +void Telemetry::run() { + if (numRegisters<1) + return; + if (!headerSent) { + sendHeader(); + headerSent = true; + } + if (downsampleCnt++ < downsample) return; + downsampleCnt = 0; + if (min_elapsed_time > 0) { + long now = _micros(); + if (now - last_run_time < min_elapsed_time) return; + last_run_time = now; + } + sendTelemetry(); +} + + + +void Telemetry::addMotor(FOCMotor* motor) { + if (numMotors < TELEMETRY_MAX_MOTORS) { + motors[numMotors] = motor; + numMotors++; + } +}; + diff --git a/src/comms/telemetry/Telemetry.h b/src/comms/telemetry/Telemetry.h new file mode 100644 index 0000000..256036a --- /dev/null +++ b/src/comms/telemetry/Telemetry.h @@ -0,0 +1,53 @@ + +#pragma once + +#include "../SimpleFOCRegisters.h" +#include "../RegisterSender.h" + +#ifndef TELEMETRY_MAX_REGISTERS +#define TELEMETRY_MAX_REGISTERS 8 +#endif + +#ifndef TELEMETRY_MAX_MOTORS +#define TELEMETRY_MAX_MOTORS 4 +#endif + + +#define DEF_TELEMETRY_DOWNSAMPLE 100 + + +typedef enum : uint8_t { + TELEMETRY_FRAMETYPE_DATA = 0x01, + TELEMETRY_FRAMETYPE_HEADER = 0x02 +} TelemetryFrameType; + + + + +class Telemetry : public RegisterSender { +public: + Telemetry(); + virtual ~Telemetry(); + virtual void init(); + void addMotor(FOCMotor* motor); + void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL); + void run(); + + uint16_t downsample = DEF_TELEMETRY_DOWNSAMPLE; + uint32_t min_elapsed_time = 0; +protected: + virtual void sendTelemetry() = 0; + virtual void sendHeader() = 0; + + FOCMotor* motors[TELEMETRY_MAX_MOTORS]; + uint8_t numMotors = 0; + + uint8_t numRegisters; + uint8_t registers[TELEMETRY_MAX_REGISTERS]; + uint8_t registers_motor[TELEMETRY_MAX_REGISTERS]; + uint8_t frameSize; + bool headerSent; + long last_run_time = 0; + uint16_t downsampleCnt = 0; +}; + diff --git a/src/drivers/tmc6200/README.md b/src/drivers/tmc6200/README.md new file mode 100644 index 0000000..b34ba7a --- /dev/null +++ b/src/drivers/tmc6200/README.md @@ -0,0 +1,147 @@ + +# TMC6200 SimpleFOC Driver + +by [@YaseenTwati](https://github.com/YaseenTwati) + +The TMC6200 is a Universal high voltage BLDC/PMSM/Servo MOSFET 3-halfbridge gate-driver with in line motor current +sensing. External MOSFETs for up to 100A motor current. + +See https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC6200_datasheet_Rev1.05.pdf for more information. + +## Hardware setup + +To use the TMC6200 you will have to connect the following: + +- GND +- VCC_IO - 3.3V or 5V +- SPE - pull up to VCC to enable SPI mode +- SPI MOSI ( SDI ) +- SPI MISO ( SDO ) +- SPI CLK +- SPI nCS +- DRV_EN - connect to digital out (or pull up to VCC) +- UH - connect to motor PWM pin +- VH - connect to motor PWM pin +- WH - connect to motor PWM pin +- UL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation +- VL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation +- WL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation + +Optionally, but useful: + +- FAULT - digital in, active high + +For current sensing: + +- CURU - connect to analog in +- CURV - connect to analog in +- CURW - connect to analog in + +## Usage + +Usage is quite easy, especially if you already know SimpleFOC. See also the [examples](https://github.com/simplefoc/Arduino-FOC-drivers/examples/drivers/drv8316/) + +```c++ +#include "Arduino.h" +#include +#include "SimpleFOCDrivers.h" +#include "drivers/tmc6200/TMC6200.hpp" + +BLDCMotor motor = BLDCMotor(15); +TMC6200Driver6PWM driver = DRV8316Driver6PWM(UH, UL, VH, VL, WH, WL, nCS, DRV_EN); + +//... normal simpleFOC init code... +``` + +Or, for 3-PWM: + +```c++ +#include "Arduino.h" +#include +#include "SimpleFOCDrivers.h" +#include "drivers/tmc6200/TMC6200.hpp" + +BLDCMotor motor = BLDCMotor(15); +TMC6200Driver3PWM driver = TMC6200Driver3PWM(UH, MOTOR_VH, MOTOR_WH, nCS, DRV_EN); + +void setup() { + + pinMode(UL, OUTPUT); + pinMode(VL, OUTPUT); + pinMode(WL, OUTPUT); + + digitalWrite(WL, HIGH); + digitalWrite(UL, HIGH); + digitalWrite(VL, HIGH); + + //... normal simpleFOC init code... +} + +``` + +### Validating the SPI Connection +You can validate the SPI connection by checking the value of VERSION field in IOIN register. The value should be 0x10. + +```c++ + if(driver.getInputs().VERSION != TMC6200_VERSION){ + // something is wrong with the spi connection + } +``` + +### Current Sensing +The gain of the internal current amplifiers can be set to 5, 10 or 20 through `setCurrentSenseGain()` + +```c++ + driver.setCurrentSenseGain(TMC6200_AmplificationGain::_5); + //driver.setCurrentSenseGain(TMC6200_AmplificationGain::_10); + //driver.setCurrentSenseGain(TMC6200_AmplificationGain::_20); +``` +The sense amplifiers can also be turned off ( they are on by default ), through `setCurrentSenseAmplifierState()` + +```c++ + driver.setCurrentSenseAmplifierState(false); +``` +### Driver Strength +The strength of the mosfet drivers can be controlled through `setDriverStrength()` + +```c++ + driver.setDriverStrength(TMC6200_DRVStrength::Weak); + //driver.setDriverStrength(TMC6200_DRVStrength::WeakTC); // (medium above OTPW level) + //driver.setDriverStrength(TMC6200_DRVStrength::Medium); + //driver.setDriverStrength(TMC6200_DRVStrength::Strong); +``` + +### Handling Faults +The fault line will go high if a fault occurs such as a short, an interrupt can be used to handle it. +Note that some faults will disable the driver and will require the DRV_EN to be cycled to clear the fault. + +```c++ + // somewhere in setup + attachInterrupt(digitalPinToInterrupt(FAULT), handleFault, RISING); +``` + +```c++ + void handleFault() + { + // you can read the status register to see what happened + TMC6200GStatus status = driver.getStatus(); + Serial.print("hasUShorted: "); Serial.println(status.hasUShorted()); + Serial.print("hasVShorted: "); Serial.println(status.hasVShorted()); + Serial.print("hasWShorted: "); Serial.println(status.hasWShorted()); + Serial.print("isUShortedToGround: "); Serial.println(status.isUShortedToGround()); + Serial.print("isUShortedToSupply: "); Serial.println(status.isUShortedToSupply()); + Serial.print("isVShortedToGround: "); Serial.println(status.isVShortedToGround()); + Serial.print("isVShortedToSupply: "); Serial.println(status.isVShortedToSupply()); + Serial.print("isWShortedToGround: "); Serial.println(status.isWShortedToGround()); + Serial.print("isWShortedToSupply: "); Serial.println(status.isWShortedToSupply()); + Serial.print("isOverTemperaturePreWarning: "); Serial.println(status.isOverTemperaturePreWarning()); + Serial.print("isChargePumpUnderVoltage: "); Serial.println(status.isChargePumpUnderVoltage()); + + // the driver must be cycled to clear the fault + digitalWrite(DRV_EN, LOW); + delayMicrosockets(someSmallDelay); + digitalWrite(DRV_EN, HIGH); + } +``` + +The driver provides other features such as controlling the tolerences of short detection and the BBM cycle time and so on, setting the options can be conveniently done via the provided setter methods. All documented registers and options are available via the driver, and the option values can be accessed via enums. diff --git a/src/drivers/tmc6200/TMC6200.cpp b/src/drivers/tmc6200/TMC6200.cpp new file mode 100644 index 0000000..659ab5d --- /dev/null +++ b/src/drivers/tmc6200/TMC6200.cpp @@ -0,0 +1,221 @@ +#include "TMC6200.hpp" + +void TMC6200Driver::init(SPIClass *_spi) { + spi = _spi; + settings = SPISettings(500000, MSBFIRST, SPI_MODE3); + + pinMode(csPin, OUTPUT); + digitalWrite(csPin, HIGH); +} + +// TMC6200_GCONF ------ + +void TMC6200Driver::setDriverState(bool state) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.DISABLE = state ? 0 : 1; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setPWMMode(TMC6200_PWMMode pwmMode) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.SINGLE_LINE = pwmMode; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setFaultDirect(TMC6200_FaultDirect faultDirect) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.FAULT_DIRECT = faultDirect; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setCurrentSenseAmplifierState(bool state) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.AMPLIFIER_OFF = state ? 0 : 1; + writeRegister(TMC6200_GCONF_REG, gConf.reg); +} + +void TMC6200Driver::setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain) { + TMC6200_GCONF gConf; + gConf.reg = readRegister(TMC6200_GCONF_REG); + + gConf.AMPLIFICATION = amplificationGain; + writeRegister(TMC6200_GCONF_REG, gConf.reg); + + gConf.reg = readRegister(TMC6200_GCONF_REG); +} + +// TMC6200_DRV_CONF ------ + +void TMC6200Driver::setOverTemperatureThreshold(TMC6200_OTSelect otLevel) { + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); + + drvConf.OT_SELECT = otLevel; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); +} + +void TMC6200Driver::setDriverStrength(TMC6200_DRVStrength strength) { + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); + + drvConf.DRV_STRENGTH = strength; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); +} + +void TMC6200Driver::setBBMCycles(uint8_t clockCycles) { + TMC6200_DRV_CONF drvConf; + drvConf.reg = readRegister(TMC6200_DRV_CONF_REG); + + drvConf.BBMCLKS = clockCycles; + writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg); +} + +// TMC6200_SHORT_CONF ------ + +void TMC6200Driver::setShortDelay(TMC6200_ShortDelay shortDelay) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.SHORT_DELAY = shortDelay; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::shortFilter(TMC6200_ShortFilter shortFilter) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.SHORT_FILTER = shortFilter; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToSupplySensitivityLevel(uint8_t level) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.S2VS_LEVEL = level; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToGroundSensitivityLevel(uint8_t level) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.S2G_LEVEL = level; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortRetries(uint8_t retries) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.RETRY = retries; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setParallelProtect(TMC6200_ParallelProtect parallelProtect) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.PROTECT_PARALLEL = parallelProtect; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToGroundDetectionState(bool state) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.DISABLE_S2G = state ? 0 : 1; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +void TMC6200Driver::setShortToSupplyDetectionState(bool state) { + TMC6200_SHORT_CONF shortConf; + shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG); + + shortConf.DISABLE_S2VS = state ? 0 : 1; + writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg); +} + +// TMC6200_IOIN ------ + +TMC6200_IOIN TMC6200Driver::getInputs() { + TMC6200_IOIN ioin; + ioin.reg = readRegister(TMC6200_IOIN_REG); + + return ioin; +} + +// TMC6200_GSTAT ------ + +TMC6200GStatus TMC6200Driver::getStatus() { + TMC6200_GSTAT gstat; + gstat.reg = readRegister(TMC6200_GSTAT_REG); + + return {gstat}; +} + +void TMC6200Driver::setStatus(TMC6200_GSTAT status) { + writeRegister(TMC6200_GSTAT_REG, status.reg); +} + +uint32_t TMC6200Driver::readRegister(uint8_t addr) { + digitalWrite(csPin, LOW); + spi->beginTransaction(settings); + + // Address + spi->transfer(TMC_ADDRESS(addr)); + + // 32bit Data + uint32_t value = 0; + value |= (spi->transfer(0x00) << 24); + value |= (spi->transfer(0x00) << 16); + value |= (spi->transfer(0x00) << 8); + value |= (spi->transfer(0x00) << 0); + + spi->end(); + digitalWrite(csPin, HIGH); + + return value; +} + +void TMC6200Driver::writeRegister(uint8_t addr, uint32_t data) { + digitalWrite(csPin, LOW); + spi->beginTransaction(settings); + + // Address + spi->transfer(addr | TMC_WRITE_BIT); + + // 32bit Data + spi->transfer(0xFF & (data >> 24)); + spi->transfer(0xFF & (data >> 16)); + spi->transfer(0xFF & (data >> 8)); + spi->transfer(0xFF & (data >> 0)); + + spi->end(); + digitalWrite(csPin, HIGH); +} + +// -------------------- + +void TMC6200Driver3PWM::init(SPIClass *_spi) { + TMC6200Driver::init(_spi); + delayMicroseconds(1); + TMC6200Driver::setPWMMode(TMC6200_PWMMode::SingleLine); + BLDCDriver3PWM::init(); +}; + + +void TMC6200Driver6PWM::init(SPIClass *_spi) { + TMC6200Driver::init(_spi); + delayMicroseconds(1); + TMC6200Driver::setPWMMode(TMC6200_PWMMode::Individual); + BLDCDriver6PWM::init(); +}; diff --git a/src/drivers/tmc6200/TMC6200.hpp b/src/drivers/tmc6200/TMC6200.hpp new file mode 100644 index 0000000..7a5215c --- /dev/null +++ b/src/drivers/tmc6200/TMC6200.hpp @@ -0,0 +1,171 @@ +#pragma once + +#include +#include +#include +#include +#include "TMC6200_Registers.hpp" + +#define TMC6200_VERSION 0x10 + +enum TMC6200_PWMMode { + Individual = 0, // 6PWM + SingleLine = 1, // 3PWM +}; + +enum TMC6200_AmplificationGain { + _5 = 0, + _10 = 1, + _Also_10 = 2, // maybe just remove this + _20 = 3, +}; + +enum TMC6200_ShortDelay { + _750nS = 0, + _1500nS = 1, +}; + +enum TMC6200_ShortFilter { + _100nS = 0, + _1uS = 1, + _2uS = 2, + _3uS = 3, +}; + +enum TMC6200_ParallelProtect { + Detected = 0, + All = 1, +}; + +enum TMC6200_FaultDirect { + AtLeastOneBridgeDown = 0, + EachProtectiveAction = 1, +}; + +enum TMC6200_OTSelect { + _150 = 0b00, + _143 = 0b01, + _136 = 0b10, + _120 = 0b11, +}; + +enum TMC6200_DRVStrength { + Weak = 0b00, + WeakTC = 0b01, // (medium above OTPW level) + Medium = 0b10, + Strong = 0b11, +}; + +class TMC6200GStatus { +public: + TMC6200GStatus(TMC6200_GSTAT status) : status(status) {}; + + bool isReset() const { return status.RESET == 0b1; }; + + bool isOverTemperaturePreWarning() const { return status.DRV_OTPW == 0b1; }; + + bool isOverTemperature() const { return status.DRV_OT == 0b1; }; + + bool isChargePumpUnderVoltage() const { return status.UV_CP == 0b1; }; + + bool hasUShorted() const { return status.SHORT_DET_U == 0b1; }; + + bool hasVShorted() const { return status.SHORT_DET_V == 0b1; }; + + bool hasWShorted() const { return status.SHORT_DET_W == 0b1; }; + + bool isUShortedToGround() const { return status.S2GU == 0b1; }; + + bool isUShortedToSupply() const { return status.S2VSU == 0b1; }; + + bool isVShortedToGround() const { return status.S2GV == 0b1; }; + + bool isVShortedToSupply() const { return status.S2VSV == 0b1; }; + + bool isWShortedToGround() const { return status.S2GW == 0b1; }; + + bool isWShortedToSupply() const { return status.S2VSW == 0b1; }; + + TMC6200_GSTAT status; +}; + +class TMC6200Driver { + +public: + TMC6200Driver(int csPin) : csPin(csPin), spi(&SPI), settings(1000000, MSBFIRST, SPI_MODE3) {}; + + virtual ~TMC6200Driver() {}; + + virtual void init(SPIClass *_spi); + + void setDriverState(bool state); + + void setPWMMode(TMC6200_PWMMode pwmMode); + + void setFaultDirect(TMC6200_FaultDirect faultDirect); + + void setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain); + + void setOverTemperatureThreshold(TMC6200_OTSelect otLevel); + + void setDriverStrength(TMC6200_DRVStrength strength); + + void setCurrentSenseAmplifierState(bool state); + + void setShortDelay(TMC6200_ShortDelay shortDelay); + + void shortFilter(TMC6200_ShortFilter shortFilter); + + void setShortToSupplySensitivityLevel(uint8_t level); + + void setShortToGroundSensitivityLevel(uint8_t level); + + void setShortRetries(uint8_t retries); + + void setParallelProtect(TMC6200_ParallelProtect parallelProtect); + + void setShortToGroundDetectionState(bool state); + + void setShortToSupplyDetectionState(bool state); + + void setBBMCycles(uint8_t clockCycles); + + void setStatus(TMC6200_GSTAT status); + + TMC6200GStatus getStatus(); + + TMC6200_IOIN getInputs(); + +private: + uint32_t readRegister(uint8_t addr); + + void writeRegister(uint8_t addr, uint32_t data); + + int csPin; + SPIClass *spi; + SPISettings settings; +}; + + +class TMC6200Driver3PWM : public TMC6200Driver, public BLDCDriver3PWM { + +public: + TMC6200Driver3PWM(int phA, int phB, int phC, int cs, int en = NOT_SET) : + TMC6200Driver(cs), BLDCDriver3PWM(phA, phB, phC, en) {}; + + ~TMC6200Driver3PWM() override = default; + + virtual void init(SPIClass *_spi = &SPI) override; +}; + + +class TMC6200Driver6PWM : public TMC6200Driver, public BLDCDriver6PWM { + +public: + TMC6200Driver6PWM(int phA_h, int phA_l, int phB_h, int phB_l, int phC_h, int phC_l, int cs, int en = NOT_SET) : + TMC6200Driver(cs), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) {}; + + ~TMC6200Driver6PWM() override = default; + + virtual void init(SPIClass *_spi = &SPI) override; +}; \ No newline at end of file diff --git a/src/drivers/tmc6200/TMC6200_Registers.hpp b/src/drivers/tmc6200/TMC6200_Registers.hpp new file mode 100644 index 0000000..aff9fed --- /dev/null +++ b/src/drivers/tmc6200/TMC6200_Registers.hpp @@ -0,0 +1,111 @@ +#pragma once + +// ------ + +#define TMC_WRITE_BIT 0x80 +#define TMC_ADDRESS_MASK 0x7F +#define TMC_ADDRESS(x) ((x) & (TMC_ADDRESS_MASK)) + +// ------ + +#define TMC6200_GCONF_REG 0x00 // RW +#define TMC6200_GSTAT_REG 0x01 // RW + WC +#define TMC6200_IOIN_REG 0x04 // R + +#define TMC6200_OTP_PROG_REG 0x06 // not used +#define TMC6200_OTP_READ_REG 0x07 // not used +#define TMC6200_FACTORY_CONF_REG 0x08 // not used + +#define TMC6200_SHORT_CONF_REG 0x09 // RW +#define TMC6200_DRV_CONF_REG 0x0A // RW + +typedef union { + struct { + uint32_t DISABLE: 1; + uint32_t SINGLE_LINE: 1; + uint32_t FAULT_DIRECT: 1; + uint32_t : 1; + uint32_t AMPLIFICATION: 2; + uint32_t AMPLIFIER_OFF: 1; + uint32_t TMC6200_TEST_MODE: 1; + uint32_t : 24; + }; + uint32_t reg; +} TMC6200_GCONF; + +typedef union { + struct { + uint32_t RESET: 1; + uint32_t DRV_OTPW: 1; + uint32_t DRV_OT: 1; + uint32_t UV_CP: 1; + + uint32_t SHORT_DET_U: 1; + uint32_t S2GU: 1; + uint32_t S2VSU: 1; + uint32_t : 1; + + uint32_t SHORT_DET_V: 1; + uint32_t S2GV: 1; + uint32_t S2VSV: 1; + uint32_t : 1; + + uint32_t SHORT_DET_W: 1; + uint32_t S2GW: 1; + uint32_t S2VSW: 1; + uint32_t : 1; + }; + uint32_t reg; +} TMC6200_GSTAT; + +typedef union { + struct { + unsigned UL: 1; + uint32_t UH: 1; + uint32_t VL: 1; + uint32_t VH: 1; + uint32_t WL: 1; + uint32_t WH: 1; + uint32_t DRV_EN: 1; + uint32_t _reserved1: 1; + uint32_t OTPW: 1; + uint32_t OT136C: 1; + uint32_t OT143C: 1; + uint32_t OT150C: 1; + uint32_t _reserved2: 12; + uint32_t VERSION: 8; + }; + + uint32_t reg; +} TMC6200_IOIN; + +typedef union { + struct { + uint32_t S2VS_LEVEL: 4; + uint32_t : 4; + uint32_t S2G_LEVEL: 4; + uint32_t : 4; + uint32_t SHORT_FILTER: 2; + uint32_t : 2; + uint32_t SHORT_DELAY: 1; + uint32_t : 4; + uint32_t RETRY: 2; + uint32_t : 2; + uint32_t PROTECT_PARALLEL: 1; + uint32_t DISABLE_S2G: 1; + uint32_t DISABLE_S2VS: 1; + uint32_t : 2; + }; + uint32_t reg; +} TMC6200_SHORT_CONF; + +typedef union { + struct { + uint32_t BBMCLKS: 5; + uint32_t : 11; + uint32_t OT_SELECT: 2; + uint32_t DRV_STRENGTH: 2; + uint32_t : 12; + }; + uint32_t reg; +} TMC6200_DRV_CONF; \ No newline at end of file diff --git a/src/encoders/as5600/AS5600.cpp b/src/encoders/as5600/AS5600.cpp new file mode 100644 index 0000000..84e66e2 --- /dev/null +++ b/src/encoders/as5600/AS5600.cpp @@ -0,0 +1,175 @@ + + +#include "./AS5600.h" + + +AS5600::AS5600(uint8_t address) : _address(address) {}; +AS5600::~AS5600() {}; + +void AS5600::init(TwoWire* wire){ + _wire = wire; + _wire->begin(); + if (!closeTransactions) { + setAngleRegister(); + } +}; + + +void AS5600::setAngleRegister() { + _wire->beginTransmission(_address); + if (useHysteresis) + _wire->write(AS5600_REG_ANGLE); + else + _wire->write(AS5600_REG_ANGLE_RAW); + _wire->endTransmission(false); +} + + +uint16_t AS5600::angle() { + uint16_t result = 0; + if (!closeTransactions) { + setAngleRegister(); + } + _wire->requestFrom(_address, (uint8_t)2, (uint8_t)closeTransactions); + result = _wire->read()<<8; + result |= _wire->read(); + return result; +}; + + +uint16_t AS5600::readRawAngle() { + return readRegister(AS5600_REG_ANGLE_RAW, 2); +}; + + +uint16_t AS5600::readAngle() { + return readRegister(AS5600_REG_ANGLE, 2); +}; + + + +uint16_t AS5600::readMagnitude() { + return readRegister(AS5600_REG_MAGNITUDE, 2); +}; + + +AS5600Status AS5600::readStatus() { + AS5600Status result; + result.reg = (uint8_t)readRegister(AS5600_REG_STATUS, 1); // TODO: shift bits around + return result; +}; + + +uint8_t AS5600::readAGC() { + return (uint8_t)readRegister(AS5600_REG_AGC, 1); +}; + + + +AS5600Conf AS5600::readConf() { + AS5600Conf result; + result.reg = readRegister(AS5600_REG_CONF, 2); + return result; +}; + + +uint16_t AS5600::readMang() { + return readRegister(AS5600_REG_MANG, 2); +}; + + +uint16_t AS5600::readMPos() { + return readRegister(AS5600_REG_MPOS, 2); +}; + + +uint16_t AS5600::readZPos() { + return readRegister(AS5600_REG_ZPOS, 2); +}; + + +uint8_t AS5600::readZMCO() { + return (readRegister(AS5600_REG_ZMCO, 1)&0x03); +}; + +uint8_t AS5600::readI2CAddr() { + return (readRegister(AS5600_REG_I2CADDR, 1)>>1); +}; + + +// set registers +void AS5600::setConf(AS5600Conf value) { + // TODO: read before write + writeRegister(AS5600_REG_CONF, value.reg); +}; + + +void AS5600::setMang(uint16_t value) { + // TODO: read before write + writeRegister(AS5600_REG_MANG, value); +}; + + +void AS5600::setMPos(uint16_t value) { + // TODO: read before write + writeRegister(AS5600_REG_MPOS, value); +}; + + +void AS5600::setZPos(uint16_t value) { + // TODO: read before write + writeRegister(AS5600_REG_ZPOS, value); +}; + +void AS5600::setI2CAddr(uint8_t value) { + uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CADDR, 1); + val = (value<<1) | (val&0x01); + writeRegister(AS5600_REG_I2CADDR, val); +}; + +void AS5600::setI2CUpdt(uint8_t value) { + uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CUPDT, 1); + val = (value<<1) | (val&0x01); + writeRegister(AS5600_REG_I2CUPDT, val); +}; + + +void AS5600::burnSettings(){ + writeRegister(AS5600_REG_BURN, 0x40, 1); +} + + + +uint16_t AS5600::readRegister(uint8_t reg, uint8_t len){ + uint16_t result = 0; + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->endTransmission(false); + _wire->requestFrom(_address, len, (uint8_t)closeTransactions); + if (!closeTransactions) { + setAngleRegister(); + } + result = _wire->read(); + if (len == 2) { + result <<= 8; + result |= _wire->read(); + } + return result; +}; + + + +void AS5600::writeRegister(uint8_t reg, uint16_t val, uint8_t len){ + _wire->beginTransmission(_address); + _wire->write(reg); + if (len == 2) { + _wire->write(val>>8); + } + _wire->write(val&0xFF); + _wire->endTransmission(closeTransactions); + if (!closeTransactions) { + setAngleRegister(); + } +}; + + diff --git a/src/encoders/as5600/AS5600.h b/src/encoders/as5600/AS5600.h new file mode 100644 index 0000000..8518c17 --- /dev/null +++ b/src/encoders/as5600/AS5600.h @@ -0,0 +1,104 @@ + +#pragma once + + +#include +#include + + +#define AS5600_REG_ZMCO 0x00 +#define AS5600_REG_ZPOS 0x01 +#define AS5600_REG_MPOS 0x03 +#define AS5600_REG_MANG 0x05 +#define AS5600_REG_CONF 0x07 + +#define AS5600_REG_I2CADDR 0x20 +#define AS5600_REG_I2CUPDT 0x21 + +#define AS5600_REG_ANGLE 0x0E +#define AS5600_REG_ANGLE_RAW 0x0C + +#define AS5600_REG_STATUS 0x0B +#define AS5600_REG_AGC 0x1A +#define AS5600_REG_MAGNITUDE 0x1B + +#define AS5600_REG_BURN 0xFF + +#define AS5600_CPR (4096.0f) + + +union AS5600Conf { + struct { + uint16_t pm:2; + uint16_t hyst:2; + uint16_t outs:2; + uint16_t pwmf:2; + uint16_t sf:2; + uint16_t fth:3; + uint16_t wd:1; + uint16_t unused:2; + }; + uint16_t reg; +}; + +union AS5600Status { + struct { + uint8_t unused:3; + uint8_t mh:1; + uint8_t ml:1; + uint8_t md:1; + uint8_t unused2:2; + }; + uint8_t reg; +}; + + + + +class AS5600 { +public: + AS5600(uint8_t address = 0x36); + ~AS5600(); + + virtual void init(TwoWire* wire = &Wire); + + // read an angle, either with or without hysteresis, depending on the useHysteresis flag + // and using fast mode (not closing transactions) if so configured + uint16_t angle(); + + // read registers + uint16_t readRawAngle(); + uint16_t readAngle(); + + uint16_t readMagnitude(); + AS5600Status readStatus(); + uint8_t readAGC(); + + AS5600Conf readConf(); + uint16_t readMang(); + uint16_t readMPos(); + uint16_t readZPos(); + uint8_t readZMCO(); + uint8_t readI2CAddr(); + + // set registers + void setConf(AS5600Conf value); + void setMang(uint16_t value); + void setMPos(uint16_t value); + void setZPos(uint16_t value); + void setI2CAddr(uint8_t value); + void setI2CUpdt(uint8_t value); + + void burnSettings(); + + bool closeTransactions = true; + bool useHysteresis = true; + uint8_t _address; +protected: + TwoWire* _wire; + + void setAngleRegister(); + uint16_t readRegister(uint8_t reg, uint8_t len); + void writeRegister(uint8_t reg, uint16_t val, uint8_t len = 2); +}; + diff --git a/src/encoders/as5600/MagneticSensorAS5600.cpp b/src/encoders/as5600/MagneticSensorAS5600.cpp new file mode 100644 index 0000000..4cea5cf --- /dev/null +++ b/src/encoders/as5600/MagneticSensorAS5600.cpp @@ -0,0 +1,18 @@ + + +#include "./MagneticSensorAS5600.h" +#include "common/foc_utils.h" + +MagneticSensorAS5600::MagneticSensorAS5600(uint8_t _address) : AS5600(_address) {}; +MagneticSensorAS5600::~MagneticSensorAS5600() {}; + +void MagneticSensorAS5600::init(TwoWire* wire) { + AS5600::init(wire); + Sensor::init(); +}; + + +float MagneticSensorAS5600::getSensorAngle() { + uint16_t raw = readRawAngle(); + return raw / AS5600_CPR * _2PI; +}; \ No newline at end of file diff --git a/src/encoders/as5600/MagneticSensorAS5600.h b/src/encoders/as5600/MagneticSensorAS5600.h new file mode 100644 index 0000000..82a8419 --- /dev/null +++ b/src/encoders/as5600/MagneticSensorAS5600.h @@ -0,0 +1,22 @@ + +#pragma once + + +#include "./AS5600.h" +#include "common/base_classes/Sensor.h" + + +class MagneticSensorAS5600 : public Sensor, public AS5600 { + +public: + MagneticSensorAS5600(uint8_t _address = 0x36); + ~MagneticSensorAS5600(); + + virtual void init(TwoWire* wire = &Wire); + + virtual float getSensorAngle() override; + +protected: + +}; + diff --git a/src/encoders/as5600/README.md b/src/encoders/as5600/README.md new file mode 100644 index 0000000..eb0f37a --- /dev/null +++ b/src/encoders/as5600/README.md @@ -0,0 +1,131 @@ +# AS5600 SimpleFOC driver + +I2C protocol driver for the AMS AS5600 magnetic encoder (digital potentiometer). Also supports the newer AS5600L variant. + +:warning: work in progress + +## Hardware setup + +Connect the sensor to 3.3V or 5V power as appropriate, and to I2C (SDA and SCL). + +Important: please make sure the direction pin (DIR) is either pulled up to VDD, or down to GND. Do not leave the direction pin floating. + +## Software setup + +The sensor driver is easy to use. + +```c++ +#include +#include +#include +#include "encoders/as5600/MagneticSensorAS5600.h" + +MagneticSensorAS5600 sensor; + +void setup() { + sensor.init(); +} + +long ts; + +void loop() { + sensor.update(); + if (millis() - ts > 1000) { + Serial.println(sensor.getAngle(), 3); + ts = millis(); + } + delay(1); +} +``` + +You can use a different I2C bus by passing a pointer to its TwoWire object in the init method: + +```c++ +MagneticSensorAS5600 sensor1; +MagneticSensorAS5600 sensor2; +TwoWire myI2C; + +void setup() { + + ... + + sensor1.init(&myI2C); + sensor2.init(&Wire2); +} +``` + +Using the sensor without releasing the bus should give you considerably more speed. Setting closeTransactions to false means the sensor will not release the bus between angle reads, and also will not re-write the register to the device, which boosts angle reading throughput significantly. + +```c++ +MagneticSensorAS5600 sensor; + +void setup() { + sensor.closeTransactions = false; + sensor.init(); +} +``` + +The sensor's other registers are exposed. Note that using the setter functions to set register values only performs a normal write, not a permanent programing of the register. Permanent programming (BURN function) is not supported by this driver. + +```c++ + +MagneticSensorAS5600 sensor; + +void setup() { + sensor.closeTransactions = false; + sensor.init(); + + uint16_t magnitude = sensor.readMagnitude(); + AS5600Status status = sensor.readStatus(); + // print the values or something +} + +``` + +The AS5600L has a default address of 0x40, and you can set the I2C address. To temporarily change the address (resets to default on restart): + +```c++ + +MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40 + +void setup() { + Serial.begin(115200); + delay(2000); + + sensor.closeTransactions = true; // use normal transactons + sensor.init(); + sensor.setI2CAddr(0x41); // set address to 0x41 + sensor.setI2CUpdt(0x41); // i2c address is now 0x41 + sensor._address = 0x41; + delay(10); + // sensor is using new address + uint16_t magnitude = sensor.readMagnitude(); +} +``` + + +You can program an I2C address permanently (AS5600L only). The address will remain after reset. This programming operation can only be done once. + +```c++ + +MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40 + +void setup() { + Serial.begin(115200); + delay(2000); + + sensor.closeTransactions = true; // use normal transactons + sensor.init(); + + uint8_t addr = sensor.readI2CAddr(); // read I2C address, should be 0x40 + Serial.print("Current Address: "); + Serial.println(addr); + + sensor.setI2CAddr(0x41); // set address to 0x41 + sensor.burnSettings(); // permanently set new address + Serial.println("Permanently changed address to 0x41. Restart the system."); + while (1); +} + + +``` \ No newline at end of file diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 859e919..e3d579c 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -153,7 +153,7 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ if(i==(k*128+96)) { _delay(50); - avg_elec_angle += _normalizeAngle(_wrapped.getMechanicalAngle()*_NPP); + avg_elec_angle += _normalizeAngle(directionSensor*_wrapped.getMechanicalAngle()*_NPP); k += 1; } } diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h index 61d1e6b..32abd1d 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.h +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -10,7 +10,7 @@ #define MT6701_BITORDER MSBFIRST -#if defined(TARGET_RP2040)||defined(ESP_H) +#if defined(TARGET_RP2040)||defined(ESP_H)||defined(CORE_TEENSY) #define MT6701_DATA_POS 1 #else #define MT6701_DATA_POS 2 diff --git a/src/encoders/sc60228/README.md b/src/encoders/sc60228/README.md index e0e832d..49f7901 100644 --- a/src/encoders/sc60228/README.md +++ b/src/encoders/sc60228/README.md @@ -31,9 +31,10 @@ void setup() { } ``` -Set some options: +Set some SPI options: ```c++ +SPISettings mySPISettings(1000000, SC60228_BITORDER, SPI_MODE0); // lower speed to 1Mhz MagneticSensorSC60228 sensor1(SENSOR1_CS, mySPISettings); ``` diff --git a/src/encoders/sc60228/SC60228.h b/src/encoders/sc60228/SC60228.h index f476102..03b4e50 100644 --- a/src/encoders/sc60228/SC60228.h +++ b/src/encoders/sc60228/SC60228.h @@ -21,7 +21,7 @@ typedef union { #define SC60228_CPR 4096 #define SC60228_BITORDER MSBFIRST -static SPISettings SC60228SPISettings(8000000, SC60228_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") +static SPISettings SC60228SPISettings(8000000, SC60228_BITORDER, SPI_MODE0); // @suppress("Invalid arguments") diff --git a/src/encoders/smoothing/README.md b/src/encoders/smoothing/README.md new file mode 100644 index 0000000..733284c --- /dev/null +++ b/src/encoders/smoothing/README.md @@ -0,0 +1,69 @@ +# Smoothing Sensor + +by [@dekutree64](https://github.com/dekutree64) + +A SimpleFOC Sensor wrapper implementation which adds angle extrapolation + +Please also see our [forum thread](https://community.simplefoc.com/t/smoothingsensor-experimental-sensor-angle-extrapoltion/3105) on this topic. + + +SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better quality angle readings from sensors that are low resolution or are slow to communicate. It provides no improvement for sensors that are high resolution and quick to update. It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity from the motor to predict what the angle should be at the current time. + + +## Hardware setup + +Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without smoothing first. Once things are working and tuned without smoothing, you can add the SmoothingSensor to see if you get an improvement. + + +## Softwate setup + +The SmoothingSensor acts as a wrapper to the actual sensor class. When creating the SmoothingSensor object, provide the real sensor to the constructor of SmoothingSensor. Initialize the real sensor instance as normal. Then link the SmoothingSensor to the motor and call motor.initFOC(). + + +```c++ +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); +// real sensor instance +MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904); +// instantiate the smoothing sensor, providing the real sensor as a constructor argument +SmoothingSensor smooth = SmoothingSensor(sensor, motor); + +void doPWM(){sensor.handlePWM();} + +void setup() { + sensor.init(); + sensor.enableInterrupt(doPWM); + // Link motor to sensor + motor.linkSensor(&smooth); + // power supply voltage + driver.voltage_power_supply = 20; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage + motor.voltage_sensor_align = 8; + motor.voltage_limit = 20; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_VEL; + motor.monitor_downsample = 10; // default 10 + + // initialize motor + motor.init(); + motor.initFOC(); +} +``` + + +## Roadmap + +Possible future improvements we've thought about: + +- Add extrapolation of acceleration as well +- Switch to open loop control at very low speed with hall sensors, which otherwise move in steps even with smoothing. + diff --git a/src/encoders/smoothing/SmoothingSensor.cpp b/src/encoders/smoothing/SmoothingSensor.cpp new file mode 100644 index 0000000..ef0d322 --- /dev/null +++ b/src/encoders/smoothing/SmoothingSensor.cpp @@ -0,0 +1,57 @@ +#include "SmoothingSensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + + +SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m) +{ +} + + +void SmoothingSensor::update() { + // Update sensor, with optional downsampling of update rate + if(sensor_cnt++ >= sensor_downsample) { + sensor_cnt = 0; + _wrapped.update(); + } + + // Copy state variables from the sensor + angle_prev = _wrapped.angle_prev; + angle_prev_ts = _wrapped.angle_prev_ts; + full_rotations = _wrapped.full_rotations; + + // Perform angle prediction, using low-pass filtered velocity. But don't advance more than + // pi/3 (equivalent to one step of block commutation) from the last true angle reading. + float dt = (_micros() - angle_prev_ts) * 1e-6f; + angle_prev += _motor.sensor_direction * _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs); + + // Apply phase correction if needed + if (phase_correction != 0) { + if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs; + else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs; + } + + // Handle wraparound of the projected angle + if (angle_prev < 0) full_rotations -= 1, angle_prev += _2PI; + else if (angle_prev >= _2PI) full_rotations += 1, angle_prev -= _2PI; +} + + +float SmoothingSensor::getVelocity() { + return _wrapped.getVelocity(); +} + + +int SmoothingSensor::needsSearch() { + return _wrapped.needsSearch(); +} + + +float SmoothingSensor::getSensorAngle() { + return _wrapped.getSensorAngle(); +} + + +void SmoothingSensor::init() { + _wrapped.init(); +} diff --git a/src/encoders/smoothing/SmoothingSensor.h b/src/encoders/smoothing/SmoothingSensor.h new file mode 100644 index 0000000..c10c2a0 --- /dev/null +++ b/src/encoders/smoothing/SmoothingSensor.h @@ -0,0 +1,46 @@ +#ifndef SMOOTHING_SENSOR_H +#define SMOOTHING_SENSOR_H + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" + +/** + SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better + quality angle readings from sensors that are low resolution or are slow to communicate. It provides + no improvement for sensors that are high resolution and quick to update. + It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity + from the motor to predict what the angle should be at the current time. +*/ + +class SmoothingSensor : public Sensor +{ + public: + /** + SmoothingSensor class constructor + @param s Wrapped sensor + @param m Motor that the SmoothingSensor will be linked to + */ + SmoothingSensor(Sensor& s, const FOCMotor& m); + + void update() override; + float getVelocity() override; + int needsSearch() override; + + // For sensors with slow communication, use these to poll less often + unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update + unsigned int sensor_cnt = 0; // counting variable for downsampling + + // For hall sensors, the predicted angle is always 0 to 60 electrical degrees ahead of where it would be without + // smoothing, so offset it backward by 30 degrees (set this to -PI_6) to get the average in phase with the rotor + float phase_correction = 0; + + protected: + float getSensorAngle() override; + void init() override; + + Sensor& _wrapped; + const FOCMotor& _motor; +}; + +#endif diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.cpp b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp new file mode 100644 index 0000000..b7fea27 --- /dev/null +++ b/src/motors/HybridStepperMotor/HybridStepperMotor.cpp @@ -0,0 +1,548 @@ +#include "HybridStepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + +// HybridStepperMotor(int pp) +// - pp - pole pair number +// - R - motor phase resistance +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance [H] +HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance) + : FOCMotor() +{ + // number od pole pairs + pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + // usually used rms + KV_rating = _KV * _SQRT2; + // save phase inductance + phase_inductance = _inductance; + + // torque control type is voltage by default + // current and foc_current not supported yet + torque_controller = TorqueControlType::voltage; +} + +/** + Link the driver which controls the motor +*/ +void HybridStepperMotor::linkDriver(BLDCDriver *_driver) +{ + driver = _driver; +} + +// init hardware pins +void HybridStepperMotor::init() +{ + if (!driver || !driver->initialized) + { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return; + } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + + // sanity check for the voltage limit configuration + if (voltage_limit > driver->voltage_limit) + voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if (voltage_sensor_align > voltage_limit) + voltage_sensor_align = voltage_limit; + + // update the controller limits + if (_isset(phase_resistance)) + { + // velocity control loop controls current + PID_velocity.limit = current_limit; + } + else + { + PID_velocity.limit = voltage_limit; + } + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + SIMPLEFOC_DEBUG("MOT: Enable driver."); + enable(); + _delay(500); + + motor_status = FOCMotorStatus::motor_uncalibrated; +} + +// disable motor driver +void HybridStepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable driver + driver->disable(); + // motor status update + enabled = 0; +} +// enable motor driver +void HybridStepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + // motor status update + enabled = 1; +} + +/** + * FOC functions + */ +int HybridStepperMotor::initFOC() +{ + int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + + // sensor and motor alignment - can be skipped + // by setting motor.sensor_direction and motor.zero_electric_angle + _delay(500); + if (sensor) + { + exit_flag *= alignSensor(); + // added the shaft_angle update + sensor->update(); + shaft_angle = sensor->getAngle(); + } + else + SIMPLEFOC_DEBUG("MOT: No sensor."); + + if (exit_flag) + { + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; + } + else + { + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; + disable(); + } + + return exit_flag; +} + +// Encoder alignment to electrical 0 angle +int HybridStepperMotor::alignSensor() +{ + int exit_flag = 1; // success + SIMPLEFOC_DEBUG("MOT: Align sensor."); + + // if unknown natural direction + if(sensor_direction==Direction::UNKNOWN) { + // check if sensor needs zero search + if (sensor->needsSearch()) + exit_flag = absoluteZeroSearch(); + // stop init if not found index + if (!exit_flag) + return exit_flag; + + // find natural direction + // move one electrical revolution forward + for (int i = 0; i <= 500; i++) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + // take and angle in the middle + sensor->update(); + float mid_angle = sensor->getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >= 0; i--) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + sensor->update(); + float end_angle = sensor->getAngle(); + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle == end_angle) + { + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); + return 0; // failed calibration + } + else if (mid_angle < end_angle) + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); + sensor_direction = Direction::CCW; + } + else + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); + sensor_direction = Direction::CW; + } + // check pole pair number + float moved = fabs(mid_angle - end_angle); + if (fabs(moved * pole_pairs - _2PI) > 0.5f) + { // 0.5f is arbitrary number it can be lower or higher! + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI / moved); + } + else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } + else + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + + // zero electric angle not known + if (!_isset(zero_electric_angle)) + { + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + _delay(700); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); + _delay(20); + if (monitor_port) + { + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); + } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); + } + else + SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + return exit_flag; +} + +// Encoder alignment the absolute zero angle +// - to the index +int HybridStepperMotor::absoluteZeroSearch() +{ + + SIMPLEFOC_DEBUG("MOT: Index search..."); + // search the absolute zero with small velocity + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; + velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; + shaft_angle = 0; + while (sensor->needsSearch() && shaft_angle < _2PI) + { + angleOpenloop(1.5f * _2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->update(); + } + // disable motor + setPhaseVoltage(0, 0, 0); + // reinit the limits + velocity_limit = limit_vel; + voltage_limit = limit_volt; + // check if the zero found + if (monitor_port) + { + if (sensor->needsSearch()) + SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else + SIMPLEFOC_DEBUG("MOT: Success!"); + } + return !sensor->needsSearch(); +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void HybridStepperMotor::loopFOC() +{ + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) + sensor->update(); + + // if open-loop do nothing + if (controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop) + return; + // shaft angle + shaft_angle = shaftAngle(); + + // if disabled do nothing + if (!enabled) + return; + + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void HybridStepperMotor::move(float new_target) +{ + + // downsampling (optional) + if (motion_cnt++ < motion_downsample) + return; + motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if (controller != MotionControlType::angle_openloop && controller != MotionControlType::velocity_openloop) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if (!enabled) + return; + + // set internal target variable + if (_isset(new_target)) + target = new_target; + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + if (_isset(KV_rating)) + voltage_bemf = shaft_velocity / KV_rating / _RPM_TO_RADS; + // estimate the motor current if phase reistance available and current_sense not available + if (!current_sense && _isset(phase_resistance)) + current.q = (voltage.q - voltage_bemf) / phase_resistance; + + // choose control loop + switch (controller) + { + case MotionControlType::torque: + if (!_isset(phase_resistance)) + voltage.q = target; // if voltage torque control + else + voltage.q = target * phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if (!_isset(phase_inductance)) + voltage.d = 0; + else + voltage.d = _constrain(-target * shaft_velocity * pole_pairs * phase_inductance, -voltage_limit, voltage_limit); + break; + case MotionControlType::angle: + // angle set point + shaft_angle_sp = target; + // calculate velocity set point + shaft_velocity_sp = P_angle(shaft_angle_sp - shaft_angle); + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + // if torque controlled through voltage + // use voltage if phase-resistance not provided + if (!_isset(phase_resistance)) + voltage.q = current_sp; + else + voltage.q = _constrain(current_sp * phase_resistance + voltage_bemf, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if (!_isset(phase_inductance)) + voltage.d = 0; + else + voltage.d = _constrain(-current_sp * shaft_velocity * pole_pairs * phase_inductance, -voltage_limit, voltage_limit); + break; + case MotionControlType::velocity: + // velocity set point + shaft_velocity_sp = target; + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control + // use voltage if phase-resistance not provided + if (!_isset(phase_resistance)) + voltage.q = current_sp; + else + voltage.q = _constrain(current_sp * phase_resistance + voltage_bemf, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if (!_isset(phase_inductance)) + voltage.d = 0; + else + voltage.d = _constrain(-current_sp * shaft_velocity * pole_pairs * phase_inductance, -voltage_limit, voltage_limit); + break; + case MotionControlType::velocity_openloop: + // velocity control in open loop + shaft_velocity_sp = target; + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor + voltage.d = 0; // TODO d-component lag-compensation + break; + case MotionControlType::angle_openloop: + // angle control in open loop + shaft_angle_sp = target; + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor + voltage.d = 0; // TODO d-component lag-compensation + break; + } +} + +void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) +{ + angle_el = _normalizeAngle(angle_el); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + float center; + + switch (foc_modulation) { + case FOCModulationType::Trapezoid_120: + case FOCModulationType::Trapezoid_150: + // not handled + Ua = 0; + Ub = 0; + Uc = 0; + break; + case FOCModulationType::SinePWM: + // C phase is fixed at half-rail to provide bias point for A, B legs + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + center = driver->voltage_limit / 2; + + Ua += center; + Ub += center; + Uc = center; + break; + + case FOCModulationType::SpaceVectorPWM: + // C phase moves in order to increase max bias on coils + uint8_t sector = floor(4.0 * angle_el / _PI) + 1; + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + // Determine max/ min of [Ua, Ub, 0] based on phase angle. + switch (sector) + { + case 1: + case 8: + center = (driver->voltage_limit - Ua - Ub) / 2; + break; + + case 2: + center = (driver->voltage_limit - Ua - 0) / 2; + break; + + case 3: + center = (driver->voltage_limit - Ub - 0) / 2; + break; + + case 4: + case 5: + center = (driver->voltage_limit - Ub - Ua) / 2; + break; + + case 6: + center = (driver->voltage_limit - 0 - Ua) / 2; + break; + + case 7: + center = (driver->voltage_limit - 0 - Ub) / 2; + break; + + default: // this case does not occur, but compilers complain about uninitialized variables + center = (driver->voltage_limit - 0) / 2; + break; + } + + Ua += center; + Ub += center; + Uc = center; + break; + } + + driver->setPwm(Ua, Ub, Uc); +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +float HybridStepperMotor::velocityOpenloop(float target_velocity) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to achieve target velocity + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts); + // for display purposes + shaft_velocity = target_velocity; + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +float HybridStepperMotor::angleOpenloop(float target_angle) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if (abs(target_angle - shaft_angle) > abs(velocity_limit * Ts)) + { + shaft_angle += _sign(target_angle - shaft_angle) * abs(velocity_limit) * Ts; + shaft_velocity = velocity_limit; + } + else + { + shaft_angle = target_angle; + shaft_velocity = 0; + } + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} diff --git a/src/motors/HybridStepperMotor/HybridStepperMotor.h b/src/motors/HybridStepperMotor/HybridStepperMotor.h new file mode 100644 index 0000000..71e8792 --- /dev/null +++ b/src/motors/HybridStepperMotor/HybridStepperMotor.h @@ -0,0 +1,112 @@ +/** + * @file HybridStepperMotor.h + * + */ + +#ifndef HybridStepperMotor_h +#define HybridStepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class HybridStepperMotor : public FOCMotor +{ +public: + /** + HybridStepperMotor class constructor + @param pp pole pair number + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] + */ + HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver handle for hardware peripheral control + */ + void linkDriver(BLDCDriver *driver); + + /** + * BLDCDriver link: + */ + BLDCDriver *driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC() override; + + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the HybridStepperMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua, Ub, Uc; //!< Phase voltages used for inverse Park and Clarke transform + + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + +private: + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroSearch(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + float velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + float angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + +#endif diff --git a/src/motors/HybridStepperMotor/README.md b/src/motors/HybridStepperMotor/README.md new file mode 100644 index 0000000..4176539 --- /dev/null +++ b/src/motors/HybridStepperMotor/README.md @@ -0,0 +1,45 @@ +# Three Phase / Hybrid Stepper Motor Class + +By tying two phases together, you can use a standard bipolar stepper motor with three-phase drivers! +Of course, it requires some different math for commutation. Another limitation is that with SinePWM the motor voltage is limited to 50% of the supply voltage. Using some tricks in SpaceVectorPWM, this max bias can be increased to about 70%. + +## Warning + +This code has not been tested much! +It has killed one B-G431B-ESC1 board before. +However, it has been succesfully used on a few different hardware platforms, so it does work. +BEMF may be the danger here, so take caution with fast decelerations. +Keep in mind that potentially twice the current can be sunk in the shorted phase as the two other legs. + +## Hardware setup + +Tie two of the phases of the motor together. This is phase "C". The other two phases are A & B. Phase C must go last in the driver constructor. + + +## Software setup + +Usage is quite simple: + +```c++ +#include "Arduino.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "motors/HybridStepperMotor/HybridStepperMotor.h" + +HybridStepperMotor motor = HybridStepperMotor(pole_pairs, phase_resistance, kv, winding_inductance); +BLDCDriver3PWM driver = BLDCDriver3PWM(phase_A, phase_B, phase_C, enable); + +void setup() { + driver.init(); + motor.linkDriver(&driver); + + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; //or SinePWM + motor.init(); + motor.initFOC(); +} + +void loop(){ + motor.loopFOC(); + motor.move(); +} +``` \ No newline at end of file diff --git a/src/settings/README.md b/src/settings/README.md new file mode 100644 index 0000000..9bd70fb --- /dev/null +++ b/src/settings/README.md @@ -0,0 +1,143 @@ +# SimpleFOC Settings Storage + +Code to persist calibration data and motor settings to and from storage. The SettingsStorage is abstract, and different implementations exist for different concete storage media. + +The SettingsStorage is based on the SimpleFOCRegisters abstraction, so this means you can load or store any of the registers (representing SimpleFOC settings) that is both readable and writeable (e.g. while the current shaft velocity is available as a register, you can't store it as a setting because its read-only). + +The SettingsStorage can save the settings for one or more motors, and you can customize which settings are saved. If you don't customize, only the motor +calibration will be stored (REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION). + +## Hardware requirements + +Obviously, to save settings you need a place to store them where they won't be lost when you power down. There can be many solutions to this question, and we attempt to provide some simple drivers for the common storage solutions people might want. + +But its also quite easy to extend the system to support your specific needs. + +Supported hardware: + +- [SAMD NVM](samd/) \ +Some SAMD chips have a built-in NVRAM flash, which can be written from user code. +- [CAT24 I2C Flash EEPROMs](i2c/) \ :warning: UNFINISHED +Simple programmable memories with I2C interface. Memory space is quite limited, but enough for storing settings. + +See Roadmap, below, for systems we may support in the future. + + +## Basic Usage + +Using SettingsStorage is simple. Just add a `settings.loadSettings()` after setting up your defaults, but before calling init() on the objects. And call `saveSettings()` after initialization of the motor has (successfully) completed. + +This example saves just the motor calibration, but you might want to add additional code to save other registers, or make your solution more user-friendly. See the other examples below. + +```c++ + +#include "settings/samd/SAMDNVMSettingsStorage.h" + +SAMDNVMSettingsStorage settings = SAMDNVMSettingsStorage(); +BLDCMotor motor = BLDCMotor(...); + +void setup() { + SimpleFOCDebug::enable(); // show debug messages from settings + + // initialize the settings early in the setup + settings.addMotor(&motor); + settings.init(); + + // first set your defaults + driver.voltage_power_supply = 12.0f; + driver.voltage_limit = 12.0f; // 12V driver limit, like PSU + motor.voltage_limit = 6.0f; // 6V motor limit + + // then try to load the settings + SettingsStatus loadStatus = settings.loadSettings(); + + // driver init code, etc... + ... + + // then init the motor + motor.init(); + motor.initFOC(); + + // and if the settings were not loaded earlier, then save them + if (motor.motor_status == FOCMotorStatus::motor_ready && loadStatus != SFOC_SETTINGS_SUCCESS) { + settings.saveSettings(); + } + + // any other initialization + ... +} + +``` + +### Choosing registers + +By default, only the calibration data (motor native direction, electrical zero position) are saved. To save additional registers, you can choose them before calling `SettingsStorage.init()`, as in the example below: + +```c++ + +SimpleFOCRegister settingsRegisters[] = { REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION, REG_VEL_PID_P, REG_VEL_PID_I, REG_VEL_PID_D, REG_VEL_LPF_T, REG_VEL_LIMIT, REG_VEL_MAX_RAMP, REG_VOLTAGE_LIMIT, REG_MOTION_DOWNSAMPLE, REG_CONTROL_MODE, REG_TORQUE_MODE, REG_PHASE_RESISTANCE, REG_KV, REG_INDUCTANCE }; +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); +BLDCMotor motor = BLDCMotor(...); + +void setup() { + SimpleFOCDebug::enable(); // show debug messages from settings + + // initialize the settings early in the setup + settings.setRegisters(mySettingsRegisters, sizeof(mySettingsRegisters)); + settings.addMotor(&motor); + settings.init(); + ... +} +``` + + + +### Using settings with commander + +You can easily interact with the settings via the commander, for example to save your PID tuning values: + +```c++ + +SimpleFOCRegister settingsRegisters[] = { REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION, REG_VEL_PID_P, REG_VEL_PID_I, REG_VEL_PID_D, REG_VEL_LPF_T }; + +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); + +Commander commander; + +void doSaveSettings(char *cmd) { settings.saveSettings(); } +void doLoadSettings(char *cmd) { settings.loadSettings(); } + + +void setup() { + ... + + settings.setRegisters(settingsRegisters, sizeof(settingsRegisters)); + settings.addMotor(&motor); + settings.init(); + + ... + + commander.addCommand('s', doSaveSettings, "Save settings"); + commander.addCommand('l', doLoadSettings, "Load settings"); + + ... + +} + + +void run() { + motor.move(); + motor.loopFOC(); + commander.run(); +} +``` + +## Roadmap + +The following ideas for storage backends are on the roadmap, with no definate schedule for implementation: + +- ESP32 SPIFFs Filesystem +- ESP32 Preferences +- Arduino EEPROM library +- In-Memory Buffer (for use with other libraries or abstractions) +- JSON String (for printing to console) diff --git a/src/settings/SettingsStorage.cpp b/src/settings/SettingsStorage.cpp new file mode 100644 index 0000000..5df2bcf --- /dev/null +++ b/src/settings/SettingsStorage.cpp @@ -0,0 +1,120 @@ + +#include "./SettingsStorage.h" +#include "communication/SimpleFOCDebug.h" + + +SettingsStorage::SettingsStorage() { + +}; + + +SettingsStorage::~SettingsStorage() { + +}; + + + +void SettingsStorage::addMotor(BLDCMotor* motor) { + if (numMotors < SIMPLEFOC_SETTINGS_MAX_MOTORS) { + motors[numMotors] = motor; + numMotors++; + } + else + SimpleFOCDebug::println("SS: too many motors"); +}; + + +void SettingsStorage::setRegisters(SimpleFOCRegister* registers, int numRegisters){ + this->registers = registers; + this->numRegisters = numRegisters; +}; + + +void SettingsStorage::init() { + // make sure we have motors and registers + if (numMotors < 1) { + SimpleFOCDebug::println("SS: no motors"); + return; + } + if (registers==NULL || numRegisters < 1) { + SimpleFOCDebug::println("SS: no registers"); + return; + } +}; + + + +SettingsStatus SettingsStorage::loadSettings() { + SimpleFOCDebug::println("Loading settings..."); + beforeLoad(); + uint8_t magic; readByte(&magic); + if (magic != SIMPLEFOC_SETTINGS_MAGIC_BYTE) { + SimpleFOCDebug::println("No settings found "); + return SFOC_SETTINGS_NONE; + } + uint8_t rversion; readByte(&rversion); + if (rversion != SIMPLEFOC_REGISTERS_VERSION) { + SimpleFOCDebug::println("Registers version mismatch"); + return SFOC_SETTINGS_OLD; + } + uint8_t version; readByte(&version); + if (version != settings_version) { + SimpleFOCDebug::println("Settings version mismatch"); + return SFOC_SETTINGS_OLD; + } + for (int m = 0; m < numMotors; m++) { + if (numMotors>1) + SimpleFOCDebug::println("Loading settings for motor ", m); + startLoadMotor(m); + for (int i = 0; i < numRegisters; i++) { + SimpleFOCRegister reg = registers[i]; + startLoadRegister(reg); + setRegister(reg, motors[m]); + endLoadRegister(); + } + endLoadMotor(); + } + afterLoad(); + SimpleFOCDebug::println("Settings loaded"); + return SFOC_SETTINGS_SUCCESS; +}; + + +SettingsStatus SettingsStorage::saveSettings() { + SimpleFOCDebug::println("Saving settings..."); + beforeSave(); + writeByte(SIMPLEFOC_SETTINGS_MAGIC_BYTE); + writeByte(SIMPLEFOC_REGISTERS_VERSION); + writeByte(settings_version); + for (int m = 0; m < numMotors; m++) { + if (numMotors>1) + SimpleFOCDebug::println("Saving settings for motor ", m); + startSaveMotor(m); + for (int i = 0; i < numRegisters; i++) { + SimpleFOCRegister reg = registers[i]; + startSaveRegister(reg); + sendRegister(reg, motors[m]); + endSaveRegister(); + } + endSaveMotor(); + } + afterSave(); + SimpleFOCDebug::println("Settings saved"); + return SFOC_SETTINGS_SUCCESS; +}; + +// empty implementation for these + +void SettingsStorage::startSaveMotor(uint8_t num) {}; +void SettingsStorage::endSaveMotor() {}; +void SettingsStorage::startSaveRegister(SimpleFOCRegister reg) {}; +void SettingsStorage::endSaveRegister() {}; +void SettingsStorage::startLoadMotor(uint8_t num) {}; +void SettingsStorage::endLoadMotor() {}; +void SettingsStorage::startLoadRegister(SimpleFOCRegister reg) {}; +void SettingsStorage::endLoadRegister() {}; + +void SettingsStorage::beforeLoad() {}; +void SettingsStorage::afterLoad() {}; +void SettingsStorage::beforeSave() {}; +void SettingsStorage::afterSave() {}; diff --git a/src/settings/SettingsStorage.h b/src/settings/SettingsStorage.h new file mode 100644 index 0000000..8506c3b --- /dev/null +++ b/src/settings/SettingsStorage.h @@ -0,0 +1,70 @@ + +#pragma once + + +#include "../comms/SimpleFOCRegisters.h" +#include "../comms/RegisterReceiver.h" +#include "../comms/RegisterSender.h" +#include "BLDCMotor.h" + +#define SIMPLEFOC_SETTINGS_MAGIC_BYTE 0x42 +#define SIMPLEFOC_SETTINGS_VERSION 0x01 + +#define SIMPLEFOC_SETTINGS_REGISTERS_MINIMAL { REG_ZERO_ELECTRIC_ANGLE, REG_SENSOR_DIRECTION } + + +#if !defined(SIMPLEFOC_SETTINGS_MAX_MOTORS) +#define SIMPLEFOC_SETTINGS_MAX_MOTORS 4 +#endif + + +typedef enum : uint8_t { + SFOC_SETTINGS_SUCCESS = 0, // settings loaded/saved successfully + SFOC_SETTINGS_NONE = 1, // no settings found to load + SFOC_SETTINGS_OLD = 2, // settings found, but version is old + SFOC_SETTINGS_ERROR = 3 // other error +} SettingsStatus; + + + + +class SettingsStorage : public RegisterReceiver, public RegisterSender { +public: + SettingsStorage(); + ~SettingsStorage(); + + void addMotor(BLDCMotor* motor); + void setRegisters(SimpleFOCRegister* registers, int numRegisters); + + virtual void init(); + + SettingsStatus loadSettings(); + SettingsStatus saveSettings(); + + /* + Change this value if you make changes to the registers saved in code, and have saved settings on existing devices. + This will cause the device to reset to defaults on the next boot. + */ + uint8_t settings_version = SIMPLEFOC_SETTINGS_VERSION; + +protected: + virtual void startSaveMotor(uint8_t num); + virtual void endSaveMotor(); + virtual void startSaveRegister(SimpleFOCRegister reg); + virtual void endSaveRegister(); + virtual void startLoadMotor(uint8_t num); + virtual void endLoadMotor(); + virtual void startLoadRegister(SimpleFOCRegister reg); + virtual void endLoadRegister(); + + + virtual void beforeLoad(); + virtual void afterLoad(); + virtual void beforeSave(); + virtual void afterSave(); + + FOCMotor* motors[SIMPLEFOC_SETTINGS_MAX_MOTORS]; + int numMotors = 0; + SimpleFOCRegister* registers = NULL; + int numRegisters = 0; +}; diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp new file mode 100644 index 0000000..5d4b336 --- /dev/null +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.cpp @@ -0,0 +1,98 @@ + +#include "./CAT24I2CFlashSettingsStorage.h" + + +CAT24I2CFlashSettingsStorage::CAT24I2CFlashSettingsStorage(uint8_t address, uint16_t offset) { + _address = address; + _offset = offset; +}; + + + +CAT24I2CFlashSettingsStorage::~CAT24I2CFlashSettingsStorage() {}; + + + +void CAT24I2CFlashSettingsStorage::init(TwoWire* wire) { + SettingsStorage::init(); + _wire = wire; + reset(); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::readByte(uint8_t* valueToSet) { + return readBytes(valueToSet, 1); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::readFloat(float* valueToSet) { + return readBytes(valueToSet, 4); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::readInt(uint32_t* valueToSet) { + return readBytes(valueToSet, 4); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::writeByte(uint8_t value) { + return writeBytes(&value, 1); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::writeFloat(float value) { + return writeBytes(&value, 4); +}; + + + +uint8_t CAT24I2CFlashSettingsStorage::writeInt(uint32_t value) { + return writeBytes(&value, 4); +}; + + + +void CAT24I2CFlashSettingsStorage::beforeSave() { + reset(); +}; + + +void CAT24I2CFlashSettingsStorage::beforeLoad() { + reset(); + _wire->beginTransmission(_address); + _wire->write(_currptr >> 8); + _wire->write(_currptr & 0xFF); + _wire->endTransmission(false); +}; + + + +void CAT24I2CFlashSettingsStorage::reset() { + _currptr = _offset; +}; + + +int CAT24I2CFlashSettingsStorage::readBytes(void* valueToSet, int numBytes) { + int read = _wire->requestFrom((uint8_t)_address, (uint8_t)numBytes, (uint8_t)true); + _currptr += read; + if (read==numBytes) + _wire->readBytes((uint8_t*)valueToSet, numBytes); + return read; +}; + + +int CAT24I2CFlashSettingsStorage::writeBytes(void* value, int numBytes) { + _wire->beginTransmission(_address); + _wire->write(_currptr >> 8); + _wire->write(_currptr & 0xFF); + size_t written = _wire->write((uint8_t*)value, numBytes); + _wire->endTransmission(true); + _currptr += written; + delay(5); // write-cycle time + return written; +}; diff --git a/src/settings/i2c/CAT24I2CFlashSettingsStorage.h b/src/settings/i2c/CAT24I2CFlashSettingsStorage.h new file mode 100644 index 0000000..4f6c511 --- /dev/null +++ b/src/settings/i2c/CAT24I2CFlashSettingsStorage.h @@ -0,0 +1,35 @@ + +#pragma once + +#include "../SettingsStorage.h" +#include "Wire.h" + +class CAT24I2CFlashSettingsStorage : public SettingsStorage { +public: + CAT24I2CFlashSettingsStorage(uint8_t address = 0xA0, uint16_t offset = 0x0); + ~CAT24I2CFlashSettingsStorage(); + + void init(TwoWire* wire = &Wire); + +protected: + uint8_t readByte(uint8_t* valueToSet) override; + uint8_t readFloat(float* valueToSet) override; + uint8_t readInt(uint32_t* valueToSet) override; + + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + + void beforeSave() override; + void beforeLoad() override; + + void reset(); + int readBytes(void* valueToSet, int numBytes); + int writeBytes(void* value, int numBytes); + + TwoWire* _wire; + uint8_t _address; // i2c address + uint16_t _offset; // offset into NVRAM to store settings + uint16_t _currptr = 0; // pointer to current location in NVRAM + +}; diff --git a/src/settings/i2c/README.md b/src/settings/i2c/README.md new file mode 100644 index 0000000..9cb33bd --- /dev/null +++ b/src/settings/i2c/README.md @@ -0,0 +1,26 @@ + +# I2C memories settings drivers + +Store settings to I2C EEPROMs. + +## CAT24C64 I2C EEPROMs + +:warning: not yet finished or tested! + +Datasheet describes the CAT24C64 as: _The CAT24C64 is a 64 Kb CMOS Serial EEPROM device, internally organized as 8192 words of 8 bits each._ + +Provide I2C address in constructor: + +`CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0);` + +If you want to use a different I2C bus than the default, you can pass it to the constructor: + +`settings.init(&Wire2);` + +You can use multiple settings objects in the same EEPROM, by providing an offset to the constructor: + +```c++ +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0); +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0, 80); +CAT24I2CFlashSettingsStorage settings = CAT24I2CFlashSettingsStorage(0xA0, 160); +``` diff --git a/src/settings/samd/README.md b/src/settings/samd/README.md new file mode 100644 index 0000000..7382751 --- /dev/null +++ b/src/settings/samd/README.md @@ -0,0 +1,49 @@ +# SAMD21 settings driver + +Driver for storing settings on SAMD21 chips. + +There are two options: + +1. **RWW Flash** \ +Some SAMD21 chips have a seperate RWW flash area, of 2kB-8kB size. This area is seperate from the main flash array. \ +Note: this method is implemented and tested. + +2. **EEPROM emulation** \ +All SAMD21s support EEPROM emulation, where an area of the main flash is reserved for the application storage. \ +:warning: This method is not yet implemented fully, and untested. + +## Using RWW flash + +To use the RWW flash to store settings, you have to make sure the flash region is not locked (you can set the lock fuses using MCP Studio or other SAM development tools). + +Otherwise there is nothing special to do. When reprogramming the chip, be careful not to erase the RWW area. + +You can use multiple settings objects to store to different areas of the RWW. +To do so, specify an offset to the constructor: + +```c++ +SAMDNVMSettingsStorage settings0 = SAMDNVMSettingsStorage(); +SAMDNVMSettingsStorage settings1 = SAMDNVMSettingsStorage(NVMCTRL_ROW_SIZE); +SAMDNVMSettingsStorage settings2 = SAMDNVMSettingsStorage(NVMCTRL_ROW_SIZE*2); +``` + +Note: the offset must be a multiple of the flash memory row size, typically 256 bytes. + + +## Using EEPROM emulation + +TODO implement and test this method + +To use the EEPROM emulation, use a tool like MCP Studio to set the chip fuses to reserve an area of flash for EEPROM emulation: + +![](eeprom_emulation_screenshot.jpg) + +Normally 256 bytes (one flash page) should be plenty for SimpleFOC settings... Obviously, the area you reserve for EEPROM can't be used for your main program. + +Add a build-flag to your build to set the base address of your EEPROM memory: + +`-DSIMPLEFOC_SAMDNVMSETTINGS_BASE=0x003FF800` + +Then use the settings as normal. + +You'll have to adjust your board files to exclude the EEPROM area in the ldscript to prevent it being erased when you re-program, if you care to keep your settings when reflashing. diff --git a/src/settings/samd/SAMDNVMSettingsStorage.cpp b/src/settings/samd/SAMDNVMSettingsStorage.cpp new file mode 100644 index 0000000..5569600 --- /dev/null +++ b/src/settings/samd/SAMDNVMSettingsStorage.cpp @@ -0,0 +1,164 @@ + + + +#include "./SAMDNVMSettingsStorage.h" + + +#if defined(_SAMD21_) + +#include "communication/SimpleFOCDebug.h" + + +SAMDNVMSettingsStorage::SAMDNVMSettingsStorage(uint32_t offset) { + _offset = offset; +}; + +SAMDNVMSettingsStorage::~SAMDNVMSettingsStorage(){}; + +void SAMDNVMSettingsStorage::init(){ + SettingsStorage::init(); + reset(); +}; + + + +void SAMDNVMSettingsStorage::reset(){ + _currptr = (uint8_t*) SIMPLEFOC_SAMDNVMSETTINGS_BASE + _offset; // add offset to NVM base address +}; + + + +void SAMDNVMSettingsStorage::beforeLoad(){ + reset(); +}; + + +void SAMDNVMSettingsStorage::beforeSave(){ + reset(); + _writeIndex = 0; + + // set manual write mode + _ctlB = NVMCTRL->CTRLB.reg; + NVMCTRL->CTRLB.reg |= NVMCTRL_CTRLB_MANW; + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + // unlock region - user can do it with fuses + // erase rows + // TODO handle case that it is more than one row + NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + NVMCTRL->STATUS.reg |= NVMCTRL_STATUS_MASK; + NVMCTRL->INTFLAG.bit.ERROR = 0; + NVMCTRL->CTRLA.reg = 0x1A | NVMCTRL_CTRLA_CMDEX_KEY; + delay(1); + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + if (NVMCTRL->INTFLAG.bit.ERROR == 1) + SimpleFOCDebug::println("NVM erase error ", NVMCTRL->STATUS.reg); + else + SimpleFOCDebug::println("NVM erased row @", (int)_currptr); +}; + + +void SAMDNVMSettingsStorage::afterSave() { + if (_writeIndex>0) + flushPage(); + // restore ctlb + delay(1); + NVMCTRL->CTRLB.reg =_ctlB; + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; +} + + +void SAMDNVMSettingsStorage::flushPage(){ + // if we have an odd number of bytes, pad with 0xFF + if (_writeIndex%2==1) { + _writeBuffer[_writeIndex++] = 0xFF; + } + // erase page buffer + // NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + // NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMD_PBC | NVMCTRL_CTRLA_CMDEX_KEY; + // while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + // copy writeBuffer to NVM, using 16-bit writes + uint16_t* src = (uint16_t*)_writeBuffer; + uint16_t* dst = (uint16_t*)_currptr; + for (int i=0;i<_writeIndex/2;i++) { + *dst++ = *src++; + } + // write page + NVMCTRL->ADDR.reg = ((uint32_t)_currptr - RWWEE_BASE) / 2; + NVMCTRL->STATUS.reg |= NVMCTRL_STATUS_MASK; + NVMCTRL->INTFLAG.bit.ERROR = 0; + NVMCTRL->CTRLA.reg = 0x1C | NVMCTRL_CTRLA_CMDEX_KEY; + delay(1); + while (NVMCTRL->INTFLAG.bit.READY == 0) {}; + if (NVMCTRL->INTFLAG.bit.ERROR == 1) + SimpleFOCDebug::println("NVM write error ", NVMCTRL->STATUS.reg); + else + SimpleFOCDebug::println("NVM wrote page @", (int)_currptr); + // reset writeBuffer pointer and advance currptr to next page + _writeIndex = 0; + _currptr+=NVMCTRL_PAGE_SIZE; +} + + +int SAMDNVMSettingsStorage::readBytes(void* valueToSet, int numBytes) { + uint8_t* bytes = (uint8_t*)valueToSet; + for (int i=0;i + +#if defined(_SAMD21_) + +#include "../SettingsStorage.h" + +#define RWWEE_BASE 0x00400000 + +#if !defined(SIMPLEFOC_SAMDNVMSETTINGS_BASE) +#define SIMPLEFOC_SAMDNVMSETTINGS_BASE RWWEE_BASE +#endif + + +class SAMDNVMSettingsStorage : public SettingsStorage { +public: + SAMDNVMSettingsStorage(uint32_t offset = 0x0); + ~SAMDNVMSettingsStorage(); + + void init() override; + +protected: + uint8_t readByte(uint8_t* valueToSet) override; + uint8_t readFloat(float* valueToSet) override; + uint8_t readInt(uint32_t* valueToSet) override; + + uint8_t writeByte(uint8_t value) override; + uint8_t writeFloat(float value) override; + uint8_t writeInt(uint32_t value) override; + + void beforeSave() override; + void afterSave() override; + void beforeLoad() override; + + + void reset(); + int readBytes(void* valueToSet, int numBytes); + int writeBytes(void* value, int numBytes); + void flushPage(); + + uint32_t _offset; // offset into NVRAM to store settings + + uint32_t _ctlB; + uint8_t* _currptr; // pointer to current location in NVM + int _writeIndex; // index into current page being written + uint8_t _writeBuffer[NVMCTRL_PAGE_SIZE]; // buffer for writing to NVM +}; + +#endif diff --git a/src/settings/samd/eeprom_emulation_screenshot.jpg b/src/settings/samd/eeprom_emulation_screenshot.jpg new file mode 100644 index 0000000..c743439 Binary files /dev/null and b/src/settings/samd/eeprom_emulation_screenshot.jpg differ diff --git a/src/utilities/stm32pwm/STM32PWMInput.cpp b/src/utilities/stm32pwm/STM32PWMInput.cpp index 5d04b4c..c530c04 100644 --- a/src/utilities/stm32pwm/STM32PWMInput.cpp +++ b/src/utilities/stm32pwm/STM32PWMInput.cpp @@ -25,6 +25,9 @@ int STM32PWMInput::initialize(){ timer.Init.Period = 4.294967295E9; // TODO max period, depends on which timer is used - 32 bits or 16 bits timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; timer.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (channel!=1 && channel!=2) // only channels 1 & 2 supported + return -10; + useChannel2 = (channel==2);// remember the channel if (HAL_TIM_Base_Init(&timer) != HAL_OK) { return -1; } @@ -47,7 +50,7 @@ int STM32PWMInput::initialize(){ return -4; } TIM_IC_InitTypeDef icCfg = { - .ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING, + .ICPolarity = (channel==1)?TIM_INPUTCHANNELPOLARITY_RISING:TIM_INPUTCHANNELPOLARITY_FALLING, .ICSelection = (channel==1)?TIM_ICSELECTION_DIRECTTI:TIM_ICSELECTION_INDIRECTTI, .ICPrescaler = TIM_ICPSC_DIV1, .ICFilter = 0 @@ -55,7 +58,7 @@ int STM32PWMInput::initialize(){ if (HAL_TIM_IC_ConfigChannel(&timer, &icCfg, TIM_CHANNEL_1) != HAL_OK) { return -5; } - icCfg.ICPolarity = TIM_INPUTCHANNELPOLARITY_FALLING; + icCfg.ICPolarity = (channel==1)?TIM_INPUTCHANNELPOLARITY_FALLING:TIM_INPUTCHANNELPOLARITY_RISING; icCfg.ICSelection = (channel==1)?TIM_ICSELECTION_INDIRECTTI:TIM_ICSELECTION_DIRECTTI; if (HAL_TIM_IC_ConfigChannel(&timer, &icCfg, TIM_CHANNEL_2) != HAL_OK) { return -6; @@ -86,12 +89,18 @@ float STM32PWMInput::getDutyCyclePercent(){ uint32_t STM32PWMInput::getDutyCycleTicks(){ - return timer.Instance->CCR2; + if (useChannel2) + return timer.Instance->CCR1; + else + return timer.Instance->CCR2; }; uint32_t STM32PWMInput::getPeriodTicks(){ - return timer.Instance->CCR1; + if (useChannel2) + return timer.Instance->CCR2; + else + return timer.Instance->CCR1; }; diff --git a/src/utilities/stm32pwm/STM32PWMInput.h b/src/utilities/stm32pwm/STM32PWMInput.h index 4a1a04d..fcf3acd 100644 --- a/src/utilities/stm32pwm/STM32PWMInput.h +++ b/src/utilities/stm32pwm/STM32PWMInput.h @@ -19,6 +19,7 @@ class STM32PWMInput { PinName _pin; protected: TIM_HandleTypeDef timer; + bool useChannel2 = false; }; diff --git a/src/voltage/GenericVoltageSense.cpp b/src/voltage/GenericVoltageSense.cpp index 20f8ffd..714a0cb 100644 --- a/src/voltage/GenericVoltageSense.cpp +++ b/src/voltage/GenericVoltageSense.cpp @@ -18,16 +18,16 @@ GenericVoltageSense::GenericVoltageSense(int pin, float gain, float offset, floa bool GenericVoltageSense::init(int resolution){ pinMode(pin, INPUT_ANALOG); -#ifndef ARDUINO_ARCH_AVR +#if !defined(ARDUINO_ARCH_AVR) && !defined(ARDUINO_ARCH_MEGAAVR) if (resolution>0) { analogReadResolution(resolution); - maxValue = pow(2, resolution); + maxValue = _powtwo(resolution); } else { - maxValue = pow(2, ADC_RESOLUTION); + maxValue = _powtwo(ADC_RESOLUTION); } #else - maxValue = pow(2, ADC_RESOLUTION); + maxValue = _powtwo(ADC_RESOLUTION); #endif return true; }; @@ -37,7 +37,7 @@ bool GenericVoltageSense::init(int resolution){ float GenericVoltageSense::readRawVoltage(){ - uint32_t val = analogRead(pin); + float val = analogRead(pin); val = (val / (float)maxValue) * fullScaleVoltage; return val; }; diff --git a/src/voltage/GenericVoltageSense.h b/src/voltage/GenericVoltageSense.h index 52e4259..673162a 100644 --- a/src/voltage/GenericVoltageSense.h +++ b/src/voltage/GenericVoltageSense.h @@ -3,6 +3,7 @@ #define __GENERICVOLTAGESENSE_H__ #include "./VoltageSense.h" +#include "common/foc_utils.h" class GenericVoltageSense : public VoltageSense { public: