Skip to content

Commit 9f689db

Browse files
committed
added initial support for portenta, intial support for esp8266 + leonardo + esp32 separation from esp8266
1 parent d226061 commit 9f689db

File tree

8 files changed

+641
-12
lines changed

8 files changed

+641
-12
lines changed

src/current_sense/hardware_specific/esp32_adc_driver.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#include "esp32_adc_driver.h"
22

3-
#ifdef ESP_H
3+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
44

55
#include "freertos/FreeRTOS.h"
66
#include "freertos/task.h"

src/current_sense/hardware_specific/esp32_adc_driver.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55

66
#include "Arduino.h"
77

8-
#ifdef ESP_H
8+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
99
/*
1010
* Get ADC value for pin
1111
* */

src/current_sense/hardware_specific/esp32_mcu.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#include "../hardware_api.h"
22
#include "../../drivers/hardware_api.h"
33

4-
#ifdef ESP_H
4+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
55

66
#include "driver/mcpwm.h"
77
#include "soc/mcpwm_reg.h"

src/drivers/hardware_specific/atmega32u4_mcu.cpp

Lines changed: 20 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ void _pinHighFrequency(const int pin){
1717
TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1
1818
else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer
1919
// PLL Configuration
20-
PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz
20+
PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
2121
TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
2222
TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
2323

@@ -93,11 +93,8 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
9393

9494

9595

96-
// Configuring PWM frequency, resolution and alignment
97-
// - BLDC driver - 6PWM setting
98-
// - hardware specific
99-
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) {
100-
96+
// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
97+
int _configureComplementaryPair(int pinH, int pinL) {
10198
if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
10299
// configure the pwm phase-corrected mode
103100
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
@@ -114,7 +111,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
114111
else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
115112
}else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){
116113
// PLL Configuration
117-
PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20) // Use 96MHz / 1.5 = 64MHz
114+
PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
118115
TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
119116
TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
120117

@@ -132,6 +129,22 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
132129
return 0;
133130
}
134131

132+
133+
// Configuring PWM frequency, resolution and alignment
134+
// - BLDC driver - 6PWM setting
135+
// - hardware specific
136+
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) {
137+
_UNUSED(pwm_frequency);
138+
_UNUSED(dead_zone);
139+
// High PWM frequency
140+
// - always max 32kHz
141+
int ret_flag = 0;
142+
ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
143+
ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
144+
ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
145+
return ret_flag; // returns -1 if not well configured
146+
}
147+
135148
// function setting the
136149
void _setPwmPair(int pinH, int pinL, float val, int dead_time)
137150
{

src/drivers/hardware_specific/esp32_mcu.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#include "../hardware_api.h"
22

3-
#if defined(ESP_H)
3+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
44

55
#include "driver/mcpwm.h"
66
#include "soc/mcpwm_reg.h"
Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
#include "../hardware_api.h"
2+
3+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266)
4+
5+
#define _PWM_FREQUENCY 25000 // 25khz
6+
#define _PWM_FREQUENCY_MAX 50000 // 50khz
7+
8+
// configure High PWM frequency
9+
void _setHighFrequency(const long freq, const int pin){
10+
analogWrite(pin, 0);
11+
analogWriteFreq(freq);
12+
}
13+
14+
15+
// function setting the high pwm frequency to the supplied pins
16+
// - Stepper motor - 2PWM setting
17+
// - hardware speciffic
18+
void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
19+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
20+
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
21+
_setHighFrequency(pwm_frequency, pinA);
22+
_setHighFrequency(pwm_frequency, pinB);
23+
}
24+
25+
// function setting the high pwm frequency to the supplied pins
26+
// - BLDC motor - 3PWM setting
27+
// - hardware speciffic
28+
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
29+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
30+
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
31+
_setHighFrequency(pwm_frequency, pinA);
32+
_setHighFrequency(pwm_frequency, pinB);
33+
_setHighFrequency(pwm_frequency, pinC);
34+
}
35+
36+
// function setting the high pwm frequency to the supplied pins
37+
// - Stepper motor - 4PWM setting
38+
// - hardware speciffic
39+
void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
40+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
41+
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
42+
_setHighFrequency(pwm_frequency, pinA);
43+
_setHighFrequency(pwm_frequency, pinB);
44+
_setHighFrequency(pwm_frequency, pinC);
45+
_setHighFrequency(pwm_frequency, pinD);
46+
}
47+
48+
// function setting the pwm duty cycle to the hardware
49+
// - Stepper motor - 2PWM setting
50+
// - hardware speciffic
51+
void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){
52+
// transform duty cycle from [0,1] to [0,255]
53+
analogWrite(pinA, 255.0f*dc_a);
54+
analogWrite(pinB, 255.0f*dc_b);
55+
}
56+
// function setting the pwm duty cycle to the hardware
57+
// - BLDC motor - 3PWM setting
58+
// - hardware speciffic
59+
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
60+
// transform duty cycle from [0,1] to [0,255]
61+
analogWrite(pinA, 255.0f*dc_a);
62+
analogWrite(pinB, 255.0f*dc_b);
63+
analogWrite(pinC, 255.0f*dc_c);
64+
}
65+
66+
// function setting the pwm duty cycle to the hardware
67+
// - Stepper motor - 4PWM setting
68+
// - hardware speciffic
69+
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
70+
// transform duty cycle from [0,1] to [0,255]
71+
analogWrite(pin1A, 255.0f*dc_1a);
72+
analogWrite(pin1B, 255.0f*dc_1b);
73+
analogWrite(pin2A, 255.0f*dc_2a);
74+
analogWrite(pin2B, 255.0f*dc_2b);
75+
}
76+
77+
#endif

src/drivers/hardware_specific/generic_mcu.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#include "../hardware_api.h"
22

33
// if the mcu doen't have defiend analogWrite
4-
#if defined(ESP_H)
4+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
55
__attribute__((weak)) void analogWrite(uint8_t pin, int value){ };
66
#endif
77

0 commit comments

Comments
 (0)