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