matei/lemon-pepper-stepper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

try to fix submodule

Browse Source
This commit is contained in:
matei jordache
2023-11-09 19:02:15 -05:00
parent c1d45aa443
commit deea94b076
366 changed files with 40228 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
firmware/lib/Arduino-FOC-drivers/src/comms/README.md Normal file
View File

@@ -0,0 +1,5 @@
# SimpleFOC communications support code
This folder contains classes to support you communicating between MCUs running SimpleFOC, and other systems.

131
firmware/lib/Arduino-FOC-drivers/src/comms/RegisterReceiver.cpp Normal file
View File

@@ -0,0 +1,131 @@
#include "./RegisterReceiver.h"
#include "BLDCMotor.h"
void RegisterReceiver::setRegister(SimpleFOCRegister reg, FOCMotor* motor){
// write a register value for the given motor
switch(reg) {
case REG_ENABLE:
readByte((uint8_t*)&(motor->enabled));
break;
case REG_MODULATION_MODE:
readByte((uint8_t*)&(motor->foc_modulation));
break;
case REG_TORQUE_MODE:
readByte((uint8_t*)&(motor->torque_controller));
break;
case REG_CONTROL_MODE:
readByte((uint8_t*)&(motor->controller));
break;
case REG_TARGET:
readFloat(&(motor->target));
break;
case REG_VEL_PID_P:
readFloat(&(motor->PID_velocity.P));
break;
case REG_VEL_PID_I:
readFloat(&(motor->PID_velocity.I));
break;
case REG_VEL_PID_D:
readFloat(&(motor->PID_velocity.D));
break;
case REG_VEL_LPF_T:
readFloat(&(motor->LPF_velocity.Tf));
break;
case REG_ANG_PID_P:
readFloat(&(motor->P_angle.P));
break;
case REG_VEL_LIMIT:
readFloat(&(motor->velocity_limit));
break;
case REG_VEL_MAX_RAMP:
readFloat(&(motor->PID_velocity.output_ramp));
break;
case REG_CURQ_PID_P:
readFloat(&(motor->PID_current_q.P));
break;
case REG_CURQ_PID_I:
readFloat(&(motor->PID_current_q.I));
break;
case REG_CURQ_PID_D:
readFloat(&(motor->PID_current_q.D));
break;
case REG_CURQ_LPF_T:
readFloat(&(motor->LPF_current_q.Tf));
break;
case REG_CURD_PID_P:
readFloat(&(motor->PID_current_d.P));
break;
case REG_CURD_PID_I:
readFloat(&(motor->PID_current_d.I));
break;
case REG_CURD_PID_D:
readFloat(&(motor->PID_current_d.D));
break;
case REG_CURD_LPF_T:
readFloat(&(motor->LPF_current_d.Tf));
break;
case REG_VOLTAGE_LIMIT:
readFloat(&(motor->voltage_limit));
break;
case REG_CURRENT_LIMIT:
readFloat(&(motor->current_limit));
break;
case REG_MOTION_DOWNSAMPLE:
readByte((uint8_t*)&(motor->motion_downsample));
break;
case REG_DRIVER_VOLTAGE_LIMIT:
readFloat(&(((BLDCMotor*)motor)->driver->voltage_limit));
break;
case REG_PWM_FREQUENCY:
readInt((uint32_t*)&(((BLDCMotor*)motor)->driver->pwm_frequency));
break;
case REG_ZERO_ELECTRIC_ANGLE:
readFloat(&(motor->zero_electric_angle));
break;
case REG_SENSOR_DIRECTION:
readByte((uint8_t*)&(motor->sensor_direction));
break;
case REG_ZERO_OFFSET:
readFloat(&(motor->sensor_offset));
break;
case REG_PHASE_RESISTANCE:
readFloat(&(motor->phase_resistance));
break;
case REG_KV:
readFloat(&(motor->KV_rating));
break;
case REG_INDUCTANCE:
readFloat(&(motor->phase_inductance));
break;
case REG_POLE_PAIRS:
readByte((uint8_t*)&(motor->pole_pairs));
break;
// unknown register or read-only register (no write) or can't handle in superclass
case REG_STATUS:
case REG_ANGLE:
case REG_POSITION:
case REG_VELOCITY:
case REG_SENSOR_ANGLE:
case REG_VOLTAGE_Q:
case REG_VOLTAGE_D:
case REG_CURRENT_Q:
case REG_CURRENT_D:
case REG_CURRENT_A:
case REG_CURRENT_B:
case REG_CURRENT_C:
case REG_CURRENT_ABC:
case REG_SYS_TIME:
case REG_NUM_MOTORS:
case REG_MOTOR_ADDRESS:
case REG_ENABLE_ALL:
case REG_REPORT:
default:
break;
}
};

15
firmware/lib/Arduino-FOC-drivers/src/comms/RegisterReceiver.h Normal file
View File

@@ -0,0 +1,15 @@
#pragma once
#include "./SimpleFOCRegisters.h"
#include "common/base_classes/FOCMotor.h"
class RegisterReceiver {
protected:
virtual void setRegister(SimpleFOCRegister reg, FOCMotor* motor);
virtual uint8_t readByte(uint8_t* valueToSet) = 0;
virtual uint8_t readFloat(float* valueToSet) = 0;
virtual uint8_t readInt(uint32_t* valueToSet) = 0;
};

193
firmware/lib/Arduino-FOC-drivers/src/comms/RegisterSender.cpp Normal file
View File

@@ -0,0 +1,193 @@
#include "./RegisterSender.h"
#include "common/foc_utils.h"
#include "BLDCMotor.h"
bool RegisterSender::sendRegister(SimpleFOCRegister reg, FOCMotor* motor){
// write a register value for the given motor
switch(reg) {
case REG_STATUS:
writeByte(motor->motor_status);
break;
case REG_ENABLE:
writeByte(motor->enabled);
break;
case REG_MODULATION_MODE:
writeByte(motor->foc_modulation);
break;
case REG_TORQUE_MODE:
writeByte(motor->torque_controller);
break;
case REG_CONTROL_MODE:
writeByte(motor->controller);
break;
case REG_TARGET:
writeFloat(motor->target);
break;
case REG_ANGLE:
writeFloat(motor->shaft_angle);
break;
case REG_POSITION:
if (motor->sensor) {
writeInt(motor->sensor->getFullRotations());
writeFloat(motor->sensor->getMechanicalAngle());
}
else {
writeInt(motor->shaft_angle/_2PI);
writeFloat(fmod(motor->shaft_angle, _2PI));
}
break;
case REG_VELOCITY:
writeFloat(motor->shaft_velocity);
break;
case REG_SENSOR_ANGLE:
if (motor->sensor)
writeFloat(motor->sensor->getAngle()); // stored angle
else
writeFloat(motor->shaft_angle);
break;
case REG_VOLTAGE_Q:
writeFloat(motor->voltage.q);
break;
case REG_VOLTAGE_D:
writeFloat(motor->voltage.d);
break;
case REG_CURRENT_Q:
writeFloat(motor->current.q);
break;
case REG_CURRENT_D:
writeFloat(motor->current.d);
break;
case REG_CURRENT_A:
if (motor->current_sense)
writeFloat(motor->current_sense->getPhaseCurrents().a);
else
writeFloat(0.0f);
break;
case REG_CURRENT_B:
if (motor->current_sense)
writeFloat(motor->current_sense->getPhaseCurrents().b);
else
writeFloat(0.0f);
break;
case REG_CURRENT_C:
if (motor->current_sense)
writeFloat(motor->current_sense->getPhaseCurrents().c);
else
writeFloat(0.0f);
break;
case REG_CURRENT_ABC:
if (motor->current_sense) {
PhaseCurrent_s currents = motor->current_sense->getPhaseCurrents();
writeFloat(currents.a);
writeFloat(currents.b);
writeFloat(currents.c);
}
else {
writeFloat(0.0f);
writeFloat(0.0f);
writeFloat(0.0f);
}
break;
case REG_VEL_PID_P:
writeFloat(motor->PID_velocity.P);
break;
case REG_VEL_PID_I:
writeFloat(motor->PID_velocity.I);
break;
case REG_VEL_PID_D:
writeFloat(motor->PID_velocity.D);
break;
case REG_VEL_LPF_T:
writeFloat(motor->LPF_velocity.Tf);
break;
case REG_ANG_PID_P:
writeFloat(motor->P_angle.P);
break;
case REG_VEL_LIMIT:
writeFloat(motor->velocity_limit);
break;
case REG_VEL_MAX_RAMP:
writeFloat(motor->PID_velocity.output_ramp);
break;
case REG_CURQ_PID_P:
writeFloat(motor->PID_current_q.P);
break;
case REG_CURQ_PID_I:
writeFloat(motor->PID_current_q.I);
break;
case REG_CURQ_PID_D:
writeFloat(motor->PID_current_q.D);
break;
case REG_CURQ_LPF_T:
writeFloat(motor->LPF_current_q.Tf);
break;
case REG_CURD_PID_P:
writeFloat(motor->PID_current_d.P);
break;
case REG_CURD_PID_I:
writeFloat(motor->PID_current_d.I);
break;
case REG_CURD_PID_D:
writeFloat(motor->PID_current_d.D);
break;
case REG_CURD_LPF_T:
writeFloat(motor->LPF_current_d.Tf);
break;
case REG_VOLTAGE_LIMIT:
writeFloat(motor->voltage_limit);
break;
case REG_CURRENT_LIMIT:
writeFloat(motor->current_limit);
break;
case REG_MOTION_DOWNSAMPLE:
writeByte((uint8_t)motor->motion_downsample);
break;
case REG_DRIVER_VOLTAGE_LIMIT:
writeFloat(((BLDCMotor*)motor)->driver->voltage_limit);
break;
case REG_PWM_FREQUENCY:
writeInt(((BLDCMotor*)motor)->driver->pwm_frequency);
break;
case REG_ZERO_ELECTRIC_ANGLE:
writeFloat(motor->zero_electric_angle);
break;
case REG_SENSOR_DIRECTION:
writeByte((int8_t)motor->sensor_direction);
break;
case REG_ZERO_OFFSET:
writeFloat(motor->sensor_offset);
break;
case REG_PHASE_RESISTANCE:
writeFloat(motor->phase_resistance);
break;
case REG_KV:
writeFloat(motor->KV_rating);
break;
case REG_INDUCTANCE:
writeFloat(motor->phase_inductance);
break;
case REG_POLE_PAIRS:
writeByte((uint8_t)motor->pole_pairs);
break;
case REG_SYS_TIME:
// TODO how big is millis()? Same on all platforms?
writeInt((int)millis());
break;
// unknown register or write only register (no read) or can't handle in superclass
case REG_NUM_MOTORS:
case REG_REPORT:
case REG_MOTOR_ADDRESS:
case REG_ENABLE_ALL:
default:
writeByte(0); // TODO what to send in this case?
return false;
}
return true;
};

18
firmware/lib/Arduino-FOC-drivers/src/comms/RegisterSender.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#include "./SimpleFOCRegisters.h"
#include "common/base_classes/FOCMotor.h"
/**
* Register sending functionality is shared by Commander and Telemetry implementations.
* Since the code to access all the registers is quite large, it makes sense to abstract it out,
* and also make sure registers are addressed in the same way for all implementations.
*/
class RegisterSender {
protected:
virtual bool sendRegister(SimpleFOCRegister reg, FOCMotor* motor);
virtual uint8_t writeByte(uint8_t value) = 0;
virtual uint8_t writeFloat(float value) = 0;
virtual uint8_t writeInt(uint32_t value) = 0;
};

72
firmware/lib/Arduino-FOC-drivers/src/comms/SimpleFOCRegisters.h Normal file
View File

@@ -0,0 +1,72 @@
#pragma once
#include <inttypes.h>
// this constant is changed each time the registers definition are changed *in an incompatible way*. This means that just adding new registers
// does not change the version, but removing or changing the meaning of existing registers does, or changing the number of an existing register.
#define SIMPLEFOC_REGISTERS_VERSION 0x01
typedef enum : uint8_t {
REG_STATUS = 0x00, // RO - 1 byte (motor status)
REG_MOTOR_ADDRESS = 0x01, // R/W - 1 byte
REG_REPORT = 0x02, // R/W - Write: variable, Read: variable, up to 32 bytes
REG_ENABLE_ALL = 0x03, // WO - 1 byte
REG_ENABLE = 0x04, // R/W - 1 byte
REG_CONTROL_MODE = 0x05, // R/W - 1 byte
REG_TORQUE_MODE = 0x06, // R/W - 1 byte
REG_MODULATION_MODE = 0x07, // R/W - 1 byte
REG_TARGET = 0x08, // R/W - float
REG_ANGLE = 0x09, // RO - float
REG_POSITION = 0x10, // RO - int32_t full rotations + float position (0-2PI, in radians) (4 bytes + 4 bytes)
REG_VELOCITY = 0x11, // RO - float
REG_SENSOR_ANGLE = 0x12, // RO - float
REG_VOLTAGE_Q = 0x20, // RO - float
REG_VOLTAGE_D = 0x21, // RO - float
REG_CURRENT_Q = 0x22, // RO - float
REG_CURRENT_D = 0x23, // RO - float
REG_CURRENT_A = 0x24, // RO - float
REG_CURRENT_B = 0x25, // RO - float
REG_CURRENT_C = 0x26, // RO - float
REG_CURRENT_ABC = 0x27, // RO - 3xfloat = 12 bytes
REG_CURRENT_DC = 0x28, // RO - float
REG_VEL_PID_P = 0x30, // R/W - float
REG_VEL_PID_I = 0x31, // R/W - float
REG_VEL_PID_D = 0x32, // R/W - float
REG_VEL_LPF_T = 0x33, // R/W - float
REG_ANG_PID_P = 0x34, // R/W - float
REG_VEL_LIMIT = 0x35, // R/W - float
REG_VEL_MAX_RAMP = 0x36, // R/W - float
REG_CURQ_PID_P = 0x40, // R/W - float
REG_CURQ_PID_I = 0x41, // R/W - float
REG_CURQ_PID_D = 0x42, // R/W - float
REG_CURQ_LPF_T = 0x43, // R/W - float
REG_CURD_PID_P = 0x44, // R/W - float
REG_CURD_PID_I = 0x45, // R/W - float
REG_CURD_PID_D = 0x46, // R/W - float
REG_CURD_LPF_T = 0x47, // R/W - float
REG_VOLTAGE_LIMIT = 0x50, // R/W - float
REG_CURRENT_LIMIT = 0x51, // R/W - float
REG_MOTION_DOWNSAMPLE = 0x52, // R/W - uint32_t
REG_DRIVER_VOLTAGE_LIMIT = 0x53,// R/W - float
REG_PWM_FREQUENCY = 0x54, // R/W - uint32_t
REG_ZERO_ELECTRIC_ANGLE = 0x60, // RO - float
REG_SENSOR_DIRECTION = 0x61, // RO - 1 byte
REG_ZERO_OFFSET = 0x62, // R/W - float
REG_POLE_PAIRS = 0x63, // RO - uint32_t
REG_PHASE_RESISTANCE = 0x64, // R/W - float
REG_KV = 0x65, // R/W - float
REG_INDUCTANCE = 0x66, // R/W - float
REG_NUM_MOTORS = 0x70, // RO - 1 byte
REG_SYS_TIME = 0x71, // RO - uint32_t
} SimpleFOCRegister;

424
firmware/lib/Arduino-FOC-drivers/src/comms/i2c/I2CCommander.cpp Normal file
View File

@@ -0,0 +1,424 @@
#include "I2CCommander.h"
I2CCommander::I2CCommander(TwoWire* wire) : _wire(wire) {
};
I2CCommander::~I2CCommander(){};
void I2CCommander::init(uint8_t address) {
_address = address;
};
void I2CCommander::addMotor(FOCMotor* motor){
if (numMotors<I2CCOMMANDER_MAX_MOTORS_COMMANDABLE){
motors[numMotors] = motor;
numMotors++;
}
};
bool I2CCommander::readBytes(void* valueToSet, uint8_t numBytes){
if (_wire->available()>=numBytes){
byte* bytes = (byte*)valueToSet;
for (int i=0;i<numBytes;i++)
*bytes++ = _wire->read();
return true;
}
commanderror = true;
return false;
};
void I2CCommander::writeFloat(float value){
_wire->write((byte*)&value, 4);
};
void I2CCommander::onReceive(int numBytes){
lastcommanderror = commanderror;
lastcommandregister = curRegister;
commanderror = false;
if (numBytes>=1) { // set the current register
curRegister = static_cast<SimpleFOCRegister>(_wire->read());
}
if (numBytes>1) { // read from i2c and update value represented by register as well...
if (!receiveRegister(curMotor, curRegister, numBytes))
commanderror = true;
}
if (numBytes<1)
commanderror = true;
};
void I2CCommander::onRequest(){
commanderror = false;
if (!sendRegister(curMotor, curRegister))
commanderror = true;
};
/*
Reads values from I2C bus and updates the motor's values.
Currently this isn't really thread-safe, but works ok in practice on 32-bit MCUs.
Do not use on 8-bit architectures where the 32 bit writes may not be atomic!
Plan to make this safe: the writes should be buffered, and not actually executed
until in the main loop by calling commander->run();
the run() method disables interrupts while the updates happen.
*/
bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int numBytes) {
int val;
switch (registerNum) {
case REG_MOTOR_ADDRESS:
val = _wire->read(); // reading one more byte is definately ok, since numBytes>1
if (val>=0 && val<numMotors)
curMotor = val;
else
commanderror = true;
break;
case REG_REPORT:
if (numBytes>=3 && (numBytes&0x01)==1) { // numBytes must be odd, since we have register and n pairs of motor/register numbers
val = (numBytes-1)/2;
if (val>I2CCOMMANDER_MAX_REPORT_REGISTERS)
val = I2CCOMMANDER_MAX_REPORT_REGISTERS;
for (int i=0;i<val;i++){
reportMotors[i] = _wire->read();
reportRegisters[i] = _wire->read();
}
}
else
commanderror = true;
break;
case REG_ENABLE_ALL:
val = _wire->read();
for (int i=0; i<numMotors; i++)
(val>0)?motors[i]->enable():motors[i]->disable();
break;
case REG_ENABLE:
val = _wire->read();
(val>0)?motors[motorNum]->enable():motors[motorNum]->disable();
break;
case REG_CONTROL_MODE:
val = _wire->read();
if (val>=0 && val<=4) // TODO these enums don't have assigned constants
motors[motorNum]->controller = static_cast<MotionControlType>(val);
else
commanderror = true;
break;
case REG_TORQUE_MODE:
val = _wire->read();
if (val>=0 && val<=2)
motors[motorNum]->torque_controller = static_cast<TorqueControlType>(val);
else
commanderror = true;
break;
case REG_MODULATION_MODE:
val = _wire->read();
if (val>=0 && val<=3)
motors[motorNum]->foc_modulation = static_cast<FOCModulationType>(val);
else
commanderror = true;
break;
case REG_TARGET:
readBytes(&(motors[motorNum]->target), 4);
break;
case REG_VEL_PID_P:
readBytes(&(motors[motorNum]->PID_velocity.P), 4);
break;
case REG_VEL_PID_I:
readBytes(&(motors[motorNum]->PID_velocity.I), 4);
break;
case REG_VEL_PID_D:
readBytes(&(motors[motorNum]->PID_velocity.D), 4);
break;
case REG_VEL_LPF_T:
readBytes(&(motors[motorNum]->LPF_velocity.Tf), 4);
break;
case REG_ANG_PID_P:
readBytes(&(motors[motorNum]->P_angle.P), 4);
break;
case REG_VEL_LIMIT:
readBytes(&(motors[motorNum]->velocity_limit), 4);
break;
case REG_VEL_MAX_RAMP:
readBytes(&(motors[motorNum]->PID_velocity.output_ramp), 4);
break;
case REG_CURQ_PID_P:
readBytes(&(motors[motorNum]->PID_current_q.P), 4);
break;
case REG_CURQ_PID_I:
readBytes(&(motors[motorNum]->PID_current_q.I), 4);
break;
case REG_CURQ_PID_D:
readBytes(&(motors[motorNum]->PID_current_q.D), 4);
break;
case REG_CURQ_LPF_T:
readBytes(&(motors[motorNum]->LPF_current_q.Tf), 4);
break;
case REG_CURD_PID_P:
readBytes(&(motors[motorNum]->PID_current_d.P), 4);
break;
case REG_CURD_PID_I:
readBytes(&(motors[motorNum]->PID_current_d.I), 4);
break;
case REG_CURD_PID_D:
readBytes(&(motors[motorNum]->PID_current_d.D), 4);
break;
case REG_CURD_LPF_T:
readBytes(&(motors[motorNum]->LPF_current_d.Tf), 4);
break;
case REG_VOLTAGE_LIMIT:
readBytes(&(motors[motorNum]->voltage_limit), 4);
break;
case REG_CURRENT_LIMIT:
readBytes(&(motors[motorNum]->current_limit), 4);
break;
case REG_MOTION_DOWNSAMPLE:
readBytes(&(motors[motorNum]->motion_downsample), 4);
break;
case REG_ZERO_OFFSET:
readBytes(&(motors[motorNum]->sensor_offset), 4);
break;
// RO registers
case REG_STATUS:
case REG_ANGLE:
case REG_POSITION:
case REG_VELOCITY:
case REG_SENSOR_ANGLE:
case REG_VOLTAGE_Q:
case REG_VOLTAGE_D:
case REG_CURRENT_Q:
case REG_CURRENT_D:
case REG_CURRENT_A:
case REG_CURRENT_B:
case REG_CURRENT_C:
case REG_CURRENT_ABC:
case REG_ZERO_ELECTRIC_ANGLE:
case REG_SENSOR_DIRECTION:
case REG_POLE_PAIRS:
case REG_PHASE_RESISTANCE:
case REG_NUM_MOTORS:
case REG_SYS_TIME:
default: // unknown register
return false;
}
return true;
}
/*
Reads values from motor/sensor and writes them to I2C bus. Intended to be run
from the Wire.onRequest interrupt.
Assumes atomic 32 bit reads. On 8-bit arduino this assumption does not hold and this
code is not safe on those platforms. You might read "half-written" floats.
A solution might be to maintain a complete set of shadow registers in the commander
class, and update them in the run() method (which runs with interrupts off). Not sure
of the performance impact of all those 32 bit read/writes though. In any case, since
I use only 32 bit MCUs I'll leave it as an excercise to the one who needs it. ;-)
On 32 bit platforms the implication is that reads will occur atomically, so data will
be intact, but they can occur at any time during motor updates, so different values might
not be in a fully consistent state (i.e. phase A current might be from the current iteration
but phase B current from the previous iteration).
*/
bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) {
// read the current register
switch(registerNum) {
case REG_STATUS:
_wire->write(curMotor);
_wire->write((uint8_t)lastcommandregister);
_wire->write((uint8_t)lastcommanderror+1);
for (int i=0;(i<numMotors && i<28); i++) { // at most 28 motors, so we can fit in one packet
_wire->write(motors[i]->motor_status);
}
break;
case REG_MOTOR_ADDRESS:
_wire->write(curMotor);
break;
case REG_REPORT:
for (int i=0;i<numReportRegisters;i++)
if (reportRegisters[i]!=REG_REPORT) // prevent recursion
sendRegister(reportMotors[i], reportRegisters[i]);
break;
case REG_ENABLE:
_wire->write(motors[motorNum]->enabled);
break;
case REG_MODULATION_MODE:
_wire->write(motors[motorNum]->foc_modulation);
break;
case REG_TORQUE_MODE:
_wire->write(motors[motorNum]->torque_controller);
break;
case REG_CONTROL_MODE:
_wire->write(motors[motorNum]->controller);
break;
case REG_TARGET:
writeFloat(motors[motorNum]->target);
break;
case REG_ANGLE:
writeFloat(motors[motorNum]->shaft_angle);
break;
case REG_VELOCITY:
writeFloat(motors[motorNum]->shaft_velocity);
break;
case REG_SENSOR_ANGLE:
if (motors[motorNum]->sensor)
writeFloat(motors[motorNum]->sensor->getAngle()); // stored angle
else
writeFloat(motors[motorNum]->shaft_angle);
break;
case REG_VOLTAGE_Q:
writeFloat(motors[motorNum]->voltage.q);
break;
case REG_VOLTAGE_D:
writeFloat(motors[motorNum]->voltage.d);
break;
case REG_CURRENT_Q:
writeFloat(motors[motorNum]->current.q);
break;
case REG_CURRENT_D:
writeFloat(motors[motorNum]->current.d);
break;
case REG_CURRENT_A:
if (motors[motorNum]->current_sense) // TODO check if current sense can be called from inside this callback function
writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().a);
else
writeFloat(0.0f);
break;
case REG_CURRENT_B:
if (motors[motorNum]->current_sense)
writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().b);
else
writeFloat(0.0f);
break;
case REG_CURRENT_C:
if (motors[motorNum]->current_sense)
writeFloat(motors[motorNum]->current_sense->getPhaseCurrents().c);
else
writeFloat(0.0f);
break;
case REG_CURRENT_ABC:
if (motors[motorNum]->current_sense) {
PhaseCurrent_s currents = motors[motorNum]->current_sense->getPhaseCurrents();
writeFloat(currents.a);
writeFloat(currents.b);
writeFloat(currents.c);
}
else {
writeFloat(0.0f);
writeFloat(0.0f);
writeFloat(0.0f);
}
break;
case REG_VEL_PID_P:
writeFloat(motors[motorNum]->PID_velocity.P);
break;
case REG_VEL_PID_I:
writeFloat(motors[motorNum]->PID_velocity.I);
break;
case REG_VEL_PID_D:
writeFloat(motors[motorNum]->PID_velocity.D);
break;
case REG_VEL_LPF_T:
writeFloat(motors[motorNum]->LPF_velocity.Tf);
break;
case REG_ANG_PID_P:
writeFloat(motors[motorNum]->P_angle.P);
break;
case REG_VEL_LIMIT:
writeFloat(motors[motorNum]->velocity_limit);
break;
case REG_VEL_MAX_RAMP:
writeFloat(motors[motorNum]->PID_velocity.output_ramp);
break;
case REG_CURQ_PID_P:
writeFloat(motors[motorNum]->PID_current_q.P);
break;
case REG_CURQ_PID_I:
writeFloat(motors[motorNum]->PID_current_q.I);
break;
case REG_CURQ_PID_D:
writeFloat(motors[motorNum]->PID_current_q.D);
break;
case REG_CURQ_LPF_T:
writeFloat(motors[motorNum]->LPF_current_q.Tf);
break;
case REG_CURD_PID_P:
writeFloat(motors[motorNum]->PID_current_d.P);
break;
case REG_CURD_PID_I:
writeFloat(motors[motorNum]->PID_current_d.I);
break;
case REG_CURD_PID_D:
writeFloat(motors[motorNum]->PID_current_d.D);
break;
case REG_CURD_LPF_T:
writeFloat(motors[motorNum]->LPF_current_d.Tf);
break;
case REG_VOLTAGE_LIMIT:
writeFloat(motors[motorNum]->voltage_limit);
break;
case REG_CURRENT_LIMIT:
writeFloat(motors[motorNum]->current_limit);
break;
case REG_MOTION_DOWNSAMPLE:
_wire->write((int)motors[motorNum]->motion_downsample); // TODO int will have different sizes on different platforms
// but using uint32 doesn't compile clean on all, e.g. RP2040
break;
case REG_ZERO_ELECTRIC_ANGLE:
writeFloat(motors[motorNum]->zero_electric_angle);
break;
case REG_SENSOR_DIRECTION:
_wire->write((int8_t)motors[motorNum]->sensor_direction);
break;
case REG_ZERO_OFFSET:
writeFloat(motors[motorNum]->sensor_offset);
break;
case REG_PHASE_RESISTANCE:
writeFloat(motors[motorNum]->phase_resistance);
break;
case REG_POLE_PAIRS:
_wire->write((int)motors[motorNum]->pole_pairs);
break;
case REG_SYS_TIME:
// TODO how big is millis()? Same on all platforms?
_wire->write((int)millis());
break;
case REG_NUM_MOTORS:
_wire->write(numMotors);
break;
// unknown register or write only register (no read)
case REG_ENABLE_ALL:
default:
_wire->write(0); // TODO what to send in this case?
return false;
}
return true;
}

49
firmware/lib/Arduino-FOC-drivers/src/comms/i2c/I2CCommander.h Normal file
View File

@@ -0,0 +1,49 @@
#ifndef SIMPLEFOC_I2CCOMMANDER_H
#define SIMPLEFOC_I2CCOMMANDER_H
#include "Arduino.h"
#include "Wire.h"
#include "common/base_classes/FOCMotor.h"
#include "../SimpleFOCRegisters.h"
#ifndef I2CCOMMANDER_MAX_MOTORS_COMMANDABLE
#define I2CCOMMANDER_MAX_MOTORS_COMMANDABLE 4
#endif
#define I2CCOMMANDER_MIN_VELOCITY_FOR_MOTOR_MOVING 0.1f // in rad/s
#define I2CCOMMANDER_MAX_REPORT_REGISTERS 8
class I2CCommander {
public:
I2CCommander(TwoWire* wire = &Wire);
~I2CCommander();
void addMotor(FOCMotor* motor); // first add motors
virtual void init(uint8_t address); // then init
void onReceive(int numBytes);
void onRequest();
protected:
void writeFloat(float value);
bool readBytes(void* valueToSet, uint8_t numBytes);
virtual bool sendRegister(uint8_t motorNum, uint8_t registerNum);
virtual bool receiveRegister(uint8_t motorNum, uint8_t registerNum, int numBytes);
uint8_t _address;
TwoWire* _wire;
uint8_t numMotors = 0;
uint8_t curMotor = 0;
SimpleFOCRegister curRegister = REG_STATUS;
bool commanderror = false;
bool lastcommanderror = false;
uint8_t lastcommandregister = REG_STATUS;
FOCMotor* motors[I2CCOMMANDER_MAX_MOTORS_COMMANDABLE];
uint8_t numReportRegisters = 0;
uint8_t reportMotors[I2CCOMMANDER_MAX_REPORT_REGISTERS];
uint8_t reportRegisters[I2CCOMMANDER_MAX_REPORT_REGISTERS];
};
#endif

59
firmware/lib/Arduino-FOC-drivers/src/comms/i2c/I2CCommanderMaster.cpp Normal file
View File

@@ -0,0 +1,59 @@
#include "I2CCommanderMaster.h"
I2CCommanderMaster::I2CCommanderMaster(int maxMotors) : maxMotors(maxMotors), motors(new I2CRemoteMotor[maxMotors]) {
};
I2CCommanderMaster::~I2CCommanderMaster(){
};
void I2CCommanderMaster::init(){
};
// TODO handle multiple motors per target
void I2CCommanderMaster::addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, TwoWire *wire){
for (int i=0;i<motorCount;i++){
if (numMotors<maxMotors){
motors[numMotors] = I2CRemoteMotor{ .wire=wire, .address=i2cAddress };
numMotors++;
}
}
};
int I2CCommanderMaster::writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size){
motors[motor].wire->beginTransmission(motors[motor].address);
motors[motor].wire->write((uint8_t)registerNum);
motors[motor].wire->write((uint8_t*)data, size);
motors[motor].wire->endTransmission();
return size;
};
int I2CCommanderMaster::readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size){
motors[motor].wire->beginTransmission(motors[motor].address);
int numWrite = motors[motor].wire->write((uint8_t)registerNum); // TODO check return value
motors[motor].wire->endTransmission();
if (numWrite==1)
return readLastUsedRegister(motor, data, size);
return 0;
};
int I2CCommanderMaster::readLastUsedRegister(int motor, void* data, uint8_t size){
int numRead = motors[motor].wire->requestFrom(motors[motor].address, size);
if (numRead==size)
motors[motor].wire->readBytes((uint8_t*)data, size);
else {
return 0;
}
return numRead;
};

42
firmware/lib/Arduino-FOC-drivers/src/comms/i2c/I2CCommanderMaster.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef SIMPLEFOC_I2CCOMMANDER_H
#define SIMPLEFOC_I2CCOMMANDER_H
#include <Arduino.h>
#include <Wire.h>
#include "../SimpleFOCRegisters.h"
#define I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS 4
typedef struct {
TwoWire* wire;
uint8_t address;
} I2CRemoteMotor;
class I2CCommanderMaster {
public:
I2CCommanderMaster(int maxMotors = I2COMMANDER_DEFAULT_MAX_REMOTE_MOTORS);
~I2CCommanderMaster();
void init();
void addI2CMotors(uint8_t i2cAddress, uint8_t motorCount, TwoWire *wire = &Wire);
int writeRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size);
int readRegister(int motor, SimpleFOCRegister registerNum, void* data, uint8_t size);
int readLastUsedRegister(int motor, void* data, uint8_t size);
// Motor intialization interface for convenience - think about how this will best work
// i.e. which parameters should be set by i2c and which should be hard-coded, and where config info is saved
// TODO bool initializeMotor(int motor);
private:
int maxMotors;
int numMotors = 0;
I2CRemoteMotor* motors;
};
#endif

98
firmware/lib/Arduino-FOC-drivers/src/comms/i2c/README.md Normal file
View File

@@ -0,0 +1,98 @@
# I2CCommander
Implementation of SimpleFOC Commander for I2C communication bus.
This code takes the point of view that the motor driver (the "muscle") is the I2C target device, and another MCU/CPU (the "brain") is the I2C controller device, which is coordinating one or more motor drivers on the same or different I2C buses. Each motor driver can offer one or more motors for control via the I2CCommander. So fairly flexible setups are possible, with multiple motors per driver, multiple drivers per I2C bus and multiple I2C buses on the brain MCU.
## Warning
This is new code, and has not been extensively tested. Your milage may vary. That said, basic use cases have been tested, and we would certainly appreciate feedback and help with testing it out.
In particular, there are concurrency issues with reading/writing the SimpleFOC motor values from I2C while the motor is running. These should be solved soon in an upcoming version.
**Do not run on 8-bit MCUs!** The code currently assumes atomic 32 bit reads, so running on Arduino UNO or Nano is unfortunately a no-go.
## Using
As would be expected for I2C, each target device needs a unique I2C address on its bus, and setting up and discovering these addresses is out-of-scope for I2CCommander. Setting up and configuring the TwoWire objects (which pins, speed, etc...) is also out of scope and finished, initialized TwoWire objects must be passed to I2CCommander. If you don't specify a different reference, the standard *Wire* object is assumed.
Communication with the motor drivers happens via a register paradigm. The driver board offers many registers, some of which can be read, some can be written, and some are read/write. The controller MCU sends I2C messages to the target device to read or write a register as desired. The size of the data to be read/written depends on the register, and must be known by the controller. See Registers, below, for more details on the individual registers.
Since each target motor driver can handle multiple motors, one of the registers contains the currently selected motor. Most of the other registers then operate on the currently selected motor. There are some exceptions, like REG_ENABLE_ALL - which operates on all the motors, or REG_STATUS, which returns stati for all the motors.
### Target device (motor driver)
The target device (motor driver) initializes and uses an instance of I2CCommander. Only one instance is needed for all attached motors:
```c++
#include "Arduino.h"
#include <Wire.h>
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "comms/i2c/I2CCommander.h"
// commander instance
uint8_t i2c_addr = 0x60; // can be anything you choose
I2CCommander commander;
// interrupt callbacks
void onReceive(int numBytes) { commander.onReceive(numBytes); }
void onRequest() { commander.onRequest(); }
// ... other variables, like sensor, etc...
BLDCMotor motor = BLDCMotor(POLE_PAIRS);
void setup() {
// ...other setup code
Wire.setClock(400000); // use same speed on controller device
Wire.begin(i2c_addr, true); // initialize i2c in target mode
commander.addMotor(&motor); // add a motor
//commander.addMotor(&motor2); // you could add more than one motor
commander.init(i2c_addr); // initialize commander
Wire.onReceive(onReceive); // connect the interrupt handlers
Wire.onRequest(onRequest);
}
```
### Controller device ("brain" MCU)
On the controller device, you use an instance of I2CCommanderMaster, which you initialize by adding one or more target devices to it:
```c++
#include "Arduino.h"
#include <Wire.h>
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "comms/i2c/I2CCommanderMaster.h"
#define TARGET_I2C_ADDRESS 0x60
I2CCommanderMaster commander;
void setup() {
// ...other setup code
Wire.setClock(400000); // use same speed on target device!
Wire.begin(); // initialize i2c in controller mode
commander.addI2CMotors(TARGET_I2C_ADDRESS, 1); // add target device, it has 1 motor
//commander.addI2CMotors(TARGET_I2C_ADDRESS2, 1); // you could add another target device on the same bus
//commander.addI2CMotors(TARGET_I2C_ADDRESS, 1, &wire2); // or on a different i2c bus
commander.init(); // init commander
Wire.onReceive(onReceive); // connect the interrupt handlers
Wire.onRequest(onRequest);
}
```
## Extending
The API is somewhat opinionated, and unlike the standard serial commander, currently does not support hooking in your own variables for reading/writing to them via I2C. This is because I2C is a bit less flexible than Serial.
If you want to extend I2CCommander please subclass it and override the functions `sendRegister` and `receiveRegister` to add new registers. Use register numbers above 0x80 to prevent collisions with the standard registers.

49
firmware/lib/Arduino-FOC-drivers/src/comms/serial/README.md Normal file
View File

@@ -0,0 +1,49 @@
# Serial communications classes
Serial communications classes for register-based control and telemetry from SimpleFOC.
## SerialASCIITelemetry
:warning: unfinished, untested
Telemetry class that sends telemetry as ASCII on a serial port. Similar to the classic "monitoring" functionality of SimpleFOC, but allows you to configure telemetry based on most of the defined registers.
Usage:
```c++
SerialASCIITelemetry telemetry = SerialASCIITelemetry(); // number of float digits to display
void setup() {
...
telemetry.addMotor(&motor);
telemetry.setTelemetryRegisters(2, [REG_VELOCITY, REG_VOLTAGE_Q]);
telemetry.init();
...
}
void loop() {
motor.move();
motor.loopFOC();
telemetry.run();
}
```
Some options are supported:
```c++
telemetry.floatPrecision = 4; // send floats with 4 decimal places
telemetry.min_elapsed_time = 0; // microseconds between sending telemetry
telemetry.downsample = 100; // send every this many loop iterations
```
## SerialBinaryCommander
:warning: unfinished, untested!
Control SimpleFOC via a binary protocol over the serial port. The standard SimpleFOC registers are used.
TODO document the protocol

65
firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialASCIITelemetry.cpp Normal file
View File

@@ -0,0 +1,65 @@
#include "./SerialASCIITelemetry.h"
SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : Telemetry() {
this->floatPrecision = floatPrecision;
};
SerialASCIITelemetry::~SerialASCIITelemetry(){
};
void SerialASCIITelemetry::init(Print* print){
this->_print = print;
this->Telemetry::init();
};
void SerialASCIITelemetry::sendHeader() {
if (numRegisters > 0) {
writeChar('H');
writeChar(' ');
for (uint8_t i = 0; i < numRegisters; i++) {
writeByte(registers_motor[i]);
writeChar(':');
writeByte(registers[i]);
if (i < numRegisters-1)
writeChar(' ');
};
writeChar('\n');
};
};
void SerialASCIITelemetry::sendTelemetry(){
if (numRegisters > 0) {
for (uint8_t i = 0; i < numRegisters; i++) {
sendRegister(static_cast<SimpleFOCRegister>(registers[i]), motors[registers_motor[i]]);
if (i < numRegisters-1)
writeChar(' ');
};
writeChar('\n');
}
};
uint8_t SerialASCIITelemetry::writeChar(char value){
return _print->print(value);
};
uint8_t SerialASCIITelemetry::writeByte(uint8_t value){
return _print->print(value);
};
uint8_t SerialASCIITelemetry::writeFloat(float value){
return _print->print(value, floatPrecision);
};
uint8_t SerialASCIITelemetry::writeInt(uint32_t value){
return _print->print(value);
};

27
firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialASCIITelemetry.h Normal file
View File

@@ -0,0 +1,27 @@
#pragma once
#include "Arduino.h"
#include "../telemetry/Telemetry.h"
class SerialASCIITelemetry : public Telemetry {
public:
SerialASCIITelemetry(int floatPrecision = 2);
virtual ~SerialASCIITelemetry();
void init(Print* print = &Serial);
protected:
uint8_t writeByte(uint8_t value) override;
uint8_t writeFloat(float value) override;
uint8_t writeInt(uint32_t value) override;
uint8_t writeChar(char value);
void sendTelemetry() override;
void sendHeader() override;
Print* _print;
int floatPrecision = 2;
};

153
firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialBinaryCommander.cpp Normal file
View File

@@ -0,0 +1,153 @@
#include "SerialBinaryCommander.h"
SerialBinaryCommander::SerialBinaryCommander(bool echo) : Telemetry(), echo(echo) {
};
SerialBinaryCommander::~SerialBinaryCommander(){
};
void SerialBinaryCommander::init(Stream &serial) {
_serial = &serial;
this->Telemetry::init();
};
void SerialBinaryCommander::run() {
if (_serial->available()>2) { // TODO make this work with partial packets...
uint8_t command = _serial->read();
uint8_t registerNum = _serial->read();
uint8_t motorNum = _serial->read();
if (command==SERIALBINARY_COMMAND_READ){
startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect
sendRegister(static_cast<SimpleFOCRegister>(registerNum), motors[motorNum]);
endFrame();
}
else if (command==SERIALBINARY_COMMAND_WRITE) {
setRegister(static_cast<SimpleFOCRegister>(registerNum), motors[motorNum]);
if (echo) {
startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect
sendRegister(static_cast<SimpleFOCRegister>(registerNum), motors[motorNum]);
endFrame();
}
}
}
// and handle the telemetry
this->Telemetry::run();
};
uint8_t SerialBinaryCommander::readBytes(void* valueToSet, uint8_t numBytes){
if (_serial->available()<numBytes)
return 0;
uint8_t* value = (uint8_t*)valueToSet;
for (uint8_t i=0; i<numBytes; i++) {
value[i] = _serial->read();
}
return numBytes;
};
uint8_t SerialBinaryCommander::writeBytes(void* valueToSend, uint8_t numBytes){
uint8_t* value = (uint8_t*)valueToSend;
for (uint8_t i=0; i<numBytes; i++) {
_serial->write(value[i]);
}
return numBytes;
};
void SerialBinaryCommander::startFrame(uint8_t frameSize, uint8_t type){
_serial->write(0xA5);
_serial->write((uint8_t)((type<<6)|frameSize));
};
void SerialBinaryCommander::endFrame(){
_serial->write(0x5A);
};
uint8_t SerialBinaryCommander::writeByte(uint8_t value){
return writeBytes(&value, 1);
};
uint8_t SerialBinaryCommander::writeFloat(float value){
return writeBytes(&value, 4);
};
uint8_t SerialBinaryCommander::writeInt(uint32_t value){
return writeBytes(&value, 4);
};
uint8_t SerialBinaryCommander::readByte(uint8_t* valueToSet){
return readBytes(valueToSet, 1);
};
uint8_t SerialBinaryCommander::readFloat(float* valueToSet){
return readBytes(valueToSet, 4);
};
uint8_t SerialBinaryCommander::readInt(uint32_t* valueToSet){
return readBytes(valueToSet, 4);
};
void SerialBinaryCommander::sendHeader() {
if (numRegisters > 0) {
startFrame(numRegisters*2, TELEMETRY_FRAMETYPE_HEADER);
for (uint8_t i = 0; i < numRegisters; i++) {
writeByte(registers[i]);
writeByte(registers_motor[i]);
};
endFrame();
};
};
void SerialBinaryCommander::sendTelemetry(){
if (numRegisters > 0) {
startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA);
for (uint8_t i = 0; i < numRegisters; i++) {
sendRegister(static_cast<SimpleFOCRegister>(registers[i]), motors[registers_motor[i]]);
};
endFrame();
}
};

57
firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialBinaryCommander.h Normal file
View File

@@ -0,0 +1,57 @@
#pragma once
#include <Arduino.h>
#include "../RegisterReceiver.h"
#include "../telemetry/Telemetry.h"
#ifndef COMMS_MAX_REPORT_REGISTERS
#define COMMS_MAX_REPORT_REGISTERS 7
#endif
#define SERIALBINARY_FRAMETYPE_REGISTER 0x03
#define SERIALBINARY_COMMAND_READ 0x00
#define SERIALBINARY_COMMAND_WRITE 0x80
class SerialBinaryCommander : public Telemetry, public RegisterReceiver {
public:
SerialBinaryCommander(bool echo = false);
virtual ~SerialBinaryCommander();
void init(Stream &serial = Serial);
void run();
bool echo;
protected:
virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA);
virtual void endFrame();
uint8_t readBytes(void* valueToSet, uint8_t numBytes);
uint8_t writeBytes(void* valueToSend, uint8_t numBytes);
uint8_t writeByte(uint8_t value) override;
uint8_t writeFloat(float value) override;
uint8_t writeInt(uint32_t value) override;
uint8_t readByte(uint8_t* valueToSet) override;
uint8_t readFloat(float* valueToSet) override;
uint8_t readInt(uint32_t* valueToSet) override;
void sendTelemetry() override;
void sendHeader() override;
uint8_t curMotor = 0;
SimpleFOCRegister curRegister = REG_STATUS;
bool commanderror = false;
bool lastcommanderror = false;
uint8_t lastcommandregister = REG_STATUS;
uint8_t numReportRegisters = 0;
uint8_t reportMotors[COMMS_MAX_REPORT_REGISTERS];
SimpleFOCRegister reportRegisters[COMMS_MAX_REPORT_REGISTERS];
Stream* _serial;
};

58
firmware/lib/Arduino-FOC-drivers/src/comms/stm32speeddir/README.md Normal file
View File

@@ -0,0 +1,58 @@
# STM32SpeedDirInput
Control SimpleFOC using PWM speed and direction inputs.
Based on STM32 timer PWM-Input capabilities, which means this can only be used on STM32 MCUs. It can cover a wide range of PWM frequencies, and runs without MCU overhead in the timer hardware.
## Setup
The PWM speed input should be connected to either channel 1 or channel 2 of a general purpose or advanced control timer on your STM32 MCU. Suitable timers are:
- Advanced control timers: TIM1, TIM8
- General purpose timers (32 bit): TIM2, TIM5
- General purpose timers (16 bit): TIM3, TIM4, TIM9, TIM12
If in doubt, check in STM32CubeIDE and see if the PWM-Input mode can be enabled (under "Combined Channels") for the timer.
The optional direction input can be connected to any GPIO pin. By default a high level direction input is associated with a positive velocity value, while a low level direction input results in a negative velocity value. To reverse this, set the option `dir_positive_high = false`
The direction input is optional - if not provided, you can control the direction from software using the `direction` field.
The velocity values returned are in the range `min_speed` to `max_speed`, while the input PWM duty cycle should lie within the range `min_pwm` to `max_pwm`. Actual input values smaller than `min_pwm` will be treated as `min_pwm`, values larger than `max_pwm` will be treated as `max_pwm`. The behaviour for 100% or 0% duty cycles is undefined, and they should be avoided.
## Usage
Use it like this:
```c++
#include "comms/stm32speeddir/STM32SpeedDirInput.h"
// some example pins - the speed pin has to be on channel 1 or 2 of a timer
#define PIN_SPEED PC6
#define PIN_DIRECTION PB8
STM32SpeedDirInput speed_dir = STM32SpeedDirInput(PIN_SPEED, PIN_DIRECTION);
float target = 0.0f;
void setup(){
...
speed_dir.min_speed = 10.0f; // 10rad/s min speed
speed_dir.max_speed = 100.0f; // 100rad/s max speed
speed_dir.min_pwm = 5.0f; // 5% min duty cycle
speed_dir.max_pwm = 95.0f; // 95% max duty cycle
speed_dir.init();
...
}
void loop(){
target = speed_dir.getTargetVelocity();
motor.move(target);
motor.loopFOC();
}
```

29
firmware/lib/Arduino-FOC-drivers/src/comms/stm32speeddir/STM32SpeedDirInput.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include "./STM32SpeedDirInput.h"
#if defined(_STM32_DEF_)
STM32SpeedDirInput::STM32SpeedDirInput(int pin_speed, int pin_dir) : STM32PWMInput(pin_speed) {
_pin_speed = pin_speed;
_pin_dir = pin_dir;
};
STM32SpeedDirInput::~STM32SpeedDirInput(){};
int STM32SpeedDirInput::init(){
pinMode(_pin_dir, INPUT);
return STM32PWMInput::initialize();
};
float STM32SpeedDirInput::getTargetVelocity(){
if (_pin_dir != NOT_SET)
direction = digitalRead(_pin_dir);
float speed = getDutyCyclePercent();
speed = constrain(speed, min_pwm, max_pwm);
speed = (speed - min_pwm)/(max_pwm - min_pwm) * (max_speed - min_speed) + min_speed;
return (direction == dir_positive_high) ? speed : -speed;
};
#endif

31
firmware/lib/Arduino-FOC-drivers/src/comms/stm32speeddir/STM32SpeedDirInput.h Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include "Arduino.h"
#if defined(_STM32_DEF_)
#include "common/foc_utils.h"
#include "utilities/stm32pwm/STM32PWMInput.h"
class STM32SpeedDirInput : public STM32PWMInput {
public:
STM32SpeedDirInput(int pin_speed, int pin_dir = NOT_SET);
~STM32SpeedDirInput();
int init();
float getTargetVelocity();
float min_speed = 0; // min speed in rad/s
float max_speed = 100; // max speed in rad/s
float min_pwm = 5.0; // min duty cycle in %
float max_pwm = 95.0; // max duty cycle in %
bool dir_positive_high = true; // true if the direction pin is high when the motor is spinning in the positive direction
bool direction = true; // direction of rotation, default positive
protected:
int _pin_speed;
int _pin_dir;
};
#endif

42
firmware/lib/Arduino-FOC-drivers/src/comms/telemetry/README.md Normal file
View File

@@ -0,0 +1,42 @@
# SimpleFOC Telemetry
:warning: unfinished, untested
A flexible abstraction for telemetry (monitoring) of SimpleFOC systems.
The telemetry implementation is based on the SimpleFOC registers, and allows you to send telemetry for any (readable) register. Telemetry supports multiple motors.
The concept allows you to choose registers which are then sent by the telemetry automatically, on a regular schedule.
The method of sending depends on the type of telemetry you add to your program. There are telemetry drivers for:
- Serial ASCII telemetry
- Serial Binary telemetry
- and more drivers will be added in the future
Multiple motors can be added to the same telemetry, to monitor several motors at the same time. The registers reported by telemetry can be changed at run-time. Multiple instances of telemetry can be used to monitor different sets of values at different time intervals, or to send to multiple channels at the same time.
## Usage
Using telemetry is simple:
```c++
SerialASCIITelemetry telemetry = SerialASCIITelemetry();
...
void setup() {
...
telemetry.addMotor(&motor);
telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY });
telemetry.init();
...
}
void loop() {
motor.move();
motor.loopFOC();
telemetry.run();
}
```

67
firmware/lib/Arduino-FOC-drivers/src/comms/telemetry/Telemetry.cpp Normal file
View File

@@ -0,0 +1,67 @@
#include "./Telemetry.h"
Telemetry::Telemetry() {
this->numRegisters = 0;
};
Telemetry::~Telemetry(){
};
void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){
if (numRegisters<=TELEMETRY_MAX_REGISTERS) {
this->numRegisters = numRegisters;
for (uint8_t i=0; i<numRegisters; i++) {
this->registers[i] = registers[i];
if (motors!=NULL)
this->registers_motor[i] = motors[i];
else
this->registers_motor[i] = 0;
}
}
};
void Telemetry::init() {
headerSent = false;
};
void Telemetry::run() {
if (numRegisters<1)
return;
if (!headerSent) {
sendHeader();
headerSent = true;
}
if (downsampleCnt++ < downsample) return;
downsampleCnt = 0;
if (min_elapsed_time > 0) {
long now = _micros();
if (now - last_run_time < min_elapsed_time) return;
last_run_time = now;
}
sendTelemetry();
}
void Telemetry::addMotor(FOCMotor* motor) {
if (numMotors < TELEMETRY_MAX_MOTORS) {
motors[numMotors] = motor;
numMotors++;
}
};

53
firmware/lib/Arduino-FOC-drivers/src/comms/telemetry/Telemetry.h Normal file
View File

@@ -0,0 +1,53 @@
#pragma once
#include "../SimpleFOCRegisters.h"
#include "../RegisterSender.h"
#ifndef TELEMETRY_MAX_REGISTERS
#define TELEMETRY_MAX_REGISTERS 8
#endif
#ifndef TELEMETRY_MAX_MOTORS
#define TELEMETRY_MAX_MOTORS 4
#endif
#define DEF_TELEMETRY_DOWNSAMPLE 100
typedef enum : uint8_t {
TELEMETRY_FRAMETYPE_DATA = 0x01,
TELEMETRY_FRAMETYPE_HEADER = 0x02
} TelemetryFrameType;
class Telemetry : public RegisterSender {
public:
Telemetry();
virtual ~Telemetry();
virtual void init();
void addMotor(FOCMotor* motor);
void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL);
void run();
uint16_t downsample = DEF_TELEMETRY_DOWNSAMPLE;
uint32_t min_elapsed_time = 0;
protected:
virtual void sendTelemetry() = 0;
virtual void sendHeader() = 0;
FOCMotor* motors[TELEMETRY_MAX_MOTORS];
uint8_t numMotors = 0;
uint8_t numRegisters;
uint8_t registers[TELEMETRY_MAX_REGISTERS];
uint8_t registers_motor[TELEMETRY_MAX_REGISTERS];
uint8_t frameSize;
bool headerSent;
long last_run_time = 0;
uint16_t downsampleCnt = 0;
};