Skip to content

Commit da830c2

Browse files
authored
Merge pull request #1 from Candas1/dev
Dev
2 parents 0010b2c + 5de9220 commit da830c2

File tree

98 files changed

+4582
-1175
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

98 files changed

+4582
-1175
lines changed

.github/workflows/ccpp.yml

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ jobs:
1515
strategy:
1616
matrix:
1717
arduino-boards-fqbn:
18-
- arduino:avr:uno # arudino uno
18+
- arduino:avr:nano # arudino nano
1919
- arduino:sam:arduino_due_x # arduino due
2020
- arduino:samd:nano_33_iot # samd21
2121
- adafruit:samd:adafruit_metro_m4 # samd51
@@ -25,42 +25,42 @@ jobs:
2525
- STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo
2626
- arduino:mbed_rp2040:pico # rpi pico
2727
include:
28-
- arduino-boards-fqbn: arduino:avr:uno
29-
sketches-exclude: calibrated mt6816_spi smoothing
28+
- arduino-boards-fqbn: arduino:avr:nano
29+
sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage
3030
required-libraries: Simple FOC
3131
- arduino-boards-fqbn: arduino:sam:arduino_due_x
3232
required-libraries: Simple FOC
33-
sketches-exclude: calibrated smoothing
33+
sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega
3434
- arduino-boards-fqbn: arduino:samd:nano_33_iot
3535
required-libraries: Simple FOC
3636
sketches-exclude: calibrated smoothing
3737
- arduino-boards-fqbn: arduino:mbed_rp2040:pico
3838
required-libraries: Simple FOC
39-
sketches-exclude: calibrated smoothing
39+
sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega
4040
- arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4
4141
platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json
4242
required-libraries: Simple FOC
43-
sketches-exclude: calibrated smoothing
43+
sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega
4444
# - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1
4545
# platform-url: https://dl.espressif.com/dl/package_esp32_index.json
4646
# required-libraries: Simple FOC
4747
# sketch-names: '**.ino'
4848
- arduino-boards-fqbn: esp32:esp32:esp32 # esp32
49-
platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
49+
platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
5050
required-libraries: Simple FOC
51-
sketches-exclude: calibrated smoothing
51+
sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega linearhall
5252
- arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2
53-
platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
53+
platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
5454
required-libraries: Simple FOC
55-
sketches-exclude: calibrated smoothing
55+
sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega
5656
- arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8
5757
platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
5858
required-libraries: Simple FOC
59-
sketches-exclude: calibrated mt6816_spi smoothing
59+
sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage simplefocnano_atmega
6060
- arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE
6161
platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
6262
required-libraries: Simple FOC
63-
sketches-exclude: smoothing
63+
sketches-exclude: smoothing simplefocnano_torque_voltage simplefocnano_atmega
6464
# Do not cancel all jobs / architectures if one job fails
6565
fail-fast: false
6666
steps:

.gitignore

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,3 @@
11
.project
22
.DS_Store
3+
src/encoders/esp32hwencoder.bak/

README.md

Lines changed: 52 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -10,23 +10,29 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un
1010

1111
## New Release
1212

13-
v1.0.6 - Released July 2023, for Simple FOC 2.3.1 or later
13+
v1.0.8 - Released ??? 2024, for Simple FOC 2.3.3 or later
14+
15+
16+
What's changed since 1.0.7?
17+
- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.8+)
1418

15-
What's changed since 1.0.5?
16-
- Added AS5600 Sensor Driver
17-
- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+)
1819

1920
## What is included
2021

2122
What is here? See the sections below. Each driver or function should come with its own more detailed README.
2223

2324
### Motor/Gate driver ICs
2425

26+
Software to control gate driver ICs or integrated driver ICs which have advanced configuration and status interfaces, e.g. via I2C or SPI.
27+
2528
- [TMC6200 driver](src/drivers/tmc6200/) - SPI driver for Trinamics TMC6200 motor driver IC.
2629
- [DRV8316 driver](src/drivers/drv8316/) - SPI driver for TI's DRV8316 motor driver IC.
30+
- [STSPIN32G4 driver](src/drivers/stspin32g4/) - I2C and BLDCDriver for the STSPIN32G4 integrated gate driver MCU.
2731

2832
### Encoders
2933

34+
Drivers for various position sensor ICs. In many cases these hardware-specific drivers often support more functionality than the generic SimpleFOC sensor drivers, including reading status registers, setting configurations and more.
35+
3036
- [AS5048A SPI driver](src/encoders/as5048a/) - SPI driver for the AMS AS5048A absolute position magnetic rotary encoder IC.
3137
- [AS5047 SPI driver](src/encoders/as5047/) - SPI driver for the AMS AS5047P and AS5047D absolute position magnetic rotary encoder ICs.
3238
- [AS5047U SPI driver](src/encoders/as5047u/) - SPI driver for the AMS AS5047U absolute position magnetic rotary encoder ICs.
@@ -42,27 +48,42 @@ What is here? See the sections below. Each driver or function should come with i
4248
- [MT6701 SSI driver](src/encoders/mt6701/) - SSI driver for the MagnTek MT6701 absolute position magnetic rotary encoder IC.
4349
- [MT6835 SPI driver](src/encoders/mt6835/) - SPI driver for the MagnTek MT6835 21 bit magnetic rotary encoder IC.
4450
- [STM32 PWM sensor driver](src/encoders/stm32pwmsensor/) - STM32 native timer-based driver for PWM angle sensors.
51+
- [CalibratedSensor](src/encoders/calibrated/) - A sensor which can calibrate for eccentricity on the magnet placement.
4552
- [SmoothingSensor](src/encoders/smoothing/) - A SimpleFOC Sensor wrapper implementation which adds angle extrapolation.
4653

4754
### Communications
4855

49-
- [I2CCommander I2C driver](src/comms/i2c/) - I2C communications protocol and drivers for both controller and target devices, based on register abstraction
50-
- [STM32 SpeedDir Input](src/comms/stm32speeddir/) - Control target velocity with PWM speed and direction inputs
51-
- [SerialBinaryCommander](src/comms/serial/) - Serial communications with binary protocol, based on register abstraction
52-
- [Telemetry](src/comms/telemetry/) - Telemetry abstraction, based on registers
53-
- [SerialASCIITelemetry](src/comms/serial/) - Serial communications with ascii protocol, based on register abstraction
56+
Extended communications classes for SimpleFOC. In particular the Telemetry and PacketCommander classes, which implement ASCII or Binary communications and allow monitoring and control of multiple motors, via an easy to use "Registers" abstraction. The Binary and ASCII packet based protocols are directly supported in [PySimpleFOC](https://github.com/simplefoc/pysimplefoc).
5457

58+
- [PacketCommander](src/comms/streams/) - Serial communications with binary protocol, based on register abstraction - get or set any value in SimpleFOC
59+
- [Telemetry](src/comms/telemetry/) - Telemetry based on registers - monitor any value in SimpleFOC, and log in either ASCII or Binary, compatible with PacketCommander
60+
- [SimpleTelemetry](src/comms/telemetry/) - Register telemetry for use with Arduino Serial Plotter tool
61+
- [TeleplotTelemetry](src/comms/telemetry/) - Register telemetry for use with VSCode Teleplot extension
62+
- [I2CCommander I2C driver](src/comms/i2c/) - I2C communications based on register abstraction
63+
- [STM32 SpeedDir Input](src/comms/stm32speeddir/) - Control target velocity with PWM speed and direction inputs
64+
5565
### Settings
5666

57-
Load and store SimpleFOC motor settings, based on register abstraction.
67+
Load and store SimpleFOC motor settings, based on register abstraction. Storing the motor calibration allows you to skip the calibration phase during setup.
5868

59-
- [SAMD NVM storage driver](src/settings/samd/) - Store settings to the NVM flash memory in your SAMD MCU
69+
- [SAMD NVM storage driver](src/settings/samd/) - Store settings to either the main flash memory or the RWWEE memory (if available) in your SAMD MCU
6070
- [CAT24 I2C EEPROM storage driver](src/settings/i2c/) - Store settings to CAT24 I2C EEPROMs
71+
- [STM32 flash storage driver](src/settings/stm32/) - Store settings directly to STM32 on-board flash, currently supporting STM32G4 MCUs.
72+
73+
### Motor classes
74+
75+
Drive different kinds of motors, or use alternate algorithms to SimpleFOC's default BLDCMotor and StepperMotor classes.
76+
77+
- [HybridStepperMotor](motors/HybridStepperMotor/) - Drive stepper motors with 3 phases.
78+
6179

6280
### Utilities
6381

64-
- [STM32 PWM Input driver](src/utilities/stm32pwm/) - PWM Input driver for STM32 MCUs. Accurately measure PWM inputs with zero MCU overhead.
82+
Other support code not fitting in the above categories.
6583

84+
- [STM32 PWM Input driver](src/utilities/stm32pwm/) - PWM Input driver for STM32 MCUs. Accurately measure PWM inputs with zero MCU overhead.
85+
- [STM32 CORDIC trig driver](src/utilities/stm32math/) - CORDIC driver to accellerate sine and cosine calculations in SimpleFOC, on STM32 MCUs which have a CORDIC unit.
86+
- [TrapezoidalPlanner](src/utilities/trapezoids/) - Simple trapezoidal motion planner.
6687

6788
## How to use
6889

@@ -90,6 +111,25 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi
90111

91112
## Release History
92113

114+
What's changed since 1.0.6?
115+
- Improvements to LinearHall driver, thanks to dekutree
116+
- Fix for ESP32 compiler warning, thanks to Yannik Stradmann
117+
- Added STM32 LPTIM encoder driver
118+
- Refactored communications code
119+
- Working telemetry abstraction
120+
- Working streams communications, ASCII based
121+
- Refactored settings storage code
122+
- Refactored I2CCommander
123+
- New utility class for simple trapezoidal motion profiles
124+
- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.7+)
125+
126+
What's changed since 1.0.5?
127+
- Added STSPIN32G4 driver
128+
- Added STM32G4 CORDIC code, for greatly accellerated trig functions on supported MCUs
129+
- Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs
130+
- Improvements in the MT6835 sensor driver
131+
- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+)
132+
93133
What's changed since 1.0.4?
94134
- Added smoothing sensor by [@dekutree64](https://github.com/dekutree64)
95135
- Added TMD6200 SPI driver by [@YaseenTwati](https://github.com/YaseenTwati)
Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
#include <Arduino.h>
2+
#include <SPI.h>
3+
#include "SimpleFOC.h"
4+
#include "SimpleFOCDrivers.h"
5+
#include "drivers/simplefocnano/SimpleFOCNanoDriver.h"
6+
#include "encoders/as5048a/MagneticSensorAS5048A.h"
7+
8+
9+
MagneticSensorAS5048A sensor = MagneticSensorAS5048A(PIN_nCS);
10+
SimpleFOCNanoDriver driver = SimpleFOCNanoDriver();
11+
BLDCMotor motor = BLDCMotor(11); // 11 pole pairs
12+
13+
long loopts = 0;
14+
int iterations = 0;
15+
float volts = 0.0f;
16+
17+
void setup() {
18+
Serial.begin(115200); // enable serial port
19+
delay(5000);
20+
SimpleFOCDebug::enable(); // enable debug messages to Serial
21+
22+
sensor.init(); // init sensor on default SPI pins
23+
24+
// read voltage
25+
SimpleFOCDebug::print("Bus voltage: ");
26+
volts = driver.getBusVoltage(5.0f, 1024); // 5V reference, 10-bit ADC
27+
SimpleFOCDebug::println(volts);
28+
driver.voltage_power_supply = volts; // set driver voltage to measured value
29+
driver.voltage_limit = 10.0f; // limit voltage to 10V
30+
driver.pwm_frequency = 30000; // set pwm frequency to 30kHz
31+
driver.init(); // init driver
32+
33+
motor.linkSensor(&sensor); // link the motor to the sensor
34+
motor.linkDriver(&driver); // link the motor to the driver
35+
36+
motor.controller = MotionControlType::torque; // torque control
37+
motor.torque_controller = TorqueControlType::voltage; // use voltage torque control
38+
motor.voltage_limit = driver.voltage_limit / 2.0f; // limit voltage to 1/2 of driver limit
39+
motor.voltage_sensor_align = 4.0f; // align voltage sensor to 4V
40+
41+
motor.init(); // init motor
42+
delay(100); // wait for driver to power up
43+
motor.initFOC(); // init FOC and calibrate motor
44+
45+
Serial.println("Motor ready.");
46+
loopts = millis();
47+
48+
// Set the motor target to 2 volts
49+
motor.target = 2.0f;
50+
}
51+
52+
53+
void loop() {
54+
motor.move();
55+
motor.loopFOC();
56+
long now = millis();
57+
iterations++;
58+
if (now - loopts > 1000) {
59+
Serial.print("Iterations/s: ");
60+
Serial.println(iterations);
61+
Serial.print("Angle: ");
62+
Serial.println(sensor.getAngle());
63+
loopts = now;
64+
iterations = 0;
65+
}
66+
if (now - loopts < 0)
67+
loopts = 0;
68+
}
Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
#include <Arduino.h>
2+
#include <SPI.h>
3+
#include "SimpleFOC.h"
4+
#include "SimpleFOCDrivers.h"
5+
#include "drivers/simplefocnano/SimpleFOCNanoDriver.h"
6+
#include "encoders/as5048a/MagneticSensorAS5048A.h"
7+
8+
9+
MagneticSensorAS5048A sensor = MagneticSensorAS5048A(PIN_nCS);
10+
SimpleFOCNanoDriver driver = SimpleFOCNanoDriver();
11+
BLDCMotor motor = BLDCMotor(11); // 11 pole pairs
12+
13+
Commander commander = Commander(Serial);
14+
void doMotor(char* cmd) { commander.motor(&motor, cmd); }
15+
16+
long loopts = 0;
17+
int iterations = 0;
18+
float volts = 0.0f;
19+
20+
void setup() {
21+
Serial.begin(115200); // enable serial port
22+
delay(5000);
23+
SimpleFOCDebug::enable(); // enable debug messages to Serial
24+
25+
sensor.init(); // init sensor on default SPI pins
26+
27+
// read voltage
28+
SimpleFOCDebug::print("Bus voltage: ");
29+
volts = driver.getBusVoltage(3.3, 4096);
30+
SimpleFOCDebug::println(volts);
31+
driver.voltage_power_supply = volts; // set driver voltage to measured value
32+
driver.voltage_limit = 10.0f; // limit voltage to 10V
33+
driver.pwm_frequency = 30000; // set pwm frequency to 30kHz
34+
driver.init(); // init driver
35+
36+
motor.linkSensor(&sensor); // link the motor to the sensor
37+
motor.linkDriver(&driver); // link the motor to the driver
38+
39+
motor.controller = MotionControlType::torque; // torque control
40+
motor.torque_controller = TorqueControlType::voltage; // use voltage torque control
41+
motor.voltage_limit = driver.voltage_limit / 2.0f; // limit voltage to 1/2 of driver limit
42+
motor.voltage_sensor_align = 4.0f; // align voltage sensor to 4V
43+
44+
motor.init(); // init motor
45+
delay(100); // wait for driver to power up
46+
motor.initFOC(); // init FOC and calibrate motor
47+
48+
commander.add('M', doMotor); // add motor command
49+
50+
Serial.println("Motor ready.");
51+
loopts = millis();
52+
}
53+
54+
55+
void loop() {
56+
motor.move();
57+
motor.loopFOC();
58+
commander.run();
59+
long now = millis();
60+
iterations++;
61+
if (now - loopts > 1000) {
62+
Serial.print("Iterations/s: ");
63+
Serial.println(iterations);
64+
Serial.print("Angle: ");
65+
Serial.println(sensor.getAngle());
66+
loopts = now;
67+
iterations = 0;
68+
}
69+
if (now - loopts < 0)
70+
loopts = 0;
71+
}

0 commit comments

Comments
 (0)