Add sensor AS5045, new PCB 0.4.2 adoption

This commit is contained in:
vanyabeat 2024-05-08 10:05:29 +00:00 committed by Igor Brylyov
parent 3fb5246306
commit e876d7e56c
24 changed files with 422 additions and 189 deletions

9
.gitignore vendored
View file

@ -29,4 +29,11 @@ fp-info-cache
# Autorouter files (exported from Pcbnew)
*.dsn
*.ses
*.ses
# Visual Studio Code
*.vscode/
# Platformio .pio
.pio/
# JetBrains CLion
.idea/

View file

@ -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:

View file

@ -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 <STM32_CAN.h>

View file

@ -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<float>(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.
}

View file

@ -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;
};

View file

@ -0,0 +1,10 @@
name=AS5045
version=1.0.1
author=vanyabeat <vanyabeat@protonmail.com>
maintainer=vanyabeat <vanyabeat@protonmail.com>
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

View file

@ -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();
}

View file

@ -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;
};

View file

@ -0,0 +1,10 @@
name=DRV8313 Simple FOC
version=1.0.0
author=vanyabeat <vanyabeat@protonmail.com>
maintainer=vanyabeat <vanyabeat@protonmail.com>
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

View file

@ -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

130
firmware/embed/src/main.cpp Normal file
View file

@ -0,0 +1,130 @@
// clang-format off
#include <Arduino.h>
#include <SimpleFOC.h>
#include <STM32_CAN.h>
#include <AS5045.h>
#include <DRV8313.h>
#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<char *>(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<char *>(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, &current_sense, &command, doMotor);
_delay(1000);
}
void loop() {
run_foc(&motor, &command);
while (Can.read(CAN_inMsg)) {
read_can_message();
}
}

View file

@ -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;
}

View file

@ -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;
};

View file

@ -1,122 +0,0 @@
#include <AS5040.h>
#include <Arduino.h>
#include <SPI.h>
#include <SimpleFOC.h>
#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(&current_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();
}

34
firmware/test/send_can.sh Executable file
View file

@ -0,0 +1,34 @@
#!/bin/bash
# Read 8 byte ascii from $1 and send it to CAN bus
# <can_id>:
# 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
# <flags>:
# 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

30
firmware/test/test_driver.sh Executable file
View file

@ -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