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

Compare commits

base: nils:8170663b59a33680d071ee4e3ce61dbfc19a393e
Branches Tags
nils:main
nils:wip
nils:LarsBLDC
nils:hw_v5
nils:hw_v4
nils:hw_v3
nils:hw_v1
..
compare: nils:dee14242cd62bd8d29af5ceafd867b7a1bf04560
Branches Tags
nils:wip
nils:LarsBLDC
nils:main
nils:hw_v5
nils:hw_v4
nils:hw_v3
nils:hw_v1

2 Commits
8170663b59 ... dee14242cd

Author SHA1 Message Date
Nils Schulte
dee14242cd lars bldc parameter 2025-08-14 13:08:14 +02:00
Nils Schulte
8af848b01b wip fw 2025-08-13 21:31:18 +02:00
4 changed files with 115 additions and 110 deletions
Show Stats Expand all files Collapse all files

2
fw/src/DRV8323S.hpp
View File

@ -211,7 +211,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
/* Set 3PWM Mode */
drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
delay(1);
drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
// drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
delay(1);
drv8323s_set_sen_lvl(dev, DRV8323S_SEN_LVL_0_25V);
delay(1);

129
fw/src/main.cpp
View File

@ -1,8 +1,12 @@
// #include <Preferences.h>
/*
reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000.0, 100.0]
reg.pid_velocity: [0.05000000074505806, 5.5, 0.0, 0.009999999776482582, 10000.0, 6.0, 6.0]
*/
#include <cmath>
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/AS5047.h"
#include "esp32-hal.h"
#define NODE_NAME "N17BLDC"
@ -38,9 +42,6 @@ USBCDC usbserial;
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/MagneticSensorAS5047.h"
#include "encoders/calibrated/CalibratedSensor.h"
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
#include "DRV8323S.hpp"
#include "HybridStepperMotor.h"
@ -52,7 +53,7 @@ USBCDC usbserial;
#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
//* 20.8f/ 21033.75)
//#define ESPHWENC
#define MOTOR
#define FW_NO_WATCHDOG
@ -67,27 +68,35 @@ USBCDC usbserial;
#endif
const int voltage_lpf = 50;
const float max_voltage_limit = 6;
const float max_voltage_limit = 2.3;
// ESP32HWEncoder encoder =
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
#ifdef ESPHWENC
#include "encoders/esp32hwencoder/ESP32HWEncoder.h"
ESP32HWEncoder encoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
#else
Encoder encoder(ENC_A, ENC_B, 4096 / 4);
void doA1() { encoder.handleA(); }
void doB1() { encoder.handleB(); }
#endif
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]),
CalibratedSensor encoder_calibrated(abs_inc_sensor, 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
//HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
BLDCMotor motor = BLDCMotor(7, 0.7, 360, 0.15);
// BLDCMotor motor = BLDCMotor(7);
//BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4);
#endif
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
Wire.beginTransmission(addr);
Wire.write(reg); // MAN:0x2E
@ -130,10 +139,13 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
xSemaphoreTake(mutex, portMAX_DELAY);
if (!init_done) {
init_done = true;
state->pid_position.value.count = 3;
state->pid_position.value.count = 6;
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[2] = motor.P_angle.D;
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[5] = motor.P_angle.limit;
state->pid_velocity.value.count = 7;
state->pid_velocity.value.elements[0] = motor.PID_velocity.P;
state->pid_velocity.value.elements[1] = motor.PID_velocity.I;
@ -141,7 +153,7 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
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[5] = motor.PID_velocity.limit;
motor.motion_downsample = state->pid_velocity.value.elements[6] = 0;
state->pid_velocity.value.elements[6] = motor.motion_downsample;
}
servo_state.target_force = state->target_force;
servo_state.target_acceleration = state->target_acceleration;
@ -167,12 +179,11 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
xSemaphoreGive(mutex);
}
TaskHandle_t taskFocHandle;
void foc_task(void *parameter);
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();
@ -184,13 +195,6 @@ void setup() {
#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);
@ -211,7 +215,9 @@ void setup() {
// initialise magnetic sensor hardware
encoder.pullup = Pullup::USE_INTERN;
encoder.init();
#ifndef ESPHWENC
encoder.enableInterrupts(doA1, doB1);
#endif
encoder_absolute.init(&spi_dev);
abs_inc_sensor.init();
@ -223,12 +229,13 @@ void setup() {
digitalWrite(INLABC, 1); // enable
// driver.init(&SPI);
// status();
motor.linkSensor(&encoder_calibrated);
motor.linkSensor(&encoder_absolute);
//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_sensor_align = 1.5;
motor.voltage_limit = max_voltage_limit;
// motor.controller = MotionControlType::velocity_openloop;
// motor.controller = MotionControlType::velocity;
@ -250,8 +257,7 @@ void setup() {
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)) {
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);
@ -262,7 +268,7 @@ void setup() {
Serial.println("Skipping calibration");
}
if (!pref.isKey(zero_electric_angle_str)) {
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);
@ -287,17 +293,33 @@ void setup() {
);
// 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
digitalWrite(LED_PIN, 1); // disable
delay(100);
digitalWrite(LED_PIN, 0); // enable
delay(400);
digitalWrite(LED_PIN, 1); // disable
delay(100);
while (!init_done) {
delay(10);
}
bool armed = false;
xTaskCreatePinnedToCore(&foc_task, // Function name of the task
"foc", // Name of the task (e.g. for debugging)
65536, // Stack size (bytes)
NULL, // Parameter to pass
10, // Task priority
&taskFocHandle, // Assign task handle
1 // Run on the non-Arduino (1) core
);
}
int i = 0;
void loop() {}
void foc_task_loop() {
#ifdef MOTOR
bool armed = false;
/* adc reading */
drv8323s.focdriver->voltage_power_supply = vdrive_read;
const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
@ -314,23 +336,20 @@ void loop() {
target = servo_state.target_position;
motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
motor.voltage_limit = std::isnan(servo_state.target_force)
? max_voltage_limit
: std::min(max_voltage_limit, std::abs(servo_state.target_force));
motor.voltage_limit =
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)) {
if (motor.controller != MotionControlType::velocity) {
motor.controller = MotionControlType::velocity;
Serial.println("MotionControlType::velocity");
}
target = servo_state.target_velocity;
motor.voltage_limit = std::isnan(servo_state.target_force)
? max_voltage_limit
: std::min(max_voltage_limit, std::abs(servo_state.target_force));
motor.voltage_limit =
std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
} else {
motor.voltage_limit = std::isnan(servo_state.target_force)
? max_voltage_limit
: std::min(max_voltage_limit, std::abs(servo_state.target_force));
target = servo_state.target_force;
motor.voltage_limit =
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;
if (motor.controller != MotionControlType::torque) {
motor.controller = MotionControlType::torque;
Serial.printf("MotionControlType::torque %f\n", target);
@ -340,7 +359,7 @@ void loop() {
servo_state.controller_temperature = curr_pcb_temp_kelvin;
servo_state.motor_temperature = NAN;
float curr_angle = encoder_calibrated.getAngle();
float curr_angle = motor.sensor->getAngle();
servo_state.curr_position = curr_angle;
servo_state.curr_force = motor.voltage.d;
@ -351,18 +370,18 @@ void loop() {
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.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.limit =
servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
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.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.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.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.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
/* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */
motor.LPF_angle.Tf = 0; // default 0[=disabled]
// setting the limits
xSemaphoreGive(mutex);
@ -387,4 +406,10 @@ void loop() {
#else
encoder.update(); // optional: Update manually if not using loopfoc()
#endif
}
void foc_task(void *parameter) {
while (true) {
foc_task_loop();
}
}

92
fw/src/udral_servo.hpp
View File

@ -194,18 +194,16 @@ static CanardPortID getSubjectID(const SubjectRole role, const char *const port_
}
static void send(struct UdralServoInternalState *const state, const CanardMicrosecond tx_deadline_usec,
const CanardTransferMetadata *const metadata, const size_t payload_size,
const void *const payload_data, const CanardMicrosecond now_usec) {
const CanardTransferMetadata *const metadata, const size_t payload_size, const void *const payload_data,
const CanardMicrosecond now_usec) {
for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
const struct CanardPayload payload = {.size = payload_size, .data = payload_data};
(void)canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload, now_usec,
NULL);
(void)canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload, now_usec, NULL);
}
}
static void sendResponse(struct UdralServoInternalState *const state,
const struct CanardRxTransfer *const original_request_transfer, const size_t payload_size,
const void *const payload_data, const CanardMicrosecond now_usec) {
static void sendResponse(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const original_request_transfer,
const size_t payload_size, const void *const payload_data, const CanardMicrosecond now_usec) {
CanardTransferMetadata meta = original_request_transfer->metadata;
meta.transfer_kind = CanardTransferKindResponse;
send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload_data, now_usec);
@ -219,8 +217,7 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
// snprintf(buf, sizeof(buf),
Serial.printf("\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r", (double)state->servo.target_position,
(double)state->servo.target_velocity, (double)state->servo.target_acceleration,
(double)state->servo.target_force);
(double)state->servo.target_velocity, (double)state->servo.target_acceleration, (double)state->servo.target_force);
} else {
// fprintf(stderr, "\rDISARMED \r");
}
@ -231,15 +228,14 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
// Publish feedback if the subject is enabled and the node is non-anonymous.
if (!anonymous && (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX)) {
reg_udral_service_actuator_common_Feedback_0_1 msg = {0};
msg.heartbeat.readiness.value = state->servo.arming.armed ? reg_udral_service_common_Readiness_0_1_ENGAGED
: reg_udral_service_common_Readiness_0_1_STANDBY;
msg.heartbeat.readiness.value =
state->servo.arming.armed ? reg_udral_service_common_Readiness_0_1_ENGAGED : reg_udral_service_common_Readiness_0_1_STANDBY;
// If there are any hardware or configuration issues, report them here:
msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
// Serialize and publish the message:
uint8_t serialized[reg_udral_service_actuator_common_Feedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized);
const int8_t err =
reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size);
const int8_t err = reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size);
assert(err >= 0);
if (err >= 0) {
const CanardTransferMetadata transfer = {
@ -266,8 +262,7 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
// Serialize and publish the message:
uint8_t serialized[reg_udral_physics_dynamics_rotation_PlanarTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized);
const int8_t err =
reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
const int8_t err = reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
if (err >= 0) {
const CanardTransferMetadata transfer = {
.priority = CanardPriorityHigh,
@ -398,8 +393,8 @@ static void handle1HzLoop(struct UdralServoInternalState *const state, const Can
}
// Disarm automatically if the arming subject has not been updated in a while.
if (state->servo.arming.armed && ((now_usec - state->servo.arming.last_update_at) >
(uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) {
if (state->servo.arming.armed &&
((now_usec - state->servo.arming.last_update_at) > (uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) {
state->servo.arming.armed = false;
puts("Disarmed by timeout ");
}
@ -517,16 +512,14 @@ static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalS
reg.natural16.value.count = 1;
registerWrite("uavcan.node.id", &reg);
// We no longer need the subscriber, drop it to free up the resources (both memory and CPU time).
(void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage,
uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_);
(void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_);
} else {
Serial.println("Ignoring processMessagePlugAndPlayNodeIDAllocation response");
}
// Otherwise, ignore it: either it is a request from another node or it is a response to another node.
}
static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(
const uavcan_node_ExecuteCommand_Request_1_1 *req) {
static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(const uavcan_node_ExecuteCommand_Request_1_1 *req) {
uavcan_node_ExecuteCommand_Response_1_1 resp = {0};
switch (req->command) {
case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: {
@ -645,14 +638,13 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo() {
return resp;
}
static void processReceivedTransfer(struct UdralServoInternalState *const state,
const struct CanardRxTransfer *const transfer, const CanardMicrosecond now_usec) {
static void processReceivedTransfer(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const transfer,
const CanardMicrosecond now_usec) {
if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {
size_t size = transfer->payload.size;
if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) {
reg_udral_physics_dynamics_rotation_Planar_0_1 msg = {0};
if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >=
0) {
if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
processMessageServoSetpoint(state, &msg);
}
} else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) {
@ -765,11 +757,10 @@ int udral_loop(state_sync_f servo_state_sync_f) {
uavcan_register_Value_1_0_select_natural16_(&val);
val.natural16.value.count = 1;
val.natural16.value.elements[0] = UINT16_MAX; // This means undefined (anonymous), per Specification/libcanard.
registerRead("uavcan.node.id", &val); // The names of the standard registers are regulated by the Specification.
registerRead("uavcan.node.id", &val); // The names of the standard registers are regulated by the Specification.
assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1));
state.canard.node_id = (val.natural16.value.elements[0] > CANARD_NODE_ID_MAX)
? CANARD_NODE_ID_UNSET
: (CanardNodeID)val.natural16.value.elements[0];
state.canard.node_id =
(val.natural16.value.elements[0] > CANARD_NODE_ID_MAX) ? CANARD_NODE_ID_UNSET : (CanardNodeID)val.natural16.value.elements[0];
// 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.
@ -841,24 +832,18 @@ int udral_loop(state_sync_f servo_state_sync_f) {
// As follows from the Specification, the register group name prefix can be arbitrary; here we just use "servo".
// Publications:
state.port_id.pub.servo_feedback = // High-rate status information: all good or not, engaged or sleeping.
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.feedback",
reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_);
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.feedback", reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_);
state.port_id.pub.servo_status = // A low-rate high-level status overview: temperatures, fault flags, errors.
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.status",
reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_);
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.status", reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_);
state.port_id.pub.servo_power = // Electric power input measurements (voltage and current).
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.power",
reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_);
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.power", reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_);
state.port_id.pub.servo_dynamics = // Position/speed/acceleration/force feedback.
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics",
reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_);
getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics", reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_);
// Subscriptions:
state.port_id.sub.servo_setpoint = // This message actually commands the servo setpoint with the motion profile.
getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint",
reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_);
getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint", reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_);
state.port_id.sub.servo_readiness = // Arming subject: whether to act upon the setpoint or to stay idle.
getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness",
reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_);
getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness", reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_);
// Set up subject subscriptions and RPC-service servers.
// Message subscriptions:
@ -867,8 +852,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
static struct CanardRxSubscription rx;
const int8_t res = //
canardRxSubscribe(&state.canard, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&rx);
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
if (res < 0) {
return -res;
}
@ -907,8 +891,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
static struct CanardRxSubscription rx;
const int8_t res = //
canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&rx);
uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
if (res < 0) {
return -res;
}
@ -917,8 +900,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
static struct CanardRxSubscription rx;
const int8_t res = //
canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&rx);
uavcan_register_Access_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
if (res < 0) {
return -res;
}
@ -966,14 +948,13 @@ int udral_loop(state_sync_f servo_state_sync_f) {
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
// Otherwise just drop it and move on to the next one.
if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) {
const struct CanardFrame canard_frame = {
.extended_can_id = tqi->frame.extended_can_id,
.payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}};
const struct CanardFrame canard_frame = {.extended_can_id = tqi->frame.extended_can_id,
.payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}};
// const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); // Non-blocking write attempt.
const int16_t result = esp32twaicanPush(
&canard_frame,
0); // Non-blocking write attempt.
// digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
const int16_t result =
esp32twaicanPush(&canard_frame,
0); // Non-blocking write attempt.
// digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
// const int16_t result = 0;
if (result == 0) {
@ -1014,8 +995,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL);
if (canard_result > 0) {
processReceivedTransfer(&state, &transfer, now_usec);
state.canard.memory.deallocate(state.canard.memory.user_reference, transfer.payload.allocated_size,
transfer.payload.data);
state.canard.memory.deallocate(state.canard.memory.user_reference, transfer.payload.allocated_size, transfer.payload.data);
} else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) {
(void)0; // The frame did not complete a transfer so there is nothing to do.
// OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API.

2
pcb/stepper-closed-loop.kicad_prl
View File

@ -5,7 +5,7 @@
"auto_track_width": false,
"hidden_netclasses": [],
"hidden_nets": [],
"high_contrast_mode": 0,
"high_contrast_mode": 1,
"net_color_mode": 1,
"opacity": {
"images": 0.6,