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

wip doenst work

Browse Source
This commit is contained in:
Nils Schulte 2025-08-17 00:18:50 +02:00
parent dee14242cd
commit 3aa2916aef
7 changed files with 383 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
fw/src/.clang-format
View File

@ -1,3 +1,3 @@
Language: Cpp Language: Cpp
BasedOnStyle: google BasedOnStyle: google
ColumnLimit: 120 ColumnLimit: 140

5
fw/src/DRV8323S.hpp
View File

@ -199,6 +199,9 @@ void drv8323s_set_sen_lvl(struct drv8323s_foc_driver *dev,
void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC, void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
int CSn, int en, SPIClass *spi) { int CSn, int en, SPIClass *spi) {
if(dev == NULL || spi == NULL) {
return;
}
dev->CSn = CSn; dev->CSn = CSn;
dev->spi = spi; dev->spi = spi;
dev->en = en; dev->en = en;
@ -211,7 +214,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
/* Set 3PWM Mode */ /* Set 3PWM Mode */
drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM); drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
delay(1); delay(1);
// drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED); drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
delay(1); delay(1);
drv8323s_set_sen_lvl(dev, DRV8323S_SEN_LVL_0_25V); drv8323s_set_sen_lvl(dev, DRV8323S_SEN_LVL_0_25V);
delay(1); delay(1);

319
fw/src/main.cpp
View File

@ -5,6 +5,7 @@ reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000
#include <cmath> #include <cmath>
#include "BLDCMotor.h"
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/AS5047.h" #include "encoders/as5047/AS5047.h"
#include "esp32-hal.h" #include "esp32-hal.h"
@ -15,7 +16,7 @@ reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000
#define VCS_REVISION_ID 0 #define VCS_REVISION_ID 0
#include "Arduino.h" #include "Arduino.h"
#include "pin_def.h" #include "pin_def_v5.h"
#define USE_USBSERIAL #define USE_USBSERIAL
#ifdef USE_USBSERIAL #ifdef USE_USBSERIAL
@ -35,24 +36,25 @@ USBCDC usbserial;
#include "canard.c" #include "canard.c"
#include "udral_servo.hpp" #include "udral_servo.hpp"
#define spi_dev SPI #define spi_dev SPI
SPIClass spi_drv(FSPI);
// #include "Wire.h" // #include "Wire.h"
#include <SimpleFOC.h> #include <SimpleFOC.h>
#include "DRV8323S.hpp"
#include "HybridStepperMotor.h"
#include "SimpleFOCDrivers.h" #include "SimpleFOCDrivers.h"
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/MagneticSensorAS5047.h" #include "encoders/as5047/MagneticSensorAS5047.h"
#include "encoders/calibrated/CalibratedSensor.h" #include "encoders/calibrated/CalibratedSensor.h"
#include "DRV8323S.hpp"
#include "HybridStepperMotor.h"
#include "esp32-hal-adc.h" #include "esp32-hal-adc.h"
#include "esp32-hal-gpio.h" #include "esp32-hal-gpio.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(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 temp_pcb (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3))
#define temp_mot (analogRead(TERMISTOR_EXT) > 0.95 ? NAN : (TEMP(analogRead(TERMISTOR_EXT) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3)))
#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
//* 20.8f/ 21033.75)
// #define ESPHWENC // #define ESPHWENC
#define MOTOR #define MOTOR
#define FW_NO_WATCHDOG #define FW_NO_WATCHDOG
@ -67,9 +69,7 @@ USBCDC usbserial;
// }; // };
#endif #endif
const int voltage_lpf = 50; float max_voltage_limit = 0;
const float max_voltage_limit = 2.3;
#ifdef ESPHWENC #ifdef ESPHWENC
#include "encoders/esp32hwencoder/ESP32HWEncoder.h" #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
@ -87,16 +87,15 @@ CalibratedSensor encoder_calibrated(abs_inc_sensor, sizeof(encoder_calibration_l
encoder_calibration_lut); encoder_calibration_lut);
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36); // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
#ifdef MOTOR
struct drv8323s_foc_driver drv8323s; struct drv8323s_foc_driver drv8323s;
// BLDCMotor motor = BLDCMotor(100); // 24N22P // BLDCMotor motor = BLDCMotor(100); // 24N22P
//HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
HybridStepperMotor motorHybridStepper = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
BLDCMotor motor = BLDCMotor(7, 0.7, 360, 0.15); BLDCMotor motorBLDC = BLDCMotor(7, 0.7, 360, 0.15);
FOCMotor *motor = NULL;
// BLDCMotor motor = BLDCMotor(7); // BLDCMotor motor = BLDCMotor(7);
// BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4); // BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4);
#endif
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
Wire.beginTransmission(addr); Wire.beginTransmission(addr);
Wire.write(reg); // MAN:0x2E Wire.write(reg); // MAN:0x2E
@ -139,21 +138,25 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
xSemaphoreTake(mutex, portMAX_DELAY); xSemaphoreTake(mutex, portMAX_DELAY);
if (!init_done) { if (!init_done) {
init_done = true; init_done = true;
motor = state->motor_type == MOTOR_TYPE_HYBRID_STEPPER ? (FOCMotor *)&motorHybridStepper : (FOCMotor *)&motorBLDC;
state->max_voltage.value.count = 2;
state->max_voltage.value.elements[0] = 0.01; /* max voltage*/
state->max_voltage.value.elements[1] = 0.01; /* aligning voltage */
state->pid_position.value.count = 6; state->pid_position.value.count = 6;
state->pid_position.value.elements[0] = motor.P_angle.P; state->pid_position.value.elements[0] = motor->P_angle.P;
state->pid_position.value.elements[1] = motor.P_angle.I; state->pid_position.value.elements[1] = motor->P_angle.I;
state->pid_position.value.elements[2] = motor.P_angle.D; state->pid_position.value.elements[2] = motor->P_angle.D;
state->pid_position.value.elements[3] = motor.LPF_angle.Tf; state->pid_position.value.elements[3] = motor->LPF_angle.Tf;
state->pid_position.value.elements[4] = motor.P_angle.output_ramp; state->pid_position.value.elements[4] = motor->P_angle.output_ramp;
state->pid_position.value.elements[5] = motor.P_angle.limit; state->pid_position.value.elements[5] = motor->P_angle.limit;
state->pid_velocity.value.count = 7; state->pid_velocity.value.count = 7;
state->pid_velocity.value.elements[0] = motor.PID_velocity.P; state->pid_velocity.value.elements[0] = motor->PID_velocity.P;
state->pid_velocity.value.elements[1] = motor.PID_velocity.I; state->pid_velocity.value.elements[1] = motor->PID_velocity.I;
state->pid_velocity.value.elements[2] = motor.PID_velocity.D; state->pid_velocity.value.elements[2] = motor->PID_velocity.D;
state->pid_velocity.value.elements[3] = motor.LPF_velocity.Tf; state->pid_velocity.value.elements[3] = motor->LPF_velocity.Tf;
state->pid_velocity.value.elements[4] = motor.PID_velocity.output_ramp; state->pid_velocity.value.elements[4] = motor->PID_velocity.output_ramp;
state->pid_velocity.value.elements[5] = motor.PID_velocity.limit; state->pid_velocity.value.elements[5] = motor->PID_velocity.limit;
state->pid_velocity.value.elements[6] = motor.motion_downsample; state->pid_velocity.value.elements[6] = motor->motion_downsample;
} }
servo_state.target_force = state->target_force; servo_state.target_force = state->target_force;
servo_state.target_acceleration = state->target_acceleration; servo_state.target_acceleration = state->target_acceleration;
@ -210,7 +213,8 @@ void setup() {
digitalWrite(CAL_PIN, 0); // enable digitalWrite(CAL_PIN, 0); // enable
pinMode(SPI_MISO, INPUT_PULLUP); pinMode(SPI_MISO, INPUT_PULLUP);
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC); spi_dev.begin(SPI_CLK, SPI_MISO, SPI_MOSI);
spi_drv.begin(SPI_DRV_CLK, SPI_DRV_MISO, SPI_DRV_MOSI, SPI_DRV_SC);
// SPI.setFrequency(100000); // SPI.setFrequency(100000);
// initialise magnetic sensor hardware // initialise magnetic sensor hardware
encoder.pullup = Pullup::USE_INTERN; encoder.pullup = Pullup::USE_INTERN;
@ -222,67 +226,22 @@ void setup() {
abs_inc_sensor.init(); abs_inc_sensor.init();
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN,&spi_drv); //&spi_drv);
motorHybridStepper.linkSensor(&encoder_absolute);
motorHybridStepper.linkDriver(drv8323s.focdriver);
motorBLDC.linkSensor(&encoder_absolute);
motorBLDC.linkDriver(drv8323s.focdriver);
Serial.printf("offset: %f\nvalue: %d", abs_inc_sensor.offset, abs_inc_sensor.getAngle()); Serial.printf("offset: %f\nvalue: %d", abs_inc_sensor.offset, abs_inc_sensor.getAngle());
#ifdef MOTOR
SimpleFOCDebug::enable(&Serial); SimpleFOCDebug::enable(&Serial);
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev); digitalWrite(LED_PIN, 1); // disable
digitalWrite(INLABC, 1); // enable delay(100);
// driver.init(&SPI); digitalWrite(LED_PIN, 0); // enable
// status(); delay(400);
motor.linkSensor(&encoder_absolute); digitalWrite(LED_PIN, 1); // disable
//motor.linkSensor(&encoder_calibrated); delay(4000);
motor.linkDriver(drv8323s.focdriver);
// motor.foc_modulation = FOCModulationType::SinePWM;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.voltage_sensor_align = 1.5;
motor.voltage_limit = max_voltage_limit;
// 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 (motor.sensor == &encoder_calibrated && (!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.sensor != &encoder_calibrated) {
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 xTaskCreatePinnedToCore(&comm_task, // Function name of the task
"comm", // Name of the task (e.g. for debugging) "comm", // Name of the task (e.g. for debugging)
65536, // Stack size (bytes) 65536, // Stack size (bytes)
@ -294,12 +253,7 @@ void setup() {
// esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts // esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts
// esp_task_wdt_add(NULL); // add current thread to WDT watch // esp_task_wdt_add(NULL); // add current thread to WDT watch
digitalWrite(LED_PIN, 1); // disable
delay(100);
digitalWrite(LED_PIN, 0); // enable
delay(400);
digitalWrite(LED_PIN, 1); // disable
delay(100);
while (!init_done) { while (!init_done) {
delay(10); delay(10);
} }
@ -314,100 +268,189 @@ void setup() {
); );
} }
bool init_motor() {
#ifndef MOTOR
return false;
#endif
if (motor == NULL) {
return false;
}
static bool is_init = false;
if (!is_init) {
if (motor->voltage_limit <= 0 || motor->voltage_sensor_align <= 0) {
return false;
}
is_init = true;
} else {
return true;
}
digitalWrite(INLABC, 1); // enable
// driver.init(&SPI);
// status();
// motor->foc_modulation = FOCModulationType::SinePWM;
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
// 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 = motor->voltage_sensor_align;
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;
const bool calibration_saveable = motor->sensor == &encoder_calibrated;
if (calibration_saveable &&
(!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) && calibration_saveable) {
motor->zero_electric_angle = pref.getFloat(zero_electric_angle_str);
motor->sensor_direction = (Direction)pref.getInt(sensor_direction_str);
motor->initFOC();
} else {
motor->initFOC();
if (calibration_saveable) {
pref.putFloat(zero_electric_angle_str, motor->zero_electric_angle);
pref.putInt(sensor_direction_str, (int)motor->sensor_direction);
}
}
pref.end();
motor->move(0);
digitalWrite(INLABC, 0); // enable
return true;
}
int i = 0; int i = 0;
void loop() {} void loop() {}
void foc_task_loop() { void foc_task_loop() {
#ifdef MOTOR if(motor == NULL) {
delay(10);
return;
}
bool armed = false; bool armed = false;
/* adc reading */ /* adc reading */
drv8323s.focdriver->voltage_power_supply = vdrive_read; drv8323s.focdriver->voltage_power_supply = vdrive_read;
const float curr_pcb_temp_kelvin = temp_pcb + 273.15f; const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
const float curr_mot_temp_kelvin = temp_mot + 273.15f;
xSemaphoreTake(mutex, portMAX_DELAY); xSemaphoreTake(mutex, portMAX_DELAY);
armed = servo_state.arming.armed; armed = servo_state.arming.armed;
max_voltage_limit = servo_state.max_voltage.value.count <= 0 ? 0 : servo_state.max_voltage.value.elements[0];
motor->voltage_sensor_align = servo_state.max_voltage.value.count <= 1 ? 0 : servo_state.max_voltage.value.elements[1];
float target; float target;
if (!std::isnan(servo_state.target_position)) { if (!std::isnan(servo_state.target_position)) {
if (motor.controller != MotionControlType::angle) { auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::angle_openloop : MotionControlType::angle);
motor.controller = MotionControlType::angle; if (motor->controller != MotionControlType::angle) {
Serial.println("MotionControlType::angle"); motor->controller = MotionControlType::angle;
Serial.printf("MotionControlType::angle%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
} }
target = servo_state.target_position; target = servo_state.target_position;
motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity; motor->velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
motor.voltage_limit = motor->voltage_limit =
std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force)); std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
} else if (!std::isnan(servo_state.target_velocity)) { } else if (!std::isnan(servo_state.target_velocity)) {
if (motor.controller != MotionControlType::velocity) { auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::velocity_openloop : MotionControlType::velocity);
motor.controller = MotionControlType::velocity; if (motor->controller != target_control_type) {
Serial.println("MotionControlType::velocity"); motor->controller = target_control_type;
Serial.printf("MotionControlType::velocity%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
} }
target = servo_state.target_velocity; target = servo_state.target_velocity;
motor.voltage_limit = motor->voltage_limit =
std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force)); std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
} else { } else {
motor.voltage_limit = if (is_openloop(servo_state.motor_type)) {
armed = false;
}
motor->voltage_limit =
std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force)); std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
target = std::isnan(servo_state.target_force) ? 0 : servo_state.target_force; target = std::isnan(servo_state.target_force) ? 0 : servo_state.target_force;
if (motor.controller != MotionControlType::torque) { if (motor->controller != MotionControlType::torque) {
motor.controller = MotionControlType::torque; motor->controller = MotionControlType::torque;
Serial.printf("MotionControlType::torque %f\n", target); Serial.printf("MotionControlType::torque %f\n", target);
} }
} }
servo_state.controller_temperature = curr_pcb_temp_kelvin; servo_state.controller_temperature = curr_pcb_temp_kelvin;
servo_state.motor_temperature = NAN; servo_state.motor_temperature = curr_mot_temp_kelvin;
float curr_angle = motor.sensor->getAngle(); float curr_angle = motor->sensor->getAngle();
servo_state.curr_position = curr_angle; servo_state.curr_position = curr_angle;
servo_state.curr_force = motor.voltage.d; servo_state.curr_force = motor->voltage.d;
servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply; servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
servo_state.current = motor.current.d; servo_state.current = motor->current.d;
motor.PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0]; motor->PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
motor.PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1]; motor->PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
motor.PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2]; motor->PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
motor.LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3]; motor->LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
motor.PID_velocity.output_ramp = servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4]; motor->PID_velocity.output_ramp = servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
motor.PID_velocity.limit = servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5]; motor->PID_velocity.limit = servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
motor.motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6]; motor->motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];
motor.P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0]; motor->P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
motor.P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1]; motor->P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
motor.P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2]; motor->P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
motor.LPF_angle.Tf = servo_state.pid_position.value.count <= 3 ? 0 : servo_state.pid_position.value.elements[3]; motor->LPF_angle.Tf = servo_state.pid_position.value.count <= 3 ? 0 : servo_state.pid_position.value.elements[3];
motor.P_angle.output_ramp = servo_state.pid_position.value.count <= 4 ? 1e6 : servo_state.pid_position.value.elements[4]; motor->P_angle.output_ramp = servo_state.pid_position.value.count <= 4 ? 1e6 : servo_state.pid_position.value.elements[4];
motor.P_angle.limit = servo_state.pid_position.value.count <= 5 ? 1000 : servo_state.pid_position.value.elements[5]; motor->P_angle.limit = servo_state.pid_position.value.count <= 5 ? 1000 : servo_state.pid_position.value.elements[5];
// motor.P_angle.output_ramp = 10000; // default 1e6 rad/s^2
// motor->P_angle.output_ramp = 10000; // default 1e6 rad/s^2
/* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */ /* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */
// setting the limits // setting the limits
xSemaphoreGive(mutex); xSemaphoreGive(mutex);
if (init_motor()) {
if (!armed) { if (!armed) {
if (motor.enabled) { if (motor->enabled) {
motor.move(0); motor->move(0);
motor.disable(); motor->disable();
digitalWrite(INLABC, 0); digitalWrite(INLABC, 0);
digitalWrite(LED_PIN, HIGH);
} }
} else { } else {
if (!motor.enabled) { if (!motor->enabled) {
digitalWrite(INLABC, 1); digitalWrite(INLABC, 1);
motor.enable(); motor->enable();
digitalWrite(LED_PIN, LOW);
} }
motor.move(target); motor->move(target);
}
motor->loopFOC();
} else {
digitalWrite(LED_PIN, HIGH);
delay(10);
//if(motor && motor->sensor) {
// motor->sensor->update();
//}
} }
digitalWrite(LED_PIN, armed ? LOW : HIGH);
motor.loopFOC();
#else
encoder.update(); // optional: Update manually if not using loopfoc()
#endif
} }
void foc_task(void *parameter) { void foc_task(void *parameter) {
while (true) { while (true) {
foc_task_loop(); foc_task_loop();

0
fw/src/pin_def.h → fw/src/pin_def_v4.h
View File

35
fw/src/pin_def_v5.h Normal file
View File

@ -0,0 +1,35 @@
#pragma once
#define ENC_A 11
#define ENC_B 8
#define SPI_ACC_nCS 0
#define SPI_ENC_nCS 18
#define SPI_CLK 40
#define SPI_MISO 38
#define SPI_MOSI 39
#define DRV_nFAULT 15
#define DRVEN 16
#define TERMISTOR_PCB 3
#define TERMISTOR_EXT 9
#define SOA 1
#define SOB 2
#define SOC 10
#define INHA 14
#define INHB 12
#define INHC 13
#define INLABC 46
#define CAN_TX 6
#define CAN_RX 7
#define SPI_DRV_MISO 45
#define SPI_DRV_MOSI 48
#define SPI_DRV_CLK 47
#define SPI_DRV_SC 21
#define LED_PIN 42
#define VSENSE_PIN 5
#define CAL_PIN 17

67
fw/src/ringbuf.hpp Normal file
View File

@ -0,0 +1,67 @@
/*
*
* Copyright (c) [2022] by InvenSense, Inc.
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*
*/
#include "ICM42670P.h"
// Instantiate an ICM42670 with SPI interface and CS on pin 8
ICM42670 IMU(SPI,8);
void setup() {
int ret;
Serial.begin(115200);
while(!Serial) {}
// Initializing the ICM42670
ret = IMU.begin();
if (ret != 0) {
Serial.print("ICM42670 initialization failed: ");
Serial.println(ret);
while(1);
}
// Accel ODR = 100 Hz and Full Scale Range = 16G
IMU.startAccel(100,16);
// Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
IMU.startGyro(100,2000);
// Wait IMU to start
delay(100);
}
void loop() {
inv_imu_sensor_event_t imu_event;
// Get last event
IMU.getDataFromRegisters(imu_event);
// Format data for Serial Plotter
Serial.print("AccelX:");
Serial.println(imu_event.accel[0]);
Serial.print("AccelY:");
Serial.println(imu_event.accel[1]);
Serial.print("AccelZ:");
Serial.println(imu_event.accel[2]);
Serial.print("GyroX:");
Serial.println(imu_event.gyro[0]);
Serial.print("GyroY:");
Serial.println(imu_event.gyro[1]);
Serial.print("GyroZ:");
Serial.println(imu_event.gyro[2]);
Serial.print("Temperature:");
Serial.println(imu_event.temperature);
// Run @ ODR 100Hz
delay(10);
}

85
fw/src/udral_servo.hpp
View File

@ -16,7 +16,7 @@
#define NUNAVUT_ASSERT(x) assert(x) #define NUNAVUT_ASSERT(x) assert(x)
#include "canard.h" #include "canard.h"
#include "esp32-hal.h" #include "esp32-hal.h"
#include "pin_def.h" #include "pin_def_v5.h"
// #include "socketcan.h" // #include "socketcan.h"
#include <assert.h> #include <assert.h>
@ -60,6 +60,17 @@ const unsigned int hz = 50;
/// We keep the state of the application here. Feel free to use static variables /// We keep the state of the application here. Feel free to use static variables
/// instead if desired. /// instead if desired.
enum motor_type_t {
MOTOR_TYPE_BLDC_OPENLOOP = 0,
MOTOR_TYPE_BLDC = 1,
MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP = 2,
MOTOR_TYPE_HYBRID_STEPPER = 3,
};
bool is_openloop(const enum motor_type_t t) {
return !(((int)t)&0b1);
}
struct UdralServoState { struct UdralServoState {
/// Whether the servo is supposed to actuate the load or to stay idle (safe /// Whether the servo is supposed to actuate the load or to stay idle (safe
/// low-power mode). /// low-power mode).
@ -93,6 +104,9 @@ struct UdralServoState {
uavcan_primitive_array_Real32_1_0 pid_position; uavcan_primitive_array_Real32_1_0 pid_position;
uavcan_primitive_array_Real32_1_0 pid_velocity; uavcan_primitive_array_Real32_1_0 pid_velocity;
uavcan_primitive_array_Real32_1_0 max_voltage;
enum motor_type_t motor_type;
}; };
}; };
@ -143,6 +157,8 @@ struct UdralServoInternalState {
state_sync_f user_state_sync_f; state_sync_f user_state_sync_f;
}; };
static struct UdralServoInternalState state;
/// This flag is raised when the node is requested to restart. /// This flag is raised when the node is requested to restart.
static volatile bool g_restart_required = false; static volatile bool g_restart_required = false;
@ -583,10 +599,43 @@ static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const ua
name[req->name.name.count] = '\0'; name[req->name.name.count] = '\0';
uavcan_register_Access_Response_1_0 resp = {0}; uavcan_register_Access_Response_1_0 resp = {0};
// Serial.println("processRequestRegisterAccess"); Serial.printf("processRequestRegisterAccess name: %s\n\r", (char *)&req->name.name);
// Serial.print("name: "); if (strcmp((char*)&req->name.name.elements[0], "reg.motor_type") == 0) {
// Serial.println((char *)&req->name.name); uavcan_register_Value_1_0_select_string_(&resp.value);
if (!uavcan_register_Value_1_0_is_string_(&req->value)) {
if (state.servo.motor_type == MOTOR_TYPE_HYBRID_STEPPER) {
strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper");
} else if (state.servo.motor_type == MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP) {
strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop");
} else if (state.servo.motor_type == MOTOR_TYPE_BLDC) {
strcpy((char*)&resp.value._string.value.elements[0], "BLDC");
} else {
strcpy((char*)&resp.value._string.value.elements[0], "BLDC_openloop");
}
resp.value._string.value.count = strlen((char*)&resp.value._string.value.elements[0]);
//Serial.printf("resp reg.motor_type: %s (%d)\n\r",(char*)&resp.value._string.value.elements[0], resp.value._string.value.count);
Serial.printf("req reg.motor_type: %s (%d)\n\r",(char*)&req->value._string.value.elements[0], req->value._string.value.count);
registerWrite("reg.motor_type", &resp.value);
}
if (strcmp((char*)&resp.value._string.value.elements[0], "HybridStepper") == 0) {
state.servo.motor_type = MOTOR_TYPE_HYBRID_STEPPER;
strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper");
} else if (strcmp((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop") == 0) {
state.servo.motor_type = MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP;
strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop");
} else if (strcmp((char*)&resp.value._string.value.elements[0], "BLDC") == 0) {
state.servo.motor_type = MOTOR_TYPE_BLDC;
strcpy((char*)&resp.value._string.value.elements[0], "BLDC");
} else {
state.servo.motor_type = MOTOR_TYPE_BLDC_OPENLOOP;
strcpy((char*)&resp.value._string.value.elements[0], "BLDC_openloop");
}
resp.value._string.value.count = strlen((char*)&resp.value._string.value.elements[0]);
registerWrite("reg.motor_type", &resp.value);
} else {
// If we're asked to write a new value, do it now: // If we're asked to write a new value, do it now:
if (!uavcan_register_Value_1_0_is_empty_(&req->value)) { if (!uavcan_register_Value_1_0_is_empty_(&req->value)) {
uavcan_register_Value_1_0_select_empty_(&resp.value); uavcan_register_Value_1_0_select_empty_(&resp.value);
@ -601,7 +650,7 @@ static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const ua
// The client will determine if the write was successful or not by comparing the request value with response. // The client will determine if the write was successful or not by comparing the request value with response.
uavcan_register_Value_1_0_select_empty_(&resp.value); uavcan_register_Value_1_0_select_empty_(&resp.value);
registerRead(&name[0], &resp.value); registerRead(&name[0], &resp.value);
}
// Currently, all registers we implement are mutable and persistent. This is an acceptable simplification, // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification,
// but more advanced implementations will need to differentiate between them to support advanced features like // but more advanced implementations will need to differentiate between them to support advanced features like
// exposing internal states via registers, perfcounters, etc. // exposing internal states via registers, perfcounters, etc.
@ -731,9 +780,7 @@ static void canardDeallocate(void *const user_reference, const size_t amount, vo
int udral_loop(state_sync_f servo_state_sync_f) { int udral_loop(state_sync_f servo_state_sync_f) {
srand(micros()); srand(micros());
struct UdralServoInternalState state{ state.user_state_sync_f = servo_state_sync_f;
.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. // 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: // For the background and related theory refer to the following resources:
// - https://github.com/OpenCyphal/libcanard/blob/master/README.md // - https://github.com/OpenCyphal/libcanard/blob/master/README.md
@ -782,6 +829,12 @@ int udral_loop(state_sync_f servo_state_sync_f) {
val._string.value.count = strlen((const char *)val._string.value.elements); val._string.value.count = strlen((const char *)val._string.value.elements);
registerWrite("reg.udral.service.actuator.servo", &val); registerWrite("reg.udral.service.actuator.servo", &val);
uavcan_register_Access_Request_1_0 req;
strcpy((char*)&req.name.name.elements[0], "reg.motor_type");
req.name.name.count = strlen((char*)&req.name.name.elements[0]);
uavcan_register_Value_1_0_select_empty_(&req.value);
processRequestRegisterAccess(&req);
// PID // PID
state.user_state_sync_f(&state.servo, hz); state.user_state_sync_f(&state.servo, hz);
@ -811,6 +864,22 @@ int udral_loop(state_sync_f servo_state_sync_f) {
} }
state.servo.pid_velocity = val.real32; state.servo.pid_velocity = val.real32;
uavcan_register_Value_1_0_select_real32_(&val);
val.real32 = state.servo.max_voltage;
registerRead("reg.max_voltage", &val);
Serial.printf("reg.max_voltage count %d %d\n\r", state.servo.max_voltage.value.count, val.real32.value.count);
if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.max_voltage.value.count) {
for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) {
state.servo.max_voltage.value.elements[i] = val.real32.value.elements[i];
}
uavcan_register_Value_1_0_select_real32_(&val);
val.real32 = state.servo.max_voltage;
registerWrite("reg.max_voltage", &val);
}
state.servo.max_voltage = val.real32;
// The description register is optional but recommended because it helps constructing/maintaining large networks.
// It simply keeps a human-readable description of the node that should be empty by default.
// Configure the transport by reading the appropriate standard registers. // Configure the transport by reading the appropriate standard registers.
uavcan_register_Value_1_0_select_natural16_(&val); uavcan_register_Value_1_0_select_natural16_(&val);
val.natural16.value.count = 1; val.natural16.value.count = 1;