nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

save lut

Browse Source
This commit is contained in:
Nils Schulte 2025-06-29 17:24:42 +02:00
parent 7dfe4041fb
commit fbd1b57b35
5 changed files with 78 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitmodules vendored
View File

@ -1,3 +1,4 @@
[submodule "fw/lib/libcanard"]
path = fw/lib/libcanard
url = git@git.nilsschulte.de:nils/libcanard.git
branch = main

8
fw/platformio.ini
View File

@ -33,13 +33,13 @@ lib_archive = false
# ;platform_packages = espressif/toolchain-xtensa-esp32@8.4.0+2021r2-patch5
monitor_speed = 115200
build_type = debug
;build_type = debug
monitor_filters = esp32_exception_decoder
lib_deps =
https://github.com/schnilz/Arduino-FOC.git#dev
https://github.com/schnilz/Arduino-FOC-drivers.git#dev
# https://github.com/handmade0octopus/ESP32-TWAI-CAN
https://github.com/schnilz/Arduino-FOC-drivers.git#absinc
; https://github.com/handmade0octopus/ESP32-TWAI-CAN
https://github.com/tdk-invn-oss/motion.arduino.ICM42670P
; https://github.com/simplefoc/Arduino-FOC-dcmotor.git
; https://github.com/eric-wieser/packet-io.git
@ -48,6 +48,7 @@ lib_deps =
https://github.com/OpenCyphal/public_regulated_data_types#f9f6790
;https://github.com/OpenCyphal/nunavut.git#11785de
https://github.com/pavel-kirienko/o1heap.git#bd93277
;https://github.com/MajenkoLibraries/SoftSPI
lib_deps_external =
https://github.com/OpenCyphal/libcanard.git#551af7f
@ -56,6 +57,7 @@ build_flags =
-I".pio/build/nilsdriverv1/nunavut/generated-src"
-I"lib/libcanard/libcanard"
-DARDUINO_LOOP_STACK_SIZE=65536
-O3
; -DARDUINO_USB_MODE
;-DSIMPLEFOC_ESP32_USELEDC

18
fw/src/DRV8323S.hpp
View File

@ -125,17 +125,18 @@ enum drv8323s_idrive_shift_e {
DRV8323S_IDRIVEN = 0,
};
unsigned int drv8323s_idrivep_mA_map[] = {
const unsigned int drv8323s_idrivep_mA_map[] = {
10, 30, 60, 80, 120, 140, 170, 190, 260, 330, 370, 440, 570, 680, 820, 1000,
};
unsigned int drv8323s_idriven_mA_map[] = {
const unsigned int drv8323s_idriven_mA_map[] = {
20, 60, 120, 160, 240, 280, 340, 380,
520, 660, 740, 880, 1140, 1360, 1640, 2000,
};
struct drv8323s_foc_driver {
SPIClass *spi;
int CSn, en;
int CSn;
int en;
BLDCDriver3PWM *focdriver;
};
uint16_t drv8323s_read_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
@ -144,13 +145,8 @@ uint16_t drv8323s_read_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE1));
uint16_t data = ((uint16_t)(1 << 15)) | ((((uint16_t)addr)) << 11);
*result = dev->spi->transfer16(data);
// *result = data;
dev->spi->endTransaction();
digitalWrite(dev->CSn, 1);
// Serial.print("SPI Read Result: ");
// Serial.print(data, HEX);
// Serial.print(" -> ");
// Serial.println(result, HEX);
return 0;
}
@ -162,10 +158,6 @@ uint16_t drv8323s_write_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
*result = dev->spi->transfer16(data);
dev->spi->endTransaction();
digitalWrite(dev->CSn, 1);
// Serial.print("SPI Write Result: ");
// Serial.print(data, HEX);
// Serial.print(" -> ");
// Serial.println(result, HEX);
return 0;
}
@ -225,7 +217,9 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
delay(1);
drv8323s_lock_spi(dev, true);
if(!dev->focdriver) {
dev->focdriver = new BLDCDriver3PWM(phA, phB, phC);
}
dev->focdriver->init();
}

99
fw/src/main.cpp
View File

@ -1,5 +1,6 @@
// #include <Preferences.h>
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "esp32-hal.h"
#include <cmath>
#define NODE_NAME "N17BLDC"
@ -9,6 +10,8 @@
#define VCS_REVISION_ID 0
#include "Arduino.h"
#include "pin_def.h"
#define USE_USBSERIAL
#ifdef USE_USBSERIAL
@ -25,12 +28,15 @@ USBCDC usbserial;
#include "canard.c"
#include "SPI.h"
#include <SPI.h>
#define spi_dev SPI
// #include "Wire.h"
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "encoders/calibrated/CalibratedSensor.h"
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/MagneticSensorAS5047.h"
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
#include "DRV8323S.hpp"
@ -43,8 +49,6 @@ USBCDC usbserial;
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
#include "pin_def.h"
#define TEMP(v0, b, rt, r1, vdd) \
((1.0 / \
((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - \
@ -73,12 +77,14 @@ const int voltage_lpf = 50;
// ESP32HWEncoder encoder =
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
auto encoder_absolute = MagneticSensorSPI(AS5047_SPI, 18);
Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4, 0, &encoder_absolute);
MagneticSensorAS5047 encoder_absolute(18);
Encoder encoder(ENC_A, ENC_B, 4096 / 4, 0);
AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute);
float encoder_calibration_lut[90];
CalibratedSensor encoder_calibrated =
CalibratedSensor(encoder, sizeof(encoder_calibration_lut) /
sizeof(encoder_calibration_lut[0]));
CalibratedSensor encoder_calibrated(encoder_absolute, sizeof(encoder_calibration_lut) /
sizeof(encoder_calibration_lut[0]), encoder_calibration_lut);
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
#ifdef MOTOR
@ -154,11 +160,12 @@ void setup() {
Serial.begin(460800, SERIAL_8N1, 44, 43);
#endif
digitalWrite(LED_PIN, 0); // enable
delay(100);
delay(500);
digitalWrite(LED_PIN, 1); // disable
delay(1000);
delay(100);
Serial.println("Start");
delay(1000);
pinMode(SPI_DRV_SC, OUTPUT);
@ -173,20 +180,25 @@ void setup() {
pinMode(CAL_PIN, OUTPUT);
digitalWrite(CAL_PIN, 0); // enable
pinMode(SPI_MISO, INPUT_PULLUP);
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
//SPI.setFrequency(100000);
// initialise magnetic sensor hardware
encoder.pullup = Pullup::USE_INTERN;
encoder.init();
encoder.enableInterrupts(doA1, doB1);
pinMode(SPI_MISO, INPUT_PULLUP);
encoder_absolute.init(&spi_dev);
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
abs_inc_sensor.init();
Serial.printf("offset: %f\nvalue: %d", abs_inc_sensor.offset, abs_inc_sensor.getAngle());
#ifdef MOTOR
SimpleFOCDebug::enable(&Serial);
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &SPI);
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
digitalWrite(INLABC, 1); // enable
// driver.init(&SPI);
// status();
motor.linkSensor(&encoder);
motor.linkSensor(&encoder_calibrated);
motor.linkDriver(drv8323s.focdriver);
// motor.foc_modulation = FOCModulationType::SinePWM;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
@ -207,6 +219,7 @@ void setup() {
// set voltage to run calibration
encoder_calibrated.voltage_calibration = 3;
encoder_calibrated.init();
// Running calibration
const char *encoder_calibration_lut_str = "enc_cal_lut";
const char *zero_electric_angle_str = "zero_el_angle";
@ -215,19 +228,17 @@ void setup() {
if (!pref.isKey(encoder_calibration_lut_str) ||
pref.getBytesLength(encoder_calibration_lut_str) !=
sizeof(encoder_calibration_lut)) {
encoder_calibrated.calibrate(motor, NULL, 0.0, Direction::UNKNOWN,
settle_time_ms);
encoder_calibrated.calibrate(motor, settle_time_ms);
pref.putBytes(encoder_calibration_lut_str,
&encoder_calibrated.calibrationLut[0],
&encoder_calibration_lut[0],
sizeof(encoder_calibration_lut));
pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
} else {
pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0],
sizeof(encoder_calibration_lut));
encoder_calibrated.calibrate(motor, &encoder_calibration_lut[0],
motor.zero_electric_angle, Direction::CW,
settle_time_ms);
Serial.println("Skipping calibration");
}
if (!pref.isKey(zero_electric_angle_str)) {
@ -249,7 +260,7 @@ void setup() {
"comm", // Name of the task (e.g. for debugging)
65536, // Stack size (bytes)
NULL, // Parameter to pass
1, // Task priority
10, // Task priority
&taskCommHandle, // Assign task handle
0 // Run on the non-Arduino (1) core
);
@ -260,25 +271,14 @@ void setup() {
int i = 0;
void loop() {
#ifdef MOTOR
bool led = false;
bool armed = false;
xSemaphoreGive(mutex);
led = servo_state.arming.armed;
if (!servo_state.arming.armed) {
if (motor.enabled) {
motor.move(0);
digitalWrite(INLABC, 0);
motor.disable();
}
} else {
if (!motor.enabled) {
digitalWrite(INLABC, 1);
motor.enable();
}
motor.move(servo_state.target_force);
}
float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
xSemaphoreTake(mutex, portMAX_DELAY);
armed = servo_state.arming.armed;
float target = servo_state.target_force;
servo_state.controller_temperature = temp_pcb + 273.15f; /* Kelvin */
servo_state.controller_temperature = curr_pcb_temp_kelvin;
servo_state.motor_temperature = NAN;
float curr_angle = encoder_calibrated.getAngle();
@ -296,11 +296,30 @@ void loop() {
servo_state.curr_position = curr_angle;
servo_state.curr_force = servo_state.target_force;
servo_state.curr_velocity = encoder.getAngle();
servo_state.curr_acceleration = encoder_absolute.getAngle();
servo_state.curr_force = abs_inc_sensor.getAngle();
servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
servo_state.current = NAN;
xSemaphoreTake(mutex, portMAX_DELAY);
xSemaphoreGive(mutex);
digitalWrite(LED_PIN, led ? LOW : HIGH);
if (!armed) {
if (motor.enabled) {
motor.move(0);
motor.disable();
digitalWrite(INLABC, 0);
}
} else {
if (!motor.enabled) {
digitalWrite(INLABC, 1);
motor.enable();
}
motor.move(target);
}
digitalWrite(LED_PIN, armed ? LOW : HIGH);
drv8323s.focdriver->voltage_power_supply = vdrive_read;
motor.loopFOC();

8
fw/src/udral_servo.hpp
View File

@ -144,6 +144,8 @@ struct UdralServoInternalState {
uint64_t servo_fast_loop;
uint64_t servo_1Hz_loop;
} next_transfer_id;
state_sync_f user_state_sync_f;
};
/// This flag is raised when the node is requested to restart.
@ -856,7 +858,9 @@ static void canardDeallocate(void *const user_reference, const size_t amount, vo
int udral_loop(state_sync_f servo_state_sync_f)
{
srand(micros());
struct UdralServoInternalState state;
struct UdralServoInternalState state {
.user_state_sync_f = servo_state_sync_f,
};
// A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack.
// For the background and related theory refer to the following resources:
// - https://github.com/OpenCyphal/libcanard/blob/master/README.md
@ -1069,7 +1073,7 @@ int udral_loop(state_sync_f servo_state_sync_f)
CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10;
do
{
servo_state_sync_f(&state.servo);
state.servo_state_sync_f(&state.servo);
// Run a trivial scheduler polling the loops that run the business logic.
CanardMicrosecond now_usec = getMonotonicMicroseconds();
if (now_usec >= next_fast_iter_at)