save lut?
This commit is contained in:
parent
dc902aa9b3
commit
7dfe4041fb
@ -1,7 +1,7 @@
|
||||
#!/bin/bash
|
||||
sudo ip link set can0 type can bitrate 500000 && sudo ip link set can0 up
|
||||
|
||||
rm ~/allocation_table.db
|
||||
# rm ~/allocation_table.db
|
||||
|
||||
export UAVCAN__CAN__IFACE="socketcan:can0"
|
||||
export UAVCAN__NODE__ID=127
|
||||
|
||||
@ -216,6 +216,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
|
||||
delay(1);
|
||||
drv8323s_lock_spi(dev, false);
|
||||
delay(1);
|
||||
/* Set 3PWM Mode */
|
||||
drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
|
||||
delay(1);
|
||||
drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
|
||||
@ -227,7 +228,6 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
|
||||
dev->focdriver = new BLDCDriver3PWM(phA, phB, phC);
|
||||
dev->focdriver->init();
|
||||
|
||||
/* Set 3PWM Mode */
|
||||
}
|
||||
|
||||
void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i,
|
||||
@ -260,8 +260,6 @@ void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i,
|
||||
reg_value |= i << DRV8323S_IDRIVEN;
|
||||
uint16_t result;
|
||||
drv8323s_write_spi(dev, reg, reg_value, &result);
|
||||
|
||||
// TODO SPI call
|
||||
}
|
||||
|
||||
#endif /* _DRV8323S_H */
|
||||
|
||||
481
fw/src/main.cpp
481
fw/src/main.cpp
@ -1,5 +1,7 @@
|
||||
// #include <Preferences.h>
|
||||
|
||||
#include "esp32-hal.h"
|
||||
#include <cmath>
|
||||
#define NODE_NAME "N17BLDC"
|
||||
|
||||
#define VERSION_MAJOR 0
|
||||
@ -8,49 +10,37 @@
|
||||
|
||||
#include "Arduino.h"
|
||||
|
||||
#define SSIF_USBSERIAL
|
||||
#ifdef SSIF_USBSERIAL
|
||||
#define USE_USBSERIAL
|
||||
#ifdef USE_USBSERIAL
|
||||
#include "USB.h"
|
||||
USBCDC usbserial;
|
||||
#define SSYNCIF usbserial
|
||||
#ifdef Serial
|
||||
#undef Serial
|
||||
#endif
|
||||
#define Serial usbserial
|
||||
#else
|
||||
#define SSYNCIF Serial1
|
||||
#define Serial Serial1
|
||||
#endif
|
||||
// #define SSYNCIF Serial
|
||||
// #define Serial Serial
|
||||
|
||||
#include "canard.c"
|
||||
|
||||
#include "SPI.h"
|
||||
// #include "Wire.h"
|
||||
// #include "Wire.h"
|
||||
#include <SimpleFOC.h>
|
||||
// #include <INA3221.h>
|
||||
|
||||
#include "SimpleFOCDrivers.h"
|
||||
// #include "comms/streams/BinaryIO.h"
|
||||
// #include "encoders/as5600/MagneticSensorAS5600.h"
|
||||
#include "encoders/calibrated/CalibratedSensor.h"
|
||||
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
|
||||
|
||||
#include "DRV8323S.hpp"
|
||||
|
||||
#include "HybridStepperMotor.h"
|
||||
#include "esp32-hal-adc.h"
|
||||
#include "esp32-hal-gpio.h"
|
||||
// #include "motors/HybridStepperMotor/HybridStepperMotor.h"
|
||||
#include "HybridStepperMotor.h"
|
||||
#include <cmath>
|
||||
|
||||
#include "udral_servo.hpp"
|
||||
|
||||
// #include "encoders/sc60228/MagneticSensorSC60228.h"
|
||||
// #include "esp32-hal-adc.h"
|
||||
// #include "esp32-hal-gpio.h"
|
||||
// #include "esp32-hal-uart.h"
|
||||
// #include "esp32-hal.h"
|
||||
// #include "simplesync.hpp"
|
||||
|
||||
|
||||
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
|
||||
|
||||
#include "pin_def.h"
|
||||
@ -63,7 +53,6 @@ USBCDC usbserial;
|
||||
(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)
|
||||
|
||||
@ -81,28 +70,22 @@ USBCDC usbserial;
|
||||
#endif
|
||||
|
||||
const int voltage_lpf = 50;
|
||||
// BLDC motor & driver instance
|
||||
// BLDCMotor motor = BLDCMotor(pole pair number);
|
||||
|
||||
// #include "ICM42670P.h"
|
||||
// ICM42670 IMU(SPI, 0, 100000);
|
||||
|
||||
// ESP32HWEncoder encoder =
|
||||
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
|
||||
Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4);
|
||||
CalibratedSensor encoder_calibrated = CalibratedSensor(encoder);
|
||||
auto encoder_absolute = MagneticSensorSPI(AS5047_SPI, 18);
|
||||
Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4, 0, &encoder_absolute);
|
||||
float encoder_calibration_lut[90];
|
||||
CalibratedSensor encoder_calibrated =
|
||||
CalibratedSensor(encoder, sizeof(encoder_calibration_lut) /
|
||||
sizeof(encoder_calibration_lut[0]));
|
||||
// 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);
|
||||
// HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
|
||||
// DRV8316Driver3PWM driver = DRV8316Driver3PWM(INHA, INHB, INHC, SPI_DRV_SC);
|
||||
// BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC);
|
||||
#endif
|
||||
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
|
||||
|
||||
|
||||
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
|
||||
Wire.beginTransmission(addr);
|
||||
@ -138,76 +121,44 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
|
||||
}
|
||||
|
||||
SemaphoreHandle_t mutex;
|
||||
struct sync_params {};
|
||||
struct UdralServoState servo_state = {0};
|
||||
|
||||
TaskHandle_t taskFocHandle;
|
||||
void foc_task(void *parameter) {
|
||||
long last_comm = 0;
|
||||
while (true) {
|
||||
#ifdef MOTOR
|
||||
// drivers[0].voltage_power_supply = s_foc.vcc_voltage_mV / 1000.0f;
|
||||
// for (int i = 1; i < NUM_MOTORS; i += 1)
|
||||
// drivers[i].voltage_power_supply = drivers[0].voltage_power_supply;
|
||||
#endif
|
||||
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);
|
||||
}
|
||||
|
||||
#ifdef MOTOR
|
||||
// motors.target = s_foc.vt[i] / anglefactor;
|
||||
// motors.loopFOC();
|
||||
// motors.move();
|
||||
#else
|
||||
// sensors.update();
|
||||
#endif
|
||||
vTaskDelay(1);
|
||||
}
|
||||
}
|
||||
TaskHandle_t taskCommHandle;
|
||||
void comm_task(void *parameter) {
|
||||
while (true) {
|
||||
long now = millis();
|
||||
|
||||
vTaskDelay(1);
|
||||
}
|
||||
}
|
||||
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();
|
||||
delay(1000);
|
||||
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(100);
|
||||
digitalWrite(LED_PIN, 1); // enable
|
||||
delay(2000);
|
||||
//Serial.println("Start");
|
||||
|
||||
//digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
|
||||
return;
|
||||
|
||||
for (int i = 0; i < 1; i++) {
|
||||
digitalWrite(LED_PIN, 0); // enable
|
||||
digitalWrite(LED_PIN, 1); // disable
|
||||
delay(1000);
|
||||
digitalWrite(LED_PIN, 1); // enable
|
||||
delay(1000);
|
||||
}
|
||||
mutex = xSemaphoreCreateMutex();
|
||||
#ifdef SSIF_USBSERIAL
|
||||
SSYNCIF.begin();
|
||||
USB.begin();
|
||||
SSYNCIF.setDebugOutput(
|
||||
true); // sends all log_e(), log_i() messages to USB HW CDC
|
||||
SSYNCIF.setTxTimeoutMs(0);
|
||||
#else
|
||||
SSYNCIF.begin(460800, SERIAL_8N1, 44, 43);
|
||||
#endif
|
||||
// prefs.begin("foc");
|
||||
// SSYNCIF.print(__cplusplus);
|
||||
|
||||
// Wire.setTimeOut(10);
|
||||
// Wire.begin(I2C_SDA, I2C_SCL, 1000000);
|
||||
Serial.println("Start");
|
||||
|
||||
pinMode(SPI_DRV_SC, OUTPUT);
|
||||
|
||||
@ -222,17 +173,15 @@ void setup() {
|
||||
pinMode(CAL_PIN, OUTPUT);
|
||||
digitalWrite(CAL_PIN, 0); // enable
|
||||
|
||||
// write_i2c(0x36,0x07, 0b00000011);
|
||||
// write_i2c(0x36, 0x09, 0xFF);
|
||||
// encoder.init(&Wire);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
encoder.pullup = Pullup::USE_INTERN;
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA1, doB1);
|
||||
pinMode(SPI_MISO, INPUT_PULLUP);
|
||||
|
||||
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
|
||||
#ifdef MOTOR
|
||||
SimpleFOCDebug::enable(&SSYNCIF);
|
||||
SimpleFOCDebug::enable(&Serial);
|
||||
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &SPI);
|
||||
digitalWrite(INLABC, 1); // enable
|
||||
// driver.init(&SPI);
|
||||
@ -242,278 +191,120 @@ void setup() {
|
||||
// motor.foc_modulation = FOCModulationType::SinePWM;
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
motor.voltage_sensor_align = 3;
|
||||
motor.voltage_limit = 5;
|
||||
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(SSYNCIF);
|
||||
// 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);
|
||||
|
||||
// // set voltage to run calibration
|
||||
encoder_calibrated.voltage_calibration = 3;
|
||||
// // Running calibration
|
||||
encoder_calibrated.calibrate(motor);
|
||||
static Preferences pref;
|
||||
pref.begin("simpleFOC");
|
||||
|
||||
// //Serial.println("Calibrating Sensor Done.");
|
||||
// // Linking sensor to motor object
|
||||
// motor.linkSensor(&encoder_calibrated);
|
||||
// set voltage to run calibration
|
||||
encoder_calibrated.voltage_calibration = 3;
|
||||
// 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, NULL, 0.0, Direction::UNKNOWN,
|
||||
settle_time_ms);
|
||||
pref.putBytes(encoder_calibration_lut_str,
|
||||
&encoder_calibrated.calibrationLut[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);
|
||||
}
|
||||
|
||||
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);
|
||||
// sensor.init(&SPI);
|
||||
// motors[i].linkSensor(&sensors[i]);
|
||||
digitalWrite(INLABC, 0); // enable
|
||||
#endif
|
||||
|
||||
// delay(5000);
|
||||
// SSYNCIF.println("IMU.begin()");
|
||||
// int ret = IMU.begin();
|
||||
// if (ret != 0) {
|
||||
// SSYNCIF.print("ICM42670 initialization failed: ");
|
||||
// SSYNCIF.println(ret);
|
||||
// while (1) {
|
||||
// SSYNCIF.print("ICM42670 initialization failed: ");
|
||||
// SSYNCIF.println(ret);
|
||||
// delay(100);
|
||||
// }
|
||||
// }
|
||||
// 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);
|
||||
// ina.getCurrent(INA3221_CH1) * 1000; ina.getVoltage(INA3221_CH1);
|
||||
// ina.getCurrent(INA3221_CH2) * 1000; ina.getVoltage(INA3221_CH2);
|
||||
// ina.getCurrent(INA3221_CH3) * 1000; ina.getVoltage(INA3221_CH3);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
|
||||
#ifdef MOTOR
|
||||
// pinMode(nRESET, OUTPUT);
|
||||
// digitalWrite(nRESET, 1);
|
||||
#endif
|
||||
// power supply voltage [V]
|
||||
// s_exchange.vcc_voltage_mV = GET_VCC;
|
||||
// delay(100);
|
||||
// s_exchange.vcc_voltage_mV = GET_VCC;
|
||||
// drivers[0].voltage_power_supply = s_exchange.vcc_voltage_mV / 1000.0f;
|
||||
// for (int i = 1; i < NUM_MOTORS; i += 1)
|
||||
// drivers[i].voltage_power_supply = drivers[0].voltage_power_supply;
|
||||
// s_foc = s_exchange;
|
||||
|
||||
// for (int i = 0; i < NUM_MOTORS; i += 1) {
|
||||
#ifdef MOTOR
|
||||
// drivers[i].init();
|
||||
// link driver
|
||||
// motors[i].linkDriver(&drivers[i]);
|
||||
// motors[i].voltage_sensor_align = 5;
|
||||
|
||||
// set control loop type to be used
|
||||
// motors[i].torque_controller = TorqueControlType::voltage;
|
||||
// motors[i].controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration based on the control type
|
||||
// motors[i].PID_velocity.P = 0.35;
|
||||
// motors[i].PID_velocity.I = 8;
|
||||
// motors[i].PID_velocity.D = 0;
|
||||
|
||||
// default voltage_power_supply
|
||||
// motors[i].voltage_limit = 3.4 * 3;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
// motors[i].LPF_velocity.Tf = 0.01;
|
||||
#endif
|
||||
// }
|
||||
#ifdef MOTOR
|
||||
// drivers[2].init();
|
||||
// drivers[2].voltage_limit = 3.4 * 3;
|
||||
// drivers[2].setPwm(s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV);
|
||||
|
||||
// pinMode(4, OUTPUT);
|
||||
// digitalWrite(4, 1);
|
||||
// pinMode(5, OUTPUT);
|
||||
// digitalWrite(5, 1);
|
||||
// pinMode(6, OUTPUT);
|
||||
// digitalWrite(6, 1);
|
||||
#endif
|
||||
// angle loop controller
|
||||
// motor.P_angle.P = 20;
|
||||
// angle loop velocity limit
|
||||
// motor.velocity_limit = 150;
|
||||
|
||||
// initialise motor
|
||||
// align encoder and start FOC
|
||||
#ifdef MOTOR
|
||||
// for (int i = 0; i < NUM_MOTORS; i += 1) {
|
||||
// motors[i].init();
|
||||
// // std::string pref_zero_key = "m" + std::to_string(i) + ".zero";
|
||||
// // std::string pref_dir_key = "m" + std::to_string(i) + ".dir";
|
||||
// // if (prefs.isKey(pref_zero_key.c_str()) &&
|
||||
// // prefs.isKey(pref_dir_key.c_str())) {
|
||||
// // motors[i].zero_electric_angle =
|
||||
// // prefs.get(pref_zero_key.c_str()); motors[i].sensor_direction
|
||||
// =
|
||||
// // (Direction)prefs.getInt(pref_dir_key.c_str());
|
||||
// // }
|
||||
// motors[i].initFOC();
|
||||
// // if (!prefs.isKey(pref_zero_key.c_str()) ||
|
||||
// // !prefs.isKey(pref_dir_key.c_str())) {
|
||||
// // prefs.clear();
|
||||
// // prefs.putFloat(pref_zero_key.c_str(),
|
||||
// 7/
|
||||
// motors[i].zero_electric_angle);
|
||||
// // prefs.putInt(pref_dir_key.c_str(),
|
||||
// (int)motors[i].sensor_direction);
|
||||
// // }
|
||||
// }
|
||||
#endif
|
||||
// prefs.end();
|
||||
// set the inital target value
|
||||
#ifdef MOTOR
|
||||
// for (int i = 0; i < NUM_MOTORS; i += 1)
|
||||
// motors[i].target = 0;
|
||||
#endif
|
||||
|
||||
// xTaskCreatePinnedToCore(&comm_task, // Function name of the task
|
||||
// "Comm", // Name of the task (e.g. for
|
||||
// debugging) 4096, // Stack size (bytes) NULL,
|
||||
// // Parameter to pass 1, // Task priority
|
||||
// &taskCommHandle, // Assign task handle
|
||||
// 1 // Run on the non-Arduino (1) core
|
||||
// );
|
||||
// xTaskCreatePinnedToCore(&foc_task, // Function name of the task
|
||||
// "foc", // Name of the task (e.g. for debugging)
|
||||
// 2048, // Stack size (bytes)
|
||||
// NULL, // Parameter to pass
|
||||
// 1, // Task priority
|
||||
// &taskFocHandle, // Assign task handle
|
||||
// 0 // Run on the non-Arduino (1) core
|
||||
// );
|
||||
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
|
||||
1, // 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() {
|
||||
mainloop();
|
||||
return;
|
||||
#ifdef MOTOR
|
||||
// drv8323s.focdriver->voltage_power_supply = vdrive_read;
|
||||
// digitalWrite(LED_PIN, 0); // enable
|
||||
motor.loopFOC();
|
||||
bool led = false;
|
||||
|
||||
// motor.move(3);
|
||||
// digitalWrite(LED_PIN, 1); // enable
|
||||
motor.move();
|
||||
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);
|
||||
}
|
||||
|
||||
servo_state.controller_temperature = temp_pcb + 273.15f; /* 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.vcc_volts = drv8323s.focdriver->voltage_power_supply;
|
||||
servo_state.current = NAN;
|
||||
xSemaphoreTake(mutex, portMAX_DELAY);
|
||||
|
||||
digitalWrite(LED_PIN, led ? LOW : HIGH);
|
||||
drv8323s.focdriver->voltage_power_supply = vdrive_read;
|
||||
|
||||
motor.loopFOC();
|
||||
#else
|
||||
encoder.update(); // optional: Update manually if not using loopfoc()
|
||||
#endif
|
||||
if(i++ % 100 == 0) {
|
||||
static float angle = 0;
|
||||
static float old_angle = 0;
|
||||
angle = encoder.getAngle();
|
||||
|
||||
SSYNCIF.print("enc: ");
|
||||
SSYNCIF.print(angle * 180.0 / 3.1415);
|
||||
SSYNCIF.print("mec: ");
|
||||
SSYNCIF.print(encoder.getMechanicalAngle() * 180.0 / 3.1415);
|
||||
SSYNCIF.print("\tcal: ");
|
||||
SSYNCIF.print(encoder_calibrated.getAngle() * 180.0 / 3.1415);
|
||||
SSYNCIF.println();
|
||||
|
||||
}
|
||||
|
||||
// inv_imu_sensor_event_t imu_event;
|
||||
|
||||
// // Get last event
|
||||
// IMU.getDataFromRegisters(imu_event);
|
||||
|
||||
// // Format data for Serial Plotter
|
||||
// SSYNCIF.print(imu_event.accel[0]);
|
||||
// SSYNCIF.print(", ");
|
||||
// SSYNCIF.print(imu_event.accel[1]);
|
||||
// SSYNCIF.print(", ");
|
||||
// SSYNCIF.print(imu_event.accel[2]);
|
||||
// SSYNCIF.print(", ");
|
||||
// SSYNCIF.print(imu_event.gyro[0]);
|
||||
// SSYNCIF.print(", ");
|
||||
// SSYNCIF.print(imu_event.gyro[1]);
|
||||
// SSYNCIF.print(", ");
|
||||
// SSYNCIF.print(imu_event.gyro[2]);
|
||||
// SSYNCIF.print(", ");
|
||||
// SSYNCIF.print(imu_event.temperature / 128 + 25);
|
||||
// SSYNCIF.println();
|
||||
|
||||
// // Run @ ODR 100Hz
|
||||
// delay(100);
|
||||
// // SSYNCIF.print("V: ");
|
||||
// // SSYNCIF.println(vdrive_read);
|
||||
#ifdef MOTOR
|
||||
// SSYNCIF.print(" ");
|
||||
// uint16_t result = 0;
|
||||
// SSYNCIF.print(" ");
|
||||
// result = 0;
|
||||
// delay(1);
|
||||
// drv8323s_read_spi(&drv8323s, 0x01, &result);
|
||||
// SSYNCIF.print(result, HEX);
|
||||
// SSYNCIF.print(" ");
|
||||
// result = 0;
|
||||
// delay(1);
|
||||
// drv8323s_read_spi(&drv8323s, 0x02, &result);
|
||||
// SSYNCIF.print(result, HEX);
|
||||
// result = 0;
|
||||
// SSYNCIF.print(" ");
|
||||
// drv8323s_read_spi(&drv8323s, 0x03, &result);
|
||||
// SSYNCIF.print(result, HEX);
|
||||
// SSYNCIF.print(" ");
|
||||
// result = 0;
|
||||
// delay(1);
|
||||
// drv8323s_read_spi(&drv8323s, 0x04, &result);
|
||||
// SSYNCIF.print(result, HEX);
|
||||
// SSYNCIF.print(" ");
|
||||
// result = 0;
|
||||
// delay(1);
|
||||
// drv8323s_read_spi(&drv8323s, 0x05, &result);
|
||||
// SSYNCIF.print(result, HEX);
|
||||
// SSYNCIF.print(" ");
|
||||
// result = 0;
|
||||
// delay(1);
|
||||
// drv8323s_read_spi(&drv8323s, 0x06, &result);
|
||||
// SSYNCIF.print(result, HEX);
|
||||
#endif
|
||||
// SSYNCIF.print(" ");
|
||||
// SSYNCIF.print(analogRead(TERMISTOR_PCB));
|
||||
// SSYNCIF.print(" ");
|
||||
// SSYNCIF.print(analogRead(SOA));
|
||||
// SSYNCIF.print(" ");
|
||||
// SSYNCIF.print(analogRead(SOB));
|
||||
// SSYNCIF.print(" ");
|
||||
// SSYNCIF.print(analogRead(SOC));
|
||||
// SSYNCIF.print(" ");
|
||||
// static float t = temp_pcb;
|
||||
// t = t * 0.95 + 0.05 * temp_pcb;
|
||||
// SSYNCIF.print(t);
|
||||
// SSYNCIF.print(" ");
|
||||
// SSYNCIF.println(vdrive_read);
|
||||
// analogWrite(INHA, 255/3);
|
||||
// analogWrite(INHB, 255/2);
|
||||
// analogWrite(INHC, 255*2/3);
|
||||
// static int i = 0;
|
||||
// if (i++ % 100 == 0)
|
||||
// status();
|
||||
}
|
||||
|
||||
// int i = 0;
|
||||
// void loop() {
|
||||
|
||||
// //encoder.update(); // optional: Update manually if not using loopfoc()
|
||||
// //
|
||||
// // Access the sensor value
|
||||
// SSYNCIF.println(encoder.getAngle());
|
||||
// delay(10);
|
||||
// }
|
||||
// esp_task_wdt_reset();
|
||||
|
||||
@ -1,14 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
#include "esp_mac.h"
|
||||
|
||||
#ifdef RAM_PREFERENCES
|
||||
#include "hash_preferences.hpp"
|
||||
#else
|
||||
#include <Preferences.h>
|
||||
#endif
|
||||
|
||||
#include <uavcan/node/GetInfo_1_0.h>
|
||||
#include <uavcan/_register/Value_1_0.h>
|
||||
#include <uavcan/_register/Name_1_0.h>
|
||||
#include <Arduino.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
static Preferences preferences;
|
||||
static bool preferences_open = false;
|
||||
@ -16,8 +23,7 @@ static bool preferences_open = false;
|
||||
#define REGISTER_LIST_KEY "__reglist"
|
||||
#define REGISTER_LIST_DELIM "|"
|
||||
|
||||
String
|
||||
hash(const char *str)
|
||||
String hash(const char *str)
|
||||
{
|
||||
unsigned long hash = 5381;
|
||||
int c;
|
||||
@ -112,9 +118,35 @@ bool registerAssign(uavcan_register_Value_1_0 *const dst, const uavcan_register_
|
||||
return false;
|
||||
}
|
||||
|
||||
#include <nvs_flash.h>
|
||||
|
||||
void registerDoFactoryReset(void)
|
||||
{
|
||||
open_preferences_if_needed();
|
||||
preferences.clear();
|
||||
close_preferences_if_needed();
|
||||
nvs_flash_erase(); // erase the NVS partition and...
|
||||
nvs_flash_init(); // initialize the NVS partition.
|
||||
esp_restart();
|
||||
}
|
||||
|
||||
// Returns the 128-bit unique-ID of the local node. This value is used in uavcan.node.GetInfo.Response and during the
|
||||
// plug-and-play node-ID allocation by uavcan.pnp.NodeIDAllocationData. The function is infallible.
|
||||
void getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_])
|
||||
{
|
||||
// A real hardware node would read its unique-ID from some hardware-specific source (typically stored in ROM).
|
||||
// This example is a software-only node, so we store the unique-ID in a (read-only) register instead.
|
||||
uavcan_register_Value_1_0 value = {0};
|
||||
uavcan_register_Value_1_0_select_unstructured_(&value);
|
||||
if(esp_base_mac_addr_get(&value.unstructured.value.elements[0]) == ESP_OK) {
|
||||
value.unstructured.value.count = 8;
|
||||
}
|
||||
for (uint8_t i = value.unstructured.value.count; i < uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_; i += 1)
|
||||
{
|
||||
value.unstructured.value.elements[value.unstructured.value.count++] = (uint8_t)rand(); // NOLINT
|
||||
}
|
||||
// Populate the default; it is only used at the first run if there is no such register.
|
||||
|
||||
registerRead("uavcan.node.unique_id", &value);
|
||||
assert(uavcan_register_Value_1_0_is_unstructured_(&value) &&
|
||||
value.unstructured.value.count == uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
|
||||
memcpy(&out[0], &value.unstructured.value, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
|
||||
}
|
||||
|
||||
|
||||
@ -13,8 +13,8 @@
|
||||
/// Copyright (C) 2021 OpenCyphal <maintainers@opencyphal.org>
|
||||
/// Author: Pavel Kirienko <pavel@opencyphal.org>
|
||||
|
||||
#include "esp32-hal.h"
|
||||
#define NUNAVUT_ASSERT(x) assert(x)
|
||||
#include "esp32-hal.h"
|
||||
|
||||
#include "pin_def.h"
|
||||
|
||||
@ -32,12 +32,14 @@
|
||||
#include <uavcan/_register/Access_1_0.h>
|
||||
#include <uavcan/_register/List_1_0.h>
|
||||
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
|
||||
#include <uavcan/primitive/scalar/Real32_1_0.h>
|
||||
|
||||
#include <reg/udral/service/common/Readiness_0_1.h>
|
||||
#include <reg/udral/service/actuator/common/_0_1.h>
|
||||
#include <reg/udral/service/actuator/common/Feedback_0_1.h>
|
||||
#include <reg/udral/service/actuator/common/Status_0_1.h>
|
||||
#include <reg/udral/physics/dynamics/translation/LinearTs_0_1.h>
|
||||
#include "reg/udral/physics/dynamics/rotation/Planar_0_1.h"
|
||||
#include <reg/udral/physics/dynamics/rotation/PlanarTs_0_1.h>
|
||||
#include <reg/udral/physics/electricity/PowerTs_0_1.h>
|
||||
|
||||
#include <stdio.h>
|
||||
@ -47,7 +49,6 @@
|
||||
#include <unistd.h>
|
||||
|
||||
#include "USB.h"
|
||||
|
||||
extern USBCDC usbserial;
|
||||
|
||||
#define KILO 1000L
|
||||
@ -57,9 +58,51 @@ extern USBCDC usbserial;
|
||||
/// For CAN FD the queue can be smaller.
|
||||
#define CAN_TX_QUEUE_CAPACITY 100
|
||||
|
||||
/// We keep the state of the application here. Feel free to use static variables instead if desired.
|
||||
typedef struct State
|
||||
{
|
||||
/// We keep the state of the application here. Feel free to use static variables
|
||||
/// instead if desired.
|
||||
|
||||
struct UdralServoState {
|
||||
/// Whether the servo is supposed to actuate the load or to stay idle (safe
|
||||
/// low-power mode).
|
||||
struct {
|
||||
struct {
|
||||
bool armed;
|
||||
CanardMicrosecond last_update_at;
|
||||
} arming;
|
||||
|
||||
float vcc_volts;
|
||||
float current;
|
||||
|
||||
float curr_position; ///< [meter] [radian]
|
||||
float curr_velocity; ///< [meter/second] [radian/second]
|
||||
float curr_acceleration; ///< [(meter/second)^2] [(radian/second)^2]
|
||||
float curr_force; ///< [newton] [netwon*meter]
|
||||
|
||||
float motor_temperature;
|
||||
float controller_temperature;
|
||||
};
|
||||
|
||||
struct {
|
||||
/// Setpoint & motion profile (unsupported constraints are to be ignored).
|
||||
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
|
||||
/// As described in the linked documentation, there are two kinds of servos
|
||||
/// supported: linear and rotary. Units per-kind are: LINEAR ROTARY
|
||||
float target_position; ///< [meter] [radian]
|
||||
float target_velocity; ///< [meter/second] [radian/second]
|
||||
float target_acceleration; ///< [(meter/second)^2] [(radian/second)^2]
|
||||
float target_force; ///< [newton] [netwon*meter]
|
||||
|
||||
struct {
|
||||
uavcan_primitive_scalar_Real32_1_0 p;
|
||||
uavcan_primitive_scalar_Real32_1_0 i;
|
||||
uavcan_primitive_scalar_Real32_1_0 d;
|
||||
} pid;
|
||||
};
|
||||
};
|
||||
|
||||
typedef void (*state_sync_f)(UdralServoState *const);
|
||||
|
||||
struct UdralServoInternalState {
|
||||
CanardMicrosecond started_at;
|
||||
|
||||
O1HeapInstance *heap;
|
||||
@ -67,50 +110,33 @@ typedef struct State
|
||||
struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR];
|
||||
|
||||
/// The state of the business logic.
|
||||
struct
|
||||
{
|
||||
/// Whether the servo is supposed to actuate the load or to stay idle (safe low-power mode).
|
||||
struct
|
||||
{
|
||||
bool armed;
|
||||
CanardMicrosecond last_update_at;
|
||||
} arming;
|
||||
UdralServoState servo;
|
||||
|
||||
/// Setpoint & motion profile (unsupported constraints are to be ignored).
|
||||
/// These values are read from the registers at startup. You can also
|
||||
/// implement hot reloading if desired. The subjects of the servo network
|
||||
/// service are defined in the UDRAL data type definitions here:
|
||||
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
|
||||
/// As described in the linked documentation, there are two kinds of servos supported: linear and rotary.
|
||||
/// Units per-kind are: LINEAR ROTARY
|
||||
float position; ///< [meter] [radian]
|
||||
float velocity; ///< [meter/second] [radian/second]
|
||||
float acceleration; ///< [(meter/second)^2] [(radian/second)^2]
|
||||
float force; ///< [newton] [netwon*meter]
|
||||
} servo;
|
||||
|
||||
/// These values are read from the registers at startup. You can also implement hot reloading if desired.
|
||||
/// The subjects of the servo network service are defined in the UDRAL data type definitions here:
|
||||
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
|
||||
struct
|
||||
{
|
||||
struct
|
||||
{
|
||||
CanardPortID servo_feedback; //< reg.udral.service.actuator.common.Feedback
|
||||
struct {
|
||||
struct {
|
||||
CanardPortID
|
||||
servo_feedback; //< reg.udral.service.actuator.common.Feedback
|
||||
CanardPortID servo_status; //< reg.udral.service.actuator.common.Status
|
||||
CanardPortID servo_power; //< reg.udral.physics.electricity.PowerTs
|
||||
CanardPortID servo_dynamics; //< (timestamped dynamics)
|
||||
} pub;
|
||||
struct
|
||||
{
|
||||
struct {
|
||||
CanardPortID servo_setpoint; //< (non-timestamped dynamics)
|
||||
CanardPortID servo_readiness; //< reg.udral.service.common.Readiness
|
||||
} sub;
|
||||
} port_id;
|
||||
|
||||
/// A transfer-ID is an integer that is incremented whenever a new message is published on a given subject.
|
||||
/// It is used by the protocol for deduplication, message loss detection, and other critical things.
|
||||
/// For CAN, each value can be of type uint8_t, but we use larger types for genericity and for statistical purposes,
|
||||
/// as large values naturally contain the number of times each subject was published to.
|
||||
struct
|
||||
{
|
||||
/// A transfer-ID is an integer that is incremented whenever a new message is
|
||||
/// published on a given subject. It is used by the protocol for
|
||||
/// deduplication, message loss detection, and other critical things. For CAN,
|
||||
/// each value can be of type uint8_t, but we use larger types for genericity
|
||||
/// and for statistical purposes, as large values naturally contain the number
|
||||
/// of times each subject was published to.
|
||||
struct {
|
||||
uint64_t uavcan_node_heartbeat;
|
||||
uint64_t uavcan_node_port_list;
|
||||
uint64_t uavcan_pnp_allocation;
|
||||
@ -118,7 +144,7 @@ typedef struct State
|
||||
uint64_t servo_fast_loop;
|
||||
uint64_t servo_1Hz_loop;
|
||||
} next_transfer_id;
|
||||
} State;
|
||||
};
|
||||
|
||||
/// This flag is raised when the node is requested to restart.
|
||||
static volatile bool g_restart_required = false;
|
||||
@ -134,26 +160,6 @@ static CanardMicrosecond getMonotonicMicroseconds()
|
||||
return micros();
|
||||
}
|
||||
|
||||
// Returns the 128-bit unique-ID of the local node. This value is used in uavcan.node.GetInfo.Response and during the
|
||||
// plug-and-play node-ID allocation by uavcan.pnp.NodeIDAllocationData. The function is infallible.
|
||||
static void getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_])
|
||||
{
|
||||
// A real hardware node would read its unique-ID from some hardware-specific source (typically stored in ROM).
|
||||
// This example is a software-only node, so we store the unique-ID in a (read-only) register instead.
|
||||
uavcan_register_Value_1_0 value = {0};
|
||||
uavcan_register_Value_1_0_select_unstructured_(&value);
|
||||
// Populate the default; it is only used at the first run if there is no such register.
|
||||
for (uint8_t i = 0; i < uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_; i++)
|
||||
{
|
||||
value.unstructured.value.elements[value.unstructured.value.count++] = (uint8_t)rand(); // NOLINT
|
||||
}
|
||||
registerRead("uavcan.node.unique_id", &value);
|
||||
assert(uavcan_register_Value_1_0_is_unstructured_(&value) &&
|
||||
value.unstructured.value.count == uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
|
||||
memcpy(&out[0], &value.unstructured.value, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
|
||||
memset(out, 0xAB, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
|
||||
}
|
||||
|
||||
typedef enum SubjectRole
|
||||
{
|
||||
SUBJECT_ROLE_PUBLISHER,
|
||||
@ -195,7 +201,7 @@ static CanardPortID getSubjectID(const SubjectRole role, const char *const port_
|
||||
return result;
|
||||
}
|
||||
|
||||
static void send(State *const state,
|
||||
static void send(struct UdralServoInternalState *const state,
|
||||
const CanardMicrosecond tx_deadline_usec,
|
||||
const CanardTransferMetadata *const metadata,
|
||||
const size_t payload_size,
|
||||
@ -214,7 +220,7 @@ static void send(State *const state,
|
||||
}
|
||||
}
|
||||
|
||||
static void sendResponse(State *const state,
|
||||
static void sendResponse(struct UdralServoInternalState *const state,
|
||||
const struct CanardRxTransfer *const original_request_transfer,
|
||||
const size_t payload_size,
|
||||
const void *const payload_data,
|
||||
@ -226,17 +232,17 @@ static void sendResponse(State *const state,
|
||||
}
|
||||
|
||||
/// Invoked at the rate of the fastest loop.
|
||||
static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
static void handleFastLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec)
|
||||
{
|
||||
// Apply control inputs if armed.
|
||||
if (state->servo.arming.armed)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r",
|
||||
(double)state->servo.position,
|
||||
(double)state->servo.velocity,
|
||||
(double)state->servo.acceleration,
|
||||
(double)state->servo.force);
|
||||
(double)state->servo.target_position,
|
||||
(double)state->servo.target_velocity,
|
||||
(double)state->servo.target_acceleration,
|
||||
(double)state->servo.target_force);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -276,21 +282,19 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
// Publish dynamics if the subject is enabled and the node is non-anonymous.
|
||||
if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX))
|
||||
{
|
||||
reg_udral_physics_dynamics_translation_LinearTs_0_1 msg = {0};
|
||||
reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg = {0};
|
||||
// Our node does not synchronize its clock with the network, so we cannot timestamp our publications:
|
||||
msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;
|
||||
// A real application would source these values from the hardware; we republish the setpoint for demo purposes.
|
||||
// TODO populate real values:
|
||||
msg.value.kinematics.position.meter = state->servo.position;
|
||||
msg.value.kinematics.velocity.meter_per_second = state->servo.velocity;
|
||||
msg.value.kinematics.acceleration.meter_per_second_per_second = state->servo.acceleration;
|
||||
msg.value.force.newton = state->servo.force;
|
||||
|
||||
msg.value.kinematics.angular_position.radian = state->servo.curr_position;
|
||||
msg.value.kinematics.angular_velocity.radian_per_second = state->servo.curr_velocity;
|
||||
msg.value.kinematics.angular_acceleration.radian_per_second_per_second = state->servo.curr_acceleration;
|
||||
msg.value._torque.newton_meter = state->servo.curr_force;
|
||||
// Serialize and publish the message:
|
||||
uint8_t serialized[reg_udral_physics_dynamics_translation_LinearTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
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_translation_LinearTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
|
||||
assert(err >= 0);
|
||||
reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
|
||||
if (err >= 0)
|
||||
{
|
||||
const CanardTransferMetadata transfer = {
|
||||
@ -311,13 +315,12 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
// Our node does not synchronize its clock with the network, so we cannot timestamp our publications:
|
||||
msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;
|
||||
// TODO populate real values:
|
||||
msg.value.current.ampere = 20.315F;
|
||||
msg.value.voltage.volt = 51.3F;
|
||||
msg.value.current.ampere = state->servo.current;
|
||||
msg.value.voltage.volt = state->servo.vcc_volts;
|
||||
// Serialize and publish the message:
|
||||
uint8_t serialized[reg_udral_physics_dynamics_translation_LinearTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
uint8_t serialized[reg_udral_physics_electricity_PowerTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
size_t serialized_size = sizeof(serialized);
|
||||
const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
|
||||
assert(err >= 0);
|
||||
if (err >= 0)
|
||||
{
|
||||
const CanardTransferMetadata transfer = {
|
||||
@ -333,7 +336,7 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
}
|
||||
|
||||
/// Invoked every second.
|
||||
static void handle1HzLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
static void handle1HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec)
|
||||
{
|
||||
const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX;
|
||||
// Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat.
|
||||
@ -422,6 +425,8 @@ static void handle1HzLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
// Publish the servo status -- this is a low-rate message with low-severity diagnostics.
|
||||
reg_udral_service_actuator_common_Status_0_1 msg = {0};
|
||||
// TODO: POPULATE THE MESSAGE: temperature, errors, etc.
|
||||
msg.motor_temperature.kelvin = state->servo.motor_temperature;
|
||||
msg.controller_temperature.kelvin = state->servo.controller_temperature;
|
||||
uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
size_t serialized_size = sizeof(serialized);
|
||||
const int8_t err =
|
||||
@ -479,7 +484,7 @@ static void fillServers(const struct CanardTreeNode *const tree, // NOLINT(misc-
|
||||
}
|
||||
|
||||
/// Invoked every 10 seconds.
|
||||
static void handle01HzLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
static void handle01HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec)
|
||||
{
|
||||
// Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous.
|
||||
// The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU.
|
||||
@ -537,18 +542,18 @@ static void handle01HzLoop(State *const state, const CanardMicrosecond now_usec)
|
||||
}
|
||||
|
||||
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
|
||||
static void processMessageServoSetpoint(State *const state,
|
||||
const reg_udral_physics_dynamics_translation_Linear_0_1 *const msg)
|
||||
static void processMessageServoSetpoint(struct UdralServoInternalState *const state,
|
||||
const reg_udral_physics_dynamics_rotation_Planar_0_1 *const msg)
|
||||
{
|
||||
Serial.println("processMessageServoSetpoint");
|
||||
state->servo.position = msg->kinematics.position.meter;
|
||||
state->servo.velocity = msg->kinematics.velocity.meter_per_second;
|
||||
state->servo.acceleration = msg->kinematics.acceleration.meter_per_second_per_second;
|
||||
state->servo.force = msg->force.newton;
|
||||
state->servo.target_position = msg->kinematics.angular_position.radian;
|
||||
state->servo.target_velocity = msg->kinematics.angular_velocity.radian_per_second;
|
||||
state->servo.target_acceleration = msg->kinematics.angular_acceleration.radian_per_second_per_second;
|
||||
state->servo.target_force = msg->_torque.newton_meter;
|
||||
}
|
||||
|
||||
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/common/Readiness.0.1.dsdl
|
||||
static void processMessageServiceReadiness(State *const state,
|
||||
static void processMessageServiceReadiness(struct UdralServoInternalState *const state,
|
||||
const reg_udral_service_common_Readiness_0_1 *const msg,
|
||||
const CanardMicrosecond monotonic_time)
|
||||
{
|
||||
@ -557,7 +562,7 @@ static void processMessageServiceReadiness(State *const state,
|
||||
state->servo.arming.last_update_at = monotonic_time;
|
||||
}
|
||||
|
||||
static void processMessagePlugAndPlayNodeIDAllocation(State *const state,
|
||||
static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalState *const state,
|
||||
const uavcan_pnp_NodeIDAllocationData_1_0 *const msg)
|
||||
{
|
||||
uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0};
|
||||
@ -718,7 +723,7 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo()
|
||||
return resp;
|
||||
}
|
||||
|
||||
static void processReceivedTransfer(State *const state,
|
||||
static void processReceivedTransfer(struct UdralServoInternalState *const state,
|
||||
const struct CanardRxTransfer *const transfer,
|
||||
const CanardMicrosecond now_usec)
|
||||
{
|
||||
@ -727,8 +732,8 @@ static void processReceivedTransfer(State *const state,
|
||||
size_t size = transfer->payload.size;
|
||||
if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint)
|
||||
{
|
||||
reg_udral_physics_dynamics_translation_Linear_0_1 msg = {0};
|
||||
if (reg_udral_physics_dynamics_translation_Linear_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >=
|
||||
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)
|
||||
{
|
||||
processMessageServoSetpoint(state, &msg);
|
||||
@ -836,7 +841,7 @@ static void processReceivedTransfer(State *const state,
|
||||
|
||||
static void *canardAllocate(void *const user_reference, const size_t amount)
|
||||
{
|
||||
O1HeapInstance *const heap = ((State *)user_reference)->heap;
|
||||
O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap;
|
||||
assert(o1heapDoInvariantsHold(heap));
|
||||
return o1heapAllocate(heap, amount);
|
||||
}
|
||||
@ -844,18 +849,14 @@ static void *canardAllocate(void *const user_reference, const size_t amount)
|
||||
static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer)
|
||||
{
|
||||
(void)amount;
|
||||
O1HeapInstance *const heap = ((State *)user_reference)->heap;
|
||||
O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap;
|
||||
o1heapFree(heap, pointer);
|
||||
}
|
||||
|
||||
int mainloop()
|
||||
int udral_loop(state_sync_f servo_state_sync_f)
|
||||
{
|
||||
struct timespec ts;
|
||||
srand(micros());
|
||||
|
||||
State state = {0};
|
||||
|
||||
//registerDoFactoryReset();
|
||||
struct UdralServoInternalState state;
|
||||
// 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
|
||||
@ -943,12 +944,12 @@ int mainloop()
|
||||
state.port_id.pub.servo_dynamics = // Position/speed/acceleration/force feedback.
|
||||
getSubjectID(SUBJECT_ROLE_PUBLISHER,
|
||||
"servo.dynamics",
|
||||
reg_udral_physics_dynamics_translation_LinearTs_0_1_FULL_NAME_AND_VERSION_);
|
||||
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_translation_Linear_0_1_FULL_NAME_AND_VERSION_);
|
||||
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",
|
||||
@ -979,7 +980,7 @@ int mainloop()
|
||||
canardRxSubscribe(&state.canard,
|
||||
CanardTransferKindMessage,
|
||||
state.port_id.sub.servo_setpoint,
|
||||
reg_udral_physics_dynamics_translation_Linear_0_1_EXTENT_BYTES_,
|
||||
reg_udral_physics_dynamics_rotation_Planar_0_1_EXTENT_BYTES_,
|
||||
servo_transfer_id_timeout,
|
||||
&rx);
|
||||
if (res < 0)
|
||||
@ -1068,6 +1069,7 @@ int mainloop()
|
||||
CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10;
|
||||
do
|
||||
{
|
||||
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)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user