Skip to content

Commit 28fb9de

Browse files
runger1101001runger1101001
committed
Merge branch 'Polyphe-master' into dev
2 parents 748ff92 + 236b955 commit 28fb9de

File tree

1 file changed

+353
-0
lines changed

1 file changed

+353
-0
lines changed

‎src/drivers/hardware_specific/nrf52_mcu.cpp

Lines changed: 353 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,353 @@
1+
#include "../hardware_api.h"
2+
3+
#if defined(NRF52_SERIES)
4+
5+
#define PWM_CLK (16000000)
6+
#define PWM_FREQ (40000)
7+
#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ)
8+
#define PWM_MAX_FREQ (62500)
9+
#define DEAD_ZONE (250) // in ns
10+
#define DEAD_TIME (DEAD_ZONE / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM
11+
12+
#ifdef NRF_PWM3
13+
#define PWM_COUNT 4
14+
#else
15+
#define PWM_COUNT 3
16+
#endif
17+
18+
// empty motor slot
19+
#define _EMPTY_SLOT (-0xAA)
20+
#define _TAKEN_SLOT (-0x55)
21+
22+
int pwm_range;
23+
float dead_time;
24+
25+
static NRF_PWM_Type* pwms[PWM_COUNT] = {
26+
NRF_PWM0,
27+
NRF_PWM1,
28+
NRF_PWM2,
29+
#ifdef NRF_PWM3
30+
NRF_PWM3
31+
#endif
32+
};
33+
34+
typedef struct {
35+
int pinA;
36+
NRF_PWM_Type* mcpwm;
37+
uint16_t mcpwm_channel_sequence[4];
38+
} bldc_3pwm_motor_slots_t;
39+
40+
typedef struct {
41+
int pin1A;
42+
NRF_PWM_Type* mcpwm;
43+
uint16_t mcpwm_channel_sequence[4];
44+
} stepper_motor_slots_t;
45+
46+
typedef struct {
47+
int pinAH;
48+
NRF_PWM_Type* mcpwm1;
49+
NRF_PWM_Type* mcpwm2;
50+
uint16_t mcpwm_channel_sequence[8];
51+
} bldc_6pwm_motor_slots_t;
52+
53+
// define bldc motor slots array
54+
bldc_3pwm_motor_slots_t nrf52_bldc_3pwm_motor_slots[4] = {
55+
{_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be PWM0
56+
{_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 2nd motor will be PWM1
57+
{_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 3rd motor will be PWM2
58+
{_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 4th motor will be PWM3
59+
};
60+
61+
// define stepper motor slots array
62+
stepper_motor_slots_t nrf52_stepper_motor_slots[4] = {
63+
{_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be on PWM0
64+
{_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 1st motor will be on PWM1
65+
{_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 1st motor will be on PWM2
66+
{_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 1st motor will be on PWM3
67+
};
68+
69+
// define BLDC motor slots array
70+
bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] = {
71+
{_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1
72+
{_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2
73+
};
74+
75+
// configuring high frequency pwm timer
76+
void _configureHwPwm(NRF_PWM_Type* mcpwm1, NRF_PWM_Type* mcpwm2){
77+
78+
mcpwm1->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
79+
mcpwm1->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
80+
mcpwm1->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos);
81+
mcpwm1->COUNTERTOP = pwm_range; //pwm freq.
82+
mcpwm1->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
83+
mcpwm1->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
84+
mcpwm1->SEQ[0].REFRESH = 0;
85+
mcpwm1->SEQ[0].ENDDELAY = 0;
86+
87+
if(mcpwm1 != mcpwm2){
88+
mcpwm2->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
89+
mcpwm2->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
90+
mcpwm2->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos);
91+
mcpwm2->COUNTERTOP = pwm_range; //pwm freq.
92+
mcpwm2->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
93+
mcpwm2->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
94+
mcpwm2->SEQ[0].REFRESH = 0;
95+
mcpwm2->SEQ[0].ENDDELAY = 0;
96+
}else{
97+
mcpwm1->MODE = (PWM_MODE_UPDOWN_Up << PWM_MODE_UPDOWN_Pos);
98+
}
99+
}
100+
101+
// function setting the high pwm frequency to the supplied pins
102+
// - BLDC motor - 3PWM setting
103+
// - hardware speciffic
104+
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
105+
106+
if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
107+
else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256
108+
109+
pwm_range = (PWM_CLK / pwm_frequency);
110+
111+
int pA = g_ADigitalPinMap[pinA];
112+
int pB = g_ADigitalPinMap[pinB];
113+
int pC = g_ADigitalPinMap[pinC];
114+
115+
// determine which motor are we connecting
116+
// and set the appropriate configuration parameters
117+
int slot_num;
118+
for(slot_num = 0; slot_num < 4; slot_num++){
119+
if(nrf52_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
120+
nrf52_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
121+
break;
122+
}
123+
}
124+
// disable all the slots with the same MCPWM
125+
if(slot_num < 2){
126+
// slot 0 of the stepper
127+
nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
128+
// slot 0 of the 6pwm bldc
129+
nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
130+
//NRF_PPI->CHEN &= ~1UL;
131+
}else{
132+
// slot 1 of the stepper
133+
nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
134+
// slot 0 of the 6pwm bldc
135+
nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
136+
//NRF_PPI->CHEN &= ~2UL;
137+
}
138+
139+
// configure pwm outputs
140+
141+
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
142+
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
143+
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
144+
145+
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
146+
nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
147+
148+
// configure the pwm
149+
_configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm);
150+
}
151+
152+
// function setting the high pwm frequency to the supplied pins
153+
// - Stepper motor - 4PWM setting
154+
// - hardware speciffic
155+
void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
156+
157+
if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
158+
else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256
159+
160+
pwm_range = (PWM_CLK / pwm_frequency);
161+
162+
int pA = g_ADigitalPinMap[pinA];
163+
int pB = g_ADigitalPinMap[pinB];
164+
int pC = g_ADigitalPinMap[pinC];
165+
int pD = g_ADigitalPinMap[pinD];
166+
167+
// determine which motor are we connecting
168+
// and set the appropriate configuration parameters
169+
int slot_num;
170+
for(slot_num = 0; slot_num < 4; slot_num++){
171+
if(nrf52_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
172+
nrf52_stepper_motor_slots[slot_num].pin1A = pinA;
173+
break;
174+
}
175+
}
176+
// disable all the slots with the same MCPWM
177+
if( slot_num < 2 ){
178+
// slots 0 and 1 of the 3pwm bldc
179+
nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
180+
// slot 0 of the 6pwm bldc
181+
nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
182+
//NRF_PPI->CHEN &= ~1UL;
183+
}else{
184+
// slots 2 and 3 of the 3pwm bldc
185+
nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
186+
// slot 1 of the 6pwm bldc
187+
nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
188+
//NRF_PPI->CHEN &= ~2UL;
189+
}
190+
191+
// configure pwm outputs
192+
193+
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
194+
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
195+
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
196+
nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[3] = pD;
197+
198+
nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_stepper_motor_slots[slot_num].mcpwm_channel_sequence[0];
199+
nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
200+
201+
// configure the pwm
202+
_configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm);
203+
}
204+
205+
// function setting the pwm duty cycle to the hardware
206+
// - BLDC motor - 3PWM setting
207+
// - hardware speciffic
208+
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
209+
// determine which motor slot is the motor connected to
210+
for(int i = 0; i < 4; i++){
211+
if(nrf52_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found
212+
// se the PWM on the slot timers
213+
// transform duty cycle from [0,1] to [0,range]
214+
215+
nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000;
216+
nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000;
217+
nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000;
218+
219+
nrf52_bldc_3pwm_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1;
220+
break;
221+
}
222+
}
223+
}
224+
225+
// function setting the pwm duty cycle to the hardware
226+
// - Stepper motor - 4PWM setting
227+
// - hardware speciffic
228+
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A){
229+
// determine which motor slot is the motor connected to
230+
for(int i = 0; i < 4; i++){
231+
if(nrf52_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found
232+
// se the PWM on the slot timers
233+
// transform duty cycle from [0,1] to [0,range]
234+
235+
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000;
236+
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000;
237+
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000;
238+
nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000;
239+
240+
nrf52_stepper_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1;
241+
break;
242+
}
243+
}
244+
}
245+
246+
/* Configuring PWM frequency, resolution and alignment
247+
// - BLDC driver - 6PWM setting
248+
// - hardware specific
249+
*/
250+
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){
251+
252+
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
253+
else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256
254+
255+
pwm_range = (PWM_CLK / pwm_frequency);
256+
pwm_range /= 2; // scale the frequency (centered PWM)
257+
258+
if (dead_zone != NOT_SET){
259+
dead_time = dead_zone/2;
260+
}else{
261+
dead_time = DEAD_TIME/2;
262+
}
263+
264+
int pA_l = g_ADigitalPinMap[pinA_l];
265+
int pA_h = g_ADigitalPinMap[pinA_h];
266+
int pB_l = g_ADigitalPinMap[pinB_l];
267+
int pB_h = g_ADigitalPinMap[pinB_h];
268+
int pC_l = g_ADigitalPinMap[pinC_l];
269+
int pC_h = g_ADigitalPinMap[pinC_h];
270+
271+
272+
// determine which motor are we connecting
273+
// and set the appropriate configuration parameters
274+
int slot_num;
275+
for(slot_num = 0; slot_num < 2; slot_num++){
276+
if(nrf52_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
277+
nrf52_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
278+
break;
279+
}
280+
}
281+
// if no slots available
282+
if(slot_num >= 2) return -1;
283+
284+
// disable all the slots with the same MCPWM
285+
if( slot_num == 0 ){
286+
// slots 0 and 1 of the 3pwm bldc
287+
nrf52_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
288+
nrf52_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
289+
// slot 0 and 1 of the stepper
290+
nrf52_stepper_motor_slots[0].pin1A = _TAKEN_SLOT;
291+
nrf52_stepper_motor_slots[1].pin1A = _TAKEN_SLOT;
292+
}else{
293+
// slots 2 and 3 of the 3pwm bldc
294+
nrf52_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
295+
nrf52_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
296+
// slot 1 of the stepper
297+
nrf52_stepper_motor_slots[2].pin1A = _TAKEN_SLOT;
298+
nrf52_stepper_motor_slots[3].pin1A = _TAKEN_SLOT;
299+
}
300+
301+
// Configure pwm outputs
302+
303+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[0] = pA_h;
304+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[1] = pA_l;
305+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[2] = pB_h;
306+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[3] = pB_l;
307+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
308+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].CNT = 4;
309+
310+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[0] = pC_h;
311+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[1] = pC_l;
312+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[4];
313+
nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].CNT = 4;
314+
315+
// Initializing the PPI peripheral for sync the pwm slots
316+
317+
NRF_PPI->CH[slot_num].EEP = (uint32_t)&NRF_EGU0->EVENTS_TRIGGERED[0];
318+
NRF_PPI->CH[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->TASKS_SEQSTART[0];
319+
NRF_PPI->FORK[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->TASKS_SEQSTART[0];
320+
NRF_PPI->CHEN = 1UL << slot_num;
321+
322+
// configure the pwm type
323+
_configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2);
324+
325+
// return
326+
return 0;
327+
}
328+
329+
/* Function setting the duty cycle to the pwm pin
330+
// - BLDC driver - 6PWM setting
331+
// - hardware specific
332+
*/
333+
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){
334+
for(int i = 0; i < 2; i++){
335+
if(nrf52_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
336+
// se the PWM on the slot timers
337+
// transform duty cycle from [0,1] to [0,range]
338+
339+
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000;
340+
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range);
341+
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000;
342+
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range);
343+
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000;
344+
nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range);
345+
346+
NRF_EGU0->TASKS_TRIGGER[0] = 1;
347+
break;
348+
}
349+
}
350+
}
351+
352+
353+
#endif

0 commit comments

Comments
 (0)