mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-09-26 23:09:27 +08:00
NanoFOC/ESP32-S3 support (#117)
- New PlatformIO env (`nanofoc`) for the NanoFOC hardware - Huge thanks to @lgc00 for the initial NanoFOC configuration and prototyping! - Uses `HWCDC` rather than `UartStream` as the serial `Stream` interface (ESP32-S3 specific change) - Support for MAQ430 magnetic encoder sensor - Motor-specific PID tuning constants moved to separate header files --------- Co-authored-by: lgc00 <lgcasd1+1@gmail.com>
This commit is contained in:
parent
4a1d2bee77
commit
9903ac63e6
12
README.md
12
README.md
@ -138,9 +138,17 @@ Latest auto-generated (untested and likely broken!) artifacts⚠️:
|
|||||||
⚠️ For tested/stable/recommended artifacts, use a [release](https://github.com/scottbez1/smartknob/releases) instead.
|
⚠️ For tested/stable/recommended artifacts, use a [release](https://github.com/scottbez1/smartknob/releases) instead.
|
||||||
|
|
||||||
|
|
||||||
## SmartKnob Mini
|
## NanoFOC (3rd party)
|
||||||
Planned for the future.
|
If you're looking to tinker with FOC/haptic feedback, but don't want to build a full SmartKnob View yourself, I can
|
||||||
|
recommend the NanoFOC DevKit++, an [open-source design](https://github.com/katbinaris/nanofoc_devkit) made and
|
||||||
|
[sold](https://store.binaris.io/products/nanofoc-devkit) by a member of the SmartKnob community! It's super compact
|
||||||
|
and is a great testbed or core for building your own BLDC-based haptic input device.
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
|
||||||
|
The NanoFOC uses an ESP32-S3, and the SmartKnob firmware works on it out of the box; just select the `nanofoc`
|
||||||
|
environment in PlatformIO rather than the `view` environment when uploading.
|
||||||
|
|
||||||
# Frequently Asked Questions (FAQ)
|
# Frequently Asked Questions (FAQ)
|
||||||
|
|
||||||
|
@ -24,7 +24,11 @@ class InterfaceTask : public Task<InterfaceTask>, public Logger {
|
|||||||
void run();
|
void run();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
#ifdef CONFIG_IDF_TARGET_ESP32S3
|
||||||
|
HWCDC stream_;
|
||||||
|
#else
|
||||||
UartStream stream_;
|
UartStream stream_;
|
||||||
|
#endif
|
||||||
MotorTask& motor_task_;
|
MotorTask& motor_task_;
|
||||||
DisplayTask* display_task_;
|
DisplayTask* display_task_;
|
||||||
char buf_[64];
|
char buf_[64];
|
||||||
|
13
firmware/src/maq430_sensor.h
Normal file
13
firmware/src/maq430_sensor.h
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <sensors/MagneticSensorSPI.h>
|
||||||
|
|
||||||
|
/** Configured fro 12bit MA710 and MAQ430 magnetic sensor over SPI interface*/
|
||||||
|
MagneticSensorSPIConfig_s MAQ430_SPI = {
|
||||||
|
.spi_mode = SPI_MODE3,
|
||||||
|
.clock_speed = 1000000,
|
||||||
|
.bit_resolution = 12,
|
||||||
|
.angle_register = 0x0000,
|
||||||
|
.data_start_bit = 15,
|
||||||
|
.command_rw_bit = 0, // not required
|
||||||
|
.command_parity_bit = 17 // parity not implemented
|
||||||
|
};
|
@ -3,12 +3,14 @@
|
|||||||
#include "motor_task.h"
|
#include "motor_task.h"
|
||||||
#if SENSOR_MT6701
|
#if SENSOR_MT6701
|
||||||
#include "mt6701_sensor.h"
|
#include "mt6701_sensor.h"
|
||||||
#endif
|
#elif SENSOR_TLV
|
||||||
#if SENSOR_TLV
|
|
||||||
#include "tlv_sensor.h"
|
#include "tlv_sensor.h"
|
||||||
|
#elif SENSOR_MAQ430
|
||||||
|
#include "maq430_sensor.h"
|
||||||
#endif
|
#endif
|
||||||
#include "util.h"
|
|
||||||
|
|
||||||
|
#include "motors/motor_config.h"
|
||||||
|
#include "util.h"
|
||||||
|
|
||||||
// ####
|
// ####
|
||||||
// Hardware-specific motor calibration constants.
|
// Hardware-specific motor calibration constants.
|
||||||
@ -41,6 +43,8 @@ MotorTask::~MotorTask() {}
|
|||||||
TlvSensor encoder = TlvSensor();
|
TlvSensor encoder = TlvSensor();
|
||||||
#elif SENSOR_MT6701
|
#elif SENSOR_MT6701
|
||||||
MT6701Sensor encoder = MT6701Sensor();
|
MT6701Sensor encoder = MT6701Sensor();
|
||||||
|
#elif SENSOR_MAQ430
|
||||||
|
MagneticSensorSPI encoder = MagneticSensorSPI(MAQ430_SPI, PIN_MAQ_SS);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void MotorTask::run() {
|
void MotorTask::run() {
|
||||||
@ -50,26 +54,32 @@ void MotorTask::run() {
|
|||||||
|
|
||||||
#if SENSOR_TLV
|
#if SENSOR_TLV
|
||||||
encoder.init(&Wire, false);
|
encoder.init(&Wire, false);
|
||||||
#endif
|
#elif SENSOR_MT6701
|
||||||
|
|
||||||
#if SENSOR_MT6701
|
|
||||||
encoder.init();
|
encoder.init();
|
||||||
|
#elif SENSOR_MAQ430
|
||||||
|
SPIClass* spi = new SPIClass(HSPI);
|
||||||
|
spi->begin(PIN_MAQ_SCK, PIN_MAQ_MISO, PIN_MAQ_MOSI, PIN_MAQ_SS);
|
||||||
|
encoder.init(spi);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
motor.linkDriver(&driver);
|
motor.linkDriver(&driver);
|
||||||
|
|
||||||
motor.controller = MotionControlType::torque;
|
motor.controller = MotionControlType::torque;
|
||||||
motor.voltage_limit = 5;
|
motor.voltage_limit = FOC_VOLTAGE_LIMIT;
|
||||||
motor.velocity_limit = 10000;
|
motor.velocity_limit = 10000;
|
||||||
motor.linkSensor(&encoder);
|
motor.linkSensor(&encoder);
|
||||||
|
|
||||||
// Not actually using the velocity loop built into SimpleFOC; but I'm using those PID variables
|
// 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)
|
// to run PID for torque (and SimpleFOC studio supports updating them easily over serial for tuning)
|
||||||
motor.PID_velocity.P = 4;
|
motor.PID_velocity.P = FOC_PID_P;
|
||||||
motor.PID_velocity.I = 0;
|
motor.PID_velocity.I = FOC_PID_I;
|
||||||
motor.PID_velocity.D = 0.04;
|
motor.PID_velocity.D = FOC_PID_D;
|
||||||
motor.PID_velocity.output_ramp = 10000;
|
motor.PID_velocity.output_ramp = FOC_PID_OUTPUT_RAMP;
|
||||||
motor.PID_velocity.limit = 10;
|
motor.PID_velocity.limit = FOC_PID_LIMIT;
|
||||||
|
|
||||||
|
#ifdef FOC_LPF
|
||||||
|
motor.LPF_angle.Tf = FOC_LPF;
|
||||||
|
#endif
|
||||||
|
|
||||||
motor.init();
|
motor.init();
|
||||||
|
|
||||||
@ -371,6 +381,8 @@ void MotorTask::calibrate() {
|
|||||||
log("NO, Direction=CCW");
|
log("NO, Direction=CCW");
|
||||||
motor.initFOC(0, Direction::CCW);
|
motor.initFOC(0, Direction::CCW);
|
||||||
}
|
}
|
||||||
|
snprintf(buf_, sizeof(buf_), " (start was %.1f, end was %.1f)", start_sensor, end_sensor);
|
||||||
|
log(buf_);
|
||||||
|
|
||||||
|
|
||||||
// #### Determine pole-pairs
|
// #### Determine pole-pairs
|
||||||
@ -378,7 +390,7 @@ void MotorTask::calibrate() {
|
|||||||
uint8_t electrical_revolutions = 20;
|
uint8_t electrical_revolutions = 20;
|
||||||
snprintf(buf_, sizeof(buf_), "Going to measure %d electrical revolutions...", electrical_revolutions);
|
snprintf(buf_, sizeof(buf_), "Going to measure %d electrical revolutions...", electrical_revolutions);
|
||||||
log(buf_);
|
log(buf_);
|
||||||
motor.voltage_limit = 5;
|
motor.voltage_limit = FOC_VOLTAGE_LIMIT;
|
||||||
motor.move(a);
|
motor.move(a);
|
||||||
log("Going to electrical zero...");
|
log("Going to electrical zero...");
|
||||||
float destination = a + _2PI;
|
float destination = a + _2PI;
|
||||||
@ -428,7 +440,7 @@ void MotorTask::calibrate() {
|
|||||||
|
|
||||||
// #### Determine mechanical offset to electrical zero
|
// #### Determine mechanical offset to electrical zero
|
||||||
// Measure mechanical angle at every electrical zero for several revolutions
|
// Measure mechanical angle at every electrical zero for several revolutions
|
||||||
motor.voltage_limit = 5;
|
motor.voltage_limit = FOC_VOLTAGE_LIMIT;
|
||||||
motor.move(a);
|
motor.move(a);
|
||||||
float offset_x = 0;
|
float offset_x = 0;
|
||||||
float offset_y = 0;
|
float offset_y = 0;
|
||||||
@ -478,7 +490,7 @@ void MotorTask::calibrate() {
|
|||||||
// TODO: save to non-volatile storage
|
// TODO: save to non-volatile storage
|
||||||
motor.pole_pairs = measured_pole_pairs;
|
motor.pole_pairs = measured_pole_pairs;
|
||||||
motor.zero_electric_angle = avg_offset_angle + _3PI_2;
|
motor.zero_electric_angle = avg_offset_angle + _3PI_2;
|
||||||
motor.voltage_limit = 5;
|
motor.voltage_limit = FOC_VOLTAGE_LIMIT;
|
||||||
motor.controller = MotionControlType::torque;
|
motor.controller = MotionControlType::torque;
|
||||||
|
|
||||||
log("\n\nRESULTS:\n Update these constants at the top of " __FILE__);
|
log("\n\nRESULTS:\n Update these constants at the top of " __FILE__);
|
||||||
|
12
firmware/src/motors/mad2804.h
Normal file
12
firmware/src/motors/mad2804.h
Normal file
@ -0,0 +1,12 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
// Tuning parameters for the MAD2804 motor (orange stator).
|
||||||
|
|
||||||
|
#define FOC_PID_P 1
|
||||||
|
#define FOC_PID_I 0
|
||||||
|
#define FOC_PID_D 0.148
|
||||||
|
#define FOC_PID_OUTPUT_RAMP 5000
|
||||||
|
#define FOC_PID_LIMIT 3
|
||||||
|
|
||||||
|
#define FOC_VOLTAGE_LIMIT 3
|
||||||
|
#define FOC_LPF 0.0075
|
9
firmware/src/motors/motor_config.h
Normal file
9
firmware/src/motors/motor_config.h
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#if MOTOR_WANZHIDA_ONCE_TOP
|
||||||
|
#include "motors/wanzhida_once_top.h"
|
||||||
|
#elif MOTOR_MAD2804
|
||||||
|
#include "motors/mad2804.h"
|
||||||
|
#else
|
||||||
|
#error "No motor configuration specified!"
|
||||||
|
#endif
|
12
firmware/src/motors/wanzhida_once_top.h
Normal file
12
firmware/src/motors/wanzhida_once_top.h
Normal file
@ -0,0 +1,12 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
// Tuning parameters for the Wanzhida/Oncetop OT-EM3215D2450Y1R,
|
||||||
|
// sold by SparkFun (ROB-20441).
|
||||||
|
|
||||||
|
#define FOC_PID_P 4
|
||||||
|
#define FOC_PID_I 0
|
||||||
|
#define FOC_PID_D 0.04
|
||||||
|
#define FOC_PID_OUTPUT_RAMP 10000
|
||||||
|
#define FOC_PID_LIMIT 10
|
||||||
|
|
||||||
|
#define FOC_VOLTAGE_LIMIT 5
|
@ -24,29 +24,29 @@ monitor_flags =
|
|||||||
--eol=CRLF
|
--eol=CRLF
|
||||||
--echo
|
--echo
|
||||||
--filter=esp32_exception_decoder
|
--filter=esp32_exception_decoder
|
||||||
|
upload_speed = 921600
|
||||||
lib_deps =
|
lib_deps =
|
||||||
askuric/Simple FOC @ 2.2.0
|
|
||||||
infineon/TLV493D-Magnetic-Sensor @ 1.0.3
|
infineon/TLV493D-Magnetic-Sensor @ 1.0.3
|
||||||
bxparks/AceButton @ 1.9.1
|
bxparks/AceButton @ 1.9.1
|
||||||
|
bakercp/PacketSerial @ 1.4.0
|
||||||
|
nanopb/Nanopb @ 0.4.6 ; Ideally this would reference the nanopb submodule, but that would require
|
||||||
|
; everyone to check out submodules to just compile, so we use the library
|
||||||
|
; registry for the runtime. The submodule is available for manually updating
|
||||||
|
; the pre-compiled (checked in) .pb.h/c files when proto files change, but is
|
||||||
|
; otherwise not used during application firmware compilation.
|
||||||
build_flags =
|
build_flags =
|
||||||
-DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
|
-DMONITOR_SPEED=921600
|
||||||
-DMONITOR_SPEED=921600
|
|
||||||
|
|
||||||
[env:view]
|
[env:view]
|
||||||
extends = base_config
|
extends = base_config
|
||||||
board = esp32doit-devkit-v1
|
board = esp32doit-devkit-v1
|
||||||
lib_deps =
|
lib_deps =
|
||||||
${base_config.lib_deps}
|
${base_config.lib_deps}
|
||||||
|
askuric/Simple FOC @ 2.2.0
|
||||||
bodmer/TFT_eSPI@2.4.25
|
bodmer/TFT_eSPI@2.4.25
|
||||||
fastled/FastLED @ 3.5.0
|
fastled/FastLED @ 3.5.0
|
||||||
bogde/HX711 @ 0.7.5
|
bogde/HX711 @ 0.7.5
|
||||||
adafruit/Adafruit VEML7700 Library @ 1.1.1
|
adafruit/Adafruit VEML7700 Library @ 1.1.1
|
||||||
bakercp/PacketSerial @ 1.4.0
|
|
||||||
nanopb/Nanopb @ 0.4.6 ; Ideally this would reference the nanopb submodule, but that would require
|
|
||||||
; everyone to check out submodules to just compile, so we use the library
|
|
||||||
; registry for the runtime. The submodule is available for manually updating
|
|
||||||
; the pre-compiled (checked in) .pb.h/c files when proto files change, but is
|
|
||||||
; otherwise not used during application firmware compilation.
|
|
||||||
|
|
||||||
build_flags =
|
build_flags =
|
||||||
${base_config.build_flags}
|
${base_config.build_flags}
|
||||||
@ -58,13 +58,16 @@ build_flags =
|
|||||||
-DSK_LEDS=1
|
-DSK_LEDS=1
|
||||||
; Number of LEDs
|
; Number of LEDs
|
||||||
-DNUM_LEDS=8
|
-DNUM_LEDS=8
|
||||||
-DSENSOR_MT6701=1
|
|
||||||
; Strain-gauge press input enabled: 1=enable, 0=disable
|
; Strain-gauge press input enabled: 1=enable, 0=disable
|
||||||
-DSK_STRAIN=1
|
-DSK_STRAIN=1
|
||||||
; Invert direction of angle sensor (motor direction is detected relative to angle sensor as part of the calibration procedure)
|
|
||||||
-DSK_INVERT_ROTATION=1
|
|
||||||
; Ambient light sensor (VEML7700) enabled: 1=enable (display/LEDs match ambient brightness), 0=disable (100% brightness all the time)
|
; Ambient light sensor (VEML7700) enabled: 1=enable (display/LEDs match ambient brightness), 0=disable (100% brightness all the time)
|
||||||
-DSK_ALS=1
|
-DSK_ALS=1
|
||||||
|
; Use MT6701 magnetic encoder
|
||||||
|
-DSENSOR_MT6701=1
|
||||||
|
; Invert direction of angle sensor (motor direction is detected relative to angle sensor as part of the calibration procedure)
|
||||||
|
-DSK_INVERT_ROTATION=1
|
||||||
|
|
||||||
|
-DMOTOR_WANZHIDA_ONCE_TOP=1
|
||||||
|
|
||||||
; Pin configurations
|
; Pin configurations
|
||||||
-DPIN_UH=26
|
-DPIN_UH=26
|
||||||
@ -108,6 +111,8 @@ build_flags =
|
|||||||
; Reduce loop task stack size (only works on newer IDF Arduino core)
|
; Reduce loop task stack size (only works on newer IDF Arduino core)
|
||||||
; -DARDUINO_LOOP_STACK_SIZE=2048
|
; -DARDUINO_LOOP_STACK_SIZE=2048
|
||||||
|
|
||||||
|
-DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
|
||||||
|
|
||||||
; FastLED setup
|
; FastLED setup
|
||||||
; Modify the default unusable pin mask to allow GPIO 7 (allowed to use on ESP32-PICO-V3-02)
|
; Modify the default unusable pin mask to allow GPIO 7 (allowed to use on ESP32-PICO-V3-02)
|
||||||
; Unusable bits: 6, 8, 9, 10, 20
|
; Unusable bits: 6, 8, 9, 10, 20
|
||||||
@ -119,3 +124,61 @@ build_flags =
|
|||||||
; GPIO >= 34 are input only
|
; 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)))
|
; (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
|
-DSOC_GPIO_VALID_OUTPUT_GPIO_MASK=0x30EFFFFFF
|
||||||
|
|
||||||
|
|
||||||
|
[env:nanofoc]
|
||||||
|
extends = base_config
|
||||||
|
platform = espressif32
|
||||||
|
board = adafruit_feather_esp32s3
|
||||||
|
lib_deps =
|
||||||
|
${base_config.lib_deps}
|
||||||
|
askuric/Simple FOC@^2.3.0
|
||||||
|
bodmer/TFT_eSPI@2.5.0
|
||||||
|
|
||||||
|
build_flags =
|
||||||
|
${base_config.build_flags}
|
||||||
|
; Display enabled: 1=enable, 0=disable
|
||||||
|
-DSK_DISPLAY=0
|
||||||
|
; Display orientation: 0=usb bottom, 2=usb top
|
||||||
|
-DSK_DISPLAY_ROTATION=0
|
||||||
|
; LEDs enabled: 1=enable, 0=disable
|
||||||
|
-DSK_LEDS=0
|
||||||
|
; Number of LEDs
|
||||||
|
-DNUM_LEDS=8
|
||||||
|
; Strain-gauge press input enabled: 1=enable, 0=disable
|
||||||
|
-DSK_STRAIN=0
|
||||||
|
; Ambient light sensor (VEML7700) enabled: 1=enable (display/LEDs match ambient brightness), 0=disable (100% brightness all the time)
|
||||||
|
-DSK_ALS=0
|
||||||
|
|
||||||
|
-DSENSOR_MAQ430=1
|
||||||
|
-DPIN_MAQ_SCK=6
|
||||||
|
-DPIN_MAQ_MISO=7
|
||||||
|
-DPIN_MAQ_MOSI=5
|
||||||
|
-DPIN_MAQ_SS=4
|
||||||
|
; Invert direction of angle sensor (motor direction is detected relative to angle sensor as part of the calibration procedure)
|
||||||
|
-DSK_INVERT_ROTATION=1
|
||||||
|
|
||||||
|
-DMOTOR_MAD2804=1
|
||||||
|
|
||||||
|
; Pin configurations
|
||||||
|
-DPIN_UH=21
|
||||||
|
-DPIN_UL=12
|
||||||
|
-DPIN_VH=14
|
||||||
|
-DPIN_VL=10
|
||||||
|
-DPIN_WH=13
|
||||||
|
-DPIN_WL=11
|
||||||
|
-DPIN_BUTTON_NEXT=-1
|
||||||
|
-DPIN_BUTTON_PREV=-1
|
||||||
|
-DPIN_LED_DATA=7
|
||||||
|
-DPIN_LCD_BACKLIGHT=08
|
||||||
|
|
||||||
|
-DPIO_FRAMEWORK_ARDUINO_ENABLE_CDC=1
|
||||||
|
-DUSBCON=1
|
||||||
|
-DARDUINO_USB_CDC_ON_BOOT=1
|
||||||
|
-DARDUINO_USB_MODE=1
|
||||||
|
-DCORE_DEBUG_LEVEL=2
|
||||||
|
-DHSPI_SPEED=100000 ; MA/MAQ Nominal SPI Speed in Mhz (HSPI)
|
||||||
|
-DVSPI_SPEED=400000 ; TFt Nominal SPI Speed in Mhz (VSPI)
|
||||||
|
|
||||||
|
; Reduce loop task stack size (only works on newer IDF Arduino core)
|
||||||
|
; -DARDUINO_LOOP_STACK_SIZE=2048
|
||||||
|
Loading…
Reference in New Issue
Block a user