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

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.