nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
N17BLDC/fw/src/main.cpp
Nils Schulte 64f4d4078d save lut
2025-06-29 17:35:47 +02:00

329 lines
9.6 KiB
C++
Raw Blame History

// #include <Preferences.h>
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "esp32-hal.h"
#include <cmath>
#define NODE_NAME "N17BLDC"
#define VERSION_MAJOR 0
#define VERSION_MINOR 1
#define VCS_REVISION_ID 0
#include "Arduino.h"
#include "pin_def.h"
#define USE_USBSERIAL
#ifdef USE_USBSERIAL
#include "USB.h"
USBCDC usbserial;
#ifdef Serial
#undef Serial
#endif
#define Serial usbserial
#else
#define Serial Serial1
#endif
// #define Serial Serial
#include "canard.c"
#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"
#include "HybridStepperMotor.h"
#include "esp32-hal-adc.h"
#include "esp32-hal-gpio.h"
#include "udral_servo.hpp"
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
#define TEMP(v0, b, rt, r1, vdd) \
((1.0 / \
((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - \
273.15)
#define temp_pcb \
(TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, \
3.3))
#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
//* 20.8f/ 21033.75)
#define MOTOR
#define FW_NO_WATCHDOG
#ifndef FW_NO_WATCHDOG
#include <esp_task_wdt.h>
#define WDT_TIMEOUT_s 1
// struct esp_task_wdt_config_t wd_config = {
// .timeout_ms = 100,
// .idle_core_mask = ~0x00000000,
// .trigger_panic = true
// };
#endif
const int voltage_lpf = 50;
// ESP32HWEncoder encoder =
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
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(encoder_absolute, sizeof(encoder_calibration_lut) /
sizeof(encoder_calibration_lut[0]), encoder_calibration_lut);
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
#ifdef MOTOR
struct drv8323s_foc_driver drv8323s;
// BLDCMotor motor = BLDCMotor(100); // 24N22P
HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
#endif
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
Wire.beginTransmission(addr);
Wire.write(reg); // MAN:0x2E
int error = Wire.endTransmission();
if (error != 0) {
return;
}
delayMicroseconds(50);
int ret = Wire.requestFrom(addr, len);
Wire.readBytes(buf, ret);
return;
}
void write_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val) {
Wire.beginTransmission(addr);
uint8_t data[] = {reg_lsb, val};
Wire.write(data, sizeof data);
int error = Wire.endTransmission();
if (error != 0) {
return;
}
}
void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
Wire.beginTransmission(addr);
uint8_t data[] = {reg_lsb, val1, val2};
Wire.write(data, sizeof data);
int error = Wire.endTransmission();
if (error != 0) {
return;
}
}
SemaphoreHandle_t mutex;
struct UdralServoState servo_state = {0};
void mutexed_state_sync(UdralServoState *const state) {
xSemaphoreTake(mutex, portMAX_DELAY);
servo_state.target_force = state->target_force;
servo_state.target_acceleration = state->target_acceleration;
servo_state.target_velocity = state->target_velocity;
servo_state.target_position = state->target_position;
servo_state.arming = state->arming;
servo_state.pid = state->pid;
*state = servo_state;
xSemaphoreGive(mutex);
}
TaskHandle_t taskCommHandle;
void comm_task(void *parameter) { udral_loop(mutexed_state_sync); }
void doA1() { encoder.handleA(); }
void doB1() { encoder.handleB(); }
void setup() {
pinMode(LED_PIN, OUTPUT);
mutex = xSemaphoreCreateMutex();
#ifdef USE_USBSERIAL
Serial.begin();
USB.begin();
Serial.setDebugOutput(
true); // sends all log_e(), log_i() messages to USB HW CDC
Serial.setTxTimeoutMs(0);
#else
Serial.begin(460800, SERIAL_8N1, 44, 43);
#endif
digitalWrite(LED_PIN, 0); // enable
delay(500);
digitalWrite(LED_PIN, 1); // disable
delay(100);
Serial.println("Start");
delay(1000);
pinMode(SPI_DRV_SC, OUTPUT);
pinMode(INHA, OUTPUT);
// digitalWrite(INHA, 0); // enable
pinMode(INHB, OUTPUT);
// digitalWrite(INHB, 0); // enable
pinMode(INHC, OUTPUT);
// digitalWrite(INHC, 0); // enable
pinMode(INLABC, OUTPUT);
digitalWrite(INLABC, 0); // enable
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);
encoder_absolute.init(&spi_dev);
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_dev);
digitalWrite(INLABC, 1); // enable
// driver.init(&SPI);
// status();
motor.linkSensor(&encoder_calibrated);
motor.linkDriver(drv8323s.focdriver);
// motor.foc_modulation = FOCModulationType::SinePWM;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.voltage_sensor_align = 4;
motor.voltage_limit = 6;
// motor.controller = MotionControlType::velocity_openloop;
// motor.controller = MotionControlType::velocity;
motor.controller = MotionControlType::torque;
motor.torque_controller = TorqueControlType::voltage;
motor.useMonitoring(Serial);
drv8323s.focdriver->voltage_power_supply = vdrive_read;
motor.init();
delay(1000);
static Preferences pref;
pref.begin("simpleFOC");
// 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";
const char *sensor_direction_str = "enc_dir_str";
const int settle_time_ms = 150;
if (!pref.isKey(encoder_calibration_lut_str) ||
pref.getBytesLength(encoder_calibration_lut_str) !=
sizeof(encoder_calibration_lut)) {
encoder_calibrated.calibrate(motor, settle_time_ms);
pref.putBytes(encoder_calibration_lut_str,
&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));
Serial.println("Skipping calibration");
}
if (!pref.isKey(zero_electric_angle_str)) {
motor.initFOC();
pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
} else {
motor.zero_electric_angle = pref.getFloat(zero_electric_angle_str);
motor.sensor_direction = (Direction)pref.getInt(sensor_direction_str);
motor.initFOC();
}
pref.end();
motor.move(0);
digitalWrite(INLABC, 0); // enable
#endif
xTaskCreatePinnedToCore(&comm_task, // Function name of the task
"comm", // Name of the task (e.g. for debugging)
65536, // Stack size (bytes)
NULL, // Parameter to pass
10, // Task priority
&taskCommHandle, // Assign task handle
0 // Run on the non-Arduino (1) core
);
// esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts
// esp_task_wdt_add(NULL); // add current thread to WDT watch
}
int i = 0;
void loop() {
#ifdef MOTOR
bool armed = false;
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 = curr_pcb_temp_kelvin;
servo_state.motor_temperature = NAN;
float curr_angle = encoder_calibrated.getAngle();
float curr_vel = encoder_calibrated.getVelocity();
unsigned long now = millis();
static unsigned long last_update = now;
unsigned long tdelta = now - last_update;
last_update = now;
float curr_accel =
tdelta ? (curr_vel - servo_state.curr_velocity) / tdelta : 0;
servo_state.curr_acceleration = curr_accel;
servo_state.curr_velocity = curr_vel;
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;
xSemaphoreGive(mutex);
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();
#else
encoder.update(); // optional: Update manually if not using loopfoc()
#endif
}
Reference in New Issue View Git Blame Copy Permalink