Add sensor AS5045, new PCB 0.4.2 adoption
This commit is contained in:
parent
3fb5246306
commit
e876d7e56c
24 changed files with 422 additions and 189 deletions
9
.gitignore
vendored
9
.gitignore
vendored
|
@ -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/
|
||||
|
|
|
@ -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:
|
||||
|
|
37
firmware/embed/include/hal_conf_extra.h
Normal file
37
firmware/embed/include/hal_conf_extra.h
Normal 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>
|
44
firmware/embed/lib/AS5045/AS5045.cpp
Normal file
44
firmware/embed/lib/AS5045/AS5045.cpp
Normal 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.
|
||||
}
|
32
firmware/embed/lib/AS5045/AS5045.h
Normal file
32
firmware/embed/lib/AS5045/AS5045.h
Normal 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;
|
||||
};
|
10
firmware/embed/lib/AS5045/library.properties
Normal file
10
firmware/embed/lib/AS5045/library.properties
Normal 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
|
42
firmware/embed/lib/DRV8313/DRV8313.cpp
Normal file
42
firmware/embed/lib/DRV8313/DRV8313.cpp
Normal 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();
|
||||
}
|
20
firmware/embed/lib/DRV8313/DRV8313.h
Normal file
20
firmware/embed/lib/DRV8313/DRV8313.h
Normal 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;
|
||||
};
|
10
firmware/embed/lib/DRV8313/library.properties
Normal file
10
firmware/embed/lib/DRV8313/library.properties
Normal 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
|
|
@ -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
130
firmware/embed/src/main.cpp
Normal 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, ¤t_sense, &command, doMotor);
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
run_foc(&motor, &command);
|
||||
while (Can.read(CAN_inMsg)) {
|
||||
read_can_message();
|
||||
}
|
||||
}
|
|
@ -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;
|
||||
}
|
|
@ -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;
|
||||
};
|
|
@ -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(¤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();
|
||||
}
|
34
firmware/test/send_can.sh
Executable file
34
firmware/test/send_can.sh
Executable 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
30
firmware/test/test_driver.sh
Executable 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
|
Loading…
Add table
Add a link
Reference in a new issue