diff --git a/.gitmodules b/.gitmodules index b301240..e7acdd2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ [submodule "fw/lib/libcanard"] path = fw/lib/libcanard url = git@git.nilsschulte.de:nils/libcanard.git + branch = main diff --git a/fw/platformio.ini b/fw/platformio.ini index f77bb8c..bcd30da 100644 --- a/fw/platformio.ini +++ b/fw/platformio.ini @@ -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 diff --git a/fw/src/DRV8323S.hpp b/fw/src/DRV8323S.hpp index 64afc04..6158283 100644 --- a/fw/src/DRV8323S.hpp +++ b/fw/src/DRV8323S.hpp @@ -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); - dev->focdriver = new BLDCDriver3PWM(phA, phB, phC); + if(!dev->focdriver) { + dev->focdriver = new BLDCDriver3PWM(phA, phB, phC); + } dev->focdriver->init(); } diff --git a/fw/src/main.cpp b/fw/src/main.cpp index f4c9097..43d7feb 100644 --- a/fw/src/main.cpp +++ b/fw/src/main.cpp @@ -1,5 +1,6 @@ // #include +#include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "esp32-hal.h" #include #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 +#define spi_dev SPI // #include "Wire.h" #include #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 -#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(); diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp index 4c79d94..323ea04 100644 --- a/fw/src/udral_servo.hpp +++ b/fw/src/udral_servo.hpp @@ -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)