Skip to content

Commit 366d8d5

Browse files
askuricaskuric
committed
FEAT Teensy 3.x inital implementation of 6PWM
1 parent 9f6e5fd commit 366d8d5

File tree

3 files changed

+228
-5
lines changed

3 files changed

+228
-5
lines changed

‎src/drivers/hardware_specific/teensy3_mcu.cpp

Lines changed: 216 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,216 @@
1+
#include "teensy_mcu.h"
2+
3+
// if defined
4+
// - Teensy 3.0 MK20DX128
5+
// - Teensy 3.1/3.2 MK20DX256
6+
// - Teensy 3.5 MK20DX128
7+
// - Teensy LC MKL26Z64
8+
// - Teensy 3.5 MK64FX512
9+
// - Teensy 3.6 MK66FX1M0
10+
#if defined(__arm__) && defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__))
11+
12+
13+
// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c
14+
#if defined(__MK20DX128__)
15+
#define FTM0_CH0_PIN 22
16+
#define FTM0_CH1_PIN 23
17+
#define FTM0_CH2_PIN 9
18+
#define FTM0_CH3_PIN 10
19+
#define FTM0_CH4_PIN 6
20+
#define FTM0_CH5_PIN 20
21+
#define FTM0_CH6_PIN 21
22+
#define FTM0_CH7_PIN 5
23+
#define FTM1_CH0_PIN 3
24+
#define FTM1_CH1_PIN 4
25+
#elif defined(__MK20DX256__)
26+
#define FTM0_CH0_PIN 22
27+
#define FTM0_CH1_PIN 23
28+
#define FTM0_CH2_PIN 9
29+
#define FTM0_CH3_PIN 10
30+
#define FTM0_CH4_PIN 6
31+
#define FTM0_CH5_PIN 20
32+
#define FTM0_CH6_PIN 21
33+
#define FTM0_CH7_PIN 5
34+
#define FTM1_CH0_PIN 3
35+
#define FTM1_CH1_PIN 4
36+
#define FTM2_CH0_PIN 32
37+
#define FTM2_CH1_PIN 25
38+
#elif defined(__MKL26Z64__)
39+
#define FTM0_CH0_PIN 22
40+
#define FTM0_CH1_PIN 23
41+
#define FTM0_CH2_PIN 9
42+
#define FTM0_CH3_PIN 10
43+
#define FTM0_CH4_PIN 6
44+
#define FTM0_CH5_PIN 20
45+
#define FTM1_CH0_PIN 16
46+
#define FTM1_CH1_PIN 17
47+
#define FTM2_CH0_PIN 3
48+
#define FTM2_CH1_PIN 4
49+
#elif defined(__MK64FX512__)
50+
#define FTM0_CH0_PIN 22
51+
#define FTM0_CH1_PIN 23
52+
#define FTM0_CH2_PIN 9
53+
#define FTM0_CH3_PIN 10
54+
#define FTM0_CH4_PIN 6
55+
#define FTM0_CH5_PIN 20
56+
#define FTM0_CH6_PIN 21
57+
#define FTM0_CH7_PIN 5
58+
#define FTM1_CH0_PIN 3
59+
#define FTM1_CH1_PIN 4
60+
#define FTM2_CH0_PIN 29
61+
#define FTM2_CH1_PIN 30
62+
#define FTM3_CH0_PIN 2
63+
#define FTM3_CH1_PIN 14
64+
#define FTM3_CH2_PIN 7
65+
#define FTM3_CH3_PIN 8
66+
#define FTM3_CH4_PIN 35
67+
#define FTM3_CH5_PIN 36
68+
#define FTM3_CH6_PIN 37
69+
#define FTM3_CH7_PIN 38
70+
#elif defined(__MK66FX1M0__)
71+
#define FTM0_CH0_PIN 22
72+
#define FTM0_CH1_PIN 23
73+
#define FTM0_CH2_PIN 9
74+
#define FTM0_CH3_PIN 10
75+
#define FTM0_CH4_PIN 6
76+
#define FTM0_CH5_PIN 20
77+
#define FTM0_CH6_PIN 21
78+
#define FTM0_CH7_PIN 5
79+
#define FTM1_CH0_PIN 3
80+
#define FTM1_CH1_PIN 4
81+
#define FTM2_CH0_PIN 29
82+
#define FTM2_CH1_PIN 30
83+
#define FTM3_CH0_PIN 2
84+
#define FTM3_CH1_PIN 14
85+
#define FTM3_CH2_PIN 7
86+
#define FTM3_CH3_PIN 8
87+
#define FTM3_CH4_PIN 35
88+
#define FTM3_CH5_PIN 36
89+
#define FTM3_CH6_PIN 37
90+
#define FTM3_CH7_PIN 38
91+
#define TPM1_CH0_PIN 16
92+
#define TPM1_CH1_PIN 17
93+
#endif
94+
95+
int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const int Ch, const int Cl){
96+
97+
if((Ah == FTM0_CH0_PIN && Al == FTM0_CH1_PIN) ||
98+
(Ah == FTM0_CH2_PIN && Al == FTM0_CH3_PIN) ||
99+
(Ah == FTM0_CH4_PIN && Al == FTM0_CH5_PIN) ){
100+
if((Bh == FTM0_CH0_PIN && Bl == FTM0_CH1_PIN) ||
101+
(Bh == FTM0_CH2_PIN && Bl == FTM0_CH3_PIN) ||
102+
(Bh == FTM0_CH4_PIN && Bl == FTM0_CH5_PIN) ){
103+
if((Ch == FTM0_CH0_PIN && Cl == FTM0_CH1_PIN) ||
104+
(Ch == FTM0_CH2_PIN && Cl == FTM0_CH3_PIN) ||
105+
(Ch == FTM0_CH4_PIN && Cl == FTM0_CH5_PIN) ){
106+
// timer FTM0
107+
return 0;
108+
}
109+
}
110+
}
111+
112+
#ifdef FTM3_SC // if the board has FTM3 timer
113+
if((Ah == FTM3_CH0_PIN && Al == FTM3_CH1_PIN) ||
114+
(Ah == FTM3_CH2_PIN && Al == FTM3_CH3_PIN) ||
115+
(Ah == FTM3_CH4_PIN && Al == FTM3_CH5_PIN) ){
116+
if((Bh == FTM3_CH0_PIN && Bl == FTM3_CH1_PIN) ||
117+
(Bh == FTM3_CH2_PIN && Bl == FTM3_CH3_PIN) ||
118+
(Bh == FTM3_CH4_PIN && Bl == FTM3_CH5_PIN) ){
119+
if((Ch == FTM3_CH0_PIN && Cl == FTM3_CH1_PIN) ||
120+
(Ch == FTM3_CH2_PIN && Cl == FTM3_CH3_PIN) ||
121+
(Ch == FTM3_CH4_PIN && Cl == FTM3_CH5_PIN) ){
122+
// timer FTM3
123+
return 3;
124+
}
125+
}
126+
}
127+
#endif
128+
129+
return -1;
130+
131+
}
132+
133+
134+
// function setting the high pwm frequency to the supplied pins
135+
// - Stepper motor - 6PWM setting
136+
// - hardware specific
137+
void* _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) {
138+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
139+
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
140+
unsigned long pwm_freq = 4*pwm_frequency; // center-aligned pwm has 4 times lower freq
141+
_setHighFrequency(pwm_freq, pinA_h);
142+
_setHighFrequency(pwm_freq, pinA_l);
143+
_setHighFrequency(pwm_freq, pinB_h);
144+
_setHighFrequency(pwm_freq, pinB_l);
145+
_setHighFrequency(pwm_freq, pinC_h);
146+
_setHighFrequency(pwm_freq, pinC_l);
147+
148+
GenericDriverParams* params = new GenericDriverParams {
149+
.pins = { pinA_h,pinA_l, pinB_h,pinB_l, pinC_h, pinC_l },
150+
.pwm_frequency = pwm_frequency
151+
};
152+
153+
154+
int timer = _findTimer(pinA_h,pinA_l,pinB_h,pinB_l,pinC_h,pinC_l);
155+
if(timer<0) return SIMPLEFOC_DRIVER_INIT_FAILED;
156+
157+
158+
159+
// find the best combination of prescalers and counter value
160+
double dead_time = dead_zone/pwm_freq;
161+
int prescaler = 1; // initial prescaler (1,4 or 16)
162+
double count = 1; // inital count (1 - 63)
163+
for (; prescaler<=16; prescaler*=4){
164+
count = dead_time*((double)F_CPU)/((double)prescaler);
165+
if(count < 64) break; // found the solution
166+
}
167+
count = _constrain(count, 1, 63);
168+
169+
// configure the timer
170+
if(timer==0){
171+
// Configure FTM0
172+
// // inverting and deadtime insertion for FTM1
173+
FTM0_COMBINE = 0x00121212; // 0x2 - complemetary mode, 0x1 - dead timer insertion enabled
174+
175+
// Deadtime config
176+
FTM0_DEADTIME = (int)count; // set counter - 1-63
177+
FTM0_DEADTIME |= ((prescaler>1) << 7) | ((prescaler>4) << 6); // set prescaler (0b01 - 1, 0b10 - 4, 0b11 - 16)
178+
179+
// configure center aligned PWM
180+
FTM0_SC = 0x00000028; // 0x2 - center-alignment, 0x8 - fixed clock freq
181+
}else if(timer==3){
182+
// Configure FTM3
183+
// inverting and deadtime insertion for FTM1
184+
FTM3_COMBINE = 0x00121212; // 0x2 - complemetary mode, 0x1 - dead timer insertion enabled
185+
186+
// Deadtime config
187+
FTM3_DEADTIME = (int)count; // set counter - 1-63
188+
FTM3_DEADTIME |= ((prescaler>1) << 7) | ((prescaler>4) << 6); // set prescaler (0b01 - 1, 0b10 - 4, 0b11 - 16)
189+
190+
// configure center aligned PWM
191+
FTM3_SC = 0x00000028; // 0x2 - center-alignment, 0x8 - fixed clock freq
192+
}
193+
194+
return params;
195+
}
196+
197+
198+
199+
// function setting the pwm duty cycle to the hardware
200+
// - Stepper motor - 6PWM setting
201+
// - hardware specific
202+
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){
203+
// transform duty cycle from [0,1] to [0,255]
204+
// phase A
205+
analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
206+
analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_a);
207+
208+
// phase B
209+
analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_b);
210+
analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_b);
211+
212+
// phase C
213+
analogWrite(((GenericDriverParams*)params)->pins[4], 255.0f*dc_c);
214+
analogWrite(((GenericDriverParams*)params)->pins[5], 255.0f*dc_c);
215+
}
216+
#endif

‎src/drivers/hardware_specific/teensy_mcu.cpp

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

33
#if defined(__arm__) && defined(CORE_TEENSY)
44

5-
#define _PWM_FREQUENCY 25000 // 25khz
6-
#define _PWM_FREQUENCY_MAX 50000 // 50khz
7-
85
// configure High PWM frequency
96
void _setHighFrequency(const long freq, const int pin){
107
analogWrite(pin, 0);
@@ -76,7 +73,6 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i
7673
}
7774

7875

79-
8076
// function setting the pwm duty cycle to the hardware
8177
// - Stepper motor - 2PWM setting
8278
// - hardware speciffic

‎src/drivers/hardware_specific/teensy_mcu.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
#include "../hardware_api.h"
2+
3+
#if defined(__arm__) && defined(CORE_TEENSY)
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+
11+
#endif

0 commit comments

Comments
 (0)