main.c 4.89 KB
Newer Older
Jake Read's avatar
Jake Read committed
1
2
3
4
5
6
7
8
9
/*
 * mkbldcdriver-v03.c
 *
 * Created: 1/14/2018 8:20:39 PM
 * Author : Jake
 */ 


#include "sam.h"
10
11
#include "pwm_foc.h"
#include "pin.h"
Jake Read's avatar
Jake Read committed
12
13
14
#include "ringbuffer.h"
#include "uartport.h"
#include "spiport.h"
15
#include "hardware.h"
Jake Read's avatar
Jake Read committed
16
#include "sinelut.h"
Jake Read's avatar
Jake Read committed
17
18
19
20
#include <stdio.h>

// stlb is PA23

Jake Read's avatar
Jake Read committed
21
22
23
24
25
26
27
28
29
30
31
32
uint32_t encoder_read(void){
	uint8_t data_tx[2] = {0xFF, 0xFF}; // read command: 1s, noop: 0s
	uint8_t data_rx[2];
	spi_txrxchars_polled(&spi_encoder, data_tx, 2, data_rx);
	data_tx[0] = 0x00;
	data_tx[1] = 0x00;
	spi_txrxchars_polled(&spi_encoder, data_tx, 2, data_rx);
	
	uint32_t data = (data_rx[0] & 0b00111111) << 8 | data_rx[1];
	return data;
}

Jake Read's avatar
Jake Read committed
33
34
35
36
int main(void)
{
    /* Initialize the SAM system */
    SystemInit();
Jake Read's avatar
Jake Read committed
37
	SysTick_Config(2500000);
Jake Read's avatar
Jake Read committed
38
	
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
	// LED
	stlb = pin_new(&PORT->Group[0], 23);
	pin_output(&stlb);
	
	// interrupts
	__enable_irq();
	NVIC_EnableIRQ(SERCOM4_0_IRQn); //up1tx
	NVIC_EnableIRQ(SERCOM4_2_IRQn); //up1rx
	NVIC_EnableIRQ(SERCOM5_0_IRQn);
	NVIC_EnableIRQ(SERCOM5_2_IRQn);
	
	// Rinbuffers for UARTs
	rb_init(&up1_rbrx);
	rb_init(&up1_rbtx);
	rb_init(&up1_rbrx);
	rb_init(&up1_rbtx);
	
	// UARTS 
	// UP1:
	// SERCOM4 | RX: PB13 | TX: PB12
	// UP2:
	// SERCOM5 | RX: PB3 | TX: PB2
	up1 = uart_new(SERCOM4, &PORT->Group[1], &up1_rbrx, &up1_rbtx, 12, 13, HARDWARE_IS_APBD, HARDWARE_ON_PERIPHERAL_C); 
	MCLK->APBDMASK.reg |= MCLK_APBDMASK_SERCOM4;
	uart_init(&up1, 6, SERCOM4_GCLK_ID_CORE, 63018); // baud: 45402 for 921600, 63018 for 115200
	up2 = uart_new(SERCOM5, &PORT->Group[1], &up2_rbrx, &up2_rbtx, 3, 2, HARDWARE_IS_APBD, HARDWARE_ON_PERIPHERAL_D);
	MCLK->APBDMASK.reg |= MCLK_APBDMASK_SERCOM5;
	uart_init(&up2, 7, SERCOM5_GCLK_ID_CORE, 63018);
Jake Read's avatar
Jake Read committed
67
68
69
70
71
72
73
74
75
76
77
78
	
	// AS5147 SPI
	// SERCOM0
	// MISO: SER0, PA4
	// SCK: SER1, PA5
	// CSN: SER2, PA6
	// MOSI: SER3, PA7
	// AS5147 is CPOL = 0, CPHA = 1, 16 bit words (use 1 32 bit word, 2nd half will be reply: has to load)
	spi_encoder = spi_new(SERCOM0, &PORT->Group[0], 4, 7, 5, 6, HARDWARE_IS_APBA, HARDWARE_ON_PERIPHERAL_D);
	MCLK->APBAMASK.reg |= MCLK_APBAMASK_SERCOM0;
	spi_init(&spi_encoder, 8, SERCOM0_GCLK_ID_CORE, 6, 0, 2, 0, 1, 0, 0);
	
79
80
	// check all PWM lines
	// pins for enable, fault, pwmmode, etc
Jake Read's avatar
Jake Read committed
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
	pwmsetup_foc();
	
	// DRV Outputs
	// EN_GATE PA19: high to drive gates
	// M_PWM PA20: drive low for 6 channel pwm, high for 3 channel
	// DC_CAL PB16: drive high to calibrate
	// M_GAIN PB17: drive low, gain = 10, high, gain + 40
	en_gate = pin_new(&PORT->Group[0], 19);
	pin_output(&en_gate);
	pin_clear(&en_gate);
	m_pwm = pin_new(&PORT->Group[0], 20);
	pin_output(&m_pwm);
	pin_clear(&m_pwm);
	dc_cal = pin_new(&PORT->Group[1], 16);
	pin_output(&dc_cal);
	pin_clear(&dc_cal);
	m_gain = pin_new(&PORT->Group[1], 17);
	pin_output(&m_gain);
	pin_clear(&m_gain);
Jake Read's avatar
Jake Read committed
100
	
Jake Read's avatar
Jake Read committed
101
102
103
104
105
106
107
	// DRV Inputs
	// FAULT PA21: input, open drain when active, use pullup
	// OCTW PA22: input, open drain when active, use pullup
	fault = pin_new(&PORT->Group[0], 21);
	pin_input(&fault);
	octw = pin_new(&PORT->Group[0], 22);
	pin_input(&octw);
Jake Read's avatar
Jake Read committed
108
	
109
110
	// do closed loop sinucommutate, following old ATSAMS70 code
	// do BLDC closed, open loop
Jake Read's avatar
Jake Read committed
111
	
Jake Read's avatar
Jake Read committed
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
	// enable or don't
	//pin_clear(&en_gate);
	pin_set(&en_gate);
	
	// other modes
	pin_clear(&m_pwm);
	pin_clear(&m_gain);
	pin_clear(&dc_cal);
	
	uint32_t resolution = 16384;
	uint32_t modulo = 2340;
	uint32_t offset = 800;
	uint32_t reverse = 1;
	uint32_t scalar = 42; // of 255
	
	uint32_t reading;
	uint32_t elecpos;
		
	uint32_t phaseu, pwmu;
	uint32_t phasev, pwmv;
	uint32_t phasew, pwmw;
Jake Read's avatar
Jake Read committed
133
	
Jake Read's avatar
Jake Read committed
134
135
    while (1) 
    {
Jake Read's avatar
Jake Read committed
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
		// read encoder & get electric position (relative phase period, not rotation period)
		reading = encoder_read();		
		if(reverse){
			reading = resolution - reading;
		}
		elecpos = (reading + offset) % modulo;
		
		// not LUT'd yet
		phaseu = elecpos / (modulo / 1024);
		phasev = elecpos / (modulo / 1024) + 341;
		phasew = elecpos / (modulo / 1024) + 682;
		
		(phaseu > 1023) ? (phaseu -= 1023) : (0);
		(phasev > 1023) ? (phasev -= 1023) : (0);
		(phasew > 1023) ? (phasew -= 1023) : (0);
		
		pwmu = sinelut[phaseu] * scalar / 255 + 1;
		pwmv = sinelut[phasev] * scalar / 255 + 1;
		pwmw = sinelut[phasew] * scalar / 255 + 1;
		
		pwmupdate_foc(pwmu,pwmv,pwmw);
Jake Read's avatar
Jake Read committed
157
158
159
    }
}

Jake Read's avatar
Jake Read committed
160
161
162
163
164
165
166
167
168
169
170
// ok, we're commutating
/*
 - ticking: it's a PWM update error, occasionally setting some values to 100%
 - implement a timer to commutate at a set frequency
 - work through the mess of values above, go faster, cleaner
 - basic search for reverse, offset, modulo etc would be cool, hard (probably)
 - verify all gates are opening
*/

uint8_t loopcnt;

Jake Read's avatar
Jake Read committed
171
void SysTick_Handler(void){
172
	pin_toggle(&stlb);
Jake Read's avatar
Jake Read committed
173
174
175
176
177
178
179
	loopcnt ++;
	uart_sendchar_buffered(&up1, loopcnt);
	uint32_t reading = encoder_read();
	uint8_t d1 = reading >> 8;
	uint8_t d2 = reading;
	uart_sendchar_buffered(&up1, d1);
	uart_sendchar_buffered(&up1, d2);
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
}

void SERCOM4_0_Handler(void){
	uart_txhandler(&up1);
}

void SERCOM4_2_Handler(void){
	uart_rxhandler(&up1);
}

void SERCOM5_0_Handler(void){
	uart_txhandler(&up2);
}

void SERCOM5_2_Handler(void){
	uart_rxhandler(&up2);
}