velocity working?
```
y r 125 reg.pid_velocity 0.001 2.5 0.0 0.01 1000.0 3.0 100.0 \
&& y cmd 125 restart
y pub --period=0.1 \
10:reg.udral.service.common.readiness '!$ "B(1,1) * 3"' \
50:reg.udral.physics.dynamics.rotation.planar '{torque: 4, kinematics: {angular_position: NAN, angular_velocity: !$ "A(1,1) * 10"}}'
```
This commit is contained in:
parent
67609d7b31
commit
b0047fbde2
@ -3,3 +3,12 @@ create `compile_commands.json`:
|
||||
´´´
|
||||
pio run -t compiledb
|
||||
´´´
|
||||
|
||||
# Tuning
|
||||
```
|
||||
y rl 125, | y rb
|
||||
y r 125 reg.pid_velocity 0.001 2.5 0.0 0.01 1000.0 3.0 100.0 && y cmd 125 restart
|
||||
y pub --period=0.1 \
|
||||
10:reg.udral.service.common.readiness '!$ "B(1,1) * 3"' \
|
||||
50:reg.udral.physics.dynamics.rotation.planar '{torque: 4, kinematics: {angular_position: NAN, angular_velocity: !$ "A(1,1) * 10"}}'
|
||||
```
|
||||
3
fw/src/.clang-format
Normal file
3
fw/src/.clang-format
Normal file
@ -0,0 +1,3 @@
|
||||
Language: Cpp
|
||||
BasedOnStyle: google
|
||||
ColumnLimit: 120
|
||||
166
fw/src/main.cpp
166
fw/src/main.cpp
@ -1,8 +1,9 @@
|
||||
// #include <Preferences.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
|
||||
#include "esp32-hal.h"
|
||||
#include <cmath>
|
||||
#define NODE_NAME "N17BLDC"
|
||||
|
||||
#define VERSION_MAJOR 0
|
||||
@ -12,7 +13,6 @@
|
||||
#include "Arduino.h"
|
||||
#include "pin_def.h"
|
||||
|
||||
|
||||
#define USE_USBSERIAL
|
||||
#ifdef USE_USBSERIAL
|
||||
#include "USB.h"
|
||||
@ -26,36 +26,29 @@ USBCDC usbserial;
|
||||
#endif
|
||||
// #define Serial Serial
|
||||
|
||||
#include "canard.c"
|
||||
|
||||
#include <SPI.h>
|
||||
|
||||
#include "canard.c"
|
||||
#include "udral_servo.hpp"
|
||||
#define spi_dev SPI
|
||||
// #include "Wire.h"
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
#include "SimpleFOCDrivers.h"
|
||||
#include "encoders/calibrated/CalibratedSensor.h"
|
||||
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
|
||||
#include "encoders/as5047/MagneticSensorAS5047.h"
|
||||
#include "encoders/calibrated/CalibratedSensor.h"
|
||||
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
|
||||
|
||||
#include "DRV8323S.hpp"
|
||||
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
|
||||
|
||||
#include "DRV8323S.hpp"
|
||||
#include "HybridStepperMotor.h"
|
||||
#include "esp32-hal-adc.h"
|
||||
#include "esp32-hal-gpio.h"
|
||||
|
||||
#include "udral_servo.hpp"
|
||||
|
||||
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
|
||||
|
||||
#define TEMP(v0, b, rt, r1, vdd) \
|
||||
((1.0 / \
|
||||
((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - \
|
||||
273.15)
|
||||
#define temp_pcb \
|
||||
(TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, \
|
||||
3.3))
|
||||
#define TEMP(v0, b, rt, r1, vdd) ((1.0 / ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - 273.15)
|
||||
#define temp_pcb (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3))
|
||||
|
||||
#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
|
||||
//* 20.8f/ 21033.75)
|
||||
@ -74,6 +67,7 @@ USBCDC usbserial;
|
||||
#endif
|
||||
|
||||
const int voltage_lpf = 50;
|
||||
const float max_voltage_limit = 6;
|
||||
|
||||
// ESP32HWEncoder encoder =
|
||||
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
|
||||
@ -83,19 +77,20 @@ Encoder encoder(ENC_A, ENC_B, 4096 / 4, 0);
|
||||
AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute);
|
||||
|
||||
float encoder_calibration_lut[90];
|
||||
CalibratedSensor encoder_calibrated(encoder_absolute, sizeof(encoder_calibration_lut) /
|
||||
sizeof(encoder_calibration_lut[0]), encoder_calibration_lut);
|
||||
CalibratedSensor encoder_calibrated(encoder_absolute,
|
||||
sizeof(encoder_calibration_lut) / sizeof(encoder_calibration_lut[0]),
|
||||
encoder_calibration_lut);
|
||||
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
|
||||
|
||||
#ifdef MOTOR
|
||||
struct drv8323s_foc_driver drv8323s;
|
||||
// BLDCMotor motor = BLDCMotor(100); // 24N22P
|
||||
HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
|
||||
HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
|
||||
#endif
|
||||
|
||||
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg); // MAN:0x2E
|
||||
Wire.write(reg); // MAN:0x2E
|
||||
int error = Wire.endTransmission();
|
||||
if (error != 0) {
|
||||
return;
|
||||
@ -129,14 +124,32 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
|
||||
SemaphoreHandle_t mutex;
|
||||
struct UdralServoState servo_state = {0};
|
||||
|
||||
static bool init_done = false;
|
||||
|
||||
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.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_velocity.value.count = 7;
|
||||
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[2] = motor.PID_velocity.D;
|
||||
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;
|
||||
}
|
||||
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;
|
||||
servo_state.pid_position = state->pid_position;
|
||||
servo_state.pid_velocity = state->pid_velocity;
|
||||
|
||||
float curr_angle = servo_state.curr_position;
|
||||
static float curr_angle_last = curr_angle;
|
||||
@ -144,7 +157,7 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
|
||||
curr_angle_last = curr_angle;
|
||||
|
||||
static float curr_vel_last = curr_vel;
|
||||
float curr_accel =(curr_vel - curr_vel_last) * hz;
|
||||
float curr_accel = (curr_vel - curr_vel_last) * hz;
|
||||
curr_vel_last = curr_vel;
|
||||
|
||||
servo_state.curr_acceleration = curr_accel;
|
||||
@ -166,15 +179,14 @@ void setup() {
|
||||
#ifdef USE_USBSERIAL
|
||||
Serial.begin();
|
||||
USB.begin();
|
||||
Serial.setDebugOutput(
|
||||
true); // sends all log_e(), log_i() messages to USB HW CDC
|
||||
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
|
||||
digitalWrite(LED_PIN, 0); // enable
|
||||
delay(500);
|
||||
digitalWrite(LED_PIN, 1); // disable
|
||||
digitalWrite(LED_PIN, 1); // disable
|
||||
delay(100);
|
||||
|
||||
Serial.println("Start");
|
||||
@ -189,14 +201,14 @@ void setup() {
|
||||
pinMode(INHC, OUTPUT);
|
||||
// digitalWrite(INHC, 0); // enable
|
||||
pinMode(INLABC, OUTPUT);
|
||||
digitalWrite(INLABC, 0); // enable
|
||||
digitalWrite(INLABC, 0); // enable
|
||||
pinMode(CAL_PIN, OUTPUT);
|
||||
digitalWrite(CAL_PIN, 0); // enable
|
||||
digitalWrite(CAL_PIN, 0); // enable
|
||||
|
||||
pinMode(SPI_MISO, INPUT_PULLUP);
|
||||
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
|
||||
//SPI.setFrequency(100000);
|
||||
// initialise magnetic sensor hardware
|
||||
// SPI.setFrequency(100000);
|
||||
// initialise magnetic sensor hardware
|
||||
encoder.pullup = Pullup::USE_INTERN;
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA1, doB1);
|
||||
@ -208,7 +220,7 @@ void setup() {
|
||||
#ifdef MOTOR
|
||||
SimpleFOCDebug::enable(&Serial);
|
||||
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
|
||||
digitalWrite(INLABC, 1); // enable
|
||||
digitalWrite(INLABC, 1); // enable
|
||||
// driver.init(&SPI);
|
||||
// status();
|
||||
motor.linkSensor(&encoder_calibrated);
|
||||
@ -217,7 +229,7 @@ void setup() {
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
motor.voltage_sensor_align = 4;
|
||||
motor.voltage_limit = 6;
|
||||
motor.voltage_limit = max_voltage_limit;
|
||||
// motor.controller = MotionControlType::velocity_openloop;
|
||||
// motor.controller = MotionControlType::velocity;
|
||||
motor.controller = MotionControlType::torque;
|
||||
@ -239,18 +251,14 @@ void setup() {
|
||||
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)) {
|
||||
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.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));
|
||||
pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
|
||||
Serial.println("Skipping calibration");
|
||||
}
|
||||
|
||||
@ -266,16 +274,16 @@ void setup() {
|
||||
pref.end();
|
||||
|
||||
motor.move(0);
|
||||
digitalWrite(INLABC, 0); // enable
|
||||
digitalWrite(INLABC, 0); // enable
|
||||
#endif
|
||||
|
||||
xTaskCreatePinnedToCore(&comm_task, // Function name of the task
|
||||
"comm", // Name of the task (e.g. for debugging)
|
||||
65536, // Stack size (bytes)
|
||||
NULL, // Parameter to pass
|
||||
10, // Task priority
|
||||
&taskCommHandle, // Assign task handle
|
||||
0 // Run on the non-Arduino (1) core
|
||||
xTaskCreatePinnedToCore(&comm_task, // Function name of the task
|
||||
"comm", // Name of the task (e.g. for debugging)
|
||||
65536, // Stack size (bytes)
|
||||
NULL, // Parameter to pass
|
||||
10, // Task priority
|
||||
&taskCommHandle, // Assign task handle
|
||||
0 // Run on the non-Arduino (1) core
|
||||
);
|
||||
// esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts
|
||||
// esp_task_wdt_add(NULL); // add current thread to WDT watch
|
||||
@ -284,6 +292,10 @@ void setup() {
|
||||
int i = 0;
|
||||
void loop() {
|
||||
#ifdef MOTOR
|
||||
|
||||
while (!init_done) {
|
||||
delay(10);
|
||||
}
|
||||
bool armed = false;
|
||||
|
||||
/* adc reading */
|
||||
@ -292,17 +304,67 @@ void loop() {
|
||||
|
||||
xSemaphoreTake(mutex, portMAX_DELAY);
|
||||
armed = servo_state.arming.armed;
|
||||
float target = servo_state.target_force;
|
||||
|
||||
float target;
|
||||
if (!std::isnan(servo_state.target_position)) {
|
||||
if (motor.controller != MotionControlType::angle) {
|
||||
motor.controller = MotionControlType::angle;
|
||||
Serial.println("MotionControlType::angle");
|
||||
}
|
||||
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));
|
||||
} 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));
|
||||
} 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;
|
||||
if (motor.controller != MotionControlType::torque) {
|
||||
motor.controller = MotionControlType::torque;
|
||||
Serial.printf("MotionControlType::torque %f\n", target);
|
||||
}
|
||||
}
|
||||
|
||||
servo_state.controller_temperature = curr_pcb_temp_kelvin;
|
||||
servo_state.motor_temperature = NAN;
|
||||
|
||||
float curr_angle = encoder_calibrated.getAngle();
|
||||
servo_state.curr_position = curr_angle;
|
||||
servo_state.curr_force = servo_state.target_force;
|
||||
servo_state.curr_force = motor.voltage.d;
|
||||
|
||||
servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
|
||||
servo_state.current = NAN;
|
||||
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.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.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.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);
|
||||
|
||||
if (!armed) {
|
||||
@ -323,6 +385,6 @@ void loop() {
|
||||
|
||||
motor.loopFOC();
|
||||
#else
|
||||
encoder.update(); // optional: Update manually if not using loopfoc()
|
||||
encoder.update(); // optional: Update manually if not using loopfoc()
|
||||
#endif
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user