From e876d7e56c35e5a175a26f52f812a4a0bb6d1d4e Mon Sep 17 00:00:00 2001 From: vanyabeat Date: Wed, 8 May 2024 10:05:29 +0000 Subject: [PATCH] Add sensor AS5045, new PCB 0.4.2 adoption --- .gitignore | 9 +- .gitlab-ci.yml | 21 ++- firmware/{ => embed}/.gitignore | 0 firmware/{ => embed}/cubemx_config.ioc | 0 firmware/embed/include/hal_conf_extra.h | 37 +++++ .../knowledges/AS5045B_DS000397_2-00.pdf | 0 .../{ => embed}/knowledges/TI-DRV8313.pdf | Bin .../{ => embed}/knowledges/motorBoard.pdf | Bin ...armbased-32bit-mcus-stmicroelectronics.pdf | Bin firmware/embed/lib/AS5045/AS5045.cpp | 44 ++++++ firmware/embed/lib/AS5045/AS5045.h | 32 +++++ firmware/embed/lib/AS5045/library.properties | 10 ++ firmware/embed/lib/DRV8313/DRV8313.cpp | 42 ++++++ firmware/embed/lib/DRV8313/DRV8313.h | 20 +++ firmware/embed/lib/DRV8313/library.properties | 10 ++ firmware/{ => embed}/lib/README | 0 firmware/{ => embed}/platformio.ini | 8 +- firmware/embed/src/main.cpp | 130 ++++++++++++++++++ firmware/{ => embed}/test/README | 0 firmware/lib/AS5040/AS5040.cpp | 36 ----- firmware/lib/AS5040/AS5040.h | 26 ---- firmware/src/main.cpp | 122 ---------------- firmware/test/send_can.sh | 34 +++++ firmware/test/test_driver.sh | 30 ++++ 24 files changed, 422 insertions(+), 189 deletions(-) rename firmware/{ => embed}/.gitignore (100%) rename firmware/{ => embed}/cubemx_config.ioc (100%) create mode 100644 firmware/embed/include/hal_conf_extra.h rename firmware/{ => embed}/knowledges/AS5045B_DS000397_2-00.pdf (100%) rename firmware/{ => embed}/knowledges/TI-DRV8313.pdf (100%) rename firmware/{ => embed}/knowledges/motorBoard.pdf (100%) rename firmware/{ => embed}/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf (100%) create mode 100644 firmware/embed/lib/AS5045/AS5045.cpp create mode 100644 firmware/embed/lib/AS5045/AS5045.h create mode 100644 firmware/embed/lib/AS5045/library.properties create mode 100644 firmware/embed/lib/DRV8313/DRV8313.cpp create mode 100644 firmware/embed/lib/DRV8313/DRV8313.h create mode 100644 firmware/embed/lib/DRV8313/library.properties rename firmware/{ => embed}/lib/README (100%) rename firmware/{ => embed}/platformio.ini (78%) create mode 100644 firmware/embed/src/main.cpp rename firmware/{ => embed}/test/README (100%) delete mode 100644 firmware/lib/AS5040/AS5040.cpp delete mode 100644 firmware/lib/AS5040/AS5040.h delete mode 100644 firmware/src/main.cpp create mode 100755 firmware/test/send_can.sh create mode 100755 firmware/test/test_driver.sh diff --git a/.gitignore b/.gitignore index 4dbfb84..80c4449 100644 --- a/.gitignore +++ b/.gitignore @@ -29,4 +29,11 @@ fp-info-cache # Autorouter files (exported from Pcbnew) *.dsn -*.ses \ No newline at end of file +*.ses + +# Visual Studio Code +*.vscode/ +# Platformio .pio +.pio/ +# JetBrains CLion +.idea/ diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index db82894..2e6bf46 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -13,7 +13,7 @@ build_firmware: - ".gitlab-ci.yml" when: on_success before_script: - - cd firmware + - cd firmware/embed - pip install -U platformio - pio update script: @@ -21,10 +21,25 @@ build_firmware: - pio run artifacts: paths: - - firmware/.pio/build/robotroller_reborn/firmware.elf - - firmware/.pio/build/robotroller_reborn/firmware.bin + - firmware/embed/.pio/build/robotroller_reborn/firmware.elf + - firmware/embed/.pio/build/robotroller_reborn/firmware.bin expire_in: 1 week +lint_firmware: + stage: gen_fabrication_files + image: python:3.9 + rules: + - changes: + - "firmware/*" + - ".gitlab-ci.yml" + when: on_success + before_script: + - cd firmware/embed/ + - pip install cpplint + script: + - pwd + - cpplint --extensions=h,hpp,cpp --recursive --linelength=120 --filter=-build/include_subdir,-legal/copyright --output=vs7 src include test lib + ### Gitlab CI/CD example for KiCad # workflow: diff --git a/firmware/.gitignore b/firmware/embed/.gitignore similarity index 100% rename from firmware/.gitignore rename to firmware/embed/.gitignore diff --git a/firmware/cubemx_config.ioc b/firmware/embed/cubemx_config.ioc similarity index 100% rename from firmware/cubemx_config.ioc rename to firmware/embed/cubemx_config.ioc diff --git a/firmware/embed/include/hal_conf_extra.h b/firmware/embed/include/hal_conf_extra.h new file mode 100644 index 0000000..cc08e41 --- /dev/null +++ b/firmware/embed/include/hal_conf_extra.h @@ -0,0 +1,37 @@ +#pragma once + +#pragma region "Motor and sensor setup" +#define LED1 PC10 +#define LED2 PC11 +#define HARDWARE_SERIAL_RX_PIN PB7 +#define HARDWARE_SERIAL_TX_PIN PB6 +#define AS5045_CS PB15 +#define AS5045_MISO PB14 +#define AS5045_MOSI PC1 +#define AS5045_SCLK PB10 +#define CURRENT_SENSOR_1 PB1 +#define CURRENT_SENSOR_2 PB0 +#define CURRENT_SENSOR_3 PC5 +#define TIM1_CH1 PA8 +#define TIM1_CH2 PA9 +#define TIM1_CH3 PA10 +#define EN_W_GATE_DRIVER PC6 +#define EN_U_GATE_DRIVER PA11 +#define EN_V_GATE_DRIVER PA12 +#define SLEEP_DRIVER PC9 +#define RESET_DRIVER PC8 +#define FAULT_DRIVER PC7 +#define POLE_PAIRS 14 +#define CAN2_TX PB13 +#define CAN2_RX PB12 +#define CAN1_TX PB9 +#define CAN1_RX PB8 + +#pragma endregion + +#if !defined(HAL_CAN_MODULE_ENABLED) +#define HAL_CAN_MODULE_ENABLED +#endif +#include "stm32f4xx_hal.h" +#include "stm32f4xx_hal_can.h" +#include diff --git a/firmware/knowledges/AS5045B_DS000397_2-00.pdf b/firmware/embed/knowledges/AS5045B_DS000397_2-00.pdf similarity index 100% rename from firmware/knowledges/AS5045B_DS000397_2-00.pdf rename to firmware/embed/knowledges/AS5045B_DS000397_2-00.pdf diff --git a/firmware/knowledges/TI-DRV8313.pdf b/firmware/embed/knowledges/TI-DRV8313.pdf similarity index 100% rename from firmware/knowledges/TI-DRV8313.pdf rename to firmware/embed/knowledges/TI-DRV8313.pdf diff --git a/firmware/knowledges/motorBoard.pdf b/firmware/embed/knowledges/motorBoard.pdf similarity index 100% rename from firmware/knowledges/motorBoard.pdf rename to firmware/embed/knowledges/motorBoard.pdf diff --git a/firmware/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf b/firmware/embed/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf similarity index 100% rename from firmware/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf rename to firmware/embed/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf diff --git a/firmware/embed/lib/AS5045/AS5045.cpp b/firmware/embed/lib/AS5045/AS5045.cpp new file mode 100644 index 0000000..83b2148 --- /dev/null +++ b/firmware/embed/lib/AS5045/AS5045.cpp @@ -0,0 +1,44 @@ +#include "AS5045.h" + + +MagneticSensorAS5045::MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso, + uint16_t as5040_sclk): AS5040_CS_(as5040_cs), + AS5040_MOSI_(as5040_mosi), + AS5040_MISO_(as5040_miso), + AS5040_SCLK_(as5040_sclk), + spi(nullptr), + settings(AS5145SSISettings) { +} + +MagneticSensorAS5045::~MagneticSensorAS5045() = default; + +auto MagneticSensorAS5045::init(SPIClass *_spi) -> void { + this->spi = _spi; + settings = AS5145SSISettings; + pinMode(AS5040_CS_, OUTPUT); + pinMode(AS5040_MISO_, INPUT); + pinMode(AS5040_MOSI_, OUTPUT); + pinMode(AS5040_SCLK_, OUTPUT); + + spi->setMISO(AS5040_MISO_); + spi->setMOSI(AS5040_MOSI_); + spi->setSCLK(AS5040_SCLK_); + spi->begin(); + this->Sensor::init(); +} + +float MagneticSensorAS5045::getSensorAngle() { + float angle_data = readRawAngleSSI(); + angle_data = (static_cast(angle_data) / AS5045_CPR) * _2PI; + return angle_data; +} + +uint16_t MagneticSensorAS5045::readRawAngleSSI() const { + spi->beginTransaction(settings); + digitalWrite(AS5040_CS_, LOW); + uint16_t value = spi->transfer16(0x0000); + digitalWrite(AS5040_CS_, HIGH); + spi->endTransaction(); + delayMicroseconds(30); + return (value >> 3) & 0x1FFF; // TODO(vanyabeat): Add error checking MAGNETIC OWERFLOW and etc. +} diff --git a/firmware/embed/lib/AS5045/AS5045.h b/firmware/embed/lib/AS5045/AS5045.h new file mode 100644 index 0000000..03dc98f --- /dev/null +++ b/firmware/embed/lib/AS5045/AS5045.h @@ -0,0 +1,32 @@ +#pragma once + +#include "SimpleFOC.h" +#include "SPI.h" + +#ifndef MSBFIRST +#define MSBFIRST BitOrder::MSBFIRST +#endif + +#define AS5045_BITORDER MSBFIRST +#define AS5045_CPR 4096.0f +#define _2PI 6.28318530718f + +static SPISettings AS5145SSISettings(1000000, AS5045_BITORDER, SPI_MODE0); + +class MagneticSensorAS5045 final: public Sensor { + public: + MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso, uint16_t as5040_sclk); + + virtual ~MagneticSensorAS5045(); + + float getSensorAngle() override; + + virtual void init(SPIClass *_spi = &SPI); + + [[nodiscard]] uint16_t readRawAngleSSI() const; + + private: + uint16_t AS5040_CS_, AS5040_MOSI_, AS5040_MISO_, AS5040_SCLK_; + SPIClass *spi; + SPISettings settings; +}; diff --git a/firmware/embed/lib/AS5045/library.properties b/firmware/embed/lib/AS5045/library.properties new file mode 100644 index 0000000..da9ea63 --- /dev/null +++ b/firmware/embed/lib/AS5045/library.properties @@ -0,0 +1,10 @@ +name=AS5045 +version=1.0.1 +author=vanyabeat +maintainer=vanyabeat +sentence=Simple library to work with AS5040 and Simple FOC (for Robotroller in Robosemmbler) Fork from https://github.com/runger1101001 +paragraph=Simple library to work with AS5040 and Simple FOC and simple library intended for hobby comunity to run the AS5040 magnetic sensor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards. +category=Device Control +url=https://docs.simplefoc.com +architectures=* +includes=SimpleFOC.h \ No newline at end of file diff --git a/firmware/embed/lib/DRV8313/DRV8313.cpp b/firmware/embed/lib/DRV8313/DRV8313.cpp new file mode 100644 index 0000000..dc4a50f --- /dev/null +++ b/firmware/embed/lib/DRV8313/DRV8313.cpp @@ -0,0 +1,42 @@ +#include "DRV8313.h" + +DRV8313Driver::DRV8313Driver(int phA, int phB, int phC, int en1, int en2, int en3, int slp, int rst, + int flt) : BLDCDriver3PWM(phA, phB, phC, en1, en2, en3), slp_pin(slp), rst_pin(rst), + flt_pin(flt) { +} + +int DRV8313Driver::init() { + // Get state from flt pin + if (_isset(flt_pin)) { + pinMode(flt_pin, INPUT); + if (digitalRead(flt_pin) == HIGH) { + // if the fault pin is high the driver is in fault state + // reset the driver + if (_isset(rst_pin)) { + pinMode(rst_pin, OUTPUT); + digitalWrite(rst_pin, LOW); + delay(1); + digitalWrite(rst_pin, HIGH); + delay(1); + } + } + } + return BLDCDriver3PWM::init(); +} + +void DRV8313Driver::enable() { + // Enable the driver + if (_isset(slp_pin)) { + pinMode(slp_pin, OUTPUT); + digitalWrite(slp_pin, HIGH); + } + BLDCDriver3PWM::enable(); +} + +void DRV8313Driver::disable() { + if (_isset(slp_pin)) { + pinMode(slp_pin, OUTPUT); + digitalWrite(slp_pin, LOW); + } + BLDCDriver3PWM::disable(); +} diff --git a/firmware/embed/lib/DRV8313/DRV8313.h b/firmware/embed/lib/DRV8313/DRV8313.h new file mode 100644 index 0000000..e74b717 --- /dev/null +++ b/firmware/embed/lib/DRV8313/DRV8313.h @@ -0,0 +1,20 @@ +#pragma once + +#include "SimpleFOC.h" + +class DRV8313Driver : public BLDCDriver3PWM { + public: + DRV8313Driver(int phA, int phB, int phC, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET, int slp = NOT_SET, + int rst = NOT_SET, int flt = NOT_SET); + + int init() override; + + void enable() override; + + void disable() override; + + private: + int slp_pin; + int rst_pin; + int flt_pin; +}; diff --git a/firmware/embed/lib/DRV8313/library.properties b/firmware/embed/lib/DRV8313/library.properties new file mode 100644 index 0000000..0c534d0 --- /dev/null +++ b/firmware/embed/lib/DRV8313/library.properties @@ -0,0 +1,10 @@ +name=DRV8313 Simple FOC +version=1.0.0 +author=vanyabeat +maintainer=vanyabeat +sentence=Simple library to work with DRV8313 and Simple FOC (for Robotroller in Robosemmbler) +paragraph=Simple library to work with DRV8313 and Simple FOC and simple library intended for hobby comunity to run the BLDC and Stepper motor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards. +category=Device Control +url=https://docs.simplefoc.com +architectures=* +includes=SimpleFOC.h \ No newline at end of file diff --git a/firmware/lib/README b/firmware/embed/lib/README similarity index 100% rename from firmware/lib/README rename to firmware/embed/lib/README diff --git a/firmware/platformio.ini b/firmware/embed/platformio.ini similarity index 78% rename from firmware/platformio.ini rename to firmware/embed/platformio.ini index 653b782..4460efc 100644 --- a/firmware/platformio.ini +++ b/firmware/embed/platformio.ini @@ -18,4 +18,10 @@ upload_protocol = stlink debug_tool = stlink monitor_speed = 115200 monitor_parity = N -lib_deps = askuric/Simple FOC@^2.3.2 +build_flags = + -DSTM32F446xx + -D HAL_CAN_MODULE_ENABLED + ; -D HAL_TIM_MODULE_ENABLED +lib_deps = + askuric/Simple FOC@^2.3.2 + pazi88/STM32_CAN@^1.1.0 diff --git a/firmware/embed/src/main.cpp b/firmware/embed/src/main.cpp new file mode 100644 index 0000000..53bc8cd --- /dev/null +++ b/firmware/embed/src/main.cpp @@ -0,0 +1,130 @@ +// clang-format off +#include +#include +#include +#include +#include +#include "hal_conf_extra.h" +// clang-format on + +STM32_CAN Can(CAN2, DEF); + +static CAN_message_t CAN_TX_msg; +static CAN_message_t CAN_inMsg; + +SPIClass spi; +MagneticSensorAS5045 sensor(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK); + +BLDCMotor motor(POLE_PAIRS); + +DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER, + EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER, + RESET_DRIVER, FAULT_DRIVER); +LowsideCurrentSense current_sense(0.0125, 20, CURRENT_SENSOR_1, CURRENT_SENSOR_2, + CURRENT_SENSOR_3); + +Commander command(Serial); + +void doMotor(char *cmd) { + command.motor(&motor, cmd); + digitalWrite(PC10, !digitalRead(PC10)); + delayMicroseconds(2); +} + +void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *driver, + LowsideCurrentSense *current_sense, Commander *commander, + CommandCallback callback) { + sensor->init(&spi); + motor->linkSensor(sensor); + driver->pwm_frequency = 20000; + driver->voltage_power_supply = 20; + driver->voltage_limit = 40; + driver->init(); + motor->linkDriver(driver); + current_sense->linkDriver(driver); + current_sense->init(); + motor->linkCurrentSense(current_sense); + SimpleFOCDebug::enable(nullptr); + commander->add('M', callback, "motor"); + motor->useMonitoring(Serial); + motor->monitor_start_char = 'M'; + motor->monitor_end_char = 'M'; + commander->verbose = VerboseMode::machine_readable; + motor->controller = MotionControlType::angle; + motor->torque_controller = TorqueControlType::voltage; + motor->foc_modulation = FOCModulationType::SinePWM; + motor->PID_velocity.P = 0.3; + motor->PID_velocity.I = 15; + motor->PID_velocity.D = 0.001; + motor->PID_velocity.output_ramp = 1000; + motor->LPF_velocity.Tf = 0.005; + motor->LPF_angle.Tf = 0.005; + motor->P_angle.P = 15; + motor->velocity_limit = 40; + motor->voltage_limit = 40; + motor->sensor_direction = Direction::CCW; + current_sense->skip_align = true; + motor->init(); + motor->initFOC(); +} + +uint8_t Counter; + +void send_data() { + // TODO(vanyabeat) add can functionability for ROS packets + if (Counter >= 255) { + Counter = 0; + } + CAN_TX_msg.id = 1; + snprintf(reinterpret_cast(CAN_TX_msg.buf), sizeof(CAN_TX_msg.buf), "CNT:%d", Counter); + CAN_TX_msg.len = 8; + Can.write(CAN_TX_msg); + // ++Counter; + // Serial.print("Sent: \n"); + digitalWrite(PC11, !digitalRead(PC11)); +} + +void read_can_message() { + Serial.print("Received: "); + Serial.print(CAN_inMsg.id); + Serial.print(" "); + Serial.print(CAN_inMsg.len); + Serial.print(" "); + CAN_inMsg.buf[7] = 0; + auto ptr = reinterpret_cast(CAN_inMsg.buf); + Serial.println(ptr); + doMotor(ptr); + Serial.print("\n"); +} + +void run_foc(BLDCMotor *motor, Commander *commander) { + motor->loopFOC(); + motor->move(); + motor->monitor(); + commander->run(); +} + +void setup() { + Serial.setRx(HARDWARE_SERIAL_RX_PIN); + Serial.setTx(HARDWARE_SERIAL_TX_PIN); + Serial.begin(115200); + // setup led pin + pinMode(PC11, OUTPUT); + pinMode(PC10, OUTPUT); + Can.begin(); + Can.setBaudRate(1000000); + TIM_TypeDef *Instance = TIM2; + HardwareTimer *SendTimer = new HardwareTimer(Instance); + SendTimer->setOverflow(1, HERTZ_FORMAT); // 50 Hz + SendTimer->attachInterrupt(send_data); + SendTimer->resume(); + setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor); + _delay(1000); +} + +void loop() { + run_foc(&motor, &command); + while (Can.read(CAN_inMsg)) { + read_can_message(); + } +} diff --git a/firmware/test/README b/firmware/embed/test/README similarity index 100% rename from firmware/test/README rename to firmware/embed/test/README diff --git a/firmware/lib/AS5040/AS5040.cpp b/firmware/lib/AS5040/AS5040.cpp deleted file mode 100644 index d48336a..0000000 --- a/firmware/lib/AS5040/AS5040.cpp +++ /dev/null @@ -1,36 +0,0 @@ -#include "AS5040.h" - -AS5040Sensor::AS5040Sensor(uint16_t cs, uint16_t mosi, uint16_t miso, uint16_t sck) { - cs_ = cs; - mosi_ = mosi; - miso_ = miso; - sck_ = sck; -} -void AS5040Sensor::init(SPIClass *_spi) { - spi = _spi; - // 1MHz clock (AMS should be able to accept up to 10MHz) - settings = SPISettings(1000000, MSBFIRST, SPI_MODE0); - //setup pins - pinMode(cs_, OUTPUT); - pinMode(miso_, INPUT); - pinMode(mosi_, OUTPUT); - pinMode(sck_, OUTPUT); - spi->setMISO(miso_); - spi->setMOSI(mosi_); - spi->setSCLK(sck_); - spi->begin(); - this->Sensor::init(); // call base class init -} - -float AS5040Sensor::getSensorAngle() { - //SPI - begin transaction - spi->beginTransaction(settings); - //Send the command - digitalWrite(cs_, LOW); - uint16_t data = spi->transfer16(0x3FFF); - digitalWrite(cs_,HIGH); -//SPI - end transaction - spi->endTransaction(); - data = (data) >> 5; - return (float)data * _2PI / cpr; -} diff --git a/firmware/lib/AS5040/AS5040.h b/firmware/lib/AS5040/AS5040.h deleted file mode 100644 index b25ea97..0000000 --- a/firmware/lib/AS5040/AS5040.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include "SimpleFOC.h" - - -class AS5040Sensor : public Sensor { -public: - - AS5040Sensor(uint16_t cs, uint16_t mosi, uint16_t miso, uint16_t sck); - - /** sensor initialise pins */ - void init(SPIClass *_spi = &SPI); - - // implementation of abstract functions of the Sensor class - /** get current angle (rad) */ - float getSensorAngle() override; - -private: - uint16_t cs_ = 0; - uint16_t mosi_ = 0; - uint16_t miso_ = 0; - uint16_t sck_ = 0; - float cpr = 1024; - SPISettings settings; - SPIClass *spi; -}; diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp deleted file mode 100644 index f8544d4..0000000 --- a/firmware/src/main.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include -#include -#include -#include - -#pragma region "Motor and sensor setup" -#define LED1 PC10 -#define LED2 PC11 -#define HARDWARE_SERIAL_RX_PIN PB7 -#define HARDWARE_SERIAL_TX_PIN PB6 -#define AS5040_CS PB15 -#define AS5040_MISO PB14 -#define AS5040_MOSI PC1 -#define AS5040_SCLK PB10 -#define CURRENT_SENSOR_1 PB1 // INA180A -#define CURRENT_SENSOR_2 PB0 -#define CURRENT_SENSOR_3 PC5 -#pragma endregion -// simple foc debug mode -#define DEBUG_SIMPLE_FOC - -// Init AS5040 -AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK); -// Init iPower 6208 -BLDCMotor motor = BLDCMotor(16); -// Init 3PWM driver DRV8313 -BLDCDriver3PWM driver = BLDCDriver3PWM(PA8, PA9, PA10, PC6, PA11, PA12); - -// Init current sense -InlineCurrentSense current_sense = InlineCurrentSense( - 0.0263, 100, CURRENT_SENSOR_2, CURRENT_SENSOR_1, CURRENT_SENSOR_3); -// Init UART Commander -Commander command = Commander(Serial); -void doMotor(char *cmd) { command.motor(&motor, cmd); } - -void setup() { - SimpleFOCDebug::enable(&Serial); - // Setup hardware serial - Serial.setRx(HARDWARE_SERIAL_RX_PIN); - Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); - // Init pins - pinMode(PC8, OUTPUT); - pinMode(PC9, OUTPUT); - pinMode(PA11, OUTPUT); - pinMode(PA12, OUTPUT); - pinMode(PA10, OUTPUT); - pinMode(PC6, OUTPUT); - pinMode(PA8, OUTPUT); - pinMode(PA9, OUTPUT); - pinMode(PA10, OUTPUT); - // end init pins - digitalWrite(PC8, HIGH); - digitalWrite(PC9, HIGH); - delay(1); - digitalWrite(PA11, HIGH); - digitalWrite(PA12, HIGH); - digitalWrite(PC6, HIGH); - - sensor.init(); - motor.linkSensor(&sensor); - - driver.pwm_frequency = 20000; - driver.voltage_power_supply = 20; - driver.voltage_limit = 20; - driver.init(); - motor.linkDriver(&driver); - current_sense.linkDriver(&driver); - motor.controller = MotionControlType::angle; - motor.torque_controller = TorqueControlType::foc_current; - motor.foc_modulation = FOCModulationType::SinePWM; - motor.voltage_limit = 20; - motor.velocity_limit = 20; - motor.useMonitoring(Serial); - motor.init(); - current_sense.init(); - motor.linkCurrentSense(¤t_sense); - motor.torque_controller = TorqueControlType::foc_current; - motor.controller = MotionControlType::angle; - motor.motion_downsample = 0.0; - motor.PID_velocity.P = 0.5; - motor.PID_velocity.I = 10.0; - motor.PID_velocity.D = 0.0; - motor.PID_velocity.output_ramp = 1000.0; - motor.PID_velocity.limit = 2.0; - motor.LPF_velocity.Tf = 0.005; - motor.P_angle.P = 0.5; - motor.P_angle.I = 10; - motor.P_angle.D = 1; - motor.P_angle.output_ramp = 0.0; - motor.P_angle.limit = 20.0; - motor.LPF_angle.Tf = 0.0; - motor.PID_current_q.P = 3.0; - motor.PID_current_q.I = 300.0; - motor.PID_current_q.D = 0.0; - motor.PID_current_q.output_ramp = 0.0; - motor.PID_current_q.limit = 12.0; - motor.LPF_current_q.Tf = 0.005; - motor.PID_current_d.P = 3.0; - motor.PID_current_d.I = 300.0; - motor.PID_current_d.D = 0.0; - motor.PID_current_d.output_ramp = 0.0; - motor.PID_current_d.limit = 12.0; - motor.LPF_current_d.Tf = 0.005; - motor.velocity_limit = 20.0; - motor.voltage_limit = 20.0; - motor.current_limit = 2.0; - motor.sensor_offset = 0.0; - motor.phase_resistance = 3.608; - motor.foc_modulation = FOCModulationType::SinePWM; - motor.modulation_centered = 1.0; - command.add('M', doMotor, "motor"); - motor.useMonitoring(Serial); - _delay(1000); -} - -void loop() { - motor.loopFOC(); - motor.move(); - motor.monitor(); - command.run(); -} \ No newline at end of file diff --git a/firmware/test/send_can.sh b/firmware/test/send_can.sh new file mode 100755 index 0000000..74a98fa --- /dev/null +++ b/firmware/test/send_can.sh @@ -0,0 +1,34 @@ +#!/bin/bash + +# Read 8 byte ascii from $1 and send it to CAN bus + +# : +# 3 (SFF) or 8 (EFF) hex chars +# {data}: +# 0..8 (0..64 CAN FD) ASCII hex-values (optionally separated by '.') +# {len}: +# an optional 0..8 value as RTR frames can contain a valid dlc field +# : +# a single ASCII Hex value (0 .. F) which defines canfd_frame.flags + +can_id=$1 +input=$2 + +if [[ ${#input} -gt 8 ]]; then + echo "Error: Input string must be no longer than 8 characters." + exit 1 +fi + +hex_output="" +for (( i=0; i<${#input}; i++ )); do + hex_char=$(printf "%02x" "'${input:$i:1}") + hex_output+="${hex_char}" +done + +hex_output=$(printf "%-16s" "$hex_output") +hex_output=${hex_output// /0} + +can_id=$(printf "%03X" $can_id) + +cansend can0 $can_id#$hex_output + diff --git a/firmware/test/test_driver.sh b/firmware/test/test_driver.sh new file mode 100755 index 0000000..e729ae5 --- /dev/null +++ b/firmware/test/test_driver.sh @@ -0,0 +1,30 @@ +#!/bin/bash + + +/bin/bash $(pwd)/send_can.sh 1 E0 + +/bin/bash $(pwd)/send_can.sh 1 E1 + +/bin/bash $(pwd)/send_can.sh 1 C2 + +/bin/bash $(pwd)/send_can.sh 1 1 +sleep 1 +/bin/bash $(pwd)/send_can.sh 1 -1 +sleep 1 +/bin/bash $(pwd)/send_can.sh 1 -1 +sleep 1 +/bin/bash $(pwd)/send_can.sh 1 2 +sleep 1 +/bin/bash $(pwd)/send_can.sh 1 C1 +sleep 1 +/bin/bash $(pwd)/send_can.sh 1 0.5 +sleep 5 +/bin/bash $(pwd)/send_can.sh 1 2 +sleep 5 +/bin/bash $(pwd)/send_can.sh 1 5 +sleep 5 +/bin/bash $(pwd)/send_can.sh 1 -5 +sleep 5 +/bin/bash $(pwd)/send_can.sh 1 20 +sleep 5 +/bin/bash $(pwd)/send_can.sh 1 E0 \ No newline at end of file