wip doenst work
This commit is contained in:
parent
dee14242cd
commit
3aa2916aef
@ -1,3 +1,3 @@
|
||||
Language: Cpp
|
||||
BasedOnStyle: google
|
||||
ColumnLimit: 120
|
||||
ColumnLimit: 140
|
||||
@ -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,
|
||||
int CSn, int en, SPIClass *spi) {
|
||||
if(dev == NULL || spi == NULL) {
|
||||
return;
|
||||
}
|
||||
dev->CSn = CSn;
|
||||
dev->spi = spi;
|
||||
dev->en = en;
|
||||
@ -211,7 +214,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);
|
||||
|
||||
319
fw/src/main.cpp
319
fw/src/main.cpp
@ -5,6 +5,7 @@ reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "BLDCMotor.h"
|
||||
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
|
||||
#include "encoders/as5047/AS5047.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
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "pin_def.h"
|
||||
#include "pin_def_v5.h"
|
||||
|
||||
#define USE_USBSERIAL
|
||||
#ifdef USE_USBSERIAL
|
||||
@ -35,24 +36,25 @@ USBCDC usbserial;
|
||||
#include "canard.c"
|
||||
#include "udral_servo.hpp"
|
||||
#define spi_dev SPI
|
||||
SPIClass spi_drv(FSPI);
|
||||
// #include "Wire.h"
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
#include "DRV8323S.hpp"
|
||||
#include "HybridStepperMotor.h"
|
||||
#include "SimpleFOCDrivers.h"
|
||||
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
|
||||
#include "encoders/as5047/MagneticSensorAS5047.h"
|
||||
#include "encoders/calibrated/CalibratedSensor.h"
|
||||
|
||||
#include "DRV8323S.hpp"
|
||||
#include "HybridStepperMotor.h"
|
||||
#include "esp32-hal-adc.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_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)
|
||||
//* 20.8f/ 21033.75)
|
||||
|
||||
// #define ESPHWENC
|
||||
#define MOTOR
|
||||
#define FW_NO_WATCHDOG
|
||||
@ -67,9 +69,7 @@ USBCDC usbserial;
|
||||
// };
|
||||
#endif
|
||||
|
||||
const int voltage_lpf = 50;
|
||||
const float max_voltage_limit = 2.3;
|
||||
|
||||
float max_voltage_limit = 0;
|
||||
|
||||
#ifdef ESPHWENC
|
||||
#include "encoders/esp32hwencoder/ESP32HWEncoder.h"
|
||||
@ -87,16 +87,15 @@ CalibratedSensor encoder_calibrated(abs_inc_sensor, sizeof(encoder_calibration_l
|
||||
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);
|
||||
|
||||
|
||||
BLDCMotor motor = BLDCMotor(7, 0.7, 360, 0.15);
|
||||
HybridStepperMotor motorHybridStepper = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
|
||||
BLDCMotor motorBLDC = BLDCMotor(7, 0.7, 360, 0.15);
|
||||
FOCMotor *motor = NULL;
|
||||
// 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
|
||||
@ -139,21 +138,25 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
|
||||
xSemaphoreTake(mutex, portMAX_DELAY);
|
||||
if (!init_done) {
|
||||
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.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_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;
|
||||
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;
|
||||
state->pid_velocity.value.elements[6] = motor.motion_downsample;
|
||||
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;
|
||||
state->pid_velocity.value.elements[6] = motor->motion_downsample;
|
||||
}
|
||||
servo_state.target_force = state->target_force;
|
||||
servo_state.target_acceleration = state->target_acceleration;
|
||||
@ -210,7 +213,8 @@ void setup() {
|
||||
digitalWrite(CAL_PIN, 0); // enable
|
||||
|
||||
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);
|
||||
// initialise magnetic sensor hardware
|
||||
encoder.pullup = Pullup::USE_INTERN;
|
||||
@ -222,67 +226,22 @@ void setup() {
|
||||
|
||||
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());
|
||||
#ifdef MOTOR
|
||||
|
||||
SimpleFOCDebug::enable(&Serial);
|
||||
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
|
||||
digitalWrite(INLABC, 1); // enable
|
||||
// driver.init(&SPI);
|
||||
// status();
|
||||
motor.linkSensor(&encoder_absolute);
|
||||
//motor.linkSensor(&encoder_calibrated);
|
||||
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
|
||||
|
||||
digitalWrite(LED_PIN, 1); // disable
|
||||
delay(100);
|
||||
digitalWrite(LED_PIN, 0); // enable
|
||||
delay(400);
|
||||
digitalWrite(LED_PIN, 1); // disable
|
||||
delay(4000);
|
||||
xTaskCreatePinnedToCore(&comm_task, // Function name of the task
|
||||
"comm", // Name of the task (e.g. for debugging)
|
||||
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_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) {
|
||||
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;
|
||||
void loop() {}
|
||||
|
||||
void foc_task_loop() {
|
||||
#ifdef MOTOR
|
||||
if(motor == NULL) {
|
||||
delay(10);
|
||||
return;
|
||||
}
|
||||
bool armed = false;
|
||||
/* adc reading */
|
||||
drv8323s.focdriver->voltage_power_supply = vdrive_read;
|
||||
const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
|
||||
const float curr_mot_temp_kelvin = temp_mot + 273.15f;
|
||||
|
||||
xSemaphoreTake(mutex, portMAX_DELAY);
|
||||
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;
|
||||
if (!std::isnan(servo_state.target_position)) {
|
||||
if (motor.controller != MotionControlType::angle) {
|
||||
motor.controller = MotionControlType::angle;
|
||||
Serial.println("MotionControlType::angle");
|
||||
auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::angle_openloop : MotionControlType::angle);
|
||||
if (motor->controller != MotionControlType::angle) {
|
||||
motor->controller = MotionControlType::angle;
|
||||
Serial.printf("MotionControlType::angle%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
|
||||
}
|
||||
target = servo_state.target_position;
|
||||
|
||||
motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
|
||||
motor.voltage_limit =
|
||||
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");
|
||||
auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::velocity_openloop : MotionControlType::velocity);
|
||||
if (motor->controller != target_control_type) {
|
||||
motor->controller = target_control_type;
|
||||
Serial.printf("MotionControlType::velocity%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
|
||||
}
|
||||
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));
|
||||
} 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));
|
||||
target = std::isnan(servo_state.target_force) ? 0 : servo_state.target_force;
|
||||
if (motor.controller != MotionControlType::torque) {
|
||||
motor.controller = MotionControlType::torque;
|
||||
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;
|
||||
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_force = motor.voltage.d;
|
||||
servo_state.curr_force = motor->voltage.d;
|
||||
|
||||
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.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->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.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
|
||||
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 */
|
||||
// setting the limits
|
||||
|
||||
xSemaphoreGive(mutex);
|
||||
|
||||
if (init_motor()) {
|
||||
if (!armed) {
|
||||
if (motor.enabled) {
|
||||
motor.move(0);
|
||||
motor.disable();
|
||||
if (motor->enabled) {
|
||||
motor->move(0);
|
||||
motor->disable();
|
||||
digitalWrite(INLABC, 0);
|
||||
digitalWrite(LED_PIN, HIGH);
|
||||
}
|
||||
} else {
|
||||
if (!motor.enabled) {
|
||||
if (!motor->enabled) {
|
||||
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) {
|
||||
while (true) {
|
||||
foc_task_loop();
|
||||
|
||||
35
fw/src/pin_def_v5.h
Normal file
35
fw/src/pin_def_v5.h
Normal 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
67
fw/src/ringbuf.hpp
Normal 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);
|
||||
}
|
||||
@ -16,7 +16,7 @@
|
||||
#define NUNAVUT_ASSERT(x) assert(x)
|
||||
#include "canard.h"
|
||||
#include "esp32-hal.h"
|
||||
#include "pin_def.h"
|
||||
#include "pin_def_v5.h"
|
||||
|
||||
// #include "socketcan.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
|
||||
/// 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 {
|
||||
/// Whether the servo is supposed to actuate the load or to stay idle (safe
|
||||
/// low-power mode).
|
||||
@ -93,6 +104,9 @@ struct UdralServoState {
|
||||
|
||||
uavcan_primitive_array_Real32_1_0 pid_position;
|
||||
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;
|
||||
};
|
||||
|
||||
static struct UdralServoInternalState state;
|
||||
|
||||
/// This flag is raised when the node is requested to restart.
|
||||
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';
|
||||
|
||||
uavcan_register_Access_Response_1_0 resp = {0};
|
||||
// Serial.println("processRequestRegisterAccess");
|
||||
// Serial.print("name: ");
|
||||
// Serial.println((char *)&req->name.name);
|
||||
Serial.printf("processRequestRegisterAccess name: %s\n\r", (char *)&req->name.name);
|
||||
if (strcmp((char*)&req->name.name.elements[0], "reg.motor_type") == 0) {
|
||||
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 (!uavcan_register_Value_1_0_is_empty_(&req->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.
|
||||
uavcan_register_Value_1_0_select_empty_(&resp.value);
|
||||
registerRead(&name[0], &resp.value);
|
||||
|
||||
}
|
||||
// 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
|
||||
// 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) {
|
||||
srand(micros());
|
||||
struct UdralServoInternalState state{
|
||||
.user_state_sync_f = servo_state_sync_f,
|
||||
};
|
||||
state.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.
|
||||
// For the background and related theory refer to the following resources:
|
||||
// - 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);
|
||||
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
|
||||
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;
|
||||
|
||||
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.
|
||||
uavcan_register_Value_1_0_select_natural16_(&val);
|
||||
val.natural16.value.count = 1;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user