diff --git a/README.md b/README.md
index 04b33478..b9042658 100644
--- a/README.md
+++ b/README.md
@@ -16,16 +16,24 @@ Therefore this is an attempt to:
- ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield).
- See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller)
-##### Next release will be: SimpleFOClibrary v2.1.1
+##### Release notes be: SimpleFOClibrary v2.1.1
> - bugfixes commander
> - bugfix `voltage_limit` when provided `phase_resistance`
> - bugfix `hall_sensor` examples
> - added examples fot the PowerShield
> - added initial support for `MagneticSensorPWM`
+> - SAMD51 support
+> - **initial support for Raspberry pi Pico**
> - added examples to find the raw max and min of the analog and pwm sensor
> - extension of the `Commander` interface
+> - added pwm centering option `WC`
+> - added pwm modulation cmd `WT`
> - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler
> - issue #62: motor.shaft_angle setting in the motor.initFOC()
+> - issue #76: esp32 division by zero
+> - issue #46: commander end of line character - by @maxlem
+> - [community fix](https://community.simplefoc.com/t/as5600-dead-spot-around-0/208) AS5600 register value
+> - renamed `Pullup::EXTERN` and `Pullup::INTERN` to `Pullup::USE_EXTERN` and `Pullup::USE_INTERN`
## Arduino *SimpleFOClibrary* v2.1
diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino
index 54608e8a..0adf4c17 100644
--- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino
+++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino
@@ -17,7 +17,7 @@ MagneticSensorSPI sensor = MagneticSensorSPI(PA4, 14, 0x3FFF);
// make sure to use the pull-ups!!
// SDA PB7
// SCL PB6
-//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
// Motor instance
BLDCMotor motor = BLDCMotor(11);
diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino
index 17b98ce5..757ae0f1 100644
--- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino
+++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino
@@ -23,9 +23,11 @@ void doB2(){encoder2.handleB();}
// inline current sensor instance
+// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01, 50.0, A0, A2);
// inline current sensor instance
+// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3);
// commander communication instance
diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino
index 70444520..c0f6a10b 100644
--- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino
+++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino
@@ -12,6 +12,7 @@ void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
// inline current sensor instance
+// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
// commander communication instance
diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino
index 8292ecf4..708e2d9e 100644
--- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino
+++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino
@@ -5,7 +5,7 @@ BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
-MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
/**
diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino
index 4f6e83cc..cc4bd4d0 100644
--- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino
+++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino
@@ -27,7 +27,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
// magnetic sensor instance - I2C
-//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
// magnetic sensor instance - analog output
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino
index a54fcea4..3ad87cca 100644
--- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino
+++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino
@@ -15,7 +15,7 @@
// magnetic sensor instance - SPI
//MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
// magnetic sensor instance - I2C
-//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
// magnetic sensor instance - analog output
MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
diff --git a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino
index 0fa87522..66aca6d8 100644
--- a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino
+++ b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino
@@ -21,7 +21,7 @@ void setup() {
encoder.quadrature = Quadrature::ON;
// check if you need internal pullups
- encoder.pullup = Pullup::EXTERN;
+ encoder.pullup = Pullup::USE_EXTERN;
// initialise encoder hardware
encoder.init();
diff --git a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino
index 7c38bb59..a979d9a0 100644
--- a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino
+++ b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino
@@ -32,7 +32,7 @@ void setup() {
encoder.quadrature = Quadrature::ON;
// check if you need internal pullups
- encoder.pullup = Pullup::EXTERN;
+ encoder.pullup = Pullup::USE_EXTERN;
// initialise encoder hardware
encoder.init();
diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino
index 965aec7a..6c19a2e9 100644
--- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino
+++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino
@@ -25,7 +25,7 @@ void setup() {
Serial.begin(115200);
// check if you need internal pullups
- sensor.pullup = Pullup::EXTERN;
+ sensor.pullup = Pullup::USE_EXTERN;
// initialise encoder hardware
sensor.init();
diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino
index cac8dc00..523af03f 100644
--- a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino
+++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino
@@ -34,7 +34,7 @@ void setup() {
Serial.begin(115200);
// check if you need internal pullups
- sensor.pullup = Pullup::EXTERN;
+ sensor.pullup = Pullup::USE_EXTERN;
// initialise encoder hardware
sensor.init();
diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino
index 27816eec..a88b7186 100644
--- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino
+++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino
@@ -16,6 +16,7 @@ void setup() {
// initialise magnetic sensor hardware
sensor.init();
+ // comment out to use sensor in blocking (non-interrupt) way
sensor.enableInterrupt(doPWM);
Serial.println("Sensor ready");
diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino
index 8d0bd54f..15a1f557 100644
--- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino
+++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino
@@ -18,6 +18,7 @@ void setup() {
// initialise magnetic sensor hardware
sensor.init();
+ // comment out to use sensor in blocking (non-interrupt) way
sensor.enableInterrupt(doPWM);
Serial.println("Sensor ready");
diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino
index 5b7665a0..6b19a9fa 100644
--- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino
+++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino
@@ -24,6 +24,7 @@ void setup() {
// initialise magnetic sensor hardware
sensor.init();
+ // comment out to use sensor in blocking (non-interrupt) way
PciManager.registerListener(&listenerPWM);
Serial.println("Sensor ready");
diff --git a/keywords.txt b/keywords.txt
index 971a3ffb..77152a36 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -164,8 +164,8 @@ pinB KEYWORD2
pinC KEYWORD2
index_pin KEYWORD2
-INTERN KEYWORD2
-EXTERN KEYWORD2
+USE_INTERN KEYWORD2
+USE_EXTERN KEYWORD2
DISABLE KEYWORD2
ENABLE KEYWORD2
SpaceVectorPWM KEYWORD2
diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h
index eee0297b..4f9f1108 100644
--- a/src/SimpleFOC.h
+++ b/src/SimpleFOC.h
@@ -109,6 +109,7 @@ void loop() {
#include "drivers/StepperDriver4PWM.h"
#include "drivers/StepperDriver2PWM.h"
#include "current_sense/InlineCurrentSense.h"
+#include "current_sense/LowsideCurrentSense.h"
#include "communication/Commander.h"
#include "communication/StepDirListener.h"
diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h
index c7771a05..3b219401 100644
--- a/src/common/base_classes/Sensor.h
+++ b/src/common/base_classes/Sensor.h
@@ -15,8 +15,8 @@ enum Direction{
* Pullup configuration structure
*/
enum Pullup{
- INTERN, //!< Use internal pullups
- EXTERN //!< Use external pullups
+ USE_INTERN, //!< Use internal pullups
+ USE_EXTERN //!< Use external pullups
};
/**
@@ -43,4 +43,4 @@ class Sensor{
long velocity_calc_timestamp=0; //!< last velocity calculation timestamp
};
-#endif
\ No newline at end of file
+#endif
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
index 06978424..30e99b4f 100644
--- a/src/common/foc_utils.h
+++ b/src/common/foc_utils.h
@@ -9,6 +9,7 @@
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define _sqrt(a) (_sqrtApprox(a))
#define _isset(a) ( (a) != (NOT_SET) )
+#define _UNUSED(v) (void) (v)
// utility defines
#define _2_SQRT3 1.15470053838
diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp
index fae5165f..8e1cb051 100644
--- a/src/communication/Commander.cpp
+++ b/src/communication/Commander.cpp
@@ -1,11 +1,14 @@
#include "Commander.h"
-Commander::Commander(Stream& serial){
+Commander::Commander(Stream& serial, char eol, bool echo){
com_port = &serial;
+ this->eol = eol;
+ this->echo = echo;
}
-Commander::Commander(){
- // do nothing
+Commander::Commander(char eol, bool echo){
+ this->eol = eol;
+ this->echo = echo;
}
@@ -19,33 +22,24 @@ void Commander::add(char id, CommandCallback onCommand, char* label ){
void Commander::run(){
if(!com_port) return;
- // a string to hold incoming data
- while (com_port->available()) {
- // get the new byte:
- received_chars[rec_cnt] = (char)com_port->read();
- // end of user input
- if (received_chars[rec_cnt++] == '\n') {
- // execute the user command
- run(received_chars);
-
- // reset the command buffer
- received_chars[0] = 0;
- rec_cnt=0;
- }
- }
+ run(*com_port, eol);
}
-void Commander::run(Stream& serial){
+void Commander::run(Stream& serial, char eol){
Stream* tmp = com_port; // save the serial instance
- // use the new serial instance to output if not available the one linked in constructor
- if(!tmp) com_port = &serial;
+ char eol_tmp = this->eol;
+ this->eol = eol;
+ com_port = &serial;
// a string to hold incoming data
while (serial.available()) {
// get the new byte:
- received_chars[rec_cnt] = (char)serial.read();
+ int ch = serial.read();
+ received_chars[rec_cnt++] = (char)ch;
// end of user input
- if (received_chars[rec_cnt++] == '\n') {
+ if(echo)
+ print((char)ch);
+ if (isSentinel(ch)) {
// execute the user command
run(received_chars);
@@ -53,9 +47,14 @@ void Commander::run(Stream& serial){
received_chars[0] = 0;
rec_cnt=0;
}
+ if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
+ received_chars[0] = 0;
+ rec_cnt=0;
+ }
}
com_port = tmp; // reset the instance to the internal value
+ this->eol = eol_tmp;
}
void Commander::run(char* user_input){
@@ -71,7 +70,7 @@ void Commander::run(char* user_input){
}
break;
case CMD_VERBOSE:
- if(user_input[1] != '\n') verbose = (VerboseMode)atoi(&user_input[1]);
+ if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]);
printVerbose(F("Verb:"));
switch (verbose){
case VerboseMode::nothing:
@@ -84,7 +83,7 @@ void Commander::run(char* user_input){
}
break;
case CMD_DECIMAL:
- if(user_input[1] != '\n') decimal_places = atoi(&user_input[1]);
+ if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]);
printVerbose(F("Decimal:"));
println(decimal_places);
break;
@@ -100,14 +99,25 @@ void Commander::run(char* user_input){
}
void Commander::motor(FOCMotor* motor, char* user_command) {
+
+ // if target setting
+ if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){
+ printVerbose(F("Target: "));
+ motor->target = atof(user_command);
+ println(motor->target);
+ return;
+ }
+
// parse command letter
char cmd = user_command[0];
char sub_cmd = user_command[1];
+ // check if there is a subcommand or not
int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1;
// check if get command
- bool GET = user_command[value_index] == '\n';
+ bool GET = isSentinel(user_command[value_index]);
// parse command values
- float value = atof(&user_command[value_index]);
+ float value = atof(&user_command[value_index]);
+
// a bit of optimisation of variable memory for Arduino UNO (atmega328)
switch(cmd){
@@ -149,10 +159,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
printVerbose(F("curr: "));
if(!GET){
motor->current_limit = value;
- // if phase resistance is set, change the voltage limit as well.
- if(_isset(motor->phase_resistance)) motor->voltage_limit = value*motor->phase_resistance;
// if phase resistance specified or the current control is on set the current limit to the velocity PID
- if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
+ if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
}
println(motor->current_limit);
break;
@@ -179,7 +187,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
break;
default:
// change control type
- if(!GET && value >= 0 && (int)value < 5)// if set command
+ if(!GET && value >= 0 && (int)value < 5) // if set command
motor->controller = (MotionControlType)value;
switch(motor->controller){
case MotionControlType::torque:
@@ -354,7 +362,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
case SCMD_SET:
if(!GET) motor->monitor_variables = (uint8_t) 0;
for(int i = 0; i < 7; i++){
- if(user_command[value_index+i] == '\n') break;
+ if(isSentinel(user_command[value_index+i])) break;
if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i);
print( (user_command[value_index+i] - '0') );
}
@@ -365,16 +373,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
break;
}
break;
- default: // target change
- printVerbose(F("Target: "));
- motor->target = atof(user_command);
- println(motor->target);
+ default: // unknown cmd
+ printVerbose(F("unknown cmd "));
+ printError();
}
}
void Commander::pid(PIDController* pid, char* user_cmd){
char cmd = user_cmd[0];
- bool GET = user_cmd[1] == '\n';
+ bool GET = isSentinel(user_cmd[1]);
float value = atof(&user_cmd[1]);
switch (cmd){
@@ -411,7 +418,7 @@ void Commander::pid(PIDController* pid, char* user_cmd){
void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
char cmd = user_cmd[0];
- bool GET = user_cmd[1] == '\n';
+ bool GET = isSentinel(user_cmd[1]);
float value = atof(&user_cmd[1]);
switch (cmd){
@@ -427,11 +434,21 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
}
void Commander::scalar(float* value, char* user_cmd){
- bool GET = user_cmd[0] == '\n';
+ bool GET = isSentinel(user_cmd[0]);
if(!GET) *value = atof(user_cmd);
println(*value);
}
+bool Commander::isSentinel(char ch)
+{
+ if(ch == eol)
+ return true;
+ else if (ch == '\r')
+ {
+ printVerbose(F("Warn: \\r detected! \n"));
+ }
+ return false;
+}
void Commander::print(const int number){
if( !com_port || verbose == VerboseMode::nothing ) return;
diff --git a/src/communication/Commander.h b/src/communication/Commander.h
index 0b07b707..aaa55b11 100644
--- a/src/communication/Commander.h
+++ b/src/communication/Commander.h
@@ -7,6 +7,10 @@
#include "../common/lowpass_filter.h"
#include "commands.h"
+
+#define MAX_COMMAND_LENGTH 20
+
+
// Commander verbose display to the user type
enum VerboseMode{
nothing = 0, // display nothing - good for monitoring
@@ -38,9 +42,11 @@ class Commander
* Also if the function run() is used it uses this serial instance to read the serial for user commands
*
* @param serial - Serial com port instance
+ * @param eol - the end of line sentinel character
+ * @param echo - echo last typed character (for command line feedback)
*/
- Commander(Stream &serial);
- Commander();
+ Commander(Stream &serial, char eol = '\n', bool echo = false);
+ Commander(char eol = '\n', bool echo = false);
/**
* Function reading the serial port and firing callbacks that have been added to the commander
@@ -61,9 +67,10 @@ class Commander
* '#' - Number of decimal places
* '?' - Scan command - displays all the labels of attached nodes
*
- * @param reader - Stream to read user input
+ * @param reader - temporary stream to read user input
+ * @param eol - temporary end of line sentinel
*/
- void run(Stream &reader);
+ void run(Stream &reader, char eol = '\n');
/**
* Function reading the string of user input and firing callbacks that have been added to the commander
* once the user has requested them - when he sends the command
@@ -91,7 +98,8 @@ class Commander
// monitoring functions
Stream* com_port = nullptr; //!< Serial terminal variable if provided
-
+ char eol = '\n'; //!< end of line sentinel character
+ bool echo = false; //!< echo last typed character (for command line feedback)
/**
*
* FOC motor (StepperMotor and BLDCMotor) command interface
@@ -174,7 +182,7 @@ class Commander
int call_count = 0;//!< number callbacks that are subscribed
// helping variable for serial communication reading
- char received_chars[20] = {0}; //!< so far received user message - waiting for newline
+ char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline
int rec_cnt = 0; //!< number of characters receives
// serial printing functions
@@ -194,6 +202,7 @@ class Commander
* @param message - number to be printed
* @param newline - if needs lewline (1) otherwise (0)
*/
+
void print(const float number);
void print(const int number);
void print(const char* message);
@@ -206,6 +215,7 @@ class Commander
void println(const char message);
void printError();
+ bool isSentinel(char ch);
};
diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp
index 2e9c4421..879395d1 100644
--- a/src/current_sense/InlineCurrentSense.cpp
+++ b/src/current_sense/InlineCurrentSense.cpp
@@ -22,7 +22,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _
// Inline sensor init function
void InlineCurrentSense::init(){
// configure ADC variables
- _configureADC(pinA,pinB,pinC);
+ _configureADCInline(pinA,pinB,pinC);
// calibrate zero offsets
calibrateOffsets();
}
@@ -36,9 +36,9 @@ void InlineCurrentSense::calibrateOffsets(){
offset_ic = 0;
// read the adc voltage 1000 times ( arbitrary number )
for (int i = 0; i < calibration_rounds; i++) {
- offset_ia += _readADCVoltage(pinA);
- offset_ib += _readADCVoltage(pinB);
- if(_isset(pinC)) offset_ic += _readADCVoltage(pinC);
+ offset_ia += _readADCVoltageInline(pinA);
+ offset_ib += _readADCVoltageInline(pinB);
+ if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC);
_delay(1);
}
// calculate the mean offsets
@@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){
// read all three phase currents (if possible 2 or 3)
PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
PhaseCurrent_s current;
- current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps
- current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps
- current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps
+ current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
+ current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
+ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
return current;
}
// Function synchronizing current sense with motor driver.
diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp
new file mode 100644
index 00000000..e0df940a
--- /dev/null
+++ b/src/current_sense/LowsideCurrentSense.cpp
@@ -0,0 +1,181 @@
+#include "LowsideCurrentSense.h"
+// LowsideCurrentSensor constructor
+// - shunt_resistor - shunt resistor value
+// - gain - current-sense op-amp gain
+// - phA - A phase adc pin
+// - phB - B phase adc pin
+// - phC - C phase adc pin (optional)
+LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+ pinA = _pinA;
+ pinB = _pinB;
+ pinC = _pinC;
+
+ shunt_resistor = _shunt_resistor;
+ amp_gain = _gain;
+ volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps
+ // gains for each phase
+ gain_a = volts_to_amps_ratio;
+ gain_b = volts_to_amps_ratio;
+ gain_c = volts_to_amps_ratio;
+}
+
+// Lowside sensor init function
+void LowsideCurrentSense::init(){
+ // configure ADC variables
+ _configureADCLowSide(pinA,pinB,pinC);
+ // calibrate zero offsets
+ calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void LowsideCurrentSense::calibrateOffsets(){
+ // find adc offset = zero current voltage
+ offset_ia =0;
+ offset_ib= 0;
+ offset_ic= 0;
+ // read the adc voltage 1000 times ( arbitrary number )
+ for (int i = 0; i < 1000; i++) {
+ _startADC3PinConversionLowSide();
+ offset_ia += _readADCVoltageLowSide(pinA);
+ offset_ib += _readADCVoltageLowSide(pinB);
+ if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC);
+ _delay(1);
+ }
+ // calculate the mean offsets
+ offset_ia = offset_ia / 1000.0;
+ offset_ib = offset_ib / 1000.0;
+ if(_isset(pinC)) offset_ic = offset_ic / 1000.0;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
+ PhaseCurrent_s current;
+ _startADC3PinConversionLowSide();
+ current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps
+ current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps
+ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps
+ return current;
+}
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int LowsideCurrentSense::driverSync(BLDCDriver *driver){
+ _driverSyncLowSide();
+ return 1;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
+ //gain_a *= -1;
+ //gain_b *= -1;
+ //gain_c *= -1;
+
+ /*
+ int exit_flag = 1;
+ if(skip_align) return exit_flag;
+
+ // set phase A active and phases B and C down
+ driver->setPwm(voltage, 0, 0);
+ _delay(2000);
+ PhaseCurrent_s c = getPhaseCurrents();
+ // read the current 100 times ( arbitrary number )
+ for (int i = 0; i < 100; i++) {
+ PhaseCurrent_s c1 = getPhaseCurrents();
+ c.a = c.a*0.6 + 0.4*c1.a;
+ c.b = c.b*0.6 + 0.4*c1.b;
+ c.c = c.c*0.6 + 0.4*c1.c;
+ _delay(3);
+ }
+ driver->setPwm(0, 0, 0);
+ // align phase A
+ float ab_ratio = fabs(c.a / c.b);
+ float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+ if( ab_ratio > 1.5 ){ // should be ~2
+ gain_a *= _sign(c.a);
+ }else if( ab_ratio < 0.7 ){ // should be ~0.5
+ // switch phase A and B
+ int tmp_pinA = pinA;
+ pinA = pinB;
+ pinB = tmp_pinA;
+ gain_a *= _sign(c.b);
+ exit_flag = 2; // signal that pins have been switched
+ }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5
+ // switch phase A and C
+ int tmp_pinA = pinA;
+ pinA = pinC;
+ pinC= tmp_pinA;
+ gain_a *= _sign(c.c);
+ exit_flag = 2;// signal that pins have been switched
+ }else{
+ // error in current sense - phase either not measured or bad connection
+ return 0;
+ }
+
+ // set phase B active and phases A and C down
+ driver->setPwm(0, voltage, 0);
+ _delay(200);
+ c = getPhaseCurrents();
+ // read the current 50 times
+ for (int i = 0; i < 100; i++) {
+ PhaseCurrent_s c1 = getPhaseCurrents();
+ c.a = c.a*0.6 + 0.4*c1.a;
+ c.b = c.b*0.6 + 0.4*c1.b;
+ c.c = c.c*0.6 + 0.4*c1.c;
+ _delay(3);
+ }
+ driver->setPwm(0, 0, 0);
+ float ba_ratio = fabs(c.b/c.a);
+ float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+ if( ba_ratio > 1.5 ){ // should be ~2
+ gain_b *= _sign(c.b);
+ }else if( ba_ratio < 0.7 ){ // it should be ~0.5
+ // switch phase A and B
+ int tmp_pinB = pinB;
+ pinB = pinA;
+ pinA = tmp_pinB;
+ gain_b *= _sign(c.a);
+ exit_flag = 2; // signal that pins have been switched
+ }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5
+ // switch phase A and C
+ int tmp_pinB = pinB;
+ pinB = pinC;
+ pinC = tmp_pinB;
+ gain_b *= _sign(c.c);
+ exit_flag = 2; // signal that pins have been switched
+ }else{
+ // error in current sense - phase either not measured or bad connection
+ return 0;
+ }
+
+ // if phase C measured
+ if(_isset(pinC)){
+ // set phase B active and phases A and C down
+ driver->setPwm(0, 0, voltage);
+ _delay(200);
+ c = getPhaseCurrents();
+ // read the adc voltage 500 times ( arbitrary number )
+ for (int i = 0; i < 50; i++) {
+ PhaseCurrent_s c1 = getPhaseCurrents();
+ c.c = (c.c+c1.c)/50.0;
+ }
+ driver->setPwm(0, 0, 0);
+ gain_c *= _sign(c.c);
+ }
+
+ if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+ // exit flag is either
+ // 0 - fail
+ // 1 - success and nothing changed
+ // 2 - success but pins reconfigured
+ // 3 - success but gains inverted
+ // 4 - success but pins reconfigured and gains inverted
+
+ return exit_flag;
+ */
+ return 1;
+}
diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h
new file mode 100644
index 00000000..2300d5f6
--- /dev/null
+++ b/src/current_sense/LowsideCurrentSense.h
@@ -0,0 +1,59 @@
+#ifndef LOWSIDE_CS_LIB_H
+#define LOWSIDE_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/base_classes/FOCMotor.h"
+#include "hardware_api.h"
+
+
+class LowsideCurrentSense: public CurrentSense{
+ public:
+ /**
+ LowsideCurrentSense class constructor
+ @param shunt_resistor shunt resistor value
+ @param gain current-sense op-amp gain
+ @param phA A phase adc pin
+ @param phB B phase adc pin
+ @param phC C phase adc pin (optional)
+ */
+ LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
+
+ // CurrentSense interface implementing functions
+ void init() override;
+ PhaseCurrent_s getPhaseCurrents() override;
+ int driverSync(BLDCDriver *driver) override;
+ int driverAlign(BLDCDriver *driver, float voltage) override;
+
+ // ADC measuremnet gain for each phase
+ // support for different gains for different phases of more commonly - inverted phase currents
+ // this should be automated later
+ float gain_a; //!< phase A gain
+ float gain_b; //!< phase B gain
+ float gain_c; //!< phase C gain
+
+ private:
+
+ // hardware variables
+ int pinA; //!< pin A analog pin for current measurement
+ int pinB; //!< pin B analog pin for current measurement
+ int pinC; //!< pin C analog pin for current measurement
+
+ // gain variables
+ double shunt_resistor; //!< Shunt resistor value
+ double amp_gain; //!< amp gain value
+ double volts_to_amps_ratio; //!< Volts to amps ratio
+
+ /**
+ * Function finding zero offsets of the ADC
+ */
+ void calibrateOffsets();
+ double offset_ia; //!< zero current A voltage value (center of the adc reading)
+ double offset_ib; //!< zero current B voltage value (center of the adc reading)
+ double offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+};
+
+#endif
diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h
index ab376e8e..7b648cde 100644
--- a/src/current_sense/hardware_api.h
+++ b/src/current_sense/hardware_api.h
@@ -9,7 +9,7 @@
*
* @param pinA - the arduino pin to be read (it has to be ADC pin)
*/
-float _readADCVoltage(const int pinA);
+float _readADCVoltageInline(const int pinA);
/**
* function reading an ADC value and returning the read voltage
@@ -18,6 +18,28 @@ float _readADCVoltage(const int pinA);
* @param pinB - adc pin B
* @param pinC - adc pin C
*/
-void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET);
+void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET);
+/**
+ * function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - adc pin A
+ * @param pinB - adc pin B
+ * @param pinC - adc pin C
+ */
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET);
+
+void _startADC3PinConversionLowSide();
+
+/**
+ * function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - the arduino pin to be read (it has to be ADC pin)
+ */
+float _readADCVoltageLowSide(const int pinA);
+
+/**
+ * function syncing the Driver with the ADC for the LowSide Sensing
+ */
+void _driverSyncLowSide();
#endif
\ No newline at end of file
diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp
new file mode 100644
index 00000000..3f7e83b3
--- /dev/null
+++ b/src/current_sense/hardware_specific/esp32_mcu.cpp
@@ -0,0 +1,83 @@
+#include "../hardware_api.h"
+
+#if defined(ESP_H)
+
+#include "driver/mcpwm.h"
+#include "soc/mcpwm_reg.h"
+#include "soc/mcpwm_struct.h"
+
+#define _ADC_VOLTAGE 3.3
+#define _ADC_RESOLUTION 4095.0
+
+static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1};
+int a1, a2, a3; //Current readings from internal current sensor amplifiers
+int _pinA, _pinB, _pinC;
+static void IRAM_ATTR isr_handler(void*);
+byte currentState = 1;
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pin){
+ uint32_t raw_adc;
+
+ if (pin == _pinA) raw_adc = a1;
+ else if (pin == _pinB) raw_adc = a2;
+ else if (pin == _pinC) raw_adc = a3;
+
+ return raw_adc * _ADC_CONV;
+}
+
+void _startADC3PinConversionLowSide(){
+
+}
+
+// function reading an ADC value and returning the read voltage
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+ _pinA = pinA;
+ _pinB = pinB;
+ if( _isset(pinC) ) _pinC = pinC;
+ pinMode(pinA, INPUT);
+ pinMode(pinB, INPUT);
+ if( _isset(pinC) ) pinMode(pinC, INPUT);
+
+}
+
+void _driverSyncLowSide(){
+ MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
+ MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt
+ //MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt
+ mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler
+}
+
+// Read currents when interrupt is triggered
+static void IRAM_ATTR isr_handler(void*){
+ uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st;
+ uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st;
+ //uint32_t mcpwm_intr_status_2 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st;
+
+ if(mcpwm_intr_status_0 > 0 && currentState == 1){
+ a1 = analogRead(_pinA);
+ //a2 = analogRead(_pinB);
+ currentState = 2;
+ }
+ else if(mcpwm_intr_status_1 > 0 && currentState == 2){
+ a2 = analogRead(_pinB);
+ //a3 = analogRead(_pinC);
+ currentState = 1;
+ }
+ /*
+ else if(mcpwm_intr_status_2 > 0 && currentState == 3){
+ a3 = analogRead(_pinC);
+ //a1 = analogRead(_pinA);
+ currentState = 1;
+ }*/
+
+ MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0;
+ MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1;
+ //MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2;
+}
+
+#endif
diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp
index 423774c2..dccd3601 100644
--- a/src/current_sense/hardware_specific/generic_mcu.cpp
+++ b/src/current_sense/hardware_specific/generic_mcu.cpp
@@ -18,8 +18,8 @@
#define _ADC_VOLTAGE 3.3
#define _ADC_RESOLUTION 1024.0
#elif defined(ESP_H) // or esp32
- #define _ADC_VOLTAGE 3.3
- #define _ADC_RESOLUTION 4095.0
+ // do nothing implemented in esp32_mcu.h
+
#elif defined(_STM32_DEF_) // or stm32
#define _ADC_VOLTAGE 3.3
#define _ADC_RESOLUTION 1024.0
@@ -33,14 +33,14 @@
#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
// function reading an ADC value and returning the read voltage
-float _readADCVoltage(const int pinA){
+float _readADCVoltageInline(const int pinA){
uint32_t raw_adc = analogRead(pinA);
return raw_adc * _ADC_CONV;
}
// function reading an ADC value and returning the read voltage
-void _configureADC(const int pinA,const int pinB,const int pinC){
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp
new file mode 100644
index 00000000..5c5de779
--- /dev/null
+++ b/src/current_sense/hardware_specific/samd21_mcu.cpp
@@ -0,0 +1,304 @@
+
+#include "samd21_mcu.h"
+#include "../hardware_api.h"
+
+
+static bool freeRunning = false;
+static int _pinA, _pinB, _pinC;
+static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode
+static SAMDCurrentSenseADCDMA instance;
+/**
+ * function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - adc pin A
+ * @param pinB - adc pin B
+ * @param pinC - adc pin C
+ */
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC)
+{
+ _pinA = pinA;
+ _pinB = pinB;
+ _pinC = pinC;
+ freeRunning = true;
+ instance.init(pinA, pinB, pinC);
+
+}
+void _startADC3PinConversionLowSide()
+{
+ instance.startADCScan();
+}
+/**
+ * function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - the arduino pin to be read (it has to be ADC pin)
+ */
+float _readADCVoltageLowSide(const int pinA)
+{
+ instance.readResults(a, b, c);
+
+ if(pinA == _pinA)
+ return instance.toVolts(a);
+ if(pinA == _pinB)
+ return instance.toVolts(b);
+ if(pinA == _pinC)
+ return instance.toVolts(c);
+
+ return NAN;
+}
+
+/**
+ * function syncing the Driver with the ADC for the LowSide Sensing
+ */
+void _driverSyncLowSide()
+{
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented"));
+ instance.startADCScan();
+ //TODO: hook with PWM interrupts
+}
+
+
+
+
+
+
+
+
+
+
+ // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless
+
+static void adcStopWithDMA(void);
+static void adcStartWithDMA(void);
+
+/**
+ * @brief ADC sync wait
+ * @retval void
+ */
+static __inline__ void ADCsync() __attribute__((always_inline, unused));
+static void ADCsync() {
+ while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free
+}
+
+// ADC DMA sequential free running (6) with Interrupts /////////////////
+
+SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance()
+{
+
+ return &instance;
+}
+
+SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA()
+{
+}
+
+void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA)
+{
+ this->pinA = pinA;
+ this->pinB = pinB;
+ this->pinC = pinC;
+ this->pinAREF = pinAREF;
+ this->channelDMA = channelDMA;
+ this->voltageAREF = voltageAREF;
+ this->maxCountsADC = 1 << adcBits;
+ countsToVolts = ( voltageAREF / maxCountsADC );
+
+ initPins();
+ initADC();
+ initDMA();
+ startADCScan(); //so we have something to read next time we call readResults()
+}
+
+
+void SAMDCurrentSenseADCDMA::startADCScan(){
+ adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize);
+ adcStartWithDMA();
+}
+
+bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){
+ if(ADC->CTRLA.bit.ENABLE)
+ return false;
+ uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
+ uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
+ a = adcBuffer[ainA];
+ b = adcBuffer[ainB];
+ if(_isset(pinC))
+ {
+ uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
+ c = adcBuffer[ainC];
+ }
+ return true;
+}
+
+
+float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) {
+ return counts * countsToVolts;
+}
+
+void SAMDCurrentSenseADCDMA::initPins(){
+
+ pinMode(pinAREF, INPUT);
+ pinMode(pinA, INPUT);
+ pinMode(pinB, INPUT);
+
+ uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
+ uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
+ firstAIN = min(ainA, ainB);
+ lastAIN = max(ainA, ainB);
+ if( _isset(pinC) )
+ {
+ uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
+ pinMode(pinC, INPUT);
+ firstAIN = min(firstAIN, ainC);
+ lastAIN = max(lastAIN, ainC);
+ }
+
+ oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout
+ BufferSize = lastAIN - oneBeforeFirstAIN + 1;
+
+}
+
+void SAMDCurrentSenseADCDMA::initADC(){
+
+ analogRead(pinA); // do some pin init pinPeripheral()
+ analogRead(pinB); // do some pin init pinPeripheral()
+ analogRead(pinC); // do some pin init pinPeripheral()
+
+ ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC
+ ADCsync();
+ //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA
+ ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X
+ // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default
+ ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA;
+ // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0;
+ ADCsync(); // ref 31.6.16
+
+ /*
+ Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan
+ This register gives the number of input sources included in the pin scan. The number of input sources included is
+ INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS +
+ INPUTOFFSET + INPUTSCAN.
+ The range of the scan mode must not exceed the number of input channels available on the device.
+ Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection
+ These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If
+ the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit
+ group in the SamplingControl register must be written.
+ Table 32-14. Positive Mux Input Selection
+ MUXPOS[4:0] Group configuration Description
+ 0x00 PIN0 ADC AIN0 pin
+ 0x01 PIN1 ADC AIN1 pin
+ 0x02 PIN2 ADC AIN2 pin
+ 0x03 PIN3 ADC AIN3 pin
+ 0x04 PIN4 ADC AIN4 pin
+ 0x05 PIN5 ADC AIN5 pin
+ 0x06 PIN6 ADC AIN6 pin
+ 0x07 PIN7 ADC AIN7 pin
+ 0x08 PIN8 ADC AIN8 pin
+ 0x09 PIN9 ADC AIN9 pin
+ 0x0A PIN10 ADC AIN10 pin
+ 0x0B PIN11 ADC AIN11 pin
+ 0x0C PIN12 ADC AIN12 pin
+ 0x0D PIN13 ADC AIN13 pin
+ 0x0E PIN14 ADC AIN14 pin
+ 0x0F PIN15 ADC AIN15 pin
+ 0x10 PIN16 ADC AIN16 pin
+ 0x11 PIN17 ADC AIN17 pin
+ 0x12 PIN18 ADC AIN18 pin
+ 0x13 PIN19 ADC AIN19 pin
+ 0x14-0x17 Reserved
+ 0x18 TEMP Temperature reference
+ 0x19 BANDGAP Bandgap voltage
+ 0x1A SCALEDCOREVCC 1/4 scaled core supply
+ 0x1B SCALEDIOVCC 1/4 scaled I/O supply
+ 0x1C DAC DAC output
+ 0x1D-0x1F Reserved
+ */
+ ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN;
+ ADCsync();
+ ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive)
+ ADCsync();
+ ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor
+ ADCsync();
+ ADC->AVGCTRL.reg = 0x00 ; //no averaging
+ ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16
+ // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU
+ ADCsync();
+ ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT;
+ ADCsync();
+}
+
+volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16)));
+
+void SAMDCurrentSenseADCDMA::initDMA() {
+ // probably on by default
+ PM->AHBMASK.reg |= PM_AHBMASK_DMAC ;
+ PM->APBBMASK.reg |= PM_APBBMASK_DMAC ;
+ NVIC_EnableIRQ( DMAC_IRQn ) ;
+ DMAC->BASEADDR.reg = (uint32_t)descriptor_section;
+ DMAC->WRBADDR.reg = (uint32_t)wrb;
+ DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf);
+}
+
+
+void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) {
+
+ DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
+ DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE;
+ DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST;
+ DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA));
+
+ DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0)
+ | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY)
+ | DMAC_CHCTRLB_TRIGACT_BEAT;
+ DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts
+ descriptor.descaddr = 0;
+ descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg;
+ descriptor.btcnt = hwords;
+ descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address
+ descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID;
+ memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor));
+
+ // start channel
+ DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
+ DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
+}
+
+
+int iii = 0;
+
+void adcStopWithDMA(void){
+ ADCsync();
+ ADC->CTRLA.bit.ENABLE = 0x00;
+ // ADCsync();
+ // if(iii++ % 1000 == 0)
+ // {
+ // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a);
+ // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
+ // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b);
+ // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
+ // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c);
+ // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!");
+ // }
+
+
+}
+
+void adcStartWithDMA(void){
+ ADCsync();
+ ADC->INPUTCTRL.bit.INPUTOFFSET = 0;
+ ADCsync();
+ ADC->SWTRIG.bit.FLUSH = 1;
+ ADCsync();
+ ADC->CTRLA.bit.ENABLE = 0x01;
+ ADCsync();
+}
+
+void DMAC_Handler() {
+ uint8_t active_channel;
+ active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number
+ DMAC->CHID.reg = DMAC_CHID_ID(active_channel);
+ adcStopWithDMA();
+ DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear
+ DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR;
+ DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP;
+
+}
\ No newline at end of file
diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h
new file mode 100644
index 00000000..a01c72e3
--- /dev/null
+++ b/src/current_sense/hardware_specific/samd21_mcu.h
@@ -0,0 +1,61 @@
+#ifndef CURRENT_SENSE_SAMD21_H
+#define CURRENT_SENSE_SAMD21_H
+
+// #define SIMPLEFOC_SAMD_DEBUG
+#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
+#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
+#endif
+
+#include
+ typedef struct {
+ uint16_t btctrl;
+ uint16_t btcnt;
+ uint32_t srcaddr;
+ uint32_t dstaddr;
+ uint32_t descaddr;
+ } dmacdescriptor ;
+
+
+class SAMDCurrentSenseADCDMA
+{
+
+public:
+ static SAMDCurrentSenseADCDMA * getHardwareAPIInstance();
+ SAMDCurrentSenseADCDMA();
+ void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3);
+ void startADCScan();
+ bool readResults(uint16_t & a, uint16_t & b, uint16_t & c);
+ float toVolts(uint16_t counts);
+private:
+
+ void adcToDMATransfer(void *rxdata, uint32_t hwords);
+
+ void initPins();
+ void initADC();
+ void initDMA();
+
+ uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout
+ uint32_t firstAIN;
+ uint32_t lastAIN;
+ uint32_t BufferSize = 0;
+
+ uint16_t adcBuffer[20];
+
+
+ uint32_t pinA;
+ uint32_t pinB;
+ uint32_t pinC;
+ uint32_t pinAREF;
+ uint32_t channelDMA; // DMA channel
+ bool freeRunning;
+
+ float voltageAREF;
+ float maxCountsADC;
+ float countsToVolts;
+
+ dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16)));
+ dmacdescriptor descriptor __attribute__ ((aligned (16)));
+
+};
+
+#endif
\ No newline at end of file
diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp
index 87eb7f5b..11215863 100644
--- a/src/drivers/BLDCDriver3PWM.cpp
+++ b/src/drivers/BLDCDriver3PWM.cpp
@@ -41,9 +41,6 @@ void BLDCDriver3PWM::disable()
// init hardware pins
int BLDCDriver3PWM::init() {
- // a bit of separation
- _delay(1000);
-
// PWM pins
pinMode(pwmA, OUTPUT);
pinMode(pwmB, OUTPUT);
diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp
index 5dd99eb2..d51eec0a 100644
--- a/src/drivers/BLDCDriver6PWM.cpp
+++ b/src/drivers/BLDCDriver6PWM.cpp
@@ -41,9 +41,7 @@ void BLDCDriver6PWM::disable()
// init hardware pins
int BLDCDriver6PWM::init() {
- // a bit of separation
- _delay(1000);
-
+
// PWM pins
pinMode(pwmA_h, OUTPUT);
pinMode(pwmB_h, OUTPUT);
diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp
index 7ca7cc31..09dd3e16 100644
--- a/src/drivers/StepperDriver2PWM.cpp
+++ b/src/drivers/StepperDriver2PWM.cpp
@@ -61,9 +61,6 @@ void StepperDriver2PWM::disable()
// init hardware pins
int StepperDriver2PWM::init() {
- // a bit of separation
- _delay(1000);
-
// PWM pins
pinMode(pwm1, OUTPUT);
pinMode(pwm2, OUTPUT);
diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp
index d3678f00..f752ea6e 100644
--- a/src/drivers/StepperDriver4PWM.cpp
+++ b/src/drivers/StepperDriver4PWM.cpp
@@ -39,8 +39,6 @@ void StepperDriver4PWM::disable()
// init hardware pins
int StepperDriver4PWM::init() {
- // a bit of separation
- _delay(1000);
// PWM pins
pinMode(pwm1A, OUTPUT);
diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp
index cf12c302..be8dadb1 100644
--- a/src/drivers/hardware_specific/atmega328_mcu.cpp
+++ b/src/drivers/hardware_specific/atmega328_mcu.cpp
@@ -6,7 +6,7 @@
void _pinHighFrequency(const int pin){
// High PWM frequency
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
- if (pin == 5 || pin == 6 ) {
+ if (pin == 5 || pin == 6 ) {
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
}
@@ -17,12 +17,12 @@ void _pinHighFrequency(const int pin){
}
-
// function setting the high pwm frequency to the supplied pins
// - Stepper motor - 2PWM setting
// - hardware speciffic
// supports Arudino/ATmega328
void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+ _UNUSED(pwm_frequency);
// High PWM frequency
// - always max 32kHz
_pinHighFrequency(pinA);
@@ -34,6 +34,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
// - hardware speciffic
// supports Arudino/ATmega328
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+ _UNUSED(pwm_frequency);
// High PWM frequency
// - always max 32kHz
_pinHighFrequency(pinA);
@@ -118,6 +119,8 @@ int _configureComplementaryPair(int pinH, int pinL) {
// - hardware specific
// supports Arudino/ATmega328
int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+ _UNUSED(pwm_frequency);
+ _UNUSED(dead_zone);
// High PWM frequency
// - always max 32kHz
int ret_flag = 0;
diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp
index 27097301..e0d39afd 100644
--- a/src/drivers/hardware_specific/due_mcu.cpp
+++ b/src/drivers/hardware_specific/due_mcu.cpp
@@ -1,6 +1,11 @@
#include "../hardware_api.h"
-#if defined(__arm__) && defined(__SAM3X8E__)
+#if defined(__arm__) && defined(__SAM3X8E__)
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+#define _PWM_RES_MIN 255 // 50khz
// pwm frequency and max duty cycle
static unsigned long _pwm_frequency;
@@ -101,7 +106,29 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){
if (!PWMEnabled) {
// PWM Startup code
pmc_enable_periph_clk(PWM_INTERFACE_ID);
- PWMC_ConfigureClocks(pwm_freq * _max_pwm_value, 0, VARIANT_MCK);
+ // this function does not work too well - I'll rewrite it
+ // PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK);
+
+ // finding the divisors an prescalers form FindClockConfiguration function
+ uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024};
+ uint8_t divisor = 0;
+ uint32_t prescaler;
+
+ /* Find prescaler and divisor values */
+ prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
+ while ((prescaler > 255) && (divisor < 11)) {
+ divisor++;
+ prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
+ }
+ // update the divisor*prescaler value
+ prescaler = prescaler | (divisor << 8);
+
+ // now calculate the real resolution timer period necessary (pwm resolution)
+ // pwm_res = bus_freq / (pwm_freq * (prescaler))
+ _max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler);
+ // set the prescaler value
+ PWM->PWM_CLK = prescaler;
+
PWMEnabled = 1;
}
@@ -112,6 +139,8 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){
g_APinDescription[ulPin].ulPinType,
g_APinDescription[ulPin].ulPin,
g_APinDescription[ulPin].ulPinConfiguration);
+ // PWM_CMR_CALG - center align
+ // PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0);
PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0);
PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value);
PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0);
@@ -123,7 +152,7 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
// We use MCLK/2 as clock.
- const uint32_t TC = VARIANT_MCK / 2 / pwm_freq;
+ const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ;
// Setup Timer for this pin
ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
uint32_t chNo = channelToChNo[channel];
@@ -131,17 +160,18 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){
Tc *chTC = channelToTC[channel];
uint32_t interfaceID = channelToId[channel];
- if (!TCChanEnabled[interfaceID]) {
- pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID);
- TC_Configure(chTC, chNo,
- TC_CMR_TCCLKS_TIMER_CLOCK1 |
- TC_CMR_WAVE | // Waveform mode
- TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC
- TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output)
- TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR |
- TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
- TC_SetRC(chTC, chNo, TC);
- }
+ if (!TCChanEnabled[interfaceID]) {
+ pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID);
+ TC_Configure(chTC, chNo,
+ TC_CMR_TCCLKS_TIMER_CLOCK1 |
+ TC_CMR_WAVE | // Waveform mode
+ TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC
+ TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output)
+ TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR |
+ TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
+ TC_SetRC(chTC, chNo, TC);
+ }
+
// disable the counter on start
if (chA){
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
@@ -197,7 +227,7 @@ void setPwm(uint32_t ulPin, uint32_t ulValue) {
const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency;
// Map value to Timer ranges 0..max_duty_cycle => 0..TC
// Setup Timer for this pin
- ulValue = ulValue * TC;
+ ulValue = ulValue * TC ;
pwm_counter_vals[channel] = ulValue / _max_pwm_value;
// enable counter
if (channelToAB[channel])
@@ -297,8 +327,8 @@ void TC8_Handler()
// - BLDC motor - 3PWM setting
// - hardware specific
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 35000; // default frequency 50khz
- else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
// save the pwm frequency
_pwm_frequency = pwm_frequency;
// cinfigure pwm pins
@@ -314,8 +344,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int
//- Stepper driver - 2PWM setting
// - hardware specific
void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
- if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz
- else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+ if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
// save the pwm frequency
_pwm_frequency = pwm_frequency;
// cinfigure pwm pins
@@ -329,8 +359,8 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
// - Stepper motor - 4PWM setting
// - hardware speciffic
void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
- if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz
- else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+ if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
// save the pwm frequency
_pwm_frequency = pwm_frequency;
// cinfigure pwm pins
@@ -373,19 +403,4 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
}
-// Configuring PWM frequency, resolution and alignment
-// - BLDC driver - 6PWM setting
-// - hardware specific
-int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
- return -1;
-}
-
-// Function setting the duty cycle to the pwm pin (ex. analogWrite())
-// - BLDC driver - 6PWM setting
-// - hardware specific
-void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
- return;
-}
-
-
#endif
\ No newline at end of file
diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp
index b9c23d9c..0bc2d863 100644
--- a/src/drivers/hardware_specific/esp32_mcu.cpp
+++ b/src/drivers/hardware_specific/esp32_mcu.cpp
@@ -20,6 +20,9 @@
#define _PWM_RES_MIN 1500
// max resolution
#define _PWM_RES_MAX 3000
+// pwm frequency
+#define _PWM_FREQUENCY 25000 // default
+#define _PWM_FREQUENCY_MAX 50000 // mqx
// structure containing motor slot configuration
// this library supports up to 4 motors
@@ -102,9 +105,10 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm
mcpwm_config_t pwm_config;
pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH
+ pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76
mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
- mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings
- mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings
+ mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings
+ mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings
if (_isset(dead_zone)){
// dead zone is configured
@@ -149,6 +153,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm
mcpwm_num->timer[1].period.upmethod = 0;
mcpwm_num->timer[2].period.upmethod = 0;
_delay(1);
+ // _delay(1);
//restart the timers
mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
@@ -171,9 +176,8 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm
// - hardware speciffic
// supports Arudino/ATmega328, STM32 and ESP32
void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
-
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency
- else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
stepper_2pwm_motor_slots_t m_slot = {};
@@ -217,9 +221,8 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
// - hardware speciffic
// supports Arudino/ATmega328, STM32 and ESP32
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
-
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency
- else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
bldc_3pwm_motor_slots_t m_slot = {};
@@ -262,9 +265,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int
// - Stepper motor - 4PWM setting
// - hardware speciffic
void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
-
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency
- else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 50kHz max - centered pwm has twice lower frequency
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
stepper_4pwm_motor_slots_t m_slot = {};
// determine which motor are we connecting
// and set the appropriate configuration parameters
@@ -367,7 +369,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency
- else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency
bldc_6pwm_motor_slots_t m_slot = {};
// determine which motor are we connecting
// and set the appropriate configuration parameters
diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp
index fd64e6af..6549de07 100644
--- a/src/drivers/hardware_specific/generic_mcu.cpp
+++ b/src/drivers/hardware_specific/generic_mcu.cpp
@@ -1,30 +1,13 @@
#include "../hardware_api.h"
-#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // if mcu is not atmega328
-
-#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560
-
-#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy
-
-#elif defined(__arm__) && defined(__SAM3X8E__) // or due
-
-#elif defined(ESP_H) // or esp32
-
-#elif defined(_STM32_DEF_) // or stm32
-
-#elif defined(_SAMD21_) // samd21
-
-#elif defined(_SAMD51_) // samd51
-
-#elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51
-
-#else
-
// function setting the high pwm frequency to the supplied pins
// - Stepper motor - 2PWM setting
// - hardware speciffic
// in generic case dont do anything
-void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+ _UNUSED(pwm_frequency);
+ _UNUSED(pinA);
+ _UNUSED(pinB);
return;
}
@@ -32,7 +15,11 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
// - BLDC motor - 3PWM setting
// - hardware speciffic
// in generic case dont do anything
-void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+__attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+ _UNUSED(pwm_frequency);
+ _UNUSED(pinA);
+ _UNUSED(pinB);
+ _UNUSED(pinC);
return;
}
@@ -41,14 +28,27 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int
// - Stepper motor - 4PWM setting
// - hardware speciffic
// in generic case dont do anything
-void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+__attribute__((weak)) void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+ _UNUSED(pwm_frequency);
+ _UNUSED(pin1A);
+ _UNUSED(pin1B);
+ _UNUSED(pin2A);
+ _UNUSED(pin2B);
return;
}
// Configuring PWM frequency, resolution and alignment
// - BLDC driver - 6PWM setting
// - hardware specific
-int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+__attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+ _UNUSED(pwm_frequency);
+ _UNUSED(dead_zone);
+ _UNUSED(pinA_h);
+ _UNUSED(pinB_h);
+ _UNUSED(pinC_h);
+ _UNUSED(pinA_l);
+ _UNUSED(pinB_l);
+ _UNUSED(pinC_l);
return -1;
}
@@ -56,7 +56,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
// function setting the pwm duty cycle to the hardware
// - Stepper motor - 2PWM setting
// - hardware speciffic
-void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
+__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
// transform duty cycle from [0,1] to [0,255]
analogWrite(pinA, 255.0*dc_a);
analogWrite(pinB, 255.0*dc_b);
@@ -65,7 +65,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
// function setting the pwm duty cycle to the hardware
// - BLDC motor - 3PWM setting
// - hardware speciffic
-void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
+__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
// transform duty cycle from [0,1] to [0,255]
analogWrite(pinA, 255.0*dc_a);
analogWrite(pinB, 255.0*dc_b);
@@ -75,7 +75,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB
// function setting the pwm duty cycle to the hardware
// - Stepper motor - 4PWM setting
// - hardware speciffic
-void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
// transform duty cycle from [0,1] to [0,255]
analogWrite(pin1A, 255.0*dc_1a);
analogWrite(pin1B, 255.0*dc_1b);
@@ -87,9 +87,17 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
// - BLDC driver - 6PWM setting
// - hardware specific
-void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+ _UNUSED(dc_a);
+ _UNUSED(dc_b);
+ _UNUSED(dc_c);
+ _UNUSED(dead_zone);
+ _UNUSED(pinA_h);
+ _UNUSED(pinB_h);
+ _UNUSED(pinC_h);
+ _UNUSED(pinA_l);
+ _UNUSED(pinB_l);
+ _UNUSED(pinC_l);
return;
}
-
-#endif
diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp
new file mode 100644
index 00000000..097b7a6d
--- /dev/null
+++ b/src/drivers/hardware_specific/rp2040_mcu.cpp
@@ -0,0 +1,163 @@
+
+/**
+ * Support for the RP2040 MCU, as found on the Raspberry Pi Pico.
+ */
+#if defined(TARGET_RP2040)
+
+#define SIMPLEFOC_DEBUG_RP2040
+
+
+#ifdef SIMPLEFOC_DEBUG_RP2040
+
+#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL
+#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial
+#endif
+
+#endif
+
+#include "Arduino.h"
+
+
+
+
+// until I can figure out if this can be quickly read from some register, keep it here.
+// it also serves as a marker for what slices are already used.
+uint16_t wrapvalues[NUM_PWM_SLICES];
+
+
+// TODO add checks which channels are already used...
+
+void setupPWM(int pin, long pwm_frequency, bool invert = false) {
+ gpio_set_function(pin, GPIO_FUNC_PWM);
+ uint slice = pwm_gpio_to_slice_num(pin);
+ uint chan = pwm_gpio_to_channel(pin);
+ pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get
+ pwm_set_phase_correct(slice, true);
+ uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L;
+ if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000
+ if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300
+#ifdef SIMPLEFOC_DEBUG_RP2040
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin ");
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin);
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice ");
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice);
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel ");
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan);
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency ");
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency);
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value ");
+ SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue);
+#endif
+ pwm_set_wrap(slice, wrapvalue);
+ wrapvalues[slice] = wrapvalue;
+ if (invert) {
+ if (chan==0)
+ hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS);
+ else
+ hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS);
+ }
+ pwm_set_chan_level(slice, chan, 0); // switch off initially
+}
+
+
+void syncSlices() {
+ for (int i=0;i1.0) ret = 1.0;
+ return ret;
+}
+
+void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) {
+ writeDutyCycle(dc_a, pinA_h);
+ writeDutyCycle(swDti(dc_a, dead_zone), pinA_l);
+ writeDutyCycle(dc_b, pinB_h);
+ writeDutyCycle(swDti(dc_b,dead_zone), pinB_l);
+ writeDutyCycle(dc_c, pinC_h);
+ writeDutyCycle(swDti(dc_c,dead_zone), pinC_l);
+}
+
+#endif
diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp
index cf8db836..ab41b745 100644
--- a/src/drivers/hardware_specific/samd21_mcu.cpp
+++ b/src/drivers/hardware_specific/samd21_mcu.cpp
@@ -163,7 +163,7 @@ void configureSAMDClock() {
while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Configured clock...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock...");
#endif
}
}
@@ -217,8 +217,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate,
tc->COUNT8.CTRLA.bit.ENABLE = 1;
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("Initialized TC ");
- Serial.println(tccConfig.tcc.tccn);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC ");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn);
#endif
}
else {
@@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate,
while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print(" Initialized TCC ");
- Serial.print(tccConfig.tcc.tccn);
- Serial.print("-");
- Serial.print(tccConfig.tcc.chan);
- Serial.print("[");
- Serial.print(tccConfig.wo);
- Serial.println("]");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC ");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
#endif
}
}
@@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate,
while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("(Re-)Initialized TCC ");
- Serial.print(tccConfig.tcc.tccn);
- Serial.print("-");
- Serial.print(tccConfig.tcc.chan);
- Serial.print("[");
- Serial.print(tccConfig.wo);
- Serial.println("]");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC ");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
#endif
}
diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp
index 69c44848..08201b97 100644
--- a/src/drivers/hardware_specific/samd51_mcu.cpp
+++ b/src/drivers/hardware_specific/samd51_mcu.cpp
@@ -188,7 +188,7 @@ void configureSAMDClock() {
while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("(Re-)Initialized TCC ");
- Serial.print(tccConfig.tcc.tccn);
- Serial.print("-");
- Serial.print(tccConfig.tcc.chan);
- Serial.print("[");
- Serial.print(tccConfig.wo);
- Serial.println("]");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC ");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
#endif
}
else if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
@@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate,
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("Initialized TC ");
- Serial.println(tccConfig.tcc.tccn);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC ");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn);
#endif
}
diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp
index fdbd7a7f..1b035dfd 100644
--- a/src/drivers/hardware_specific/samd_mcu.cpp
+++ b/src/drivers/hardware_specific/samd_mcu.cpp
@@ -287,7 +287,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
if (compatibility<0) {
// no result!
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Bad combination!");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
#endif
return;
}
@@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("Found configuration: (score=");
- Serial.print(scorePermutation(tccConfs, 2));
- Serial.println(")");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score=");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2));
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")");
printTCCConfiguration(tccConfs[0]);
printTCCConfiguration(tccConfs[1]);
#endif
@@ -308,7 +308,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
attachTCC(tccConfs[1]);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Attached pins...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
#endif
// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
@@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
configureTCC(tccConfs[0], pwm_frequency);
configureTCC(tccConfs[1], pwm_frequency);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Configured TCCs...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
#endif
return; // Someone with a stepper-setup who can test it?
@@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in
if (compatibility<0) {
// no result!
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Bad combination!");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
#endif
return;
}
@@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("Found configuration: (score=");
- Serial.print(scorePermutation(tccConfs, 3));
- Serial.println(")");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score=");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3));
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")");
printTCCConfiguration(tccConfs[0]);
printTCCConfiguration(tccConfs[1]);
printTCCConfiguration(tccConfs[2]);
@@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in
attachTCC(tccConfs[1]);
attachTCC(tccConfs[2]);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Attached pins...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
#endif
// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
@@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in
configureTCC(tccConfs[1], pwm_frequency);
configureTCC(tccConfs[2], pwm_frequency);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Configured TCCs...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
#endif
}
@@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
if (compatibility<0) {
// no result!
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Bad combination!");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
#endif
return;
}
@@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.print("Found configuration: (score=");
- Serial.print(scorePermutation(tccConfs, 4));
- Serial.println(")");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score=");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4));
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")");
printTCCConfiguration(tccConfs[0]);
printTCCConfiguration(tccConfs[1]);
printTCCConfiguration(tccConfs[2]);
@@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
attachTCC(tccConfs[2]);
attachTCC(tccConfs[3]);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Attached pins...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
#endif
// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
@@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
configureTCC(tccConfs[2], pwm_frequency);
configureTCC(tccConfs[3], pwm_frequency);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Configured TCCs...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
#endif
return; // Someone with a stepper-setup who can test it?
@@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
if (compatibility<0) {
// no result!
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Bad combination!");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
#endif
return -1;
}
@@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5));
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Found configuration: ");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: ");
printTCCConfiguration(pinAh);
printTCCConfiguration(pinAl);
printTCCConfiguration(pinBh);
@@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
if (!allAttached)
return -1;
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Attached pins...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
#endif
// set up clock - if we did this right it should be possible to get all TCC units synchronized?
// e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API
@@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo))
configureTCC(pinCl, pwm_frequency, true, -1.0);
#ifdef SIMPLEFOC_SAMD_DEBUG
- Serial.println("Configured TCCs...");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
#endif
return 0;
@@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i
* saves you hours of cross-referencing with the datasheet.
*/
void printAllPinInfos() {
- Serial.println();
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println();
for (uint8_t pin=0;pin=TCC_INST_NUM)
- Serial.print(": TC Peripheral");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral");
else
- Serial.print(": TCC Peripheral");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral");
switch (info.peripheral) {
case PIO_TIMER:
- Serial.print(" E "); break;
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break;
case PIO_TIMER_ALT:
- Serial.print(" F "); break;
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break;
#if defined(_SAMD51_)||defined(_SAME51_)
case PIO_TCC_PDEC:
- Serial.print(" G "); break;
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break;
#endif
default:
- Serial.print(" ? "); break;
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break;
}
if (info.tcc.tccn>=0) {
- Serial.print(info.tcc.tccn);
- Serial.print("-");
- Serial.print(info.tcc.chan);
- Serial.print("[");
- Serial.print(info.wo);
- Serial.println("]");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo);
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
}
else
- Serial.println(" None");
+ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None");
}
diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h
index f0fd5460..7faf7442 100644
--- a/src/drivers/hardware_specific/samd_mcu.h
+++ b/src/drivers/hardware_specific/samd_mcu.h
@@ -4,7 +4,10 @@
// uncomment to enable debug output to Serial port
-#define SIMPLEFOC_SAMD_DEBUG
+// #define SIMPLEFOC_SAMD_DEBUG
+#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
+#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
+#endif
#include "../hardware_api.h"
diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp
index e95f2066..5f70db4f 100644
--- a/src/drivers/hardware_specific/teensy_mcu.cpp
+++ b/src/drivers/hardware_specific/teensy_mcu.cpp
@@ -2,6 +2,9 @@
#if defined(__arm__) && defined(CORE_TEENSY)
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
// configure High PWM frequency
void _setHighFrequency(const long freq, const int pin){
analogWrite(pin, 0);
@@ -13,8 +16,8 @@ void _setHighFrequency(const long freq, const int pin){
// - Stepper motor - 2PWM setting
// - hardware speciffic
void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 50000; // default frequency 50khz
- else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
_setHighFrequency(pwm_frequency, pinA);
_setHighFrequency(pwm_frequency, pinB);
}
@@ -23,8 +26,8 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
// - BLDC motor - 3PWM setting
// - hardware speciffic
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 50000; // default frequency 50khz
- else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
_setHighFrequency(pwm_frequency, pinA);
_setHighFrequency(pwm_frequency, pinB);
_setHighFrequency(pwm_frequency, pinC);
@@ -34,8 +37,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int
// - Stepper motor - 4PWM setting
// - hardware speciffic
void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
- if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 50000; // default frequency 50khz
- else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max
+ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+ else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
_setHighFrequency(pwm_frequency, pinA);
_setHighFrequency(pwm_frequency, pinB);
_setHighFrequency(pwm_frequency, pinC);
@@ -71,18 +74,4 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
analogWrite(pin2B, 255.0*dc_2b);
}
-
-// Configuring PWM frequency, resolution and alignment
-// - BLDC driver - 6PWM setting
-// - hardware specific
-int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
- return -1;
-}
-
-// Function setting the duty cycle to the pwm pin (ex. analogWrite())
-// - BLDC driver - 6PWM setting
-// - hardware specific
-void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
- return;
-}
#endif
\ No newline at end of file
diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp
index 5b196d57..001330ba 100644
--- a/src/sensors/Encoder.cpp
+++ b/src/sensors/Encoder.cpp
@@ -32,7 +32,7 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){
prev_timestamp_us = _micros();
// extern pullup as default
- pullup = Pullup::EXTERN;
+ pullup = Pullup::USE_EXTERN;
// enable quadrature encoder by default
quadrature = Quadrature::ON;
}
@@ -160,7 +160,7 @@ int Encoder::hasIndex(){
void Encoder::init(){
// Encoder - check if pullup needed for your encoder
- if(pullup == Pullup::INTERN){
+ if(pullup == Pullup::USE_INTERN){
pinMode(pinA, INPUT_PULLUP);
pinMode(pinB, INPUT_PULLUP);
if(hasIndex()) pinMode(index_pin,INPUT_PULLUP);
diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp
index e39fd514..2ceb22cf 100644
--- a/src/sensors/HallSensor.cpp
+++ b/src/sensors/HallSensor.cpp
@@ -17,7 +17,7 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){
cpr = _pp * 6;
// extern pullup as default
- pullup = Pullup::EXTERN;
+ pullup = Pullup::USE_EXTERN;
}
// HallSensor interrupt callback functions
@@ -119,7 +119,7 @@ void HallSensor::init(){
electric_rotations = 0;
// HallSensor - check if pullup needed for your HallSensor
- if(pullup == Pullup::INTERN){
+ if(pullup == Pullup::USE_INTERN){
pinMode(pinA, INPUT_PULLUP);
pinMode(pinB, INPUT_PULLUP);
pinMode(pinC, INPUT_PULLUP);
diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp
index 0216392a..35f6d73c 100644
--- a/src/sensors/MagneticSensorAnalog.cpp
+++ b/src/sensors/MagneticSensorAnalog.cpp
@@ -13,7 +13,7 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun
min_raw_count = _min_raw_count;
max_raw_count = _max_raw_count;
- if(pullup == Pullup::INTERN){
+ if(pullup == Pullup::USE_INTERN){
pinMode(pinAnalog, INPUT_PULLUP);
}else{
pinMode(pinAnalog, INPUT);
diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp
index 531a06f9..c9029f6e 100644
--- a/src/sensors/MagneticSensorI2C.cpp
+++ b/src/sensors/MagneticSensorI2C.cpp
@@ -4,7 +4,7 @@
MagneticSensorI2CConfig_s AS5600_I2C = {
.chip_address = 0x36,
.bit_resolution = 12,
- .angle_register = 0x0E,
+ .angle_register = 0x0C,
.data_start_bit = 11
};
diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h
index 6ff2c761..9966d6eb 100644
--- a/src/sensors/MagneticSensorI2C.h
+++ b/src/sensors/MagneticSensorI2C.h
@@ -16,6 +16,12 @@ struct MagneticSensorI2CConfig_s {
// some predefined structures
extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C;
+#if defined(TARGET_RP2040)
+#define SDA I2C_SDA
+#define SCL I2C_SCL
+#endif
+
+
class MagneticSensorI2C: public Sensor{
public:
/**
diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp
index 4e9f944e..da4ebf89 100644
--- a/src/sensors/MagneticSensorPWM.cpp
+++ b/src/sensors/MagneticSensorPWM.cpp
@@ -14,15 +14,19 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m
min_raw_count = _min_raw_count;
max_raw_count = _max_raw_count;
- pinMode(pinPWM, INPUT);
+ // define if the sensor uses interrupts
+ is_interrupt_based = false;
- // init last call
+ // define as not set
last_call_us = _micros();
}
void MagneticSensorPWM::init(){
+ // initial hardware
+ pinMode(pinPWM, INPUT);
+
// velocity calculation init
angle_prev = 0;
velocity_calc_timestamp = _micros();
@@ -66,10 +70,9 @@ float MagneticSensorPWM::getVelocity(){
// read the raw counter of the magnetic sensor
int MagneticSensorPWM::getRawCount(){
-#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560
- pulse_length_us = pulseIn(pinPWM, HIGH);
-#endif
-
+ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way
+ pulse_length_us = pulseIn(pinPWM, HIGH);
+ }
return pulse_length_us;
}
@@ -83,11 +86,15 @@ void MagneticSensorPWM::handlePWM() {
// save the currrent timestamp for the next call
last_call_us = now_us;
+ is_interrupt_based = true; // set the flag to true
}
// function enabling hardware interrupts of the for the callback provided
// if callback is not provided then the interrupt is not enabled
void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){
+ // declare it's interrupt based
+ is_interrupt_based = true;
+
#if !defined(__AVR_ATmega328P__) && !defined(__AVR_ATmega168__) && !defined(__AVR_ATmega328PB__) && !defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560
// enable interrupts on pwm input pin
attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE);
diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h
index 46053ac7..32d6daee 100644
--- a/src/sensors/MagneticSensorPWM.h
+++ b/src/sensors/MagneticSensorPWM.h
@@ -38,6 +38,10 @@ class MagneticSensorPWM: public Sensor{
int min_raw_count;
int max_raw_count;
int cpr;
+
+ // flag saying if the readings are interrupt based or not
+ bool is_interrupt_based;
+
int read();
/**
diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp
index 56833b7f..b3d82dee 100644
--- a/src/sensors/MagneticSensorSPI.cpp
+++ b/src/sensors/MagneticSensorSPI.cpp
@@ -1,3 +1,5 @@
+#ifndef TARGET_RP2040
+
#include "MagneticSensorSPI.h"
/** Typical configuration for the 14bit AMS AS5147 magnetic sensor over SPI interface */
@@ -212,3 +214,6 @@ word MagneticSensorSPI::read(word angle_register){
void MagneticSensorSPI::close(){
spi->end();
}
+
+
+#endif
diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h
index 764053df..77fcde4d 100644
--- a/src/sensors/MagneticSensorSPI.h
+++ b/src/sensors/MagneticSensorSPI.h
@@ -1,6 +1,8 @@
#ifndef MAGNETICSENSORSPI_LIB_H
#define MAGNETICSENSORSPI_LIB_H
+#ifndef TARGET_RP2040
+
#include "Arduino.h"
#include
#include "../common/base_classes/Sensor.h"
@@ -91,3 +93,4 @@ class MagneticSensorSPI: public Sensor{
#endif
+#endif