优化了平衡，尤其对于向后倾倒的几种极限情况进行优化
This commit is contained in:
myperworld committed
1 parent
10aba2664d
commit
fcb8af8163
693 files changed
+2427
-70
No files matched your search
@@ -0,0 +1,158 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(__AVR_ATmega2560__)
|
||||
|
||||
// set pwm frequency to 32KHz
|
||||
void _pinHighFrequency(const int pin){
|
||||
// High PWM frequency
|
||||
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
|
||||
// https://forum.arduino.cc/index.php?topic=72092.0
|
||||
if (pin == 13 || pin == 4 ) {
|
||||
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
|
||||
TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
|
||||
}
|
||||
else if (pin == 12 || pin == 11 )
|
||||
TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
else if (pin == 10 || pin == 9 )
|
||||
TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
else if (pin == 5 || pin == 3 || pin == 2)
|
||||
TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
else if (pin == 8 || pin == 7 || pin == 6)
|
||||
TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
else if (pin == 44 || pin == 45 || pin == 46)
|
||||
TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 2PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pinA);
|
||||
_pinHighFrequency(pinB);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pinA);
|
||||
_pinHighFrequency(pinB);
|
||||
_pinHighFrequency(pinC);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
analogWrite(pinC, 255.0f*dc_c);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 4PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pin1A);
|
||||
_pinHighFrequency(pin1B);
|
||||
_pinHighFrequency(pin2A);
|
||||
_pinHighFrequency(pin2B);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pin1A, 255.0f*dc_1a);
|
||||
analogWrite(pin1B, 255.0f*dc_1b);
|
||||
analogWrite(pin2A, 255.0f*dc_2a);
|
||||
analogWrite(pin2B, 255.0f*dc_2b);
|
||||
}
|
||||
|
||||
|
||||
// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
|
||||
// supports Arudino/ATmega2560
|
||||
int _configureComplementaryPair(int pinH, int pinL) {
|
||||
if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){
|
||||
// configure the pwm phase-corrected mode
|
||||
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
|
||||
else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
|
||||
}else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
|
||||
else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
|
||||
}else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR2B = ((TCCR2B & 0b11111000) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
|
||||
else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
|
||||
}else{
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Configuring PWM frequency, resolution and alignment
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - 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) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
int ret_flag = 0;
|
||||
ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
|
||||
ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
|
||||
ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
|
||||
return ret_flag; // returns -1 if not well configured
|
||||
}
|
||||
|
||||
// function setting the
|
||||
void _setPwmPair(int pinH, int pinL, float val, int dead_time)
|
||||
{
|
||||
int pwm_h = _constrain(val-dead_time/2,0,255);
|
||||
int pwm_l = _constrain(val+dead_time/2,0,255);
|
||||
|
||||
analogWrite(pinH, pwm_h);
|
||||
if(pwm_l == 255 || pwm_l == 0)
|
||||
digitalWrite(pinL, pwm_l ? LOW : HIGH);
|
||||
else
|
||||
analogWrite(pinL, pwm_l);
|
||||
}
|
||||
|
||||
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - hardware specific
|
||||
// supports Arudino/ATmega328
|
||||
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){
|
||||
_setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
|
||||
_setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
|
||||
_setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,156 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)
|
||||
|
||||
// set pwm frequency to 32KHz
|
||||
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 ) {
|
||||
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
|
||||
TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
|
||||
}
|
||||
if (pin == 9 || pin == 10 )
|
||||
TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
if (pin == 3 || pin == 11)
|
||||
TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1
|
||||
|
||||
}
|
||||
|
||||
// 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);
|
||||
_pinHighFrequency(pinB);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - 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);
|
||||
_pinHighFrequency(pinB);
|
||||
_pinHighFrequency(pinC);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
analogWrite(pinC, 255.0f*dc_c);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 4PWM setting
|
||||
// - hardware speciffic
|
||||
// supports Arudino/ATmega328
|
||||
void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pin1A);
|
||||
_pinHighFrequency(pin1B);
|
||||
_pinHighFrequency(pin2A);
|
||||
_pinHighFrequency(pin2B);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pin1A, 255.0f*dc_1a);
|
||||
analogWrite(pin1B, 255.0f*dc_1b);
|
||||
analogWrite(pin2A, 255.0f*dc_2a);
|
||||
analogWrite(pin2B, 255.0f*dc_2b);
|
||||
}
|
||||
|
||||
|
||||
// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
|
||||
int _configureComplementaryPair(int pinH, int pinL) {
|
||||
if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){
|
||||
// configure the pwm phase-corrected mode
|
||||
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
|
||||
else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
|
||||
}else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
|
||||
else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
|
||||
}else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR2B = ((TCCR2B & 0b11111000) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
|
||||
else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
|
||||
}else{
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Configuring PWM frequency, resolution and alignment
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - 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;
|
||||
ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
|
||||
ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
|
||||
ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
|
||||
return ret_flag; // returns -1 if not well configured
|
||||
}
|
||||
|
||||
// function setting the
|
||||
void _setPwmPair(int pinH, int pinL, float val, int dead_time)
|
||||
{
|
||||
int pwm_h = _constrain(val-dead_time/2,0,255);
|
||||
int pwm_l = _constrain(val+dead_time/2,0,255);
|
||||
|
||||
analogWrite(pinH, pwm_h);
|
||||
if(pwm_l == 255 || pwm_l == 0)
|
||||
digitalWrite(pinL, pwm_l ? LOW : HIGH);
|
||||
else
|
||||
analogWrite(pinL, pwm_l);
|
||||
}
|
||||
|
||||
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - hardware specific
|
||||
// supports Arudino/ATmega328
|
||||
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){
|
||||
_setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
|
||||
_setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
|
||||
_setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,172 @@
|
||||
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(__AVR_ATmega32U4__)
|
||||
|
||||
// set pwm frequency to 32KHz
|
||||
void _pinHighFrequency(const int pin){
|
||||
// High PWM frequency
|
||||
// reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html
|
||||
if (pin == 3 || pin == 11 ) {
|
||||
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
|
||||
TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
|
||||
}
|
||||
else if (pin == 9 || pin == 10 )
|
||||
TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
else if (pin == 5 )
|
||||
TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1
|
||||
else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer
|
||||
// PLL Configuration
|
||||
PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
|
||||
TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
|
||||
TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
|
||||
|
||||
if (pin == 6) TCCR4A = 0x82; // activate channel A - pin 13
|
||||
else if (pin == 13) TCCR4C |= 0x09; // Activate channel D - pin 6
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 2PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pinA);
|
||||
_pinHighFrequency(pinB);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pinA);
|
||||
_pinHighFrequency(pinB);
|
||||
_pinHighFrequency(pinC);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0*dc_a);
|
||||
analogWrite(pinB, 255.0*dc_b);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0*dc_a);
|
||||
analogWrite(pinB, 255.0*dc_b);
|
||||
analogWrite(pinC, 255.0*dc_c);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 4PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
_pinHighFrequency(pin1A);
|
||||
_pinHighFrequency(pin1B);
|
||||
_pinHighFrequency(pin2A);
|
||||
_pinHighFrequency(pin2B);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pin1A, 255.0*dc_1a);
|
||||
analogWrite(pin1B, 255.0*dc_1b);
|
||||
analogWrite(pin2A, 255.0*dc_2a);
|
||||
analogWrite(pin2B, 255.0*dc_2b);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
|
||||
int _configureComplementaryPair(int pinH, int pinL) {
|
||||
if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
|
||||
// configure the pwm phase-corrected mode
|
||||
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 11 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
|
||||
else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
|
||||
}else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){
|
||||
// set prescaler to 1 - 32kHz freq
|
||||
TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
|
||||
else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
|
||||
}else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){
|
||||
// PLL Configuration
|
||||
PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
|
||||
TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
|
||||
TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
|
||||
|
||||
// configure complementary pwm on low side
|
||||
if(pinH == 13 ){
|
||||
TCCR4A = 0x82; // activate channel A - pin 13
|
||||
TCCR4C |= 0x0d; // Activate complementary channel D - pin 6
|
||||
}else {
|
||||
TCCR4C |= 0x09; // Activate channel D - pin 6
|
||||
TCCR4A = 0xc2; // activate complementary channel A - pin 13
|
||||
}
|
||||
}else{
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 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) {
|
||||
_UNUSED(pwm_frequency);
|
||||
_UNUSED(dead_zone);
|
||||
// High PWM frequency
|
||||
// - always max 32kHz
|
||||
int ret_flag = 0;
|
||||
ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
|
||||
ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
|
||||
ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
|
||||
return ret_flag; // returns -1 if not well configured
|
||||
|
||||
|
||||
// function setting the
|
||||
void _setPwmPair(int pinH, int pinL, float val, int dead_time)
|
||||
{
|
||||
int pwm_h = _constrain(val-dead_time/2,0,255);
|
||||
int pwm_l = _constrain(val+dead_time/2,0,255);
|
||||
|
||||
analogWrite(pinH, pwm_h);
|
||||
if(pwm_l == 255 || pwm_l == 0)
|
||||
digitalWrite(pinL, pwm_l ? LOW : HIGH);
|
||||
else
|
||||
analogWrite(pinL, pwm_l);
|
||||
}
|
||||
|
||||
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - hardware specific
|
||||
// supports Arudino/ATmega328
|
||||
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){
|
||||
_setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
|
||||
_setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
|
||||
_setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,406 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#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;
|
||||
static int _max_pwm_value = 1023;
|
||||
|
||||
// array mapping the timer values to the interrupt handlers
|
||||
static IRQn_Type irq_type[] = {TC0_IRQn, TC0_IRQn, TC1_IRQn, TC1_IRQn, TC2_IRQn, TC2_IRQn, TC3_IRQn, TC3_IRQn, TC4_IRQn, TC4_IRQn, TC5_IRQn, TC5_IRQn, TC6_IRQn, TC6_IRQn, TC7_IRQn, TC7_IRQn, TC8_IRQn, TC8_IRQn};
|
||||
// current counter values
|
||||
static volatile uint32_t pwm_counter_vals[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
|
||||
|
||||
// variables copied from wiring_analog.cpp for arduino due
|
||||
static uint8_t PWMEnabled = 0;
|
||||
static uint8_t TCChanEnabled[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
static const uint32_t channelToChNo[] = { 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2 };
|
||||
static const uint32_t channelToAB[] = { 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0 };
|
||||
static Tc *channelToTC[] = {
|
||||
TC0, TC0, TC0, TC0, TC0, TC0,
|
||||
TC1, TC1, TC1, TC1, TC1, TC1,
|
||||
TC2, TC2, TC2, TC2, TC2, TC2 };
|
||||
static const uint32_t channelToId[] = { 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8 };
|
||||
|
||||
|
||||
// function setting the CMR register
|
||||
static void TC_SetCMR_ChannelA(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xFFF0FFFF) | v;}
|
||||
static void TC_SetCMR_ChannelB(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xF0FFFFFF) | v; }
|
||||
|
||||
|
||||
// function which starts and syncs the timers
|
||||
// if the pin is the true PWM pin this function does not do anything
|
||||
void syncTimers(uint32_t ulPin1,uint32_t ulPin2, uint32_t ulPin3 = -1, uint32_t ulPin4 = -1){
|
||||
uint32_t chNo1,chNo2,chNo3,chNo4;
|
||||
Tc *chTC1 = nullptr,*chTC2 = nullptr,*chTC3 = nullptr,*chTC4 = nullptr;
|
||||
|
||||
// configure timer channel for the first pin if it is a timer pin
|
||||
uint32_t attr = g_APinDescription[ulPin1].ulPinAttribute;
|
||||
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
|
||||
ETCChannel channel1 = g_APinDescription[ulPin1].ulTCChannel;
|
||||
chNo1 = channelToChNo[channel1];
|
||||
chTC1 = channelToTC[channel1];
|
||||
TCChanEnabled[channelToId[channel1]] = 1;
|
||||
}
|
||||
|
||||
// configure timer channel for the first pin if it is a timer pin
|
||||
attr = g_APinDescription[ulPin2].ulPinAttribute;
|
||||
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
|
||||
ETCChannel channel2 = g_APinDescription[ulPin2].ulTCChannel;
|
||||
chNo2 = channelToChNo[channel2];
|
||||
chTC2 = channelToTC[channel2];
|
||||
TCChanEnabled[channelToId[channel2]] = 1;
|
||||
}
|
||||
if(ulPin3 > 0 ){
|
||||
// configure timer channel for the first pin if it is a timer pin
|
||||
attr = g_APinDescription[ulPin3].ulPinAttribute;
|
||||
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
|
||||
ETCChannel channel3 = g_APinDescription[ulPin3].ulTCChannel;
|
||||
chNo3 = channelToChNo[channel3];
|
||||
chTC3 = channelToTC[channel3];
|
||||
TCChanEnabled[channelToId[channel3]] = 1;
|
||||
}
|
||||
}
|
||||
if(ulPin4 > 0 ){
|
||||
// configure timer channel for the first pin if it is a timer pin
|
||||
attr = g_APinDescription[ulPin4].ulPinAttribute;
|
||||
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
|
||||
ETCChannel channel4 = g_APinDescription[ulPin4].ulTCChannel;
|
||||
chNo4 = channelToChNo[channel4];
|
||||
chTC4 = channelToTC[channel4];
|
||||
TCChanEnabled[channelToId[channel4]] = 1;
|
||||
}
|
||||
}
|
||||
// start timers and make them synced
|
||||
if(chTC1){
|
||||
TC_Start(chTC1, chNo1);
|
||||
chTC1->TC_BCR = TC_BCR_SYNC;
|
||||
}
|
||||
if(chTC2){
|
||||
TC_Start(chTC2, chNo2);
|
||||
chTC2->TC_BCR = TC_BCR_SYNC;
|
||||
}
|
||||
if(chTC3 && ulPin3){
|
||||
TC_Start(chTC3, chNo3);
|
||||
chTC3->TC_BCR = TC_BCR_SYNC;
|
||||
}
|
||||
if(chTC4 && ulPin4){
|
||||
TC_Start(chTC4, chNo4);
|
||||
chTC4->TC_BCR = TC_BCR_SYNC;
|
||||
}
|
||||
}
|
||||
|
||||
// function configuring the pwm frequency for given pin
|
||||
// possible to supply the pwm pin and the timer pin
|
||||
void initPWM(uint32_t ulPin, uint32_t pwm_freq){
|
||||
// check which pin type
|
||||
uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
|
||||
if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm pin
|
||||
|
||||
if (!PWMEnabled) {
|
||||
// PWM Startup code
|
||||
pmc_enable_periph_clk(PWM_INTERFACE_ID);
|
||||
// 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;
|
||||
}
|
||||
|
||||
uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
|
||||
if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
|
||||
// Setup PWM for this pin
|
||||
PIO_Configure(g_APinDescription[ulPin].pPort,
|
||||
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);
|
||||
PWMC_EnableChannel(PWM_INTERFACE, chan);
|
||||
g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
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 ;
|
||||
// Setup Timer for this pin
|
||||
ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
|
||||
uint32_t chNo = channelToChNo[channel];
|
||||
uint32_t chA = channelToAB[channel];
|
||||
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);
|
||||
}
|
||||
|
||||
// disable the counter on start
|
||||
if (chA){
|
||||
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
|
||||
}else{
|
||||
TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
|
||||
}
|
||||
// configure input-ouput structure
|
||||
if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
|
||||
PIO_Configure(g_APinDescription[ulPin].pPort,
|
||||
g_APinDescription[ulPin].ulPinType,
|
||||
g_APinDescription[ulPin].ulPin,
|
||||
g_APinDescription[ulPin].ulPinConfiguration);
|
||||
g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
|
||||
}
|
||||
// enable interrupts
|
||||
chTC->TC_CHANNEL[chNo].TC_IER = TC_IER_CPAS // interrupt on RA compare match
|
||||
| TC_IER_CPBS // interrupt on RB compare match
|
||||
| TC_IER_CPCS; // interrupt on RC compare match
|
||||
chTC->TC_CHANNEL[chNo].TC_IDR = ~TC_IER_CPAS // interrupt on RA compare match
|
||||
& ~TC_IER_CPBS // interrupt on RB compare match
|
||||
& ~ TC_IER_CPCS; // interrupt on RC compare match
|
||||
// enable interrupts for this timer
|
||||
NVIC_EnableIRQ(irq_type[channel]);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// pwm setting function
|
||||
// it sets the duty cycle for pwm pin or timer pin
|
||||
void setPwm(uint32_t ulPin, uint32_t ulValue) {
|
||||
// check pin type
|
||||
uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
|
||||
if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm
|
||||
uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
|
||||
PWMC_SetDutyCycle(PWM_INTERFACE, chan, ulValue);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
|
||||
// get the timer variables
|
||||
ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
|
||||
Tc *chTC = channelToTC[channel];
|
||||
uint32_t chNo = channelToChNo[channel];
|
||||
if(!ulValue) {
|
||||
// if the value 0 disable counter
|
||||
if (channelToAB[channel])
|
||||
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR);
|
||||
else
|
||||
TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
|
||||
}else{
|
||||
// if the value not zero
|
||||
// calculate clock
|
||||
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 ;
|
||||
pwm_counter_vals[channel] = ulValue / _max_pwm_value;
|
||||
// enable counter
|
||||
if (channelToAB[channel])
|
||||
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
|
||||
else
|
||||
TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// interrupt handlers for seamless pwm duty-cycle setting
|
||||
void TC0_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC0, 0);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[0]) TC_SetRA(TC0, 0, pwm_counter_vals[0]);
|
||||
if(pwm_counter_vals[1]) TC_SetRB(TC0, 0, pwm_counter_vals[1]);
|
||||
}
|
||||
|
||||
void TC1_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC0, 1);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[2]) TC_SetRA(TC0, 1, pwm_counter_vals[2]);
|
||||
if(pwm_counter_vals[3]) TC_SetRB(TC0, 1, pwm_counter_vals[3]);
|
||||
}
|
||||
|
||||
void TC2_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC0, 2);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[4]) TC_SetRA(TC0, 2, pwm_counter_vals[4]);
|
||||
if(pwm_counter_vals[5]) TC_SetRB(TC0, 2, pwm_counter_vals[5]);
|
||||
}
|
||||
void TC3_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC1, 0);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[6]) TC_SetRA(TC1, 0, pwm_counter_vals[6]);
|
||||
if(pwm_counter_vals[7]) TC_SetRB(TC1, 0, pwm_counter_vals[7]);
|
||||
}
|
||||
|
||||
void TC4_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC1, 1);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[8]) TC_SetRA(TC1, 1, pwm_counter_vals[8]);
|
||||
if(pwm_counter_vals[9]) TC_SetRB(TC1, 1, pwm_counter_vals[9]);
|
||||
}
|
||||
|
||||
void TC5_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC1, 2);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[10]) TC_SetRA(TC1, 2, pwm_counter_vals[10]);
|
||||
if(pwm_counter_vals[11]) TC_SetRB(TC1, 2, pwm_counter_vals[11]);
|
||||
}
|
||||
void TC6_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC2, 0);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[12]) TC_SetRA(TC2, 0, pwm_counter_vals[12]);
|
||||
if(pwm_counter_vals[13]) TC_SetRB(TC2, 0, pwm_counter_vals[13]);
|
||||
}
|
||||
|
||||
void TC7_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC2, 1);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[14]) TC_SetRA(TC2, 1, pwm_counter_vals[14]);
|
||||
if(pwm_counter_vals[15]) TC_SetRB(TC2, 1, pwm_counter_vals[15]);
|
||||
}
|
||||
|
||||
void TC8_Handler()
|
||||
{
|
||||
// read/clear interrupt status
|
||||
TC_GetStatus(TC2, 2);
|
||||
// update the counters
|
||||
if(pwm_counter_vals[16]) TC_SetRA(TC2, 2, pwm_counter_vals[16]);
|
||||
if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]);
|
||||
}
|
||||
|
||||
// implementation of the hardware_api.cpp
|
||||
// ---------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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
|
||||
initPWM(pinA, _pwm_frequency);
|
||||
initPWM(pinB, _pwm_frequency);
|
||||
initPWM(pinC, _pwm_frequency);
|
||||
// sync the timers if possible
|
||||
syncTimers(pinA, pinB, pinC);
|
||||
}
|
||||
|
||||
|
||||
// Configuring PWM frequency, resolution and alignment
|
||||
//- 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 = _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
|
||||
initPWM(pinA, _pwm_frequency);
|
||||
initPWM(pinB, _pwm_frequency);
|
||||
// sync the timers if possible
|
||||
syncTimers(pinA, pinB);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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
|
||||
initPWM(pinA, _pwm_frequency);
|
||||
initPWM(pinB, _pwm_frequency);
|
||||
initPWM(pinC, _pwm_frequency);
|
||||
initPWM(pinD, _pwm_frequency);
|
||||
// sync the timers if possible
|
||||
syncTimers(pinA, pinB, pinC, pinD);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,_max_pwm_value]
|
||||
setPwm(pinA, _max_pwm_value*dc_a);
|
||||
setPwm(pinB, _max_pwm_value*dc_b);
|
||||
setPwm(pinC, _max_pwm_value*dc_c);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,_max_pwm_value]
|
||||
setPwm(pin1A, _max_pwm_value*dc_1a);
|
||||
setPwm(pin1B, _max_pwm_value*dc_1b);
|
||||
setPwm(pin2A, _max_pwm_value*dc_2a);
|
||||
setPwm(pin2B, _max_pwm_value*dc_2b);
|
||||
}
|
||||
|
||||
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
// - Stepper driver - 2PWM setting
|
||||
// - hardware specific
|
||||
void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
|
||||
// transform duty cycle from [0,1] to [0,_max_pwm_value]
|
||||
setPwm(pinA, _max_pwm_value*dc_a);
|
||||
setPwm(pinB, _max_pwm_value*dc_b);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,204 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED)
|
||||
|
||||
#include "driver/ledc.h"
|
||||
|
||||
#define _PWM_FREQUENCY 25000 // 25khz
|
||||
#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution
|
||||
#define _PWM_RES_BIT 10 // 10 bir resolution
|
||||
#define _PWM_RES 1023 // 2^10-1 = 1023-1
|
||||
|
||||
|
||||
// empty motor slot
|
||||
#define _EMPTY_SLOT -20
|
||||
#define _TAKEN_SLOT -21
|
||||
|
||||
// figure out how many ledc channels are avaible
|
||||
// esp32 - 2x8=16
|
||||
// esp32s2 - 8
|
||||
// esp32c3 - 6
|
||||
#include "soc/soc_caps.h"
|
||||
#ifdef SOC_LEDC_SUPPORT_HS_MODE
|
||||
#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1)
|
||||
#else
|
||||
#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM)
|
||||
#endif
|
||||
|
||||
// current channel stack index
|
||||
// support for multiple motors
|
||||
// esp32 has 16 channels
|
||||
// esp32s2 has 8 channels
|
||||
// esp32c3 has 6 channels
|
||||
int channel_index = 0;
|
||||
|
||||
// slot for the 3pwm bldc motors
|
||||
typedef struct {
|
||||
int pinID;
|
||||
int ch1;
|
||||
int ch2;
|
||||
int ch3;
|
||||
} bldc_3pwm_motor_slots_t;
|
||||
|
||||
// slot for the 2pwm stepper motors
|
||||
typedef struct {
|
||||
int pinID;
|
||||
int ch1;
|
||||
int ch2;
|
||||
} stepper_2pwm_motor_slots_t;
|
||||
|
||||
// slot for the 4pwm stepper motors
|
||||
typedef struct {
|
||||
int pinID;
|
||||
int ch1;
|
||||
int ch2;
|
||||
int ch3;
|
||||
int ch4;
|
||||
} stepper_4pwm_motor_slots_t;
|
||||
|
||||
|
||||
// define motor slots array
|
||||
bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = {
|
||||
{_EMPTY_SLOT, 0,0,0}, // 1st motor
|
||||
{_EMPTY_SLOT, 0,0,0}, // 2nd motor
|
||||
{_EMPTY_SLOT, 0,0,0}, // 3st motor // esp32s2 & esp32
|
||||
{_EMPTY_SLOT, 0,0,0}, // 4nd motor // esp32 only
|
||||
};
|
||||
stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4]={
|
||||
{_EMPTY_SLOT, 0,0}, // 1st motor
|
||||
{_EMPTY_SLOT, 0,0}, // 2nd motor
|
||||
{_EMPTY_SLOT, 0,0}, // 3rd motor
|
||||
{_EMPTY_SLOT, 0,0} // 4th motor - esp32s2 and esp32
|
||||
};
|
||||
stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[4]={
|
||||
{_EMPTY_SLOT, 0,0,0,0}, // 1st motor
|
||||
{_EMPTY_SLOT, 0,0,0,0}, // 2nd motor - esp32s2 and esp32
|
||||
{_EMPTY_SLOT, 0,0,0,0}, // 3st motor - only esp32
|
||||
{_EMPTY_SLOT, 0,0,0,0}, // 4st motor - only esp32
|
||||
};
|
||||
|
||||
// configure High PWM frequency
|
||||
void _setHighFrequency(const long freq, const int pin, const int channel){
|
||||
ledcSetup(channel, freq, _PWM_RES_BIT );
|
||||
ledcAttachPin(pin, channel);
|
||||
}
|
||||
|
||||
void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
|
||||
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
|
||||
|
||||
// check if enough channels available
|
||||
if ( channel_index + 2 >= LEDC_CHANNELS ) return;
|
||||
|
||||
stepper_2pwm_motor_slots_t m_slot = {};
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
for(int slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(esp32_stepper_2pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_stepper_2pwm_motor_slots[slot_num].pinID = pinA;
|
||||
esp32_stepper_2pwm_motor_slots[slot_num].ch1 = channel_index++;
|
||||
esp32_stepper_2pwm_motor_slots[slot_num].ch2 = channel_index++;
|
||||
m_slot = esp32_stepper_2pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_setHighFrequency(pwm_frequency, pinA, m_slot.ch1);
|
||||
_setHighFrequency(pwm_frequency, pinB, m_slot.ch2);
|
||||
}
|
||||
|
||||
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
|
||||
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
|
||||
|
||||
// check if enough channels available
|
||||
if ( channel_index + 3 >= LEDC_CHANNELS ) return;
|
||||
|
||||
bldc_3pwm_motor_slots_t m_slot = {};
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
for(int slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(esp32_bldc_3pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_bldc_3pwm_motor_slots[slot_num].pinID = pinA;
|
||||
esp32_bldc_3pwm_motor_slots[slot_num].ch1 = channel_index++;
|
||||
esp32_bldc_3pwm_motor_slots[slot_num].ch2 = channel_index++;
|
||||
esp32_bldc_3pwm_motor_slots[slot_num].ch3 = channel_index++;
|
||||
m_slot = esp32_bldc_3pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_setHighFrequency(pwm_frequency, pinA, m_slot.ch1);
|
||||
_setHighFrequency(pwm_frequency, pinB, m_slot.ch2);
|
||||
_setHighFrequency(pwm_frequency, pinC, m_slot.ch3);
|
||||
}
|
||||
|
||||
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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
|
||||
// check if enough channels available
|
||||
if ( channel_index + 4 >= LEDC_CHANNELS ) return;
|
||||
|
||||
|
||||
stepper_4pwm_motor_slots_t m_slot = {};
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
for(int slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(esp32_stepper_4pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_stepper_4pwm_motor_slots[slot_num].pinID = pinA;
|
||||
esp32_stepper_4pwm_motor_slots[slot_num].ch1 = channel_index++;
|
||||
esp32_stepper_4pwm_motor_slots[slot_num].ch2 = channel_index++;
|
||||
esp32_stepper_4pwm_motor_slots[slot_num].ch3 = channel_index++;
|
||||
esp32_stepper_4pwm_motor_slots[slot_num].ch4 = channel_index++;
|
||||
m_slot = esp32_stepper_4pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_setHighFrequency(pwm_frequency, pinA, m_slot.ch1);
|
||||
_setHighFrequency(pwm_frequency, pinB, m_slot.ch2);
|
||||
_setHighFrequency(pwm_frequency, pinC, m_slot.ch3);
|
||||
_setHighFrequency(pwm_frequency, pinD, m_slot.ch4);
|
||||
}
|
||||
|
||||
void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(esp32_stepper_2pwm_motor_slots[i].pinID == pinA){ // if motor slot found
|
||||
ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES));
|
||||
ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(esp32_bldc_3pwm_motor_slots[i].pinID == pinA){ // if motor slot found
|
||||
ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES));
|
||||
ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES));
|
||||
ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_c, 0, _PWM_RES));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(esp32_stepper_4pwm_motor_slots[i].pinID == pin1A){ // if motor slot found
|
||||
ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_1a, 0, _PWM_RES));
|
||||
ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_1b, 0, _PWM_RES));
|
||||
ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_2a, 0, _PWM_RES));
|
||||
ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch4, _constrain(_PWM_RES*dc_2b, 0, _PWM_RES));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,440 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED)
|
||||
|
||||
#include "driver/mcpwm.h"
|
||||
#include "soc/mcpwm_reg.h"
|
||||
#include "soc/mcpwm_struct.h"
|
||||
|
||||
// empty motor slot
|
||||
#define _EMPTY_SLOT -20
|
||||
#define _TAKEN_SLOT -21
|
||||
|
||||
// ABI bus frequency - would be better to take it from somewhere
|
||||
// but I did nto find a good exposed variable
|
||||
#define _MCPWM_FREQ 160e6f
|
||||
|
||||
// preferred pwm resolution default
|
||||
#define _PWM_RES_DEF 2048
|
||||
// min resolution
|
||||
#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
|
||||
typedef struct {
|
||||
int pinA;
|
||||
mcpwm_dev_t* mcpwm_num;
|
||||
mcpwm_unit_t mcpwm_unit;
|
||||
mcpwm_operator_t mcpwm_operator;
|
||||
mcpwm_io_signals_t mcpwm_a;
|
||||
mcpwm_io_signals_t mcpwm_b;
|
||||
mcpwm_io_signals_t mcpwm_c;
|
||||
} bldc_3pwm_motor_slots_t;
|
||||
typedef struct {
|
||||
int pin1A;
|
||||
mcpwm_dev_t* mcpwm_num;
|
||||
mcpwm_unit_t mcpwm_unit;
|
||||
mcpwm_operator_t mcpwm_operator1;
|
||||
mcpwm_operator_t mcpwm_operator2;
|
||||
mcpwm_io_signals_t mcpwm_1a;
|
||||
mcpwm_io_signals_t mcpwm_1b;
|
||||
mcpwm_io_signals_t mcpwm_2a;
|
||||
mcpwm_io_signals_t mcpwm_2b;
|
||||
} stepper_4pwm_motor_slots_t;
|
||||
typedef struct {
|
||||
int pin1pwm;
|
||||
mcpwm_dev_t* mcpwm_num;
|
||||
mcpwm_unit_t mcpwm_unit;
|
||||
mcpwm_operator_t mcpwm_operator;
|
||||
mcpwm_io_signals_t mcpwm_a;
|
||||
mcpwm_io_signals_t mcpwm_b;
|
||||
} stepper_2pwm_motor_slots_t;
|
||||
|
||||
typedef struct {
|
||||
int pinAH;
|
||||
mcpwm_dev_t* mcpwm_num;
|
||||
mcpwm_unit_t mcpwm_unit;
|
||||
mcpwm_operator_t mcpwm_operator1;
|
||||
mcpwm_operator_t mcpwm_operator2;
|
||||
mcpwm_io_signals_t mcpwm_ah;
|
||||
mcpwm_io_signals_t mcpwm_bh;
|
||||
mcpwm_io_signals_t mcpwm_ch;
|
||||
mcpwm_io_signals_t mcpwm_al;
|
||||
mcpwm_io_signals_t mcpwm_bl;
|
||||
mcpwm_io_signals_t mcpwm_cl;
|
||||
} bldc_6pwm_motor_slots_t;
|
||||
|
||||
// define bldc motor slots array
|
||||
bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = {
|
||||
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A
|
||||
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B
|
||||
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A
|
||||
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B
|
||||
};
|
||||
|
||||
// define stepper motor slots array
|
||||
bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = {
|
||||
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0
|
||||
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0
|
||||
};
|
||||
|
||||
// define 4pwm stepper motor slots array
|
||||
stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = {
|
||||
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0
|
||||
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1
|
||||
};
|
||||
|
||||
// define 2pwm stepper motor slots array
|
||||
stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = {
|
||||
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A
|
||||
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B
|
||||
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A
|
||||
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B
|
||||
};
|
||||
|
||||
// configuring high frequency pwm timer
|
||||
// a lot of help from this post from Paul Gauld
|
||||
// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller
|
||||
// a short tutorial for v2.0.1+
|
||||
// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html
|
||||
//
|
||||
void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){
|
||||
|
||||
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 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
|
||||
float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone;
|
||||
mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
|
||||
mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
|
||||
mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
|
||||
}
|
||||
_delay(100);
|
||||
|
||||
mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0);
|
||||
mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1);
|
||||
mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2);
|
||||
|
||||
// manual configuration due to the lack of config flexibility in mcpwm_init()
|
||||
mcpwm_num->clk_cfg.clk_prescale = 0;
|
||||
// calculate prescaler and period
|
||||
// step 1: calculate the prescaler using the default pwm resolution
|
||||
// prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1
|
||||
int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1;
|
||||
// constrain prescaler
|
||||
prescaler = _constrain(prescaler, 0, 128);
|
||||
// now calculate the real resolution timer period necessary (pwm resolution)
|
||||
// pwm_res = bus_freq / (pwm_freq * (prescaler + 1))
|
||||
int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1);
|
||||
// if pwm resolution too low lower the prescaler
|
||||
if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 )
|
||||
resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1);
|
||||
resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX);
|
||||
|
||||
// set prescaler
|
||||
mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler;
|
||||
mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler;
|
||||
mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler;
|
||||
_delay(1);
|
||||
//set period
|
||||
mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected;
|
||||
mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected;
|
||||
mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected;
|
||||
_delay(1);
|
||||
mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0;
|
||||
mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0;
|
||||
mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0;
|
||||
_delay(1);
|
||||
// _delay(1);
|
||||
//restart the timers
|
||||
mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
|
||||
mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
|
||||
mcpwm_start(mcpwm_unit, MCPWM_TIMER_2);
|
||||
_delay(1);
|
||||
|
||||
mcpwm_sync_config_t sync_conf = {
|
||||
.sync_sig = MCPWM_SELECT_TIMER0_SYNC,
|
||||
.timer_val = 0,
|
||||
.count_direction = MCPWM_TIMER_DIRECTION_UP
|
||||
};
|
||||
mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf);
|
||||
mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf);
|
||||
mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf);
|
||||
|
||||
// Enable sync event for all timers to be the TEZ of timer0
|
||||
mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 2PWM setting
|
||||
// - 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 = _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 = {};
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA;
|
||||
m_slot = esp32_stepper_2pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// disable all the slots with the same MCPWM
|
||||
// disable 3pwm bldc motor which would go in the same slot
|
||||
esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
|
||||
if( slot_num < 2 ){
|
||||
// slot 0 of the 4pwm stepper
|
||||
esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
|
||||
}else{
|
||||
// slot 1 of the stepper
|
||||
esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
|
||||
// slot 1 of the 6pwm bldc
|
||||
esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
|
||||
}
|
||||
// configure pins
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
|
||||
|
||||
// configure the timer
|
||||
_configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
|
||||
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - 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 = _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 = {};
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
|
||||
m_slot = esp32_bldc_3pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
// disable all the slots with the same MCPWM
|
||||
// disable 2pwm steppr motor which would go in the same slot
|
||||
esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT;
|
||||
if( slot_num < 2 ){
|
||||
// slot 0 of the 4pwm stepper
|
||||
esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
|
||||
}else{
|
||||
// slot 1 of the stepper
|
||||
esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
|
||||
// slot 1 of the 6pwm bldc
|
||||
esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
|
||||
}
|
||||
// configure pins
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
|
||||
|
||||
// configure the timer
|
||||
_configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
|
||||
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 2; slot_num++){
|
||||
if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA;
|
||||
m_slot = esp32_stepper_4pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
// disable all the slots with the same MCPWM
|
||||
if( slot_num == 0 ){
|
||||
// slots 0 and 1 of the 3pwm bldc
|
||||
esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
|
||||
esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
|
||||
// slots 0 and 1 of the 2pwm stepper taken
|
||||
esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT;
|
||||
esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
|
||||
}else{
|
||||
// slots 2 and 3 of the 3pwm bldc
|
||||
esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
|
||||
esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
|
||||
// slots 2 and 3 of the 2pwm stepper taken
|
||||
esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT;
|
||||
esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT;
|
||||
// slot 1 of the 6pwm bldc
|
||||
esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
|
||||
}
|
||||
|
||||
// configure pins
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD);
|
||||
|
||||
// configure the timer
|
||||
_configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
|
||||
}
|
||||
|
||||
// function setting the pwm duty cycle to the hardware
|
||||
// - Stepper motor - 2PWM setting
|
||||
// - hardware speciffic
|
||||
// ESP32 uses MCPWM
|
||||
void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,100]
|
||||
mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0);
|
||||
mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// function setting the pwm duty cycle to the hardware
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - hardware speciffic
|
||||
// ESP32 uses MCPWM
|
||||
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,100]
|
||||
mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// function setting the pwm duty cycle to the hardware
|
||||
// - Stepper motor - 4PWM setting
|
||||
// - hardware speciffic
|
||||
// ESP32 uses MCPWM
|
||||
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 2; i++){
|
||||
if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,100]
|
||||
mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0);
|
||||
mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0);
|
||||
mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0);
|
||||
mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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){
|
||||
|
||||
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, _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
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 2; slot_num++){
|
||||
if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
|
||||
m_slot = esp32_bldc_6pwm_motor_slots[slot_num];
|
||||
break;
|
||||
}
|
||||
}
|
||||
// if no slots available
|
||||
if(slot_num >= 2) return -1;
|
||||
|
||||
// disable all the slots with the same MCPWM
|
||||
if( slot_num == 0 ){
|
||||
// slots 0 and 1 of the 3pwm bldc
|
||||
esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
|
||||
esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
|
||||
}else{
|
||||
// slots 2 and 3 of the 3pwm bldc
|
||||
esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
|
||||
esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
|
||||
// slot 1 of the 6pwm bldc
|
||||
esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
|
||||
}
|
||||
|
||||
// configure pins
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h);
|
||||
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l);
|
||||
|
||||
// configure the timer
|
||||
_configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone);
|
||||
// return
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 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){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 2; i++){
|
||||
if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,100.0]
|
||||
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0);
|
||||
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,77 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266)
|
||||
|
||||
#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);
|
||||
analogWriteFreq(freq);
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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);
|
||||
_setHighFrequency(pwm_frequency, pinD);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
}
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
analogWrite(pinC, 255.0f*dc_c);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pin1A, 255.0f*dc_1a);
|
||||
analogWrite(pin1B, 255.0f*dc_1b);
|
||||
analogWrite(pin2A, 255.0f*dc_2a);
|
||||
analogWrite(pin2B, 255.0f*dc_2b);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,107 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
// if the mcu doen't have defiend analogWrite
|
||||
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
|
||||
__attribute__((weak)) void analogWrite(uint8_t pin, int value){ };
|
||||
#endif
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 2PWM setting
|
||||
// - hardware speciffic
|
||||
// in generic case dont do anything
|
||||
__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
|
||||
_UNUSED(pwm_frequency);
|
||||
_UNUSED(pinA);
|
||||
_UNUSED(pinB);
|
||||
return;
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - hardware speciffic
|
||||
// in generic case dont do anything
|
||||
__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;
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - Stepper motor - 4PWM setting
|
||||
// - hardware speciffic
|
||||
// in generic case dont do anything
|
||||
__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
|
||||
__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;
|
||||
}
|
||||
|
||||
|
||||
// function setting the pwm duty cycle to the hardware
|
||||
// - Stepper motor - 2PWM setting
|
||||
// - hardware speciffic
|
||||
__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.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
}
|
||||
|
||||
// function setting the pwm duty cycle to the hardware
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - hardware speciffic
|
||||
__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.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
analogWrite(pinC, 255.0f*dc_c);
|
||||
}
|
||||
|
||||
// function setting the pwm duty cycle to the hardware
|
||||
// - Stepper motor - 4PWM setting
|
||||
// - hardware speciffic
|
||||
__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.0f*dc_1a);
|
||||
analogWrite(pin1B, 255.0f*dc_1b);
|
||||
analogWrite(pin2A, 255.0f*dc_2a);
|
||||
analogWrite(pin2B, 255.0f*dc_2b);
|
||||
}
|
||||
|
||||
|
||||
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - hardware specific
|
||||
__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;
|
||||
}
|
||||
@@ -0,0 +1,355 @@
|
||||
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(NRF52_SERIES)
|
||||
|
||||
|
||||
#define PWM_CLK (16000000)
|
||||
#define PWM_FREQ (40000)
|
||||
#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ)
|
||||
#define PWM_MAX_FREQ (62500)
|
||||
#define DEAD_ZONE (250) // in ns
|
||||
#define DEAD_TIME (DEAD_ZONE / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM
|
||||
|
||||
#ifdef NRF_PWM3
|
||||
#define PWM_COUNT 4
|
||||
#else
|
||||
#define PWM_COUNT 3
|
||||
#endif
|
||||
|
||||
// empty motor slot
|
||||
#define _EMPTY_SLOT (-0xAA)
|
||||
#define _TAKEN_SLOT (-0x55)
|
||||
|
||||
int pwm_range;
|
||||
float dead_time;
|
||||
|
||||
static NRF_PWM_Type* pwms[PWM_COUNT] = {
|
||||
NRF_PWM0,
|
||||
NRF_PWM1,
|
||||
NRF_PWM2,
|
||||
#ifdef NRF_PWM3
|
||||
NRF_PWM3
|
||||
#endif
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
int pinA;
|
||||
NRF_PWM_Type* mcpwm;
|
||||
uint16_t mcpwm_channel_sequence[4];
|
||||
} bldc_3pwm_motor_slots_t;
|
||||
|
||||
typedef struct {
|
||||
int pin1A;
|
||||
NRF_PWM_Type* mcpwm;
|
||||
uint16_t mcpwm_channel_sequence[4];
|
||||
} stepper_motor_slots_t;
|
||||
|
||||
typedef struct {
|
||||
int pinAH;
|
||||
NRF_PWM_Type* mcpwm1;
|
||||
NRF_PWM_Type* mcpwm2;
|
||||
uint16_t mcpwm_channel_sequence[8];
|
||||
} bldc_6pwm_motor_slots_t;
|
||||
|
||||
// define bldc motor slots array
|
||||
bldc_3pwm_motor_slots_t nrf52_bldc_3pwm_motor_slots[4] = {
|
||||
{_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be PWM0
|
||||
{_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 2nd motor will be PWM1
|
||||
{_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 3rd motor will be PWM2
|
||||
{_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 4th motor will be PWM3
|
||||
};
|
||||
|
||||
// define stepper motor slots array
|
||||
stepper_motor_slots_t nrf52_stepper_motor_slots[4] = {
|
||||
{_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be on PWM0
|
||||
{_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 1st motor will be on PWM1
|
||||
{_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 1st motor will be on PWM2
|
||||
{_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 1st motor will be on PWM3
|
||||
};
|
||||
|
||||
// define BLDC motor slots array
|
||||
bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] = {
|
||||
{_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1
|
||||
{_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2
|
||||
};
|
||||
|
||||
// configuring high frequency pwm timer
|
||||
void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){
|
||||
|
||||
mcpwm1->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
|
||||
mcpwm1->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
|
||||
mcpwm1->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos);
|
||||
mcpwm1->COUNTERTOP = pwm_range; //pwm freq.
|
||||
mcpwm1->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
|
||||
mcpwm1->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
|
||||
mcpwm1->SEQ[0].REFRESH = 0;
|
||||
mcpwm1->SEQ[0].ENDDELAY = 0;
|
||||
|
||||
if(mcpwm1 != mcpwm2){
|
||||
mcpwm2->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
|
||||
mcpwm2->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
|
||||
mcpwm2->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos);
|
||||
mcpwm2->COUNTERTOP = pwm_range; //pwm freq.
|
||||
mcpwm2->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
|
||||
mcpwm2->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
|
||||
mcpwm2->SEQ[0].REFRESH = 0;
|
||||
mcpwm2->SEQ[0].ENDDELAY = 0;
|
||||
}else{
|
||||
mcpwm1->MODE = (PWM_MODE_UPDOWN_Up << PWM_MODE_UPDOWN_Pos);
|
||||
}
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - BLDC motor - 3PWM setting
|
||||
// - hardware speciffic
|
||||
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
|
||||
|
||||
if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256
|
||||
|
||||
pwm_range = (PWM_CLK / pwm_frequency);
|
||||
|
||||
int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA];
|
||||
int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB];
|
||||
int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC];
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(nrf52_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// disable all the slots with the same MCPWM
|
||||
if(slot_num < 2){
|
||||
// slot 0 of the stepper
|
||||
nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
|
||||
//NRF_PPI->CHEN &= ~1UL;
|
||||
}else{
|
||||
// slot 1 of the stepper
|
||||
nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
|
||||
//NRF_PPI->CHEN &= ~2UL;
|
||||
}
|
||||
|
||||
// configure pwm outputs
|
||||
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
|
||||
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
|
||||
|
||||
// configure the pwm
|
||||
_configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256
|
||||
|
||||
pwm_range = (PWM_CLK / pwm_frequency);
|
||||
|
||||
int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA];
|
||||
int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB];
|
||||
int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC];
|
||||
int pD = digitalPinToPinName(pinD); //g_ADigitalPinMap[pinD];
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 4; slot_num++){
|
||||
if(nrf52_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
nrf52_stepper_motor_slots[slot_num].pin1A = pinA;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// disable all the slots with the same MCPWM
|
||||
if( slot_num < 2 ){
|
||||
// slots 0 and 1 of the 3pwm bldc
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
|
||||
// slot 0 of the 6pwm bldc
|
||||
nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
|
||||
//NRF_PPI->CHEN &= ~1UL;
|
||||
}else{
|
||||
// slots 2 and 3 of the 3pwm bldc
|
||||
nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
|
||||
// slot 1 of the 6pwm bldc
|
||||
nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
|
||||
//NRF_PPI->CHEN &= ~2UL;
|
||||
}
|
||||
|
||||
// configure pwm outputs
|
||||
|
||||
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
|
||||
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
|
||||
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
|
||||
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[3] = pD;
|
||||
|
||||
nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_stepper_motor_slots[slot_num].mcpwm_channel_sequence[0];
|
||||
nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
|
||||
|
||||
// configure the pwm
|
||||
_configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(nrf52_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,range]
|
||||
|
||||
nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000;
|
||||
nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000;
|
||||
nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000;
|
||||
|
||||
nrf52_bldc_3pwm_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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){
|
||||
// determine which motor slot is the motor connected to
|
||||
for(int i = 0; i < 4; i++){
|
||||
if(nrf52_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,range]
|
||||
|
||||
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000;
|
||||
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000;
|
||||
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000;
|
||||
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000;
|
||||
|
||||
nrf52_stepper_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 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){
|
||||
|
||||
if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz - centered pwm has twice lower frequency for a resolution of 400
|
||||
else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256
|
||||
|
||||
pwm_range = (PWM_CLK / pwm_frequency);
|
||||
pwm_range /= 2; // scale the frequency (centered PWM)
|
||||
|
||||
if (dead_zone != NOT_SET){
|
||||
dead_time = dead_zone/2;
|
||||
}else{
|
||||
dead_time = DEAD_TIME/2;
|
||||
}
|
||||
|
||||
int pA_l = digitalPinToPinName(pinA_l); //g_ADigitalPinMap[pinA_l];
|
||||
int pA_h = digitalPinToPinName(pinA_h); //g_ADigitalPinMap[pinA_h];
|
||||
int pB_l = digitalPinToPinName(pinB_l); //g_ADigitalPinMap[pinB_l];
|
||||
int pB_h = digitalPinToPinName(pinB_h); //g_ADigitalPinMap[pinB_h];
|
||||
int pC_l = digitalPinToPinName(pinC_l); //g_ADigitalPinMap[pinC_l];
|
||||
int pC_h = digitalPinToPinName(pinC_h); //g_ADigitalPinMap[pinC_h];
|
||||
|
||||
|
||||
// determine which motor are we connecting
|
||||
// and set the appropriate configuration parameters
|
||||
int slot_num;
|
||||
for(slot_num = 0; slot_num < 2; slot_num++){
|
||||
if(nrf52_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// if no slots available
|
||||
if(slot_num >= 2) return -1;
|
||||
|
||||
// disable all the slots with the same MCPWM
|
||||
if( slot_num == 0 ){
|
||||
// slots 0 and 1 of the 3pwm bldc
|
||||
nrf52_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
|
||||
nrf52_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
|
||||
// slot 0 and 1 of the stepper
|
||||
nrf52_stepper_motor_slots[0].pin1A = _TAKEN_SLOT;
|
||||
nrf52_stepper_motor_slots[1].pin1A = _TAKEN_SLOT;
|
||||
}else{
|
||||
// slots 2 and 3 of the 3pwm bldc
|
||||
nrf52_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
|
||||
nrf52_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
|
||||
// slot 1 of the stepper
|
||||
nrf52_stepper_motor_slots[2].pin1A = _TAKEN_SLOT;
|
||||
nrf52_stepper_motor_slots[3].pin1A = _TAKEN_SLOT;
|
||||
}
|
||||
|
||||
// Configure pwm outputs
|
||||
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[0] = pA_h;
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[1] = pA_l;
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[2] = pB_h;
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[3] = pB_l;
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].CNT = 4;
|
||||
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[0] = pC_h;
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[1] = pC_l;
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[4];
|
||||
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].CNT = 4;
|
||||
|
||||
// Initializing the PPI peripheral for sync the pwm slots
|
||||
|
||||
NRF_PPI->CH[slot_num].EEP = (uint32_t)&NRF_EGU0->EVENTS_TRIGGERED[0];
|
||||
NRF_PPI->CH[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->TASKS_SEQSTART[0];
|
||||
NRF_PPI->FORK[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->TASKS_SEQSTART[0];
|
||||
NRF_PPI->CHEN = 1UL << slot_num;
|
||||
|
||||
// configure the pwm type
|
||||
_configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2);
|
||||
|
||||
// return
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Function setting the duty cycle to the pwm pin
|
||||
// - BLDC driver - 6PWM setting
|
||||
// - hardware specific
|
||||
*/
|
||||
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, const int pinA_h, const int, const int, const int, const int, const int){
|
||||
for(int i = 0; i < 2; i++){
|
||||
if(nrf52_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
|
||||
// se the PWM on the slot timers
|
||||
// transform duty cycle from [0,1] to [0,range]
|
||||
|
||||
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000;
|
||||
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range);
|
||||
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000;
|
||||
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range);
|
||||
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000;
|
||||
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range);
|
||||
|
||||
NRF_EGU0->TASKS_TRIGGER[0] = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,539 @@
|
||||
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(TARGET_PORTENTA_H7)
|
||||
|
||||
#include "pwmout_api.h"
|
||||
#include "pinDefinitions.h"
|
||||
#include "platform/mbed_critical.h"
|
||||
#include "platform/mbed_power_mgmt.h"
|
||||
#include "platform/mbed_assert.h"
|
||||
#include "PeripheralPins.h"
|
||||
#include "pwmout_device.h"
|
||||
|
||||
// default pwm parameters
|
||||
#define _PWM_FREQUENCY 25000 // 25khz
|
||||
#define _PWM_FREQUENCY_MAX 50000 // 50khz
|
||||
|
||||
|
||||
// // 6pwm parameters
|
||||
// #define _HARDWARE_6PWM 1
|
||||
// #define _SOFTWARE_6PWM 0
|
||||
// #define _ERROR_6PWM -1
|
||||
|
||||
typedef struct{
|
||||
int id ;
|
||||
pwmout_t pins[6];
|
||||
} portenta_h7_mcu_enty_s;
|
||||
|
||||
portenta_h7_mcu_enty_s motor_slot[10];
|
||||
int slot_index = 0;
|
||||
|
||||
|
||||
/* Convert STM32 Cube HAL channel to LL channel */
|
||||
uint32_t _TIM_ChannelConvert_HAL2LL(uint32_t channel, pwmout_t *obj)
|
||||
{
|
||||
#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED)
|
||||
if (obj->inverted) {
|
||||
switch (channel) {
|
||||
case TIM_CHANNEL_1 :
|
||||
return LL_TIM_CHANNEL_CH1N;
|
||||
case TIM_CHANNEL_2 :
|
||||
return LL_TIM_CHANNEL_CH2N;
|
||||
case TIM_CHANNEL_3 :
|
||||
return LL_TIM_CHANNEL_CH3N;
|
||||
#if defined(LL_TIM_CHANNEL_CH4N)
|
||||
case TIM_CHANNEL_4 :
|
||||
return LL_TIM_CHANNEL_CH4N;
|
||||
#endif
|
||||
default : /* Optional */
|
||||
return 0;
|
||||
}
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
switch (channel) {
|
||||
case TIM_CHANNEL_1 :
|
||||
return LL_TIM_CHANNEL_CH1;
|
||||
case TIM_CHANNEL_2 :
|
||||
return LL_TIM_CHANNEL_CH2;
|
||||
case TIM_CHANNEL_3 :
|
||||
return LL_TIM_CHANNEL_CH3;
|
||||
case TIM_CHANNEL_4 :
|
||||
return LL_TIM_CHANNEL_CH4;
|
||||
default : /* Optional */
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){
|
||||
// return _pwm_init(obj, digitalPinToPinName(pin), frequency);
|
||||
// }
|
||||
|
||||
int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){
|
||||
int peripheral = (int)pinmap_peripheral(digitalPinToPinName(pin), PinMap_PWM);
|
||||
int function = (int)pinmap_find_function(digitalPinToPinName(pin), PinMap_PWM);
|
||||
|
||||
const PinMap static_pinmap = {digitalPinToPinName(pin), peripheral, function};
|
||||
|
||||
pwmout_init_direct(obj, &static_pinmap);
|
||||
|
||||
TIM_HandleTypeDef TimHandle;
|
||||
TimHandle.Instance = (TIM_TypeDef *)(obj->pwm);
|
||||
RCC_ClkInitTypeDef RCC_ClkInitStruct;
|
||||
uint32_t PclkFreq = 0;
|
||||
uint32_t APBxCLKDivider = RCC_HCLK_DIV1;
|
||||
uint8_t i = 0;
|
||||
|
||||
|
||||
__HAL_TIM_DISABLE(&TimHandle);
|
||||
|
||||
// Get clock configuration
|
||||
// Note: PclkFreq contains here the Latency (not used after)
|
||||
HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &PclkFreq);
|
||||
|
||||
/* Parse the pwm / apb mapping table to find the right entry */
|
||||
while (pwm_apb_map_table[i].pwm != obj->pwm) i++;
|
||||
// sanity check
|
||||
if (pwm_apb_map_table[i].pwm == 0) return -1;
|
||||
|
||||
|
||||
if (pwm_apb_map_table[i].pwmoutApb == PWMOUT_ON_APB1) {
|
||||
PclkFreq = HAL_RCC_GetPCLK1Freq();
|
||||
APBxCLKDivider = RCC_ClkInitStruct.APB1CLKDivider;
|
||||
} else {
|
||||
#if !defined(PWMOUT_APB2_NOT_SUPPORTED)
|
||||
PclkFreq = HAL_RCC_GetPCLK2Freq();
|
||||
APBxCLKDivider = RCC_ClkInitStruct.APB2CLKDivider;
|
||||
#endif
|
||||
}
|
||||
|
||||
long period_us = 500000.0/((float)frequency);
|
||||
/* By default use, 1us as SW pre-scaler */
|
||||
obj->prescaler = 1;
|
||||
// TIMxCLK = PCLKx when the APB prescaler = 1 else TIMxCLK = 2 * PCLKx
|
||||
if (APBxCLKDivider == RCC_HCLK_DIV1) {
|
||||
TimHandle.Init.Prescaler = (((PclkFreq) / 1000000)) - 1; // 1 us tick
|
||||
} else {
|
||||
TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000)) - 1; // 1 us tick
|
||||
}
|
||||
TimHandle.Init.Period = (period_us - 1);
|
||||
|
||||
/* In case period or pre-scalers are out of range, loop-in to get valid values */
|
||||
while ((TimHandle.Init.Period > 0xFFFF) || (TimHandle.Init.Prescaler > 0xFFFF)) {
|
||||
obj->prescaler = obj->prescaler * 2;
|
||||
if (APBxCLKDivider == RCC_HCLK_DIV1) {
|
||||
TimHandle.Init.Prescaler = (((PclkFreq) / 1000000) * obj->prescaler) - 1;
|
||||
} else {
|
||||
TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000) * obj->prescaler) - 1;
|
||||
}
|
||||
TimHandle.Init.Period = (period_us - 1) / obj->prescaler;
|
||||
/* Period decreases and prescaler increases over loops, so check for
|
||||
* possible out of range cases */
|
||||
if ((TimHandle.Init.Period < 0xFFFF) && (TimHandle.Init.Prescaler > 0xFFFF)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
TimHandle.Init.ClockDivision = 0;
|
||||
TimHandle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; // center aligned
|
||||
|
||||
if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
TIM_OC_InitTypeDef sConfig;
|
||||
// Configure channels
|
||||
sConfig.OCMode = TIM_OCMODE_PWM1;
|
||||
sConfig.Pulse = obj->pulse / obj->prescaler;
|
||||
sConfig.OCPolarity = TIM_OCPOLARITY_HIGH;
|
||||
sConfig.OCFastMode = TIM_OCFAST_DISABLE;
|
||||
#if defined(TIM_OCIDLESTATE_RESET)
|
||||
sConfig.OCIdleState = TIM_OCIDLESTATE_RESET;
|
||||
#endif
|
||||
#if defined(TIM_OCNIDLESTATE_RESET)
|
||||
sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH;
|
||||
sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET;
|
||||
#endif
|
||||
|
||||
int channel = 0;
|
||||
switch (obj->channel) {
|
||||
case 1:
|
||||
channel = TIM_CHANNEL_1;
|
||||
break;
|
||||
case 2:
|
||||
channel = TIM_CHANNEL_2;
|
||||
break;
|
||||
case 3:
|
||||
channel = TIM_CHANNEL_3;
|
||||
break;
|
||||
case 4:
|
||||
channel = TIM_CHANNEL_4;
|
||||
break;
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (LL_TIM_CC_IsEnabledChannel(TimHandle.Instance, _TIM_ChannelConvert_HAL2LL(channel, obj)) == 0) {
|
||||
// If channel is not enabled, proceed to channel configuration
|
||||
if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, channel) != HAL_OK) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Save for future use
|
||||
obj->period = period_us;
|
||||
#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED)
|
||||
if (obj->inverted) {
|
||||
HAL_TIMEx_PWMN_Start(&TimHandle, channel);
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
HAL_TIM_PWM_Start(&TimHandle, channel);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// setting pwm to hardware pin - instead analogWrite()
|
||||
void _pwm_write(pwmout_t *obj, float value){
|
||||
TIM_HandleTypeDef TimHandle;
|
||||
int channel = 0;
|
||||
|
||||
TimHandle.Instance = (TIM_TypeDef *)(obj->pwm);
|
||||
|
||||
if (value < (float)0.0) {
|
||||
value = 0.0;
|
||||
} else if (value > (float)1.0) {
|
||||
value = 1.0;
|
||||
}
|
||||
|
||||
obj->pulse = (uint32_t)((float)obj->period * value + 0.5);
|
||||
|
||||
switch (obj->channel) {
|
||||
case 1:
|
||||
channel = TIM_CHANNEL_1;
|
||||
break;
|
||||
case 2:
|
||||
channel = TIM_CHANNEL_2;
|
||||
break;
|
||||
case 3:
|
||||
channel = TIM_CHANNEL_3;
|
||||
break;
|
||||
case 4:
|
||||
channel = TIM_CHANNEL_4;
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
// If channel already enabled, only update compare value to avoid glitch
|
||||
__HAL_TIM_SET_COMPARE(&TimHandle, channel, obj->pulse / obj->prescaler);
|
||||
}
|
||||
|
||||
// init low side pin
|
||||
// HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin)
|
||||
// {
|
||||
// PinName pin = digitalPinToPinName(ulPin);
|
||||
// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
|
||||
// uint32_t index = get_timer_index(Instance);
|
||||
|
||||
// if (HardwareTimer_Handle[index] == NULL) {
|
||||
// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
|
||||
// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
|
||||
// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
|
||||
// TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
|
||||
// sConfigOC.OCMode = TIM_OCMODE_PWM2;
|
||||
// sConfigOC.Pulse = 100;
|
||||
// sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
|
||||
// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
|
||||
// sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
|
||||
// sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET;
|
||||
// sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
|
||||
// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
|
||||
// HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
|
||||
// }
|
||||
// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
|
||||
// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
|
||||
// HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin);
|
||||
// HT->setOverflow(PWM_freq, HERTZ_FORMAT);
|
||||
// HT->pause();
|
||||
// HT->refresh();
|
||||
// return HT;
|
||||
// }
|
||||
|
||||
|
||||
// align the timers to end the init
|
||||
void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2){
|
||||
TIM_HandleTypeDef TimHandle1, TimHandle2;
|
||||
TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
|
||||
TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
|
||||
__HAL_TIM_DISABLE(&TimHandle1);
|
||||
__HAL_TIM_DISABLE(&TimHandle2);
|
||||
__HAL_TIM_ENABLE(&TimHandle1);
|
||||
__HAL_TIM_ENABLE(&TimHandle2);
|
||||
}
|
||||
|
||||
// align the timers to end the init
|
||||
void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3){
|
||||
TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3;
|
||||
TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
|
||||
TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
|
||||
TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm);
|
||||
__HAL_TIM_DISABLE(&TimHandle1);
|
||||
__HAL_TIM_DISABLE(&TimHandle2);
|
||||
__HAL_TIM_DISABLE(&TimHandle3);
|
||||
__HAL_TIM_ENABLE(&TimHandle1);
|
||||
__HAL_TIM_ENABLE(&TimHandle2);
|
||||
__HAL_TIM_ENABLE(&TimHandle3);
|
||||
}
|
||||
|
||||
// align the timers to end the init
|
||||
void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){
|
||||
TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3, TimHandle4;
|
||||
TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
|
||||
TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
|
||||
TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm);
|
||||
TimHandle4.Instance = (TIM_TypeDef *)(t4->pwm);
|
||||
__HAL_TIM_DISABLE(&TimHandle1);
|
||||
__HAL_TIM_DISABLE(&TimHandle2);
|
||||
__HAL_TIM_DISABLE(&TimHandle3);
|
||||
__HAL_TIM_DISABLE(&TimHandle4);
|
||||
__HAL_TIM_ENABLE(&TimHandle1);
|
||||
__HAL_TIM_ENABLE(&TimHandle2);
|
||||
__HAL_TIM_ENABLE(&TimHandle3);
|
||||
__HAL_TIM_ENABLE(&TimHandle4);
|
||||
}
|
||||
|
||||
// // configure hardware 6pwm interface only one timer with inverted channels
|
||||
// HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
|
||||
// {
|
||||
// PinName uhPinName = digitalPinToPinName(pinA_h);
|
||||
// PinName ulPinName = digitalPinToPinName(pinA_l);
|
||||
// PinName vhPinName = digitalPinToPinName(pinB_h);
|
||||
// PinName vlPinName = digitalPinToPinName(pinB_l);
|
||||
// PinName whPinName = digitalPinToPinName(pinC_h);
|
||||
// PinName wlPinName = digitalPinToPinName(pinC_l);
|
||||
|
||||
// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM);
|
||||
|
||||
// uint32_t index = get_timer_index(Instance);
|
||||
|
||||
// if (HardwareTimer_Handle[index] == NULL) {
|
||||
// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM));
|
||||
// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
|
||||
// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
|
||||
// ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT);
|
||||
// }
|
||||
// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
|
||||
|
||||
// HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName);
|
||||
// HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName);
|
||||
// HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName);
|
||||
// HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName);
|
||||
// HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName);
|
||||
// HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName);
|
||||
|
||||
// // dead time is set in nanoseconds
|
||||
// uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone;
|
||||
// uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
|
||||
// LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
|
||||
// LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N);
|
||||
|
||||
// HT->pause();
|
||||
// HT->refresh();
|
||||
// HT->resume();
|
||||
// return HT;
|
||||
// }
|
||||
|
||||
|
||||
// // returns 0 if each pair of pwm channels has same channel
|
||||
// // returns 1 all the channels belong to the same timer - hardware inverted channels
|
||||
// // returns -1 if neither - avoid configuring - error!!!
|
||||
// int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
|
||||
// PinName nameAH = digitalPinToPinName(pinA_h);
|
||||
// PinName nameAL = digitalPinToPinName(pinA_l);
|
||||
// PinName nameBH = digitalPinToPinName(pinB_h);
|
||||
// PinName nameBL = digitalPinToPinName(pinB_l);
|
||||
// PinName nameCH = digitalPinToPinName(pinC_h);
|
||||
// PinName nameCL = digitalPinToPinName(pinC_l);
|
||||
// int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM));
|
||||
// int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM));
|
||||
// int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM));
|
||||
// int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM));
|
||||
// int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM));
|
||||
// int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM));
|
||||
// if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6)
|
||||
// return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer
|
||||
// else if(tim1 == tim2 && tim3==tim4 && tim5==tim6)
|
||||
// return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
|
||||
// else
|
||||
// return _ERROR_6PWM; // might be error avoid configuration
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
|
||||
motor_slot[slot_index].id = pinA;
|
||||
|
||||
core_util_critical_section_enter();
|
||||
_pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency);
|
||||
_pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency);
|
||||
// allign the timers
|
||||
_alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]));
|
||||
core_util_critical_section_exit();
|
||||
slot_index++;
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
|
||||
motor_slot[slot_index].id = pinA;
|
||||
core_util_critical_section_enter();
|
||||
_pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency);
|
||||
_pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency);
|
||||
_pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency);
|
||||
// allign the timers
|
||||
_alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]));
|
||||
core_util_critical_section_exit();
|
||||
slot_index++;
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
|
||||
motor_slot[slot_index].id = pinA;
|
||||
core_util_critical_section_enter();
|
||||
_pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency);
|
||||
_pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency);
|
||||
_pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency);
|
||||
_pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency);
|
||||
// allign the timers
|
||||
_alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3]));
|
||||
core_util_critical_section_exit();
|
||||
slot_index++;
|
||||
}
|
||||
|
||||
// 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){
|
||||
for(int i=0; i<slot_index; i++){
|
||||
if(motor_slot[i].id == pinA){
|
||||
core_util_critical_section_enter();
|
||||
_pwm_write(&(motor_slot[i].pins[0]), (float)dc_a);
|
||||
_pwm_write(&(motor_slot[i].pins[1]), (float)dc_b);
|
||||
core_util_critical_section_exit();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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){
|
||||
for(int i=0; i<slot_index; i++){
|
||||
if(motor_slot[i].id == pinA){
|
||||
core_util_critical_section_enter();
|
||||
_pwm_write(&(motor_slot[i].pins[0]), (float)dc_a);
|
||||
_pwm_write(&(motor_slot[i].pins[1]), (float)dc_b);
|
||||
_pwm_write(&(motor_slot[i].pins[2]), (float)dc_c);
|
||||
core_util_critical_section_exit();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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){
|
||||
for(int i=0; i<slot_index; i++){
|
||||
if(motor_slot[i].id == pin1A){
|
||||
core_util_critical_section_enter();
|
||||
_pwm_write(&(motor_slot[i].pins[0]), (float)dc_1a);
|
||||
_pwm_write(&(motor_slot[i].pins[1]), (float)dc_1b);
|
||||
_pwm_write(&(motor_slot[i].pins[2]), (float)dc_2a);
|
||||
_pwm_write(&(motor_slot[i].pins[3]), (float)dc_2b);
|
||||
core_util_critical_section_exit();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 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){
|
||||
// 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 |%0kHz max
|
||||
// // center-aligned frequency is uses two periods
|
||||
// pwm_frequency *=2;
|
||||
|
||||
// // find configuration
|
||||
// int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
// // configure accordingly
|
||||
// switch(config){
|
||||
// case _ERROR_6PWM:
|
||||
// return -1; // fail
|
||||
// break;
|
||||
// case _HARDWARE_6PWM:
|
||||
// _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
// break;
|
||||
// case _SOFTWARE_6PWM:
|
||||
// HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
|
||||
// _initPinPWMLow(pwm_frequency, pinA_l);
|
||||
// HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
|
||||
// _initPinPWMLow(pwm_frequency, pinB_l);
|
||||
// HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
|
||||
// _initPinPWMLow(pwm_frequency, pinC_l);
|
||||
// _alignPWMTimers(HT1, HT2, HT3);
|
||||
// break;
|
||||
// }
|
||||
return -1; // success
|
||||
}
|
||||
|
||||
// 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){
|
||||
// // find configuration
|
||||
// int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
// // set pwm accordingly
|
||||
// switch(config){
|
||||
// case _HARDWARE_6PWM:
|
||||
// _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
|
||||
// _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
|
||||
// _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
|
||||
// break;
|
||||
// case _SOFTWARE_6PWM:
|
||||
// _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
// _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
// _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
// _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
// _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
// _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
// break;
|
||||
// }
|
||||
}
|
||||
#endif
|
||||
@@ -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;i<NUM_PWM_SLICES;i++) {
|
||||
pwm_set_enabled(i, false);
|
||||
pwm_set_counter(i, 0);
|
||||
}
|
||||
// enable all slices
|
||||
pwm_set_mask_enabled(0x7F);
|
||||
}
|
||||
|
||||
|
||||
void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
|
||||
setupPWM(pinA, pwm_frequency);
|
||||
setupPWM(pinB, pwm_frequency);
|
||||
syncSlices();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) {
|
||||
setupPWM(pinA, pwm_frequency);
|
||||
setupPWM(pinB, pwm_frequency);
|
||||
setupPWM(pinC, pwm_frequency);
|
||||
syncSlices();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
|
||||
setupPWM(pin1A, pwm_frequency);
|
||||
setupPWM(pin1B, pwm_frequency);
|
||||
setupPWM(pin2A, pwm_frequency);
|
||||
setupPWM(pin2B, pwm_frequency);
|
||||
syncSlices();
|
||||
}
|
||||
|
||||
|
||||
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) {
|
||||
// non-PIO solution...
|
||||
setupPWM(pinA_h, pwm_frequency);
|
||||
setupPWM(pinB_h, pwm_frequency);
|
||||
setupPWM(pinC_h, pwm_frequency);
|
||||
setupPWM(pinA_l, pwm_frequency, true);
|
||||
setupPWM(pinB_l, pwm_frequency, true);
|
||||
setupPWM(pinC_l, pwm_frequency, true);
|
||||
syncSlices();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void writeDutyCycle(float val, int pin) {
|
||||
uint slice = pwm_gpio_to_slice_num(pin);
|
||||
uint chan = pwm_gpio_to_channel(pin);
|
||||
pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) {
|
||||
writeDutyCycle(dc_a, pinA);
|
||||
writeDutyCycle(dc_b, pinB);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) {
|
||||
writeDutyCycle(dc_a, pinA);
|
||||
writeDutyCycle(dc_b, pinB);
|
||||
writeDutyCycle(dc_c, pinC);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B) {
|
||||
writeDutyCycle(dc_1a, pin1A);
|
||||
writeDutyCycle(dc_1b, pin1B);
|
||||
writeDutyCycle(dc_2a, pin2A);
|
||||
writeDutyCycle(dc_2b, pin2B);
|
||||
}
|
||||
|
||||
inline float swDti(float val, float dt) {
|
||||
float ret = dt+val;
|
||||
if (ret>1.0) ret = 1.0f;
|
||||
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
|
||||
@@ -0,0 +1,348 @@
|
||||
|
||||
|
||||
|
||||
#include "./samd_mcu.h"
|
||||
|
||||
|
||||
#ifdef _SAMD21_
|
||||
|
||||
|
||||
|
||||
#ifndef TCC3_CH0
|
||||
#define TCC3_CH0 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH1
|
||||
#define TCC3_CH1 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH2
|
||||
#define TCC3_CH2 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH3
|
||||
#define TCC3_CH3 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH4
|
||||
#define TCC3_CH4 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH5
|
||||
#define TCC3_CH5 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH6
|
||||
#define TCC3_CH6 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TCC3_CH7
|
||||
#define TCC3_CH7 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TC6_CH0
|
||||
#define TC6_CH0 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TC6_CH1
|
||||
#define TC6_CH1 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TC7_CH0
|
||||
#define TC7_CH0 NOT_ON_TIMER
|
||||
#endif
|
||||
#ifndef TC7_CH1
|
||||
#define TC7_CH1 NOT_ON_TIMER
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#define NUM_WO_ASSOCIATIONS 48
|
||||
|
||||
/*
|
||||
* For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices
|
||||
* Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway?
|
||||
*
|
||||
* Note: only the pins which have timers associated are listed in this table.
|
||||
* You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table.
|
||||
*
|
||||
* See Microchip Technology datasheet DS40001882F-page 30
|
||||
*/
|
||||
struct wo_association WO_associations[] = {
|
||||
|
||||
{ PORTA, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 2, NOT_ON_TIMER, 0, TCC3_CH0, 0},
|
||||
{ PORTA, 3, NOT_ON_TIMER, 0, TCC3_CH1, 1},
|
||||
// PB04, PB05, PB06, PB07 - no timers
|
||||
{ PORTB, 8, TC4_CH0, 0, TCC3_CH6, 6},
|
||||
{ PORTB, 9, TC4_CH1, 1, TCC3_CH7, 7},
|
||||
{ PORTA, 4, TCC0_CH0, 0, TCC3_CH2, 2},
|
||||
{ PORTA, 5, TCC0_CH1, 1, TCC3_CH3, 3},
|
||||
{ PORTA, 6, TCC1_CH0, 0, TCC3_CH4, 4},
|
||||
{ PORTA, 7, TCC1_CH1, 1, TCC3_CH5, 5},
|
||||
{ PORTA, 8, TCC0_CH0, 0, TCC1_CH2, 2},
|
||||
{ PORTA, 9, TCC0_CH1, 1, TCC1_CH3, 3},
|
||||
{ PORTA, 10, TCC1_CH0, 0, TCC0_CH2, 2},
|
||||
{ PORTA, 11, TCC1_CH1, 1, TCC0_CH3, 3},
|
||||
{ PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4},
|
||||
{ PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5},
|
||||
{ PORTB, 12, TC4_CH0, 0, TCC0_CH6, 6},
|
||||
{ PORTB, 13, TC4_CH1, 1, TCC0_CH7, 7},
|
||||
{ PORTB, 14, TC5_CH0, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 15, TC5_CH1, 1, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 12, TCC2_CH0, 0, TCC0_CH6, 6},
|
||||
{ PORTA, 13, TCC2_CH1, 1, TCC0_CH7, 7},
|
||||
{ PORTA, 14, TC3_CH0, 0, TCC0_CH4, 4},
|
||||
{ PORTA, 15, TC3_CH1, 1, TCC0_CH5, 5},
|
||||
{ PORTA, 16, TCC2_CH0, 0, TCC0_CH6, 6},
|
||||
{ PORTA, 17, TCC2_CH1, 1, TCC0_CH7, 7},
|
||||
{ PORTA, 18, TC3_CH0, 0, TCC0_CH2, 2},
|
||||
{ PORTA, 19, TC3_CH1, 1, TCC0_CH3, 3},
|
||||
{ PORTB, 16, TC6_CH0, 0, TCC0_CH4, 4},
|
||||
{ PORTB, 17, TC6_CH1, 1, TCC0_CH5, 5},
|
||||
{ PORTA, 20, TC7_CH0, 0, TCC0_CH6, 6},
|
||||
{ PORTA, 21, TC7_CH1, 1, TCC0_CH7, 7},
|
||||
{ PORTA, 22, TC4_CH0, 0, TCC0_CH4, 4},
|
||||
{ PORTA, 23, TC4_CH1, 1, TCC0_CH5, 5},
|
||||
{ PORTA, 24, TC5_CH0, 0, TCC1_CH2, 2},
|
||||
{ PORTA, 25, TC5_CH1, 1, TCC1_CH3, 3},
|
||||
{ PORTB, 22, TC7_CH0, 0, TCC3_CH0, 0},
|
||||
{ PORTB, 23, TC7_CH1, 1, TCC3_CH1, 1},
|
||||
{ PORTA, 27, NOT_ON_TIMER, 0, TCC3_CH6, 6},
|
||||
{ PORTA, 28, NOT_ON_TIMER, 0, TCC3_CH7, 7},
|
||||
{ PORTA, 30, TCC1_CH0, 0, TCC3_CH4, 4},
|
||||
{ PORTA, 31, TCC1_CH1, 1, TCC3_CH5, 5},
|
||||
{ PORTB, 30, TCC0_CH0, 0, TCC1_CH2, 2},
|
||||
{ PORTB, 31, TCC0_CH1, 1, TCC1_CH3, 3},
|
||||
{ PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 2, TC6_CH0, 0, TCC3_CH2, 2},
|
||||
{ PORTB, 3, TC6_CH1, 1, TCC3_CH3, 3}
|
||||
};
|
||||
wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0};
|
||||
|
||||
|
||||
|
||||
struct wo_association& getWOAssociation(EPortType port, uint32_t pin) {
|
||||
for (int i=0;i<NUM_WO_ASSOCIATIONS;i++) {
|
||||
if (WO_associations[i].port==port && WO_associations[i].pin==pin)
|
||||
return WO_associations[i];
|
||||
}
|
||||
return ASSOCIATION_NOT_FOUND;
|
||||
};
|
||||
|
||||
|
||||
|
||||
EPioType getPeripheralOfPermutation(int permutation, int pin_position) {
|
||||
return ((permutation>>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void syncTCC(Tcc* TCCx) {
|
||||
while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); // Wait for synchronization of registers between the clock domains
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that
|
||||
* any compatible pin combination can be used without having to worry about configuring different
|
||||
* clocks.
|
||||
*/
|
||||
void configureSAMDClock() {
|
||||
|
||||
// TODO investigate using the FDPLL96M clock to get 96MHz timer clocks... this
|
||||
// would enable 48KHz PWM clocks, and setting the frequency between 24Khz with resolution 2000, to 48KHz with resolution 1000
|
||||
|
||||
if (!SAMDClockConfigured) {
|
||||
SAMDClockConfigured = true; // mark clock as configured
|
||||
for (int i=0;i<TCC_INST_NUM;i++) // mark all the TCCs as not yet configured
|
||||
tccConfigured[i] = false;
|
||||
REG_GCLK_GENDIV = GCLK_GENDIV_DIV(1) | // Divide the 48MHz clock source by divisor N=1: 48MHz/1=48MHz
|
||||
GCLK_GENDIV_ID(4); // Select Generic Clock (GCLK) 4
|
||||
while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization
|
||||
|
||||
REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC | // Set the duty cycle to 50/50 HIGH/LOW
|
||||
GCLK_GENCTRL_GENEN | // Enable GCLK4
|
||||
GCLK_GENCTRL_SRC_DFLL48M | // Set the 48MHz clock source
|
||||
// GCLK_GENCTRL_SRC_FDPLL | // Set the 96MHz clock source
|
||||
GCLK_GENCTRL_ID(4); // Select GCLK4
|
||||
while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock...");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configure a TCC unit
|
||||
* pwm_frequency is fixed at 24kHz for now. We could go slower, but going
|
||||
* faster won't be possible without sacrificing resolution.
|
||||
*/
|
||||
void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) {
|
||||
|
||||
long pwm_resolution = (24000000) / pwm_frequency;
|
||||
if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION)
|
||||
pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION;
|
||||
if (pwm_resolution<SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION)
|
||||
pwm_resolution = SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION;
|
||||
// store for later use
|
||||
tccConfig.pwm_res = pwm_resolution;
|
||||
|
||||
// TODO for the moment we ignore the frequency...
|
||||
if (!tccConfigured[tccConfig.tcc.tccn]) {
|
||||
uint32_t GCLK_CLKCTRL_ID_ofthistcc = -1;
|
||||
switch (tccConfig.tcc.tccn>>1) {
|
||||
case 0: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1); break; //GCLK_CLKCTRL_ID_TCC0_TCC1;
|
||||
case 1: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3); break; //GCLK_CLKCTRL_ID_TCC2_TC3;
|
||||
case 2: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5); break; //GCLK_CLKCTRL_ID_TC4_TC5;
|
||||
case 3: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); break;
|
||||
default: return;
|
||||
}
|
||||
|
||||
// Feed GCLK4 to TCC
|
||||
REG_GCLK_CLKCTRL = (uint16_t) GCLK_CLKCTRL_CLKEN | // Enable GCLK4
|
||||
GCLK_CLKCTRL_GEN_GCLK4 | // Select GCLK4
|
||||
GCLK_CLKCTRL_ID_ofthistcc; // Feed GCLK4 to tcc
|
||||
while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization
|
||||
|
||||
tccConfigured[tccConfig.tcc.tccn] = true;
|
||||
|
||||
if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
|
||||
Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo);
|
||||
// disable
|
||||
tc->COUNT8.CTRLA.bit.ENABLE = 0;
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// unfortunately we need the 8-bit counter mode to use the PER register...
|
||||
tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ;
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000...
|
||||
tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ;
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// period is 250, period cannot be higher than 256!
|
||||
tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1;
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// initial duty cycle is 0
|
||||
tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0;
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// enable
|
||||
tc->COUNT8.CTRLA.bit.ENABLE = 1;
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC ");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn);
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
|
||||
|
||||
uint8_t invenMask = ~(1<<tccConfig.tcc.chan); // negate (invert) the signal if needed
|
||||
uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
|
||||
tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
|
||||
syncTCC(tcc); // wait for sync
|
||||
|
||||
tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH; // Set wave form configuration
|
||||
while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync
|
||||
|
||||
if (hw6pwm>0.0) {
|
||||
tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
|
||||
tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
|
||||
tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
|
||||
syncTCC(tcc); // wait for sync
|
||||
}
|
||||
|
||||
tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register
|
||||
while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync
|
||||
|
||||
// set all channels to 0%
|
||||
uint8_t chanCount = (tccConfig.tcc.tccn==1||tccConfig.tcc.tccn==2)?2:4;
|
||||
for (int i=0;i<chanCount;i++) {
|
||||
tcc->CC[i].reg = 0; // start off at 0% duty cycle
|
||||
uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i);
|
||||
while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
|
||||
}
|
||||
|
||||
// Enable TC
|
||||
tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz
|
||||
while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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.print("] pwm res ");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else if (tccConfig.tcc.tccn<TCC_INST_NUM) {
|
||||
// set invert bit for TCC channels in SW 6-PWM...
|
||||
Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
|
||||
|
||||
tcc->CTRLA.bit.ENABLE = 0;
|
||||
while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
|
||||
|
||||
uint8_t invenMask = ~(1<<tccConfig.tcc.chan); // negate (invert) the signal if needed
|
||||
uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
|
||||
tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
|
||||
syncTCC(tcc); // wait for sync
|
||||
|
||||
if (hw6pwm>0.0) {
|
||||
tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
|
||||
tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
|
||||
tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
|
||||
syncTCC(tcc); // wait for sync
|
||||
}
|
||||
|
||||
tcc->CTRLA.bit.ENABLE = 1;
|
||||
while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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.print("] pwm res ");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void writeSAMDDutyCycle(tccConfiguration* info, float dc) {
|
||||
uint8_t tccn = GetTCNumber(info->tcc.chaninfo);
|
||||
uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo);
|
||||
if (tccn<TCC_INST_NUM) {
|
||||
Tcc* tcc = (Tcc*)GetTC(info->tcc.chaninfo);
|
||||
// set via CC
|
||||
// tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc);
|
||||
// uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan);
|
||||
// while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
|
||||
// set via CCB
|
||||
//while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
|
||||
tcc->CCB[chan].reg = (uint32_t)((info->pwm_res-1) * dc);
|
||||
// while ( (tcc->SYNCBUSY.vec.CCB & (0x1<<chan)) > 0 );
|
||||
// tcc->STATUS.vec.CCBV |= (0x1<<chan);
|
||||
// while ( tcc->SYNCBUSY.bit.STATUS > 0 );
|
||||
// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val);
|
||||
// while ( tcc->SYNCBUSY.bit.CTRLB > 0 );
|
||||
}
|
||||
else {
|
||||
Tc* tc = (Tc*)GetTC(info->tcc.chaninfo);
|
||||
tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc);
|
||||
while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,311 @@
|
||||
|
||||
|
||||
#include "./samd_mcu.h"
|
||||
|
||||
|
||||
#if defined(_SAMD51_)||defined(_SAME51_)
|
||||
|
||||
|
||||
|
||||
// expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency.
|
||||
// for custom boards or overclockers you can override it using this define.
|
||||
#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ
|
||||
#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation
|
||||
// 0 6 8 24-bit Yes Yes Yes Yes Yes Yes
|
||||
// 1 4 8 24-bit Yes Yes Yes Yes Yes Yes
|
||||
// 2 3 3 16-bit Yes - Yes - - -
|
||||
// 3 2 2 16-bit Yes - - - - -
|
||||
// 4 2 2 16-bit Yes - - - - -
|
||||
|
||||
|
||||
#define NUM_WO_ASSOCIATIONS 72
|
||||
|
||||
struct wo_association WO_associations[] = {
|
||||
|
||||
{ PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 4, TC0_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 5, TC0_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 6, TC1_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 7, TC1_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTC, 4, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0},
|
||||
// PC05, PC06, PC07 -> no timers
|
||||
{ PORTA, 8, TC0_CH0, 0, TCC0_CH0, 0, TCC1_CH0, 4},
|
||||
{ PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5},
|
||||
{ PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6},
|
||||
{ PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7},
|
||||
{ PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0},
|
||||
{ PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1},
|
||||
{ PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0},
|
||||
{ PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1},
|
||||
{ PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2},
|
||||
{ PORTB, 15, TC5_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 3},
|
||||
{ PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0},
|
||||
{ PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0},
|
||||
{ PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0},
|
||||
{ PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0},
|
||||
{ PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0},
|
||||
{ PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4},
|
||||
{ PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5},
|
||||
{ PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6},
|
||||
{ PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7},
|
||||
{ PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0},
|
||||
{ PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1},
|
||||
{ PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2},
|
||||
{ PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3},
|
||||
{ PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2},
|
||||
{ PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3},
|
||||
{ PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4},
|
||||
{ PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5},
|
||||
{ PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6},
|
||||
{ PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH1, 7},
|
||||
{ PORTC, 16, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0
|
||||
{ PORTC, 17, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1
|
||||
{ PORTC, 18, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2
|
||||
{ PORTC, 19, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0},
|
||||
{ PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0},
|
||||
{ PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0},
|
||||
{ PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH0, 6, NOT_ON_TIMER, 0},
|
||||
{ PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH1, 7, NOT_ON_TIMER, 0},
|
||||
{ PORTD, 20, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTD, 21, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH4, 4},
|
||||
{ PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH5, 5},
|
||||
{ PORTB, 18, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0
|
||||
{ PORTB, 19, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1
|
||||
{ PORTB, 20, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2
|
||||
{ PORTB, 21, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 20, TC7_CH0, 0, TCC1_CH0, 4, TCC0_CH0, 0},
|
||||
{ PORTA, 21, TC7_CH1, 1, TCC1_CH1, 5, TCC0_CH1, 1},
|
||||
{ PORTA, 22, TC4_CH0, 0, TCC1_CH2, 6, TCC0_CH2, 2},
|
||||
{ PORTA, 23, TC4_CH1, 1, TCC1_CH3, 7, TCC0_CH3, 3},
|
||||
{ PORTA, 24, TC5_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, // PDEC0
|
||||
{ PORTA, 25, TC5_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1
|
||||
{ PORTB, 22, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2
|
||||
{ PORTB, 23, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC0
|
||||
{ PORTB, 24, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1
|
||||
{ PORTB, 25, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2
|
||||
{ PORTB, 26, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 27, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 28, NOT_ON_TIMER, 0, TCC1_CH0, 4, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 29, NOT_ON_TIMER, 1, TCC1_CH1, 5, NOT_ON_TIMER, 0},
|
||||
// PC24-PC28, PA27, RESET -> no TC/TCC peripherals
|
||||
{ PORTA, 30, TC6_CH0, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTA, 31, TC6_CH1, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH0, 6},
|
||||
{ PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH1, 7},
|
||||
// PC30, PC31 -> no TC/TCC peripherals
|
||||
{ PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0},
|
||||
{ PORTB, 2, TC6_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0},
|
||||
|
||||
};
|
||||
|
||||
wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0};
|
||||
|
||||
uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM };
|
||||
|
||||
struct wo_association& getWOAssociation(EPortType port, uint32_t pin) {
|
||||
for (int i=0;i<NUM_WO_ASSOCIATIONS;i++) {
|
||||
if (WO_associations[i].port==port && WO_associations[i].pin==pin)
|
||||
return WO_associations[i];
|
||||
}
|
||||
return ASSOCIATION_NOT_FOUND;
|
||||
};
|
||||
|
||||
|
||||
EPioType getPeripheralOfPermutation(int permutation, int pin_position) {
|
||||
return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void syncTCC(Tcc* TCCx) {
|
||||
while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void writeSAMDDutyCycle(tccConfiguration* info, float dc) {
|
||||
uint8_t tccn = GetTCNumber(info->tcc.chaninfo);
|
||||
uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo);
|
||||
if (tccn<TCC_INST_NUM) {
|
||||
Tcc* tcc = (Tcc*)GetTC(info->tcc.chaninfo);
|
||||
// set via CCBUF
|
||||
// while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
|
||||
tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency!
|
||||
}
|
||||
else {
|
||||
// we don't support the TC channels on SAMD51, isn't worth it.
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#define DPLL_CLOCK_NUM 2 // use GCLK2
|
||||
#define PWM_CLOCK_NUM 3 // use GCLK3
|
||||
|
||||
|
||||
/**
|
||||
* Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that
|
||||
* any compatible pin combination can be used without having to worry about configuring different
|
||||
* clocks.
|
||||
*/
|
||||
void configureSAMDClock() {
|
||||
if (!SAMDClockConfigured) {
|
||||
SAMDClockConfigured = true; // mark clock as configured
|
||||
for (int i=0;i<TCC_INST_NUM;i++) // mark all the TCCs as not yet configured
|
||||
tccConfigured[i] = false;
|
||||
|
||||
// GCLK->GENCTRL[DPLL_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1)
|
||||
// | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val);
|
||||
// while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<DPLL_CLOCK_NUM));
|
||||
|
||||
// GCLK->PCHCTRL[1].reg = GCLK_PCHCTRL_GEN(DPLL_CLOCK_NUM)|GCLK_PCHCTRL_CHEN;
|
||||
// while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<DPLL_CLOCK_NUM));
|
||||
|
||||
// OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 0;
|
||||
// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
|
||||
|
||||
// OSCCTRL->Dpll[0].DPLLCTRLB.bit.REFCLK = OSCCTRL_DPLLCTRLB_REFCLK_GCLK_Val;
|
||||
// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
|
||||
// OSCCTRL->Dpll[0].DPLLRATIO.reg = 3;
|
||||
// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
|
||||
|
||||
// OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 1;
|
||||
// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
|
||||
|
||||
GCLK->GENCTRL[PWM_CLOCK_NUM].bit.GENEN = 0;
|
||||
while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
|
||||
|
||||
GCLK->GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC
|
||||
//| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val);
|
||||
| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val);
|
||||
while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock...");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configure a TCC unit
|
||||
* pwm_frequency is fixed at 24kHz for now. We could go slower, but going
|
||||
* faster won't be possible without sacrificing resolution.
|
||||
*/
|
||||
void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) {
|
||||
// TODO for the moment we ignore the frequency...
|
||||
if (!tccConfigured[tccConfig.tcc.tccn]) {
|
||||
uint32_t GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_IDs[tccConfig.tcc.tccn];
|
||||
GCLK->PCHCTRL[GCLK_CLKCTRL_ID_ofthistcc].reg = GCLK_PCHCTRL_GEN(PWM_CLOCK_NUM)|GCLK_PCHCTRL_CHEN;
|
||||
while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
|
||||
}
|
||||
|
||||
if (tccConfig.tcc.tccn<TCC_INST_NUM) {
|
||||
Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
|
||||
|
||||
tcc->CTRLA.bit.ENABLE = 0; //switch off tcc
|
||||
while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
|
||||
|
||||
uint8_t invenMask = ~(1<<tccConfig.tcc.chan); // negate (invert) the signal if needed
|
||||
uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
|
||||
tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
|
||||
syncTCC(tcc); // wait for sync
|
||||
|
||||
// work out pwm resolution for desired frequency and constrain to max/min values
|
||||
long pwm_resolution = (SIMPLEFOC_SAMD51_DPLL_FREQ/2) / pwm_frequency;
|
||||
if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION)
|
||||
pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION;
|
||||
if (pwm_resolution<SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION)
|
||||
pwm_resolution = SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION;
|
||||
// store for later use
|
||||
tccConfig.pwm_res = pwm_resolution;
|
||||
|
||||
if (hw6pwm>0.0) {
|
||||
tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
|
||||
tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
|
||||
tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
|
||||
syncTCC(tcc); // wait for sync
|
||||
}
|
||||
|
||||
if (!tccConfigured[tccConfig.tcc.tccn]) {
|
||||
tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP; // Set wave form configuration - TODO check this... why set like this?
|
||||
while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync
|
||||
|
||||
tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register
|
||||
while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync
|
||||
|
||||
// set all channels to 0%
|
||||
for (int i=0;i<TCC_CHANNEL_COUNT[tccConfig.tcc.tccn];i++) {
|
||||
tcc->CC[i].reg = 0; // start off at 0% duty cycle
|
||||
uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i);
|
||||
while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
|
||||
}
|
||||
}
|
||||
|
||||
// Enable TCC
|
||||
tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz
|
||||
while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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.print("] pwm res ");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution);
|
||||
#endif
|
||||
}
|
||||
else if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
|
||||
//Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo);
|
||||
|
||||
// disable
|
||||
// tc->COUNT8.CTRLA.bit.ENABLE = 0;
|
||||
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// // unfortunately we need the 8-bit counter mode to use the PER register...
|
||||
// tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ;
|
||||
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// // meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000...
|
||||
// tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ;
|
||||
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// // period is 250, period cannot be higher than 256!
|
||||
// tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1;
|
||||
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// // initial duty cycle is 0
|
||||
// tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0;
|
||||
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
// // enable
|
||||
// tc->COUNT8.CTRLA.bit.ENABLE = 1;
|
||||
// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Not initialized: TC ");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn);
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TC units not supported on SAMD51");
|
||||
#endif
|
||||
}
|
||||
|
||||
// set as configured
|
||||
tccConfigured[tccConfig.tcc.tccn] = true;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,903 @@
|
||||
|
||||
|
||||
|
||||
#include "./samd_mcu.h"
|
||||
|
||||
#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Global state
|
||||
*/
|
||||
tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS];
|
||||
uint8_t numTccPinConfigurations = 0;
|
||||
bool SAMDClockConfigured = false;
|
||||
bool tccConfigured[TCC_INST_NUM+TC_INST_NUM];
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Attach the TCC to the pin
|
||||
*/
|
||||
bool attachTCC(tccConfiguration& tccConfig) {
|
||||
if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS)
|
||||
return false;
|
||||
pinMode(tccConfig.pin, OUTPUT);
|
||||
|
||||
pinPeripheral(tccConfig.pin, tccConfig.peripheral);
|
||||
tccPinConfigurations[numTccPinConfigurations++] = tccConfig;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int getPermutationNumber(int pins) {
|
||||
int num = 1;
|
||||
for (int i=0;i<pins;i++)
|
||||
num *= NUM_PIO_TIMER_PERIPHERALS;
|
||||
return num;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Check if the configuration is in use already.
|
||||
*/
|
||||
bool inUse(tccConfiguration& tccConfig) {
|
||||
for (int i=0;i<numTccPinConfigurations;i++) {
|
||||
if (tccPinConfigurations[i].tcc.chaninfo==tccConfig.tcc.chaninfo)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
tccConfiguration* getTccPinConfiguration(uint8_t pin) {
|
||||
for (int i=0;i<numTccPinConfigurations;i++)
|
||||
if (tccPinConfigurations[i].pin==pin)
|
||||
return &tccPinConfigurations[i];
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Get the TCC channel associated with that pin
|
||||
*/
|
||||
tccConfiguration getTCCChannelNr(int pin, EPioType peripheral) {
|
||||
tccConfiguration result;
|
||||
result.pin = pin;
|
||||
result.peripheral = peripheral;
|
||||
result.tcc.tccn = -2;
|
||||
result.tcc.chan = -2;
|
||||
const PinDescription& pinDesc = g_APinDescription[pin];
|
||||
struct wo_association& association = getWOAssociation(pinDesc.ulPort, pinDesc.ulPin);
|
||||
if (association.port==NOT_A_PORT)
|
||||
return result; // could not find the port/pin
|
||||
if (peripheral==PIO_TIMER) {
|
||||
result.tcc.chaninfo = association.tccE;
|
||||
result.wo = association.woE;
|
||||
}
|
||||
else if (peripheral==PIO_TIMER_ALT) {
|
||||
result.tcc.chaninfo = association.tccF;
|
||||
result.wo = association.woF;
|
||||
}
|
||||
#if defined(_SAMD51_)||defined(_SAME51_)
|
||||
else if (peripheral==PIO_TCC_PDEC) {
|
||||
result.tcc.chaninfo = association.tccG;
|
||||
result.wo = association.woG;
|
||||
}
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
bool checkPeripheralPermutationSameTCC6(tccConfiguration& pinAh, tccConfiguration& pinAl, tccConfiguration& pinBh, tccConfiguration& pinBl, tccConfiguration& pinCh, tccConfiguration& pinCl) {
|
||||
if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl))
|
||||
return false;
|
||||
|
||||
if (pinAh.tcc.tccn<0 || pinAh.tcc.tccn!=pinAl.tcc.tccn || pinAh.tcc.tccn!=pinBh.tcc.tccn || pinAh.tcc.tccn!=pinBl.tcc.tccn
|
||||
|| pinAh.tcc.tccn!=pinCh.tcc.tccn || pinAh.tcc.tccn!=pinCl.tcc.tccn || pinAh.tcc.tccn>=TCC_INST_NUM)
|
||||
return false;
|
||||
|
||||
if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan)
|
||||
return false;
|
||||
if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan)
|
||||
return false;
|
||||
if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan)
|
||||
return false;
|
||||
if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan)
|
||||
return false;
|
||||
|
||||
if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan)
|
||||
return false;
|
||||
if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) {
|
||||
for (int i=0;i<num;i++)
|
||||
if (pins[i].tcc.tccn<0)
|
||||
return false;
|
||||
for (int i=0;i<num-1;i++)
|
||||
for (int j=i+1;j<num;j++)
|
||||
if (pins[i].tcc.chaninfo==pins[j].tcc.chaninfo)
|
||||
return false;
|
||||
for (int i=0;i<num;i++)
|
||||
if (inUse(pins[i]))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
bool checkPeripheralPermutationCompatible6(tccConfiguration& pinAh, tccConfiguration& pinAl, tccConfiguration& pinBh, tccConfiguration& pinBl, tccConfiguration& pinCh, tccConfiguration& pinCl) {
|
||||
// check we're valid PWM pins
|
||||
if (pinAh.tcc.tccn<0 || pinAl.tcc.tccn<0 || pinBh.tcc.tccn<0 || pinBl.tcc.tccn<0 || pinCh.tcc.tccn<0 || pinCl.tcc.tccn<0)
|
||||
return false;
|
||||
// only TCC units for 6-PWM
|
||||
if (pinAh.tcc.tccn>=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM
|
||||
|| pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM)
|
||||
return false;
|
||||
|
||||
// check we're not in use
|
||||
if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl))
|
||||
return false;
|
||||
|
||||
// check pins are all different tccs/channels
|
||||
if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo)
|
||||
return false;
|
||||
if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo)
|
||||
return false;
|
||||
if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo)
|
||||
return false;
|
||||
if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo)
|
||||
return false;
|
||||
|
||||
// check H/L pins are on same timer
|
||||
if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn)
|
||||
return false;
|
||||
|
||||
// check H/L pins aren't on both the same timer and wo
|
||||
if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int checkHardware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
|
||||
for (int i=0;i<64;i++) {
|
||||
tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0));
|
||||
tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1));
|
||||
tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2));
|
||||
tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3));
|
||||
tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4));
|
||||
tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5));
|
||||
if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl))
|
||||
return i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int checkSoftware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
|
||||
for (int i=0;i<64;i++) {
|
||||
tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0));
|
||||
tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1));
|
||||
tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2));
|
||||
tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3));
|
||||
tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4));
|
||||
tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5));
|
||||
if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl))
|
||||
return i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int scorePermutation(tccConfiguration pins[], uint8_t num) {
|
||||
uint32_t usedtccs = 0;
|
||||
for (int i=0;i<num;i++)
|
||||
usedtccs |= (1<<pins[i].tcc.tccn);
|
||||
int score = 0;
|
||||
for (int i=0;i<TCC_INST_NUM;i++){
|
||||
if (usedtccs&0x1)
|
||||
score+=1;
|
||||
usedtccs = usedtccs>>1;
|
||||
}
|
||||
for (int i=0;i<TC_INST_NUM;i++){
|
||||
if (usedtccs&0x1)
|
||||
score+=(num+1);
|
||||
usedtccs = usedtccs>>1;
|
||||
}
|
||||
return score;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) {
|
||||
tccConfiguration tccConfs[num];
|
||||
int best = -1;
|
||||
int bestscore = 1000000;
|
||||
for (int i=0;i<(0x1<<num);i++) {
|
||||
for (int j=0;j<num;j++)
|
||||
tccConfs[j] = getTCCChannelNr(pins[j], getPeripheralOfPermutation(i, j));
|
||||
if (checkFunc(tccConfs, num)) {
|
||||
int score = scorePermutation(tccConfs, num);
|
||||
if (score<bestscore) {
|
||||
bestscore = score;
|
||||
best = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configuring PWM frequency, resolution and alignment
|
||||
* - Stepper driver - 2PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* @param pwm_frequency - frequency in hertz - if applicable
|
||||
* @param pinA pinA bldc driver
|
||||
* @param pinB pinB bldc driver
|
||||
*/
|
||||
void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
printAllPinInfos();
|
||||
#endif
|
||||
int pins[2] = { pinA, pinB };
|
||||
int compatibility = checkPermutations(2, pins, checkPeripheralPermutationCompatible);
|
||||
if (compatibility<0) {
|
||||
// no result!
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)),
|
||||
getTCCChannelNr(pinB, getPeripheralOfPermutation(compatibility, 1)) };
|
||||
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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
|
||||
|
||||
// attach pins to timer peripherals
|
||||
attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
|
||||
attachTCC(tccConfs[1]);
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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?
|
||||
// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
|
||||
configureSAMDClock();
|
||||
|
||||
if (pwm_frequency==NOT_SET) {
|
||||
// use default frequency
|
||||
pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
|
||||
}
|
||||
|
||||
// configure the TCC (waveform, top-value, pre-scaler = frequency)
|
||||
configureTCC(tccConfs[0], pwm_frequency);
|
||||
configureTCC(tccConfs[1], pwm_frequency);
|
||||
getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res;
|
||||
getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res;
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
|
||||
#endif
|
||||
|
||||
return; // Someone with a stepper-setup who can test it?
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configuring PWM frequency, resolution and alignment
|
||||
* - BLDC driver - 3PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* SAMD21 will support up to 2 BLDC motors in 3-PWM:
|
||||
* one on TCC0 using 3 of the channels 0,1,2 or 3
|
||||
* one on TCC3 using 3 of the channels 0,1,2 or 3
|
||||
* i.e. 8 different pins can be used, but only 4 different signals (WO[x]) on those 8 pins
|
||||
* WO[0] and WO[4] are the same
|
||||
* WO[1] and WO[5] are the same
|
||||
* WO[2] and WO[6] are the same
|
||||
* WO[3] and WO[7] are the same
|
||||
*
|
||||
* If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x]
|
||||
* signal is on which pin of the Nano. You can drive one motor on TCC0. For other boards, consult their documentation.
|
||||
*
|
||||
* Note:
|
||||
* That's if we want to keep the signals strictly in sync.
|
||||
*
|
||||
* If we can accept out-of-sync PWMs on the different phases, we could drive up to 4 BLDCs in 3-PWM mode,
|
||||
* using all the TCC channels. (TCC0 & TCC3 - 4 channels each, TCC1 & TCC2 - 2 channels each)
|
||||
*
|
||||
* All channels will use the same resolution, prescaler and clock, but they will have different start-times leading
|
||||
* to misaligned signals.
|
||||
*
|
||||
*
|
||||
* @param pwm_frequency - frequency in hertz - if applicable
|
||||
* @param pinA pinA bldc driver
|
||||
* @param pinB pinB bldc driver
|
||||
* @param pinC pinC bldc driver
|
||||
*/
|
||||
void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) {
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
printAllPinInfos();
|
||||
#endif
|
||||
int pins[3] = { pinA, pinB, pinC };
|
||||
int compatibility = checkPermutations(3, pins, checkPeripheralPermutationCompatible);
|
||||
if (compatibility<0) {
|
||||
// no result!
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)),
|
||||
getTCCChannelNr(pinB, getPeripheralOfPermutation(compatibility, 1)),
|
||||
getTCCChannelNr(pinC, getPeripheralOfPermutation(compatibility, 2)) };
|
||||
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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]);
|
||||
#endif
|
||||
|
||||
// attach pins to timer peripherals
|
||||
attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
|
||||
attachTCC(tccConfs[1]);
|
||||
attachTCC(tccConfs[2]);
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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?
|
||||
// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
|
||||
configureSAMDClock();
|
||||
|
||||
if (pwm_frequency==NOT_SET) {
|
||||
// use default frequency
|
||||
pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
|
||||
}
|
||||
|
||||
// configure the TCC (waveform, top-value, pre-scaler = frequency)
|
||||
configureTCC(tccConfs[0], pwm_frequency);
|
||||
configureTCC(tccConfs[1], pwm_frequency);
|
||||
configureTCC(tccConfs[2], pwm_frequency);
|
||||
getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res;
|
||||
getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res;
|
||||
getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res;
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configuring PWM frequency, resolution and alignment
|
||||
* - Stepper driver - 4PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* @param pwm_frequency - frequency in hertz - if applicable
|
||||
* @param pin1A pin1A stepper driver
|
||||
* @param pin1B pin1B stepper driver
|
||||
* @param pin2A pin2A stepper driver
|
||||
* @param pin2B pin2B stepper driver
|
||||
*/
|
||||
void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
printAllPinInfos();
|
||||
#endif
|
||||
int pins[4] = { pin1A, pin1B, pin2A, pin2B };
|
||||
int compatibility = checkPermutations(4, pins, checkPeripheralPermutationCompatible);
|
||||
if (compatibility<0) {
|
||||
// no result!
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, getPeripheralOfPermutation(compatibility, 0)),
|
||||
getTCCChannelNr(pin1B, getPeripheralOfPermutation(compatibility, 1)),
|
||||
getTCCChannelNr(pin2A, getPeripheralOfPermutation(compatibility, 2)),
|
||||
getTCCChannelNr(pin2B, getPeripheralOfPermutation(compatibility, 3)) };
|
||||
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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]);
|
||||
printTCCConfiguration(tccConfs[3]);
|
||||
#endif
|
||||
|
||||
// attach pins to timer peripherals
|
||||
attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
|
||||
attachTCC(tccConfs[1]);
|
||||
attachTCC(tccConfs[2]);
|
||||
attachTCC(tccConfs[3]);
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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?
|
||||
// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
|
||||
configureSAMDClock();
|
||||
|
||||
if (pwm_frequency==NOT_SET) {
|
||||
// use default frequency
|
||||
pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
|
||||
}
|
||||
|
||||
// configure the TCC (waveform, top-value, pre-scaler = frequency)
|
||||
configureTCC(tccConfs[0], pwm_frequency);
|
||||
configureTCC(tccConfs[1], pwm_frequency);
|
||||
configureTCC(tccConfs[2], pwm_frequency);
|
||||
configureTCC(tccConfs[3], pwm_frequency);
|
||||
getTccPinConfiguration(pin1A)->pwm_res = tccConfs[0].pwm_res;
|
||||
getTccPinConfiguration(pin2A)->pwm_res = tccConfs[1].pwm_res;
|
||||
getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res;
|
||||
getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res;
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
|
||||
#endif
|
||||
|
||||
return; // Someone with a stepper-setup who can test it?
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Configuring PWM frequency, resolution and alignment
|
||||
* - BLDC driver - 6PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* SAMD21 will support up to 2 BLDC motors in 6-PWM:
|
||||
* one on TCC0 using 3 of the channels 0,1,2 or 3
|
||||
* one on TCC3 using 3 of the channels 0,1,2 or 3
|
||||
* i.e. 6 out of 8 pins must be used, in the following high/low side pairs:
|
||||
* WO[0] & WO[4] (high side & low side)
|
||||
* WO[1] & WO[5]
|
||||
* WO[2] & WO[6]
|
||||
* WO[3] & WO[7]
|
||||
*
|
||||
* If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x]
|
||||
* signal is on which pin of the Nano. You can drive 1 BLDC on TCC0. For other boards, consult their documentation.
|
||||
*
|
||||
*
|
||||
* @param pwm_frequency - frequency in hertz - if applicable
|
||||
* @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable
|
||||
* @param pinA_h pinA high-side bldc driver
|
||||
* @param pinA_l pinA low-side bldc driver
|
||||
* @param pinB_h pinA high-side bldc driver
|
||||
* @param pinB_l pinA low-side bldc driver
|
||||
* @param pinC_h pinA high-side bldc driver
|
||||
* @param pinC_l pinA low-side bldc driver
|
||||
*
|
||||
* @return 0 if config good, -1 if failed
|
||||
*/
|
||||
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) {
|
||||
// we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
printAllPinInfos();
|
||||
#endif
|
||||
int compatibility = checkHardware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
if (compatibility<0) {
|
||||
compatibility = checkSoftware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
if (compatibility<0) {
|
||||
// no result!
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
|
||||
#endif
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(compatibility, 0));
|
||||
tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(compatibility, 1));
|
||||
tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(compatibility, 2));
|
||||
tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(compatibility, 3));
|
||||
tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(compatibility, 4));
|
||||
tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5));
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: ");
|
||||
printTCCConfiguration(pinAh);
|
||||
printTCCConfiguration(pinAl);
|
||||
printTCCConfiguration(pinBh);
|
||||
printTCCConfiguration(pinBl);
|
||||
printTCCConfiguration(pinCh);
|
||||
printTCCConfiguration(pinCl);
|
||||
#endif
|
||||
|
||||
// attach pins to timer peripherals
|
||||
bool allAttached = true;
|
||||
allAttached |= attachTCC(pinAh); // in theory this can fail, but there is no way to signal it...
|
||||
allAttached |= attachTCC(pinAl);
|
||||
allAttached |= attachTCC(pinBh);
|
||||
allAttached |= attachTCC(pinBl);
|
||||
allAttached |= attachTCC(pinCh);
|
||||
allAttached |= attachTCC(pinCl);
|
||||
if (!allAttached)
|
||||
return -1;
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
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
|
||||
configureSAMDClock();
|
||||
|
||||
if (pwm_frequency==NOT_SET) {
|
||||
// use default frequency
|
||||
pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
|
||||
}
|
||||
|
||||
// configure the TCC(s)
|
||||
configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1);
|
||||
if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo))
|
||||
configureTCC(pinAl, pwm_frequency, true, -1.0);
|
||||
configureTCC(pinBh, pwm_frequency, false, (pinBh.tcc.chaninfo==pinBl.tcc.chaninfo)?dead_zone:-1);
|
||||
if ((pinBh.tcc.chaninfo!=pinBl.tcc.chaninfo))
|
||||
configureTCC(pinBl, pwm_frequency, true, -1.0);
|
||||
configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1);
|
||||
if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo))
|
||||
configureTCC(pinCl, pwm_frequency, true, -1.0);
|
||||
getTccPinConfiguration(pinA_h)->pwm_res = pinAh.pwm_res;
|
||||
getTccPinConfiguration(pinA_l)->pwm_res = pinAh.pwm_res; // use the high phase resolution, in case we didn't set it
|
||||
getTccPinConfiguration(pinB_h)->pwm_res = pinBh.pwm_res;
|
||||
getTccPinConfiguration(pinB_l)->pwm_res = pinBh.pwm_res;
|
||||
getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res;
|
||||
getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res;
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
* - Stepper driver - 2PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* @param dc_a duty cycle phase A [0, 1]
|
||||
* @param dc_b duty cycle phase B [0, 1]
|
||||
* @param pinA phase A hardware pin number
|
||||
* @param pinB phase B hardware pin number
|
||||
*/
|
||||
void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) {
|
||||
tccConfiguration* tccI = getTccPinConfiguration(pinA);
|
||||
writeSAMDDutyCycle(tccI, dc_a);
|
||||
tccI = getTccPinConfiguration(pinB);
|
||||
writeSAMDDutyCycle(tccI, dc_b);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
* - BLDC driver - 3PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* @param dc_a duty cycle phase A [0, 1]
|
||||
* @param dc_b duty cycle phase B [0, 1]
|
||||
* @param dc_c duty cycle phase C [0, 1]
|
||||
* @param pinA phase A hardware pin number
|
||||
* @param pinB phase B hardware pin number
|
||||
* @param pinC phase C hardware pin number
|
||||
*/
|
||||
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) {
|
||||
tccConfiguration* tccI = getTccPinConfiguration(pinA);
|
||||
writeSAMDDutyCycle(tccI, dc_a);
|
||||
tccI = getTccPinConfiguration(pinB);
|
||||
writeSAMDDutyCycle(tccI, dc_b);
|
||||
tccI = getTccPinConfiguration(pinC);
|
||||
writeSAMDDutyCycle(tccI, dc_c);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
* - Stepper driver - 4PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* @param dc_1a duty cycle phase 1A [0, 1]
|
||||
* @param dc_1b duty cycle phase 1B [0, 1]
|
||||
* @param dc_2a duty cycle phase 2A [0, 1]
|
||||
* @param dc_2b duty cycle phase 2B [0, 1]
|
||||
* @param pin1A phase 1A hardware pin number
|
||||
* @param pin1B phase 1B hardware pin number
|
||||
* @param pin2A phase 2A hardware pin number
|
||||
* @param pin2B phase 2B hardware pin number
|
||||
*/
|
||||
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
|
||||
tccConfiguration* tccI = getTccPinConfiguration(pin1A);
|
||||
writeSAMDDutyCycle(tccI, dc_1a);
|
||||
tccI = getTccPinConfiguration(pin2A);
|
||||
writeSAMDDutyCycle(tccI, dc_2a);
|
||||
tccI = getTccPinConfiguration(pin1B);
|
||||
writeSAMDDutyCycle(tccI, dc_1b);
|
||||
tccI = getTccPinConfiguration(pin2B);
|
||||
writeSAMDDutyCycle(tccI, dc_2b);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
|
||||
* - BLDC driver - 6PWM setting
|
||||
* - hardware specific
|
||||
*
|
||||
* Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored
|
||||
* the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side
|
||||
* duty cycle.
|
||||
* No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method...
|
||||
* so use appropriately.
|
||||
*
|
||||
* @param dc_a duty cycle phase A [0, 1]
|
||||
* @param dc_b duty cycle phase B [0, 1]
|
||||
* @param dc_c duty cycle phase C [0, 1]
|
||||
* @param dead_zone duty cycle protection zone [0, 1] - both low and high side low
|
||||
* @param pinA_h phase A high-side hardware pin number
|
||||
* @param pinA_l phase A low-side hardware pin number
|
||||
* @param pinB_h phase B high-side hardware pin number
|
||||
* @param pinB_l phase B low-side hardware pin number
|
||||
* @param pinC_h phase C high-side hardware pin number
|
||||
* @param pinC_l phase C low-side hardware pin number
|
||||
*
|
||||
*/
|
||||
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){
|
||||
tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h);
|
||||
tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l);
|
||||
if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
|
||||
// low-side on a different pin of same TCC - do dead-time in software...
|
||||
float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1));
|
||||
if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
|
||||
writeSAMDDutyCycle(tcc1, dc_a);
|
||||
writeSAMDDutyCycle(tcc2, ls);
|
||||
}
|
||||
else
|
||||
writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly
|
||||
|
||||
tcc1 = getTccPinConfiguration(pinB_h);
|
||||
tcc2 = getTccPinConfiguration(pinB_l);
|
||||
if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
|
||||
float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1));
|
||||
if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
|
||||
writeSAMDDutyCycle(tcc1, dc_b);
|
||||
writeSAMDDutyCycle(tcc2, ls);
|
||||
}
|
||||
else
|
||||
writeSAMDDutyCycle(tcc1, dc_b);
|
||||
|
||||
tcc1 = getTccPinConfiguration(pinC_h);
|
||||
tcc2 = getTccPinConfiguration(pinC_l);
|
||||
if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
|
||||
float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1));
|
||||
if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
|
||||
writeSAMDDutyCycle(tcc1, dc_c);
|
||||
writeSAMDDutyCycle(tcc2, ls);
|
||||
}
|
||||
else
|
||||
writeSAMDDutyCycle(tcc1, dc_c);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
|
||||
/**
|
||||
* Prints a table of pin assignments for your SAMD MCU. Very useful since the
|
||||
* board pinout descriptions and variant.cpp are usually quite wrong, and this
|
||||
* saves you hours of cross-referencing with the datasheet.
|
||||
*/
|
||||
void printAllPinInfos() {
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println();
|
||||
for (uint8_t pin=0;pin<PINS_COUNT;pin++) {
|
||||
const PinDescription& pinDesc = g_APinDescription[pin];
|
||||
wo_association& association = getWOAssociation(pinDesc.ulPort, pinDesc.ulPin);
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Pin ");
|
||||
if (pin<10) SIMPLEFOC_SAMD_DEBUG_SERIAL.print("0");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(pin);
|
||||
switch (pinDesc.ulPort) {
|
||||
case NOT_A_PORT: SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" "); break;
|
||||
case PORTA: SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" PA"); break;
|
||||
case PORTB: SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" PB"); break;
|
||||
case PORTC: SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" PC"); break;
|
||||
#if defined(_SAMD51_)||defined(_SAME51_)
|
||||
case PORTD: SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" PD"); break;
|
||||
#endif
|
||||
}
|
||||
if (pinDesc.ulPin <10) SIMPLEFOC_SAMD_DEBUG_SERIAL.print("0");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(pinDesc.ulPin);
|
||||
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E=");
|
||||
if (association.tccE>=0) {
|
||||
int tcn = GetTCNumber(association.tccE);
|
||||
if (tcn>=TCC_INST_NUM)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" TC");
|
||||
else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn);
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccE));
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woE));
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]");
|
||||
if (tcn<10)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ");
|
||||
}
|
||||
else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" None ");
|
||||
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F=");
|
||||
if (association.tccF>=0) {
|
||||
int tcn = GetTCNumber(association.tccF);
|
||||
if (tcn>=TCC_INST_NUM)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" TC");
|
||||
else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn);
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccF));
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woF));
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]");
|
||||
if (tcn<10)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ");
|
||||
}
|
||||
else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" None ");
|
||||
|
||||
#if defined(_SAMD51_)||defined(_SAME51_)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G=");
|
||||
if (association.tccG>=0) {
|
||||
int tcn = GetTCNumber(association.tccG);
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC");
|
||||
if (tcn<10)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn);
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccG));
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woG));
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
|
||||
}
|
||||
else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None ");
|
||||
#else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println("");
|
||||
#endif
|
||||
|
||||
}
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void printTCCConfiguration(tccConfiguration& info) {
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.pin);
|
||||
if (info.tcc.tccn>=TCC_INST_NUM)
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral");
|
||||
else
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral");
|
||||
switch (info.peripheral) {
|
||||
case PIO_TIMER:
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break;
|
||||
case PIO_TIMER_ALT:
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break;
|
||||
#if defined(_SAMD51_)||defined(_SAME51_)
|
||||
case PIO_TCC_PDEC:
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break;
|
||||
#endif
|
||||
default:
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break;
|
||||
}
|
||||
if (info.tcc.tccn>=0) {
|
||||
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
|
||||
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None");
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,120 @@
|
||||
|
||||
#ifndef SAMD_MCU_H
|
||||
#define SAMD_MCU_H
|
||||
|
||||
|
||||
// uncomment to enable debug output to Serial port
|
||||
// #define SIMPLEFOC_SAMD_DEBUG
|
||||
#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
|
||||
#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
|
||||
#endif
|
||||
|
||||
#include "../hardware_api.h"
|
||||
|
||||
|
||||
#if defined(__SAME51J19A__) || defined(__ATSAME51J19A__)
|
||||
#ifndef _SAME51_
|
||||
#define _SAME51_
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "variant.h"
|
||||
#include "wiring_private.h"
|
||||
|
||||
|
||||
#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION
|
||||
#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000
|
||||
#endif
|
||||
|
||||
#define SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ 24000
|
||||
// arbitrary maximum. On SAMD51 with 120MHz clock this means 2kHz minimum pwm frequency
|
||||
#define SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION 30000
|
||||
// lets not go too low - 400 with clock speed of 120MHz on SAMD51 means 150kHz maximum PWM frequency...
|
||||
// 400 with 48MHz clock on SAMD21 means 60kHz maximum PWM frequency...
|
||||
#define SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION 400
|
||||
// this is the most we can support on the TC units
|
||||
#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250
|
||||
|
||||
#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS
|
||||
#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
struct tccConfiguration {
|
||||
uint8_t pin;
|
||||
EPioType peripheral; // 1=true, 0=false
|
||||
uint8_t wo;
|
||||
union tccChanInfo {
|
||||
struct {
|
||||
int8_t chan;
|
||||
int8_t tccn;
|
||||
};
|
||||
uint16_t chaninfo;
|
||||
} tcc;
|
||||
uint16_t pwm_res;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
struct wo_association {
|
||||
EPortType port;
|
||||
uint32_t pin;
|
||||
ETCChannel tccE;
|
||||
uint8_t woE;
|
||||
ETCChannel tccF;
|
||||
uint8_t woF;
|
||||
#if defined(_SAMD51_)||defined(_SAME51_)
|
||||
ETCChannel tccG;
|
||||
uint8_t woG;
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
|
||||
#if defined(_SAMD21_)
|
||||
#define NUM_PIO_TIMER_PERIPHERALS 2
|
||||
#elif defined(_SAMD51_)||defined(_SAME51_)
|
||||
#define NUM_PIO_TIMER_PERIPHERALS 3
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Global state
|
||||
*/
|
||||
extern struct wo_association WO_associations[];
|
||||
extern uint8_t TCC_CHANNEL_COUNT[];
|
||||
extern tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS];
|
||||
extern uint8_t numTccPinConfigurations;
|
||||
extern bool SAMDClockConfigured;
|
||||
extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM];
|
||||
|
||||
|
||||
|
||||
struct wo_association& getWOAssociation(EPortType port, uint32_t pin);
|
||||
void writeSAMDDutyCycle(tccConfiguration* info, float dc);
|
||||
void configureSAMDClock();
|
||||
void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1);
|
||||
__inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused));
|
||||
EPioType getPeripheralOfPermutation(int permutation, int pin_position);
|
||||
|
||||
#ifdef SIMPLEFOC_SAMD_DEBUG
|
||||
void printTCCConfiguration(tccConfiguration& info);
|
||||
void printAllPinInfos();
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,355 @@
|
||||
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#if defined(_STM32_DEF_)
|
||||
// default pwm parameters
|
||||
#define _PWM_RESOLUTION 12 // 12bit
|
||||
#define _PWM_RANGE 4095.0 // 2^12 -1 = 4095
|
||||
#define _PWM_FREQUENCY 25000 // 25khz
|
||||
#define _PWM_FREQUENCY_MAX 50000 // 50khz
|
||||
|
||||
// 6pwm parameters
|
||||
#define _HARDWARE_6PWM 1
|
||||
#define _SOFTWARE_6PWM 0
|
||||
#define _ERROR_6PWM -1
|
||||
|
||||
|
||||
// setting pwm to hardware pin - instead analogWrite()
|
||||
void _setPwm(int ulPin, uint32_t value, int resolution)
|
||||
{
|
||||
PinName pin = digitalPinToPinName(ulPin);
|
||||
TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
|
||||
uint32_t index = get_timer_index(Instance);
|
||||
HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
|
||||
|
||||
uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
|
||||
HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution);
|
||||
}
|
||||
|
||||
|
||||
// init pin pwm
|
||||
HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin)
|
||||
{
|
||||
PinName pin = digitalPinToPinName(ulPin);
|
||||
TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
|
||||
|
||||
uint32_t index = get_timer_index(Instance);
|
||||
if (HardwareTimer_Handle[index] == NULL) {
|
||||
HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
|
||||
HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
|
||||
HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
|
||||
}
|
||||
HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
|
||||
uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
|
||||
HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin);
|
||||
HT->setOverflow(PWM_freq, HERTZ_FORMAT);
|
||||
HT->pause();
|
||||
HT->refresh();
|
||||
return HT;
|
||||
}
|
||||
|
||||
|
||||
// init high side pin
|
||||
HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin)
|
||||
{
|
||||
return _initPinPWM(PWM_freq, ulPin);
|
||||
}
|
||||
|
||||
// init low side pin
|
||||
HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin)
|
||||
{
|
||||
PinName pin = digitalPinToPinName(ulPin);
|
||||
TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
|
||||
uint32_t index = get_timer_index(Instance);
|
||||
|
||||
if (HardwareTimer_Handle[index] == NULL) {
|
||||
HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
|
||||
HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
|
||||
HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
|
||||
TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
|
||||
sConfigOC.OCMode = TIM_OCMODE_PWM2;
|
||||
sConfigOC.Pulse = 100;
|
||||
sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
|
||||
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
|
||||
#if defined(TIM_OCIDLESTATE_SET)
|
||||
sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET;
|
||||
#endif
|
||||
#if defined(TIM_OCNIDLESTATE_RESET)
|
||||
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
|
||||
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
|
||||
#endif
|
||||
uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
|
||||
HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
|
||||
}
|
||||
HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
|
||||
uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
|
||||
HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin);
|
||||
HT->setOverflow(PWM_freq, HERTZ_FORMAT);
|
||||
HT->pause();
|
||||
HT->refresh();
|
||||
return HT;
|
||||
}
|
||||
|
||||
|
||||
// align the timers to end the init
|
||||
void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3)
|
||||
{
|
||||
HT1->pause();
|
||||
HT1->refresh();
|
||||
HT2->pause();
|
||||
HT2->refresh();
|
||||
HT3->pause();
|
||||
HT3->refresh();
|
||||
HT1->resume();
|
||||
HT2->resume();
|
||||
HT3->resume();
|
||||
}
|
||||
|
||||
// align the timers to end the init
|
||||
void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4)
|
||||
{
|
||||
HT1->pause();
|
||||
HT1->refresh();
|
||||
HT2->pause();
|
||||
HT2->refresh();
|
||||
HT3->pause();
|
||||
HT3->refresh();
|
||||
HT4->pause();
|
||||
HT4->refresh();
|
||||
HT1->resume();
|
||||
HT2->resume();
|
||||
HT3->resume();
|
||||
HT4->resume();
|
||||
}
|
||||
|
||||
// configure hardware 6pwm interface only one timer with inverted channels
|
||||
HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
|
||||
{
|
||||
|
||||
#if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface
|
||||
PinName uhPinName = digitalPinToPinName(pinA_h);
|
||||
PinName ulPinName = digitalPinToPinName(pinA_l);
|
||||
PinName vhPinName = digitalPinToPinName(pinB_h);
|
||||
PinName vlPinName = digitalPinToPinName(pinB_l);
|
||||
PinName whPinName = digitalPinToPinName(pinC_h);
|
||||
PinName wlPinName = digitalPinToPinName(pinC_l);
|
||||
|
||||
TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM);
|
||||
|
||||
uint32_t index = get_timer_index(Instance);
|
||||
|
||||
if (HardwareTimer_Handle[index] == NULL) {
|
||||
HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM));
|
||||
HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
|
||||
HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1;
|
||||
HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
|
||||
((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT);
|
||||
}
|
||||
HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
|
||||
|
||||
HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName);
|
||||
HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName);
|
||||
HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName);
|
||||
HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName);
|
||||
HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName);
|
||||
HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName);
|
||||
|
||||
// dead time is set in nanoseconds
|
||||
uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone;
|
||||
uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
|
||||
LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
|
||||
LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N);
|
||||
|
||||
// Set Trigger out for DMA transfer
|
||||
LL_TIM_SetTriggerOutput(HT->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
|
||||
|
||||
HT->pause();
|
||||
HT->refresh();
|
||||
|
||||
// adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs.
|
||||
HT->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
|
||||
HT->getHandle()->Instance->CNT = TIM1->ARR;
|
||||
|
||||
HT->resume();
|
||||
return HT;
|
||||
#else
|
||||
return nullptr; // return nothing
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// returns 0 if each pair of pwm channels has same channel
|
||||
// returns 1 all the channels belong to the same timer - hardware inverted channels
|
||||
// returns -1 if neither - avoid configuring - error!!!
|
||||
int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
|
||||
PinName nameAH = digitalPinToPinName(pinA_h);
|
||||
PinName nameAL = digitalPinToPinName(pinA_l);
|
||||
PinName nameBH = digitalPinToPinName(pinB_h);
|
||||
PinName nameBL = digitalPinToPinName(pinB_l);
|
||||
PinName nameCH = digitalPinToPinName(pinC_h);
|
||||
PinName nameCL = digitalPinToPinName(pinC_l);
|
||||
int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM));
|
||||
int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM));
|
||||
int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM));
|
||||
int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM));
|
||||
int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM));
|
||||
int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM));
|
||||
|
||||
#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface
|
||||
|
||||
if(tim1 == tim2 && tim3==tim4 && tim5==tim6)
|
||||
return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
|
||||
else
|
||||
return _ERROR_6PWM; // might be error avoid configuration
|
||||
#else // the rest of stm32 boards
|
||||
|
||||
if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6)
|
||||
return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer
|
||||
else if(tim1 == tim2 && tim3==tim4 && tim5==tim6)
|
||||
return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
|
||||
else
|
||||
return _ERROR_6PWM; // might be error avoid configuration
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
// center-aligned frequency is uses two periods
|
||||
pwm_frequency *=2;
|
||||
|
||||
HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA);
|
||||
HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB);
|
||||
// allign the timers
|
||||
_alignPWMTimers(HT1, HT2, HT2);
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
// center-aligned frequency is uses two periods
|
||||
pwm_frequency *=2;
|
||||
|
||||
HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA);
|
||||
HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB);
|
||||
HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC);
|
||||
// allign the timers
|
||||
_alignPWMTimers(HT1, HT2, HT3);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _PWM_FREQUENCY; // default frequency 25khz
|
||||
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
|
||||
// center-aligned frequency is uses two periods
|
||||
pwm_frequency *=2;
|
||||
|
||||
HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA);
|
||||
HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB);
|
||||
HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC);
|
||||
HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD);
|
||||
// allign the timers
|
||||
_alignPWMTimers(HT1, HT2, HT3, HT4);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,4095]
|
||||
_setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
|
||||
_setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,4095]
|
||||
_setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
|
||||
_setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
|
||||
_setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
|
||||
}
|
||||
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,4095]
|
||||
_setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION);
|
||||
_setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION);
|
||||
_setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION);
|
||||
_setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 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){
|
||||
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 |%0kHz max
|
||||
// center-aligned frequency is uses two periods
|
||||
pwm_frequency *=2;
|
||||
|
||||
// find configuration
|
||||
int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
// configure accordingly
|
||||
switch(config){
|
||||
case _ERROR_6PWM:
|
||||
return -1; // fail
|
||||
break;
|
||||
case _HARDWARE_6PWM:
|
||||
_initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
break;
|
||||
case _SOFTWARE_6PWM:
|
||||
HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
|
||||
_initPinPWMLow(pwm_frequency, pinA_l);
|
||||
HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
|
||||
_initPinPWMLow(pwm_frequency, pinB_l);
|
||||
HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
|
||||
_initPinPWMLow(pwm_frequency, pinC_l);
|
||||
_alignPWMTimers(HT1, HT2, HT3);
|
||||
break;
|
||||
}
|
||||
return 0; // success
|
||||
}
|
||||
|
||||
// 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){
|
||||
// find configuration
|
||||
int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
|
||||
// set pwm accordingly
|
||||
switch(config){
|
||||
case _HARDWARE_6PWM:
|
||||
_setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
|
||||
_setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
|
||||
_setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
|
||||
break;
|
||||
case _SOFTWARE_6PWM:
|
||||
_setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
_setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
_setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
_setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
_setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
_setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,77 @@
|
||||
#include "../hardware_api.h"
|
||||
|
||||
#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);
|
||||
analogWriteFrequency(pin, freq);
|
||||
}
|
||||
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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);
|
||||
}
|
||||
|
||||
// function setting the high pwm frequency to the supplied pins
|
||||
// - 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 = _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);
|
||||
_setHighFrequency(pwm_frequency, pinD);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
}
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pinA, 255.0f*dc_a);
|
||||
analogWrite(pinB, 255.0f*dc_b);
|
||||
analogWrite(pinC, 255.0f*dc_c);
|
||||
}
|
||||
|
||||
// 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){
|
||||
// transform duty cycle from [0,1] to [0,255]
|
||||
analogWrite(pin1A, 255.0f*dc_1a);
|
||||
analogWrite(pin1B, 255.0f*dc_1b);
|
||||
analogWrite(pin2A, 255.0f*dc_2a);
|
||||
analogWrite(pin2B, 255.0f*dc_2b);
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in new issue
Block a user