nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cyphal wip

Browse Source
This commit is contained in:
Nils Schulte 2024-07-24 15:10:39 +02:00
parent c4bd84cb2d
commit 64a00c5dde
10 changed files with 11199 additions and 5130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
fw-cyphal/README.md Normal file
View File

@ -0,0 +1,5 @@
## Develop
create `compile_commands.json`:
´´´
pio run -t compiledb
´´´

35
fw-cyphal/platformio.ini Normal file
View File

@ -0,0 +1,35 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:nilsdriverv1]
# platform = espressif32
platform = https://github.com/cziter15/platform-espressif32.git#68ad40f6df654fe4b8d0a50982b810df5b49b677
#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.10/platform-espressif32.zip
# board = adafruit_feather_esp32s3
board = lolin_s3_mini
framework = arduino
upload_protocol = esptool
upload_port = /dev/ttyACM*
# board_build.mcu = esp32s3
lib_archive = false
;platform_packages = espressif/toolchain-xtensa-esp32@8.4.0+2021r2-patch5
monitor_speed = 115200
lib_deps =
https://github.com/simplefoc/Arduino-FOC.git#dev
https://github.com/simplefoc/Arduino-FOC-drivers.git#dev
https://github.com/handmade0octopus/ESP32-TWAI-CAN
; https://github.com/simplefoc/Arduino-FOC-dcmotor.git
; https://github.com/eric-wieser/packet-io.git
; https://git.nilsschulte.de/nils/simplesync.git
; https://github.com/Tinyu-Zhao/INA3221.git
build_flags = -DRADIOLIB_EEPROM_UNSUPPORTED
;-DSIMPLEFOC_ESP32_USELEDC

204
fw-cyphal/src/DRV8323S.hpp Normal file
View File

@ -0,0 +1,204 @@
#ifndef _DRV8323S_H
#define _DRV8323S_H
#include <Arduino.h>
#include <SPI.h>
#include <cstdint>
#include <drivers/BLDCDriver3PWM.h>
enum {
DRV8323S_FAULT_STATUS1_REG = 0x00,
DRV8323S_FAULT_STATUS2_REG = 0x01,
DRV8323S_DRIVER_CONTROL_REG = 0x02,
DRV8323S_GATE_DRIVE_HS_REG = 0x03,
DRV8323S_GATE_DRIVE_LS_REG = 0x04,
DRV8323S_OCP_CONTROL_REG = 0x05,
DRV8323S_CSA_CONTROL_REG = 0x06,
};
enum /* 0x00 */ {
DRV8323S_FAULT_STATUS1_FAULT_MASK = 1 << 10, /* logic or of all faults */
DRV8323S_FAULT_STATUS1_VDS_OCP_MASK = 1 << 9, /* vds monitor overcurrent */
DRV8323S_FAULT_STATUS1_GDF_MASK = 1 << 8, /* gate drive fault */
DRV8323S_FAULT_STATUS1_UVLO_MASK = 1 << 7, /* undervoltage lockout fault */
DRV8323S_FAULT_STATUS1_OTSD_MASK = 1 << 6, /* over temperature shutdown */
DRV8323S_FAULT_STATUS1_VDS_HA_MASK = 1 << 5, /* overcurrent on FET */
DRV8323S_FAULT_STATUS1_VDS_LA_MASK = 1 << 4,
DRV8323S_FAULT_STATUS1_VDS_HB_MASK = 1 << 3,
DRV8323S_FAULT_STATUS1_VDS_LB_MASK = 1 << 2,
DRV8323S_FAULT_STATUS1_VDS_HC_MASK = 1 << 1,
DRV8323S_FAULT_STATUS1_VDS_LC_MASK = 1 << 0,
};
enum /* 0x01 */ {
DRV8323S_FAULT_STATUS1_SA_OC_MASK = 1 << 10, /* Sense amp OC */
DRV8323S_FAULT_STATUS1_SB_OC_MASK = 1 << 9,
DRV8323S_FAULT_STATUS1_SC_OC_MASK = 1 << 8,
DRV8323S_FAULT_STATUS1_OTW_MASK = 1 << 7, /* over temp warning */
DRV8323S_FAULT_STATUS1_CPUV_MASK = 1 << 6, /* charge pump fault */
DRV8323S_FAULT_STATUS1_VGS_HA_MASK = 1 << 5, /* FET gate drive failt */
DRV8323S_FAULT_STATUS1_VGS_LA_MASK = 1 << 4,
DRV8323S_FAULT_STATUS1_VGS_HB_MASK = 1 << 3,
DRV8323S_FAULT_STATUS1_VGS_LB_MASK = 1 << 2,
DRV8323S_FAULT_STATUS1_VGS_HC_MASK = 1 << 1,
DRV8323S_FAULT_STATUS1_VGS_LC_MASK = 1 << 0,
};
enum /* 0x02 */ {
DRV8323S_DRIVER_CONTROL_DIS_CPUV_MASK =
1 << 9, /* chrg pump uvlo fault disable */
DRV8323S_DRIVER_CONTROL_DIS_GDF_MASK = 1 << 8, /* gate drive fault disable */
DRV8323S_DRIVER_CONTROL_OTW_REP_MASK = 1
<< 7, /* over temp wariing reported */
DRV8323S_DRIVER_CONTROL_PWM_MODE_MASK = 0b11 << 5, /* PWM mode */
DRV8323S_DRIVER_CONTROL_1PWM_COM_MASK = 1 << 4, /* 1PWM mode diode freewheel*/
DRV8323S_DRIVER_CONTROL_1PWM_DIR_MASK = 1 << 3, /* 1PWM mode direction */
DRV8323S_DRIVER_CONTROL_COAST_MASK = 1 << 2, /* all FETs in Hi-Z */
DRV8323S_DRIVER_CONTROL_BRAKE_MASK = 1 << 1, /* all low FETs on */
DRV8323S_DRIVER_CONTROL_CLR_FLT_MASK = 1 << 1, /* clear latched fault bits */
};
enum /* 0x03 */ {
DRV8323S_GATE_DRIVE_HS_LOCK_MASK = 0b111 << 8, /* lock register */
DRV8323S_GATE_DRIVE_HS_IDRIVEP_HS_MASK = 0b1111 << 4, /* HS positiv current */
DRV8323S_GATE_DRIVE_HS_IDRIVEN_HS_MASK = 0b1111 << 0, /* HS negativ current */
};
enum /* 0x04 */ {
DRV8323S_GATE_DRIVE_LS_CBC_MASK = 1 << 10, /* cycle-by cycle operation */
DRV8323S_GATE_DRIVE_LS_TDRIVE_MASK = 0b11 << 8, /* peak gate curr drive t*/
DRV8323S_GATE_DRIVE_LS_IDRIVEP_LS_MASK = 0b1111 << 4, /* LS positiv current */
DRV8323S_GATE_DRIVE_LS_IDRIVEN_LS_MASK = 0b1111 << 0, /* LS negativ current */
};
enum /* 0x05 */ {
DRV8323S_OCP_CONTROL_TRETRY_MASK = 1 << 10, /* 0=4ms, 1=50us */
DRV8323S_OCP_CONTROL_DEAD_TIME_MASK = 0b11 << 8, /* 50ns*2**x dead time */
DRV8323S_OCP_CONTROL_OCP_MODE_MASK = 0b11 << 6, /* OCP Mode */
DRV8323S_OCP_CONTROL_OCP_DEG_MASK = 0b11 << 4, /* OCP deglitch t=2ns*(1+x)*/
DRV8323S_OCP_CONTROL_VDS_LVL_MASK = 0b1111 << 0, /* VDS LVL */
};
enum /* 0x06 */ {
DRV8323S_CSA_CONTROL_CSA_FET_MASK = 1 << 10, /* 0:sense posi in = SPx 1:SHx */
DRV8323S_CSA_CONTROL_VREF_DIV_MASK = 1 << 9, /* sense ref is Vref/2 */
DRV8323S_CSA_CONTROL_LS_REF_MASK = 1
<< 8, /* 0:VDS_OCP over SHx-SPx 1:SHx-SNx*/
DRV8323S_CSA_CONTROL_CSA_GAIN_MASK = 0b11 << 6, /* 2**x*5V/V */
DRV8323S_CSA_CONTROL_DIS_SEN_MASK = 1 << 5, /* disable OC sense */
DRV8323S_CSA_CONTROL_CSA_CAL_A_MASK = 1 << 4, /* cal sense offset A */
DRV8323S_CSA_CONTROL_CSA_CAL_B_MASK = 1 << 3, /* cal sense offset B */
DRV8323S_CSA_CONTROL_CSA_CAL_C_MASK = 1 << 2, /* cal sense offset C */
DRV8323S_CSA_CONTROL_SEN_LVL_MASK = 0b11 << 6, /* Sense OCP = 0.25V*(1+x)*/
};
enum drv8323s_pwm_mode_e {
DRV8323S_PWM_MODE_6PWM = 0b00 << 5,
DRV8323S_PWM_MODE_3PWM = 0b01 << 5,
DRV8323S_PWM_MODE_1PWM = 0b10 << 5,
DRV8323S_PWM_MODE_INDEPENDENT_PWM = 0b11 << 5,
};
enum drv8323s_lock_sequence_e {
DRV8323S_LOCK = 0b011 << 8,
DRV8323S_UNLOCK = 0b110 << 8,
};
enum drv8323s_idrive_shift_e {
DRV8323S_IDRIVEP = 4,
DRV8323S_IDRIVEN = 0,
};
unsigned int drv8323s_idrivep_mA_map[] = {
10, 30, 60, 80, 120, 140, 170, 190, 260, 330, 370, 440, 570, 680, 820, 1000,
};
unsigned int drv8323s_idriven_mA_map[] = {
20, 60, 120, 160, 240, 280, 340, 380,
520, 660, 740, 880, 1140, 1360, 1640, 2000,
};
struct drv8323s_foc_driver {
SPIClass *spi;
int CSn, en;
BLDCDriver3PWM *focdriver;
};
void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
int CSn, int en, SPIClass *spi) {
dev->CSn = CSn;
pinMode(CSn, OUTPUT);
dev->en = en;
pinMode(dev->en, OUTPUT);
digitalWrite(dev->en, 1);
// dev->focdriver = new BLDCDriver3PWM(phA, phB, phC, en);
dev->spi = spi;
/* Set 3PWM Mode */
}
uint16_t drv8323s_read_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
uint16_t *result) {
digitalWrite(dev->CSn, 0);
dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0));
uint16_t data =((uint16_t) (1 << 15)) | ((((uint16_t)addr) ) << 11);
*result = dev->spi->transfer16(data);
// *result = data;
dev->spi->endTransaction();
digitalWrite(dev->CSn, 1);
// Serial.print("SPI Read Result: ");
// Serial.print(data, HEX);
// Serial.print(" -> ");
// Serial.println(result, HEX);
return 0;
}
uint16_t drv8323s_write_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
uint16_t value, uint16_t *result) {
digitalWrite(dev->CSn, 0);
dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0));
uint16_t data = ((addr & 0b1111) << 11) | (value & 0b1111111111);
*result = dev->spi->transfer16(data);
dev->spi->endTransaction();
digitalWrite(dev->CSn, 1);
// Serial.print("SPI Write Result: ");
// Serial.print(data, HEX);
// Serial.print(" -> ");
// Serial.println(result, HEX);
return 0;
}
void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i,
int negative_i, bool high_side_fet) {
uint16_t reg_value = 0x0; // TODO get reg
uint16_t reg =
high_side_fet ? DRV8323S_GATE_DRIVE_HS_REG : DRV8323S_GATE_DRIVE_LS_REG;
drv8323s_read_spi(dev, reg, &reg_value);
if (high_side_fet) {
reg_value &= ~DRV8323S_GATE_DRIVE_HS_IDRIVEP_HS_MASK;
reg_value &= ~DRV8323S_GATE_DRIVE_HS_IDRIVEN_HS_MASK;
} else {
reg_value &= ~DRV8323S_GATE_DRIVE_LS_IDRIVEP_LS_MASK;
reg_value &= ~DRV8323S_GATE_DRIVE_LS_IDRIVEN_LS_MASK;
}
int i;
for (i = 0;
i < sizeof drv8323s_idrivep_mA_map / sizeof(drv8323s_idrivep_mA_map[0]);
i += 1) {
if (drv8323s_idrivep_mA_map[i] > positive_i)
break;
}
reg_value |= i << DRV8323S_IDRIVEP;
for (i = 0;
i < sizeof drv8323s_idriven_mA_map / sizeof(drv8323s_idriven_mA_map[0]);
i += 1) {
if (drv8323s_idriven_mA_map[i] > negative_i)
break;
}
reg_value |= i << DRV8323S_IDRIVEN;
uint16_t result;
drv8323s_write_spi(dev, reg, reg_value, &result);
// TODO SPI call
}
#endif /* _DRV8323S_H */

143
fw-cyphal/src/esp32twaican.c Normal file
View File

@ -0,0 +1,143 @@
/// This software is distributed under the terms of the MIT License.
/// Copyright (c) 2020 OpenCyphal
/// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>, Tom De Rybel
/// <tom.derybel@robocow.be>
// This is needed to enable the necessary declarations in sys/
#include <string.h>
#include "esp32twaican.h"
#include "driver/twai.h"
int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx,
const CanardFilter *filter_config) {
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(
(gpio_num_t)pin_tx, (gpio_num_t)pin_rx, TWAI_MODE_NORMAL);
g_config.rx_queue_len = 10;
g_config.tx_queue_len = 10;
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
if (filter_config) {
f_config.acceptance_code = filter_config->extended_can_id;
f_config.acceptance_mask = filter_config->extended_mask;
f_config.single_filter = true;
}
int ret = twai_driver_install(&g_config, &t_config, &f_config);
if (ret != ESP_OK) {
// Serial.println("CAN1 Driver initialized");
return ret;
}
ret = twai_start();
if (ret != ESP_OK) {
return ret;
}
uint32_t alerts_to_enable = TWAI_ALERT_TX_IDLE | TWAI_ALERT_TX_SUCCESS |
TWAI_ALERT_TX_FAILED | TWAI_ALERT_RX_QUEUE_FULL |
TWAI_ALERT_RX_DATA | TWAI_ALERT_ERR_PASS |
TWAI_ALERT_BUS_ERROR;
ret = twai_reconfigure_alerts(alerts_to_enable, NULL);
if (ret != ESP_OK) {
// Serial.println("Failed to reconfigure alerts");
return ret;
}
return 0;
}
int16_t esp32twaicanPush(const CanardFrame *const frame,
const CanardMicrosecond timeout_usec) {
if ((frame == NULL) || (frame->payload == NULL) ||
(frame->payload_size > UINT8_MAX)) {
return -1;
}
// Queue message for transmission
twai_message_t message;
message.extd = 1; // extended can id
message.identifier = frame->extended_can_id;
message.rtr = 0;
message.data_length_code = frame->payload_size;
memcpy(&message.data[0], &frame->payload[0], (int)frame->payload_size);
int ret =
twai_transmit(&message, (timeout_usec * (1000000 / configTICK_RATE_HZ)));
if (ret == ESP_OK) {
return 1;
// Serial.println("CAN1: Message queued for transmission");
} else if (ret == ESP_ERR_TIMEOUT) {
/* Queue full and timed out waiting for empty slot */
return 0;
}
return -1;
}
int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame,
CanardMicrosecond *const out_timestamp_usec,
const size_t payload_buffer_size,
void *const payload_buffer,
const CanardMicrosecond timeout_usec,
bool *const loopback) {
if ((out_frame == NULL) || (payload_buffer == NULL)) {
return -1;
}
uint32_t alerts_triggered;
twai_read_alerts(&alerts_triggered,
(timeout_usec * (1000000 / configTICK_RATE_HZ)));
twai_status_info_t twaistatus;
twai_get_status_info(&twaistatus);
if (alerts_triggered & TWAI_ALERT_RX_DATA) {
twai_message_t message;
if (twai_receive(&message, 0) == ESP_OK) {
// Non-blocking receive messages from the socket and validate.
const ssize_t read_size = message.data_length_code;
if (read_size < 0) {
return -1; // Error occurred -- return the negated error
// code.
}
if (read_size > payload_buffer_size) {
return -1;
}
const bool valid = (message.extd) && // Extended frame
(message.rtr) && // Not RTR frame
(true /* error frame */); // Not error frame
if (!valid) {
return 0; // Not an extended data frame -- drop silently and return
// early.
}
// Handle the loopback frame logic.
const bool loopback_frame = false;
// ((uint32_t)msg.msg_flags & (uint32_t)MSG_CONFIRM) != 0;
if (loopback == NULL && loopback_frame) {
return 0; // The loopback pointer is NULL and this is a loopback frame
// -- drop silently and return early.
}
if (loopback != NULL) {
*loopback = loopback_frame;
}
// Obtain the CAN frame time stamp from the kernel.
if (NULL != out_timestamp_usec) {
// struct timeval tv;
// gettimeofday(&tv, NULL);
// (void)memset(out_frame, 0, sizeof(CanardFrame));
// *out_timestamp_usec = (CanardMicrosecond)(((uint64_t)tv.tv_sec *
// MEGA)
// + (uint64_t)tv.tv_usec);
*out_timestamp_usec =
(CanardMicrosecond)(xTaskGetTickCount() *
(1000000 / configTICK_RATE_HZ));
}
out_frame->extended_can_id = sockcan_frame.can_id & CAN_EFF_MASK;
out_frame->payload_size = sockcan_frame.len;
out_frame->payload = payload_buffer;
(void)memcpy(payload_buffer, &message.data[0], message.data_length_code);
return 1;
}
}
return 0;
}

84
fw-cyphal/src/esp32twaican.h Normal file
View File

@ -0,0 +1,84 @@
/// ____ ______ __ __
/// / __ `____ ___ ____ / ____/_ ______ / /_ ____
/// / /
/// / / / / __ `/ _ `/ __ `/ / / / / / __ `/ __ `/ __
/// `/ /
/// / /_/ / /_/ / __/ / / / /___/ /_/ / /_/ / / / / /_/
/// / /
/// `____/ .___/`___/_/ /_/`____/`__, / .___/_/
/// /_/`__,_/_/
/// /_/ /____/_/
///
/// This is a basic adapter library that bridges Libcanard with SocketCAN.
/// Read the API documentation for usage information.
///
/// To integrate the library into your application, just copy-paste the c/h
/// files into your project tree.
///
/// --------------------------------------------------------------------------------------------------------------------
/// Changelog
///
/// v3.0 - Update for compatibility with Libcanard v3.
///
/// v2.0 - Added loop-back functionality.
/// API change in esp32twaicanPop(): loopback flag added.
/// - Changed to kernel-based time-stamping for received frames for
/// improved accuracy.
/// API change in esp32twaicanPop(): time stamp clock source is now
/// CLOCK_REALTIME, vs CLOCK_TAI before.
///
/// v1.0 - Initial release
/// --------------------------------------------------------------------------------------------------------------------
///
/// This software is distributed under the terms of the MIT License.
/// Copyright (c) 2020 OpenCyphal
/// Author: Pavel Kirienko <pavel.kirienko@zubax.com>
#ifndef ESP32TWAICAN_H_INCLUDED
#define ESP32TWAICAN_H_INCLUDED
#include "canard.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx,
const CanardFilter *filter_config);
/// Enqueue a new extended CAN data frame for transmission.
/// Block until the frame is enqueued or until the timeout is expired.
/// Zero timeout makes the operation non-blocking.
/// Returns 1 on success, 0 on timeout, negated errno on error.
int16_t esp32twaicanPush(const CanardFrame *const frame,
const CanardMillisecond timeout_msec);
/// Fetch a new extended CAN data frame from the RX queue.
/// If the received frame is not an extended-ID data frame, it will be dropped
/// and the function will return early. The payload pointer of the returned
/// frame will point to the payload_buffer. It can be a stack-allocated array.
/// The payload_buffer_size shall be large enough (64 bytes is enough for CAN
/// FD), otherwise an error is returned. The received frame timestamp will be
/// set to CLOCK_REALTIME by the kernel, sampled near the moment of its arrival.
/// The loopback flag pointer is used to both indicate and control behavior when
/// a looped-back message is received. If the flag pointer is NULL, loopback
/// frames are silently dropped; if not NULL, they are accepted and indicated
/// using this flag.
/// The function will block until a frame is received or until the timeout is
/// expired. It may return early. Zero timeout makes the operation non-blocking.
/// Returns 1 on success, 0 on timeout, negated errno on error.
int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame,
CanardMicrosecond *const out_timestamp_usec,
const size_t payload_buffer_size,
void *const payload_buffer,
const CanardMicrosecond timeout_usec,
bool *const loopback);
#ifdef __cplusplus
}
#endif
#endif

382
fw-cyphal/src/main.cpp Normal file
View File

@ -0,0 +1,382 @@
// #include <Preferences.h>
#include "Arduino.h"
#include "SPI.h"
#include "USB.h"
// #include "Wire.h"
// #include "Wire.h"
#include <SimpleFOC.h>
// #include <INA3221.h>
#include "SimpleFOCDrivers.h"
#include "comms/streams/BinaryIO.h"
#include "encoders/esp32hwencoder/ESP32HWEncoder.h"
#include "esp32-hal-adc.h"
#include "motors/HybridStepperMotor/HybridStepperMotor.h"
#include "DRV8323S.hpp"
#include <cmath>
// #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"
#define ENC_A 11
#define ENC_B 8
#define I2C_SDA 18
#define I2C_SCL 0
#define DRVEN 15
#define TERMISTOR_PCB 3
#define TERMISTOR_EXT 9
#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 SOA 1
#define SOB 2
#define SOC 10
#define INHA 14
#define INHB 13
#define INHC 12
#define INLABC 46
#define CAN_TX 6
#define CAN_RX 7
#define SPI_MISO 45
#define SPI_MOSI 48
#define SPI_CLK 47
#define SPI_DRV_SC 21
#define LED_PIN 16
#define VSENSE_PIN 5
#define CAL_PIN 17
#define vdrive_read (analogRead(VSENSE_PIN ) *20.08f / 845)
//* 20.8f/ 21033.75)
// #define MOTOR
#define SSIF_USBSERIAL
#define FW_NO_WATCHDOG
#ifndef FW_NO_WATCHDOG
#include <esp_task_wdt.h>
#define WDT_TIMEOUT_s 1
// struct esp_task_wdt_config_t wd_config = {
// .timeout_ms = 100,
// .idle_core_mask = ~0x00000000,
// .trigger_panic = true
// };
#endif
const int voltage_lpf = 50;
// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
ESP32HWEncoder encoder =
ESP32HWEncoder(ENC_A, ENC_B, 2048 / 2); // The Index pin can be omitted
#ifdef MOTOR
struct drv8323s_foc_driver drv8323s;
BLDCMotor motor = BLDCMotor(100); // 24N22P
// 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));
#ifdef SSIF_USBSERIAL
USBCDC usbserial;
#define SSYNCIF usbserial
#else
#define SSYNCIF Serial1
#endif
// #define SSYNCIF Serial
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
Wire.beginTransmission(addr);
Wire.write(reg); // MAN:0x2E
int error = Wire.endTransmission();
if (error != 0) {
return;
}
delayMicroseconds(50);
int ret = Wire.requestFrom(addr, len);
Wire.readBytes(buf, ret);
return;
}
void write_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val) {
Wire.beginTransmission(addr);
uint8_t data[] = {reg_lsb, val};
Wire.write(data, sizeof data);
int error = Wire.endTransmission();
if (error != 0) {
return;
}
}
void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
Wire.beginTransmission(addr);
uint8_t data[] = {reg_lsb, val1, val2};
Wire.write(data, sizeof data);
int error = Wire.endTransmission();
if (error != 0) {
return;
}
}
SemaphoreHandle_t mutex;
struct sync_params {};
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
#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 setup() {
mutex = xSemaphoreCreateMutex();
#ifdef SSIF_USBSERIAL
SSYNCIF.begin();
USB.begin();
#else
SSYNCIF.begin(460800, SERIAL_8N1, 44, 43);
#endif
// prefs.begin("foc");
// SSYNCIF.print(__cplusplus);
// Wire.setTimeOut(10);
Wire.begin(I2C_SDA, I2C_SCL, 400000);
pinMode(LED_PIN, OUTPUT);
pinMode(SPI_DRV_SC, OUTPUT);
digitalWrite(LED_PIN, 1); // enable
pinMode(INHA, OUTPUT);
digitalWrite(INHA, 0); // enable
pinMode(INHB, OUTPUT);
digitalWrite(INHB, 0); // enable
pinMode(INHC, OUTPUT);
digitalWrite(INHC, 0); // enable
pinMode(INLABC, OUTPUT);
digitalWrite(INLABC, 0); // enable
pinMode(CAL_PIN, OUTPUT);
digitalWrite(CAL_PIN, 0); // enable
// write_i2c(0x36,0x07, 0b00000011);
write_i2c(0x36, 0x09, 0xFF);
encoder.init();
#ifdef MOTOR
// driver.voltage_power_supply = 15;
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
drv8323s_init(&drv8323s,INHA, INHB, INHC, SPI_DRV_SC, SPI_DRV_SC/*DRVEN*/, &SPI);
// driver.init(&SPI);
// status();
// motor.linkDriver(&driver);
// motor.linkSensor(&encoder);
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.voltage_sensor_align = 5;
motor.voltage_limit = 6;
motor.controller = MotionControlType::velocity_openloop;
// motor.init();
// motor.initFOC();
// sensor.init(&SPI);
// motors[i].linkSensor(&sensors[i]);
#endif
// 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.getFloat(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(),
// 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
// );
// esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts
// esp_task_wdt_add(NULL); // add current thread to WDT watch
}
void loop() {
#ifdef MOTOR
// motor.loopFOC();
// motor.move(3);
#endif
encoder.update(); // optional: Update manually if not using loopfoc()
static float angle = 0;
static float old_angle = 0;
// SSYNCIF.print(angle = encoder.getAngle());
#ifdef MOTOR
// SSYNCIF.print(" ");
uint16_t result=0;
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);
#endif
// SSYNCIF.print(" ");
// SSYNCIF.print(analogRead(TERMISTOR_PCB));
SSYNCIF.print(" ");
static float t = temp_pcb;
t= t*0.95 + 0.05*temp_pcb;
SSYNCIF.print(t);
SSYNCIF.print(" ");
SSYNCIF.println(vdrive_read);
if (abs(angle - old_angle) > 0.01) {
digitalWrite(LED_PIN, 0);
old_angle = angle;
}
delay(100);
digitalWrite(LED_PIN, 1);
// 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();

9
fw-cyphal/src/test.h Normal file
View File

@ -0,0 +1,9 @@
#include <Arduino.h>
#define TEST(x) \
if (!(x)) { \
Serial.printf("\033[31;1mFAILED:\033[22;39m %s:%d %s\n", __FILE__, __LINE__, #x); \
status = 1; \
} else { \
Serial.printf("\033[32;1mOK:\033[22;39m %s\n", #x); \
}

13635
pcb/stepper-closed-loop.kicad_pcb
View File

File diff suppressed because it is too large Load Diff

2
pcb/stepper-closed-loop.kicad_prl
View File

@ -1,6 +1,6 @@
{
"board": {
"active_layer": 0,
"active_layer": 37,
"active_layer_preset": "",
"auto_track_width": false,
"hidden_netclasses": [],

1830
pcb/stepper-closed-loop.kicad_sch
View File

File diff suppressed because it is too large Load Diff