mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-09-26 23:09:27 +08:00
Firmware cleanup
This commit is contained in:
parent
3bee19df93
commit
b286cfabe1
@ -29,13 +29,8 @@ build_flags =
|
||||
|
||||
[env:view]
|
||||
extends = base_config
|
||||
; platform = https://github.com/platformio/platform-espressif32.git#feature/arduino-upstream
|
||||
; platform_packages =
|
||||
; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32#master
|
||||
board = esp32doit-devkit-v1
|
||||
lib_deps =
|
||||
; askuric/Simple FOC @ 2.2.1
|
||||
; bxparks/AceButton @ 1.9.1
|
||||
${base_config.lib_deps}
|
||||
bodmer/TFT_eSPI@2.4.25
|
||||
fastled/FastLED @ 3.5.0
|
||||
@ -103,54 +98,3 @@ build_flags =
|
||||
; GPIO >= 34 are input only
|
||||
; (SOC_GPIO_VALID_GPIO_MASK & ~(0ULL | _FL_BIT(34) | _FL_BIT(35) | _FL_BIT(36) | _FL_BIT(37) | _FL_BIT(38) | _FL_BIT(39)))
|
||||
-DSOC_GPIO_VALID_OUTPUT_GPIO_MASK=0x30EFFFFFF
|
||||
|
||||
|
||||
[env:handheld_tdisplay]
|
||||
extends = base_config
|
||||
board = esp32doit-devkit-v1
|
||||
lib_deps =
|
||||
${base_config.lib_deps}
|
||||
bodmer/TFT_eSPI@2.4.25
|
||||
|
||||
build_flags =
|
||||
${base_config.build_flags}
|
||||
-DSK_DISPLAY=1
|
||||
-DSK_STRAIN=0
|
||||
-DSK_LEDS=0
|
||||
|
||||
-DPIN_UH=17
|
||||
-DPIN_UL=2
|
||||
-DPIN_VH=13
|
||||
-DPIN_VL=32
|
||||
-DPIN_WH=33
|
||||
-DPIN_WL=25
|
||||
-DPIN_BUTTON_NEXT=35
|
||||
-DPIN_BUTTON_PREV=0
|
||||
-DPIN_SDA=-1
|
||||
-DPIN_SCL=-1
|
||||
-DSENSOR_MT6701=1
|
||||
-DPIN_MT_DATA=21
|
||||
-DPIN_MT_CLOCK=22
|
||||
-DPIN_MT_CSN=12
|
||||
-DPIN_LCD_BACKLIGHT=4
|
||||
|
||||
-DDESCRIPTION_FONT=FreeSans9pt7b
|
||||
-DDESCRIPTION_Y_OFFSET=80
|
||||
-DVALUE_OFFSET=0
|
||||
-DDRAW_ARC=1
|
||||
|
||||
-DUSER_SETUP_LOADED=1
|
||||
-DST7789_DRIVER=1
|
||||
-DCGRAM_OFFSET=1
|
||||
-DTFT_WIDTH=135
|
||||
-DTFT_HEIGHT=240
|
||||
-DTFT_MISO=-1
|
||||
-DTFT_MOSI=19
|
||||
-DTFT_SCLK=18
|
||||
-DTFT_CS=5
|
||||
-DTFT_DC=16
|
||||
-DTFT_RST=23
|
||||
-DTFT_BL=-1
|
||||
-DLOAD_GLCD=1
|
||||
-DLOAD_GFXFF=1
|
||||
-DSPI_FREQUENCY=40000000
|
@ -37,7 +37,7 @@ void setup() {
|
||||
|
||||
motor_task.addListener(knob_state_debug_queue);
|
||||
|
||||
// Free up the loop task
|
||||
// Free up the Arduino loop task
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
|
@ -1,11 +1,24 @@
|
||||
#include <SimpleFOC.h>
|
||||
#include <sensors/MagneticSensorI2C.h>
|
||||
|
||||
#include "motor_task.h"
|
||||
#if SENSOR_MT6701
|
||||
#include "mt6701_sensor.h"
|
||||
#endif
|
||||
#if SENSOR_TLV
|
||||
#include "tlv_sensor.h"
|
||||
#endif
|
||||
#include "util.h"
|
||||
|
||||
|
||||
// ####
|
||||
// Hardware-specific motor calibration constants.
|
||||
// Run calibration once at startup, then update these constants with the calibration results.
|
||||
static const float ZERO_ELECTRICAL_OFFSET = 2.77;
|
||||
static const Direction FOC_DIRECTION = Direction::CW;
|
||||
static const int MOTOR_POLE_PAIRS = 7;
|
||||
// ####
|
||||
|
||||
|
||||
static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
|
||||
static const float DEAD_ZONE_RAD = 1 * _PI / 180;
|
||||
|
||||
@ -24,38 +37,16 @@ MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 2048, 1, task_core
|
||||
MotorTask::~MotorTask() {}
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(1);
|
||||
BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
|
||||
|
||||
#if SENSOR_TLV
|
||||
TlvSensor encoder = TlvSensor();
|
||||
#elif SENSOR_MT6701
|
||||
MT6701Sensor encoder = MT6701Sensor();
|
||||
#endif
|
||||
// MagneticSensorI2C tlv = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
Commander command = Commander(Serial);
|
||||
|
||||
|
||||
void doMotor(char* cmd) { command.motor(&motor, cmd); }
|
||||
|
||||
void MotorTask::run() {
|
||||
// Hardware-specific configuration:
|
||||
// TODO: make this easier to configure
|
||||
// Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
|
||||
// SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
|
||||
// it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
|
||||
// So this value is based on experimentation.
|
||||
// TODO: dig into SimpleFOC calibration and find/fix the issue
|
||||
// float zero_electric_offset = -0.6; // original proto
|
||||
//float zero_electric_offset = 0.4; // handheld 1
|
||||
// float zero_electric_offset = -0.8; // handheld 2
|
||||
// float zero_electric_offset = 2.93; //0.15; // 17mm test
|
||||
// float zero_electric_offset = 0.66; // 15mm handheld
|
||||
float zero_electric_offset = 7.34;
|
||||
Direction foc_direction = Direction::CW;
|
||||
motor.pole_pairs = 7;
|
||||
|
||||
driver.voltage_power_supply = 5;
|
||||
driver.init();
|
||||
@ -66,9 +57,7 @@ void MotorTask::run() {
|
||||
|
||||
#if SENSOR_MT6701
|
||||
encoder.init();
|
||||
// motor.LPF_angle = LowPassFilter(0.05);
|
||||
#endif
|
||||
// motor.LPF_current_q = {0.01};
|
||||
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
@ -77,23 +66,21 @@ void MotorTask::run() {
|
||||
motor.velocity_limit = 10000;
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// Not actually using the velocity loop; but I'm using those PID variables
|
||||
// because SimpleFOC studio supports updating them easily over serial for tuning.
|
||||
// Not actually using the velocity loop built into SimpleFOC; but I'm using those PID variables
|
||||
// to run PID for torque (and SimpleFOC studio supports updating them easily over serial for tuning)
|
||||
motor.PID_velocity.P = 4;
|
||||
motor.PID_velocity.I = 0;
|
||||
motor.PID_velocity.D = 0.04;
|
||||
motor.PID_velocity.output_ramp = 10000;
|
||||
motor.PID_velocity.limit = 10;
|
||||
|
||||
|
||||
// motor.useMonitoring(Serial);
|
||||
|
||||
motor.init();
|
||||
|
||||
encoder.update();
|
||||
delay(10);
|
||||
|
||||
motor.initFOC(zero_electric_offset, foc_direction);
|
||||
motor.pole_pairs = MOTOR_POLE_PAIRS;
|
||||
motor.initFOC(ZERO_ELECTRICAL_OFFSET, FOC_DIRECTION);
|
||||
|
||||
bool calibrate = false;
|
||||
|
||||
@ -107,178 +94,11 @@ void MotorTask::run() {
|
||||
delay(10);
|
||||
}
|
||||
if (calibrate) {
|
||||
motor.controller = MotionControlType::angle_openloop;
|
||||
motor.pole_pairs = 1;
|
||||
motor.initFOC(0, Direction::CW);
|
||||
|
||||
|
||||
float a = 0;
|
||||
|
||||
for (uint8_t i = 0; i < 200; i++) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
float start_sensor = encoder.getAngle();
|
||||
|
||||
for (; a < 3 * _2PI; a += 0.01) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < 200; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
float end_sensor = encoder.getAngle();
|
||||
|
||||
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(a);
|
||||
// Serial.println("Did motor turn counterclockwise? Press Y to continue, otherwise change motor wiring and restart");
|
||||
// while (Serial.read() != 'Y') {
|
||||
// delay(10);
|
||||
// }
|
||||
|
||||
Serial.println();
|
||||
|
||||
// TODO: check for no motor movement!
|
||||
|
||||
Serial.print("Sensor measures positive for positive motor rotation: ");
|
||||
if (end_sensor > start_sensor) {
|
||||
Serial.println("YES, Direction=CW");
|
||||
motor.initFOC(0, Direction::CW);
|
||||
} else {
|
||||
Serial.println("NO, Direction=CCW");
|
||||
motor.initFOC(0, Direction::CCW);
|
||||
}
|
||||
|
||||
// Rotate many electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
|
||||
uint8_t electrical_revolutions = 20;
|
||||
Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
|
||||
motor.voltage_limit = 5;
|
||||
motor.move(a);
|
||||
Serial.println("Going to electrical zero...");
|
||||
float destination = a + _2PI;
|
||||
for (; a < destination; a += 0.03) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
Serial.println("pause...");
|
||||
for (uint16_t i = 0; i < 1000; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
Serial.println("Measuring...");
|
||||
|
||||
start_sensor = motor.sensor_direction * encoder.getAngle();
|
||||
destination = a + electrical_revolutions * _2PI;
|
||||
for (; a < destination; a += 0.03) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
for (uint16_t i = 0; i < 1000; i++) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
end_sensor = motor.sensor_direction * encoder.getAngle();
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(a);
|
||||
|
||||
if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
|
||||
Serial.println("ERROR: motor did not reach target!");
|
||||
while(1) {}
|
||||
}
|
||||
|
||||
float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
|
||||
Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
|
||||
Serial.println(electrical_per_mechanical);
|
||||
|
||||
int measured_pole_pairs = (int)round(electrical_per_mechanical);
|
||||
Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
|
||||
|
||||
delay(1000);
|
||||
|
||||
|
||||
|
||||
// Measure mechanical angle at every electrical zero for several revolutions
|
||||
motor.voltage_limit = 5;
|
||||
motor.move(a);
|
||||
float offset_x = 0;
|
||||
float offset_y = 0;
|
||||
float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
|
||||
float destination2 = (floor(a / _2PI)) * _2PI;
|
||||
for (; a < destination1; a += 0.4) {
|
||||
motor.move(a);
|
||||
delay(100);
|
||||
for (uint8_t i = 0; i < 100; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
float real_electrical_angle = _normalizeAngle(a);
|
||||
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
|
||||
|
||||
float offset_angle = measured_electrical_angle - real_electrical_angle;
|
||||
offset_x += cosf(offset_angle);
|
||||
offset_y += sinf(offset_angle);
|
||||
|
||||
Serial.print(degrees(real_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.print(degrees(measured_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.println(degrees(_normalizeAngle(offset_angle)));
|
||||
}
|
||||
for (; a > destination2; a -= 0.4) {
|
||||
motor.move(a);
|
||||
delay(100);
|
||||
for (uint8_t i = 0; i < 100; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
float real_electrical_angle = _normalizeAngle(a);
|
||||
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
|
||||
|
||||
float offset_angle = measured_electrical_angle - real_electrical_angle;
|
||||
offset_x += cosf(offset_angle);
|
||||
offset_y += sinf(offset_angle);
|
||||
|
||||
Serial.print(degrees(real_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.print(degrees(measured_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.println(degrees(_normalizeAngle(offset_angle)));
|
||||
}
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(a);
|
||||
|
||||
float avg_offset_angle = atan2f(offset_y, offset_x);
|
||||
|
||||
// Apply settings
|
||||
motor.pole_pairs = measured_pole_pairs;
|
||||
motor.zero_electric_angle = avg_offset_angle + _3PI_2;
|
||||
motor.voltage_limit = 5;
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
Serial.print("\n\nRESULTS:\n zero electric angle: ");
|
||||
Serial.println(motor.zero_electric_angle);
|
||||
Serial.print(" direction: ");
|
||||
if (motor.sensor_direction == Direction::CW) {
|
||||
Serial.println("CW");
|
||||
} else {
|
||||
Serial.println("CCW");
|
||||
}
|
||||
Serial.printf(" pole pairs: %d\n", motor.pole_pairs);
|
||||
delay(2000);
|
||||
this->calibrate();
|
||||
}
|
||||
|
||||
Serial.println(motor.zero_electric_angle);
|
||||
|
||||
command.add('M', &doMotor, "foo");
|
||||
// command.add('D', &doDetents, "Detents");
|
||||
motor.monitor_downsample = 0; // disable monitor at first - optional
|
||||
|
||||
// disableCore0WDT();
|
||||
@ -293,17 +113,17 @@ void MotorTask::run() {
|
||||
|
||||
float idle_check_velocity_ewma = 0;
|
||||
uint32_t last_idle_start = 0;
|
||||
uint32_t last_debug = 0;
|
||||
|
||||
uint32_t last_publish = 0;
|
||||
|
||||
while (1) {
|
||||
motor.loopFOC();
|
||||
|
||||
// Check queue for pending requests from other tasks
|
||||
Command command;
|
||||
if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
|
||||
switch (command.command_type) {
|
||||
case CommandType::CONFIG: {
|
||||
// Change haptic input mode
|
||||
config = command.data.config;
|
||||
Serial.println("Got new config");
|
||||
current_detent_center = motor.shaft_angle;
|
||||
@ -332,6 +152,7 @@ void MotorTask::run() {
|
||||
break;
|
||||
}
|
||||
case CommandType::HAPTIC: {
|
||||
// Play a hardcoded haptic "click"
|
||||
float strength = command.data.haptic.press ? 5 : 1.5;
|
||||
motor.move(strength);
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
@ -350,6 +171,7 @@ void MotorTask::run() {
|
||||
}
|
||||
}
|
||||
|
||||
// If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
|
||||
idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
|
||||
if (fabsf(idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) {
|
||||
last_idle_start = 0;
|
||||
@ -358,19 +180,11 @@ void MotorTask::run() {
|
||||
last_idle_start = millis();
|
||||
}
|
||||
}
|
||||
|
||||
// If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
|
||||
if (last_idle_start > 0 && millis() - last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(motor.shaft_angle - current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) {
|
||||
current_detent_center = motor.shaft_angle * IDLE_CORRECTION_RATE_ALPHA + current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA);
|
||||
// if (millis() - last_debug > 100) {
|
||||
// last_debug = millis();
|
||||
// Serial.print("Moving detent center. ");
|
||||
// Serial.print(current_detent_center);
|
||||
// Serial.print(" ");
|
||||
// Serial.println(motor.shaft_angle);
|
||||
// }
|
||||
}
|
||||
|
||||
// Check where we are relative to the current nearest detent; update our position if we've moved far enough to snap to another detent
|
||||
float angle_to_detent_center = motor.shaft_angle - current_detent_center;
|
||||
#if SK_INVERT_ROTATION
|
||||
angle_to_detent_center = -motor.shaft_angle - current_detent_center;
|
||||
@ -395,7 +209,7 @@ void MotorTask::run() {
|
||||
motor.PID_velocity.P = out_of_bounds ? config.endstop_strength_unit * 4 : config.detent_strength_unit * 4;
|
||||
|
||||
|
||||
|
||||
// Apply motor torque based on our angle to the nearest detent (detent strength, etc is handled by the PID_velocity parameters)
|
||||
if (fabsf(motor.shaft_velocity) > 60) {
|
||||
// Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
|
||||
motor.move(0);
|
||||
@ -407,6 +221,7 @@ void MotorTask::run() {
|
||||
motor.move(torque);
|
||||
}
|
||||
|
||||
// Publish current status to other registered tasks periodically
|
||||
if (millis() - last_publish > 10) {
|
||||
publish({
|
||||
.current_position = config.position,
|
||||
@ -417,7 +232,6 @@ void MotorTask::run() {
|
||||
}
|
||||
|
||||
motor.monitor();
|
||||
// command.run();
|
||||
|
||||
delay(1);
|
||||
}
|
||||
@ -456,3 +270,179 @@ void MotorTask::publish(const KnobState& state) {
|
||||
xQueueOverwrite(listener, &state);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorTask::calibrate() {
|
||||
// SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
|
||||
// it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
|
||||
// So this value is based on experimentation.
|
||||
// TODO: dig into SimpleFOC calibration and find/fix the issue
|
||||
|
||||
Serial.println("\n\n\nStarting calibration, please do not touch to motor until complete!");
|
||||
|
||||
motor.controller = MotionControlType::angle_openloop;
|
||||
motor.pole_pairs = 1;
|
||||
motor.initFOC(0, Direction::CW);
|
||||
|
||||
float a = 0;
|
||||
|
||||
// #### Determine direction motor rotates relative to angle sensor
|
||||
for (uint8_t i = 0; i < 200; i++) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
float start_sensor = encoder.getAngle();
|
||||
|
||||
for (; a < 3 * _2PI; a += 0.01) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < 200; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
float end_sensor = encoder.getAngle();
|
||||
|
||||
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(a);
|
||||
|
||||
Serial.println();
|
||||
|
||||
// TODO: check for no motor movement!
|
||||
|
||||
Serial.print("Sensor measures positive for positive motor rotation: ");
|
||||
if (end_sensor > start_sensor) {
|
||||
Serial.println("YES, Direction=CW");
|
||||
motor.initFOC(0, Direction::CW);
|
||||
} else {
|
||||
Serial.println("NO, Direction=CCW");
|
||||
motor.initFOC(0, Direction::CCW);
|
||||
}
|
||||
|
||||
|
||||
// #### Determine pole-pairs
|
||||
// Rotate 20 electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
|
||||
uint8_t electrical_revolutions = 20;
|
||||
Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
|
||||
motor.voltage_limit = 5;
|
||||
motor.move(a);
|
||||
Serial.println("Going to electrical zero...");
|
||||
float destination = a + _2PI;
|
||||
for (; a < destination; a += 0.03) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
Serial.println("pause..."); // Let momentum settle...
|
||||
for (uint16_t i = 0; i < 1000; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
Serial.println("Measuring...");
|
||||
|
||||
start_sensor = motor.sensor_direction * encoder.getAngle();
|
||||
destination = a + electrical_revolutions * _2PI;
|
||||
for (; a < destination; a += 0.03) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
for (uint16_t i = 0; i < 1000; i++) {
|
||||
encoder.update();
|
||||
motor.move(a);
|
||||
delay(1);
|
||||
}
|
||||
end_sensor = motor.sensor_direction * encoder.getAngle();
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(a);
|
||||
|
||||
if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
|
||||
Serial.println("ERROR: motor did not reach target!");
|
||||
while(1) {}
|
||||
}
|
||||
|
||||
float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
|
||||
Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
|
||||
Serial.println(electrical_per_mechanical);
|
||||
|
||||
int measured_pole_pairs = (int)round(electrical_per_mechanical);
|
||||
Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
|
||||
|
||||
delay(1000);
|
||||
|
||||
|
||||
// #### Determine mechanical offset to electrical zero
|
||||
// Measure mechanical angle at every electrical zero for several revolutions
|
||||
motor.voltage_limit = 5;
|
||||
motor.move(a);
|
||||
float offset_x = 0;
|
||||
float offset_y = 0;
|
||||
float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
|
||||
float destination2 = (floor(a / _2PI)) * _2PI;
|
||||
for (; a < destination1; a += 0.4) {
|
||||
motor.move(a);
|
||||
delay(100);
|
||||
for (uint8_t i = 0; i < 100; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
float real_electrical_angle = _normalizeAngle(a);
|
||||
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
|
||||
|
||||
float offset_angle = measured_electrical_angle - real_electrical_angle;
|
||||
offset_x += cosf(offset_angle);
|
||||
offset_y += sinf(offset_angle);
|
||||
|
||||
Serial.print(degrees(real_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.print(degrees(measured_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.println(degrees(_normalizeAngle(offset_angle)));
|
||||
}
|
||||
for (; a > destination2; a -= 0.4) {
|
||||
motor.move(a);
|
||||
delay(100);
|
||||
for (uint8_t i = 0; i < 100; i++) {
|
||||
encoder.update();
|
||||
delay(1);
|
||||
}
|
||||
float real_electrical_angle = _normalizeAngle(a);
|
||||
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
|
||||
|
||||
float offset_angle = measured_electrical_angle - real_electrical_angle;
|
||||
offset_x += cosf(offset_angle);
|
||||
offset_y += sinf(offset_angle);
|
||||
|
||||
Serial.print(degrees(real_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.print(degrees(measured_electrical_angle));
|
||||
Serial.print(", ");
|
||||
Serial.println(degrees(_normalizeAngle(offset_angle)));
|
||||
}
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(a);
|
||||
|
||||
float avg_offset_angle = atan2f(offset_y, offset_x);
|
||||
|
||||
|
||||
// #### Apply settings
|
||||
// TODO: save to non-volatile storage
|
||||
motor.pole_pairs = measured_pole_pairs;
|
||||
motor.zero_electric_angle = avg_offset_angle + _3PI_2;
|
||||
motor.voltage_limit = 5;
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
Serial.print("\n\nRESULTS:\n Update these constants at the top of " __FILE__ "\n ZERO_ELECTRICAL_OFFSET: ");
|
||||
Serial.println(motor.zero_electric_angle);
|
||||
Serial.print(" FOC_DIRECTION: ");
|
||||
if (motor.sensor_direction == Direction::CW) {
|
||||
Serial.println("Direction::CW");
|
||||
} else {
|
||||
Serial.println("Direction::CCW");
|
||||
}
|
||||
Serial.printf(" MOTOR_POLE_PAIRS: %d\n", motor.pole_pairs);
|
||||
delay(2000);
|
||||
}
|
||||
|
@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include <vector>
|
||||
|
||||
#include "knob_data.h"
|
||||
@ -45,5 +46,10 @@ class MotorTask : public Task<MotorTask> {
|
||||
|
||||
std::vector<QueueHandle_t> listeners_;
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(1);
|
||||
BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
|
||||
|
||||
void publish(const KnobState& state);
|
||||
void calibrate();
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user