main.c 7.69 KB
Newer Older
Jake Read's avatar
Jake Read committed
1 2 3 4 5 6 7 8 9 10 11
/*
* atkrouter.c
*
* Created: 6/17/2018 2:48:08 PM
* Author : Jake
*/

#include <avr/io.h>
#include <avr/interrupt.h>
#include "hardware.h"
#include "fastmath.h"
12
#include "sinelut.h"
Jake Read's avatar
Jake Read committed
13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46

// first setup all the pins
// want six step commutation, or sinpwm on encoder reading? 

void clock_init(void){
	OSC.XOSCCTRL = OSC_XOSCSEL_XTAL_256CLK_gc | OSC_FRQRANGE_12TO16_gc; // select external source
	OSC.CTRL = OSC_XOSCEN_bm; // enable external source
	while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for external
	OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC0_bm | OSC_PLLFAC1_bm; // select external osc for pll, do pll = source * 3
	//OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC1_bm; // pll = source * 2 for 32MHz std clock
	OSC.CTRL |= OSC_PLLEN_bm; // enable PLL
	while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL to be ready
	CCP = CCP_IOREG_gc; // enable protected register change
	CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL for main clock
}

void uarts_init(void){
	// UP0
	rb_init(&up0rbrx);
	rb_init(&up0rbtx);
	pin_init(&up0rxled, &PORTE, PIN4_bm, 4, 1);
	pin_init(&up0txled, &PORTE, PIN5_bm, 5, 1);
	uart_init(&up0, &USARTE1, &PORTE, PIN6_bm, PIN7_bm, &up0rbrx, &up0rbtx, &up0rxled, &up0txled);
	//PORTE.PIN7CTRL = PORT_SRLEN_bm;
	uart_start(&up0, SYSTEM_BAUDA, SYSTEM_BAUDB);
	
	ups[0] = &up0;
}

void atkps_init(void){
	atkport_init(&atkp0, 0, &up0);
}

void encoder_init(void){
Jake Read's avatar
Jake Read committed
47
	// startup SPI
Jake Read's avatar
Jake Read committed
48 49
	pin_init(&spiEncCsPin, &PORTD, PIN4_bm, 4, 1);
	spi_init(&spiEncoder, &USARTD1, &PORTD, PIN6_bm, PIN7_bm, PIN5_bm, &spiEncCsPin);
Jake Read's avatar
Jake Read committed
50 51 52
	spi_start(&spiEncoder, 1);
	// startup object 
	ams5047_init(&ams5047, &spiEncoder);
Jake Read's avatar
Jake Read committed
53 54
}

55 56 57 58 59 60 61 62 63 64 65 66 67 68
#define ENC_RESOLUTION 16835

// a handful of constants, probably need many less
// divisor is # of poles on motor / 2 (for each n/s pair, one set of 3 coils per full rotation)
const static uint16_t enc_resolution = ENC_RESOLUTION;
const static uint8_t enc_reverse = 1; // 1 or 0, for reverse op
const static uint16_t twoPi_enc = 2340; //ENC_RESOLUTION / 7; // divisor is # poles / 2
const static uint16_t pi_enc = 1170;
const static int16_t enc_offset = -600; // start at midpoint - adjust -ve or +ve from

static uint16_t enc_reading = 0;
static uint16_t phase_target = 0;
// 2^14 = 16,384

Jake Read's avatar
Jake Read committed
69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84
void pwm_periods(uint16_t peru, uint16_t perv, uint16_t perw){
	// check overrun
	(peru > 1024) ? peru = 1024 : (0);
	(perv > 1024) ? perv = 1024 : (0);
	(perw > 1024) ? perw = 1024 : (0);
	// hi is lo, per xmega, let's undo this
	peru = 1024 - peru;
	perv = 1024 - perv;
	perw = 1024 - perw;
	// A: 3, B: 2, C: 1 - channels to board
	TCC0.CCABUFL = (uint8_t) perw;
	TCC0.CCABUFH = (uint8_t) (perw >> 8);
	TCC0.CCBBUFL = (uint8_t) perv;
	TCC0.CCBBUFH = (uint8_t) (perv >> 8);
	TCC0.CCCBUFL = (uint8_t) peru;
	TCC0.CCCBUFH = (uint8_t) (peru >> 8);
Jake Read's avatar
Jake Read committed
85 86
}

87 88 89 90 91 92 93 94 95
void pwm_by_offset(int16_t ofu, int16_t ofv, int16_t ofw){
	// +ve offset to spend more time with hi-side off, signals are complimentary
	uint16_t peru = 512 + ofu;
	uint16_t perv = 512 + ofv;
	uint16_t perw = 512 + ofw;
	// now through business
	pwm_periods(peru, perv, perw);
}

96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112
void pwm_by_sin_duty(uint16_t phase, float duty){
	// phases respective of home
	int16_t pu = phase;
	(pu >= twoPi_enc) ? (pu -= twoPi_enc) : (0);
	int16_t pv = phase + 780;
	(pv >= twoPi_enc) ? (pv -= twoPi_enc) : (0);
	int16_t pw = phase + 1560;
	(pw >= twoPi_enc) ? (pw -= twoPi_enc) : (0);
	
	// zero-set offset
	int16_t peru = ((sinelut[pu] - 512) * duty) + 512;
	int16_t perv = ((sinelut[pv] - 512) * duty) + 512;
	int16_t perw = ((sinelut[pw] - 512) * duty) + 512;
	
	pwm_periods(peru, perv, perw);
}

Jake Read's avatar
Jake Read committed
113
void pwm_init(void){
Jake Read's avatar
Jake Read committed
114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137
	// setup awex etc
	
	pin_init(&lo1, &PORTC, PIN4_bm, 4, 1);
	pin_init(&hi1, &PORTC, PIN5_bm, 5, 1);
	pin_init(&lo2, &PORTC, PIN2_bm, 2, 1);
	pin_init(&hi2, &PORTC, PIN3_bm, 3, 1);
	pin_init(&lo3, &PORTC, PIN0_bm, 0, 1);
	pin_init(&hi3, &PORTC, PIN1_bm, 1, 1);
	
	// compare and capture at value
	uint16_t per = 1024; // at DIV1, 1024 period is 23.5kHz
	// write low first, bc bussing / xmega 8-bit oddities cc datasheet @ 3.11
	uint8_t perl = (uint8_t) per;
	uint8_t perh = (uint8_t) (per >> 8);
	
	TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
	TCC0.PERBUFL = perl;
	TCC0.PERBUFH = perh;
	
	// turnt to dual-slope pwm to have center aligned, and eventually sampling on top event
	
	TCC0.CTRLB = TC_WGMODE_DS_T_gc;// | (1 << 7) | (1 << 6) | (1 << 5); // dual slope, and enable channels a, b, c for capture
	
	AWEXC.CTRL = AWEX_DTICCAEN_bm | AWEX_DTICCBEN_bm | AWEX_DTICCCEN_bm;
Jake Read's avatar
Jake Read committed
138
	AWEXC.DTBOTHBUF = 2; // four counts of pwm clock for deadtime
Jake Read's avatar
Jake Read committed
139 140
	AWEXC.OUTOVEN = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5);
	
141
	pwm_periods(0, 0, 0);
Jake Read's avatar
Jake Read committed
142 143 144
}

void drv_init(void){
145 146 147 148 149 150 151 152 153 154 155
	// mode pins
	pin_init(&drvEnPin, &PORTD, PIN1_bm, 1, 1);
	pin_init(&drvModePwm, &PORTB, PIN6_bm, 6, 1);
	pin_init(&drvModeGain, &PORTC, PIN6_bm, 6, 1);
	pin_init(&drvDcCal, &PORTB, PIN7_bm, 7, 1);
	// status
	pin_init(&drvFault, &PORTC, PIN7_bm, 7, 0);
	pin_init(&drvOCTW, &PORTD, PIN0_bm, 0, 0);
	
	// setup drv8302 mode
	pin_clear(&drvModePwm); // low for 6-channel pwm, hi and lo sides from uc
156
	pin_set(&drvModeGain); // low for 10v/v, hi for 40v/v current sense gains
157 158 159 160 161 162 163 164 165 166
	pin_clear(&drvDcCal); // turn DC cal off, we turn this high to set midpoint on amps
	pin_clear(&drvEnPin); // disable the gate driver, to start. also broken by no/go hardware switch 
}

void drv_enable(void){
	pin_set(&drvEnPin);
}

void drv_disable(void){
	pin_clear(&drvEnPin);
Jake Read's avatar
Jake Read committed
167 168 169
}

void tickers_init(void){
Jake Read's avatar
Jake Read committed
170
	// sets up two timers
Jake Read's avatar
Jake Read committed
171 172
	
	// compare and capture at value
Jake Read's avatar
Jake Read committed
173
	uint16_t perStartSpeed = 1024; 
Jake Read's avatar
Jake Read committed
174
	// write low first, bc bussing / xmega 8-bit oddities cc datasheet @ 3.11
175 176
	uint8_t perSl = (uint8_t) perStartSpeed;
	uint8_t perSh = (uint8_t) (perStartSpeed >> 8);
Jake Read's avatar
Jake Read committed
177 178
	
	// turn on TCC0
Jake Read's avatar
Jake Read committed
179
	TCD0.CTRLA = TC_CLKSEL_DIV8_gc;
180 181
	TCD0.PERBUFL = perSl;
	TCD0.PERBUFH = perSh;
Jake Read's avatar
Jake Read committed
182 183
	
	// set cca interrupt on
Jake Read's avatar
Jake Read committed
184
	TCD0.INTCTRLA = TC_OVFINTLVL_HI_gc;
185
	
Jake Read's avatar
Jake Read committed
186
	/*
187 188
	// and a reasonable speed for acceleration ticking
	
189
	uint16_t perAccelRate = 800;
190 191 192
	uint8_t perAl = (uint8_t) perAccelRate;
	uint8_t perAh = (uint8_t) (perAccelRate >> 8);
	
Jake Read's avatar
Jake Read committed
193
	// another ticker to execute accel
194 195 196
	TCD1.CTRLA = TC_CLKSEL_DIV64_gc;
	TCD1.PERBUFL = perAl;
	TCD1.PERBUFH = perAh;
Jake Read's avatar
Jake Read committed
197
	
Jake Read's avatar
Jake Read committed
198
	TCD1.INTCTRLA = TC_OVFINTLVL_HI_gc;
Jake Read's avatar
Jake Read committed
199
	*/
Jake Read's avatar
Jake Read committed
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216
}

int main(void)
{
	clock_init();
	uarts_init();
	atkps_init();
	
	// enable interrupts
	sei();
	PMIC.CTRL |= PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;

	pin_init(&stlclk, &PORTE, PIN1_bm, 1, 1);
	pin_init(&stlerr, &PORTE, PIN0_bm, 0, 1);
	pin_set(&stlerr);
	pin_set(&stlclk);
	
Jake Read's avatar
Jake Read committed
217 218
	// startup encoder
	encoder_init();
219
	// start timers for commutation, accel tickers 
Jake Read's avatar
Jake Read committed
220
	tickers_init();
Jake Read's avatar
Jake Read committed
221
	
222
	// start pwm system
Jake Read's avatar
Jake Read committed
223
	pwm_init();
224 225
	// initialize the bldc state structure
	bldc_init(&bldc);
Jake Read's avatar
Jake Read committed
226 227
	// on startup, speed (if clcomm) and duty
	//bldc_setTargetSpeed(&bldc, 1000);
228
	// bldc_setDuty(&bldc, 10);
229 230 231 232
	// startup the driver
	drv_init();
	// and enable the gate
	drv_enable();
Jake Read's avatar
Jake Read committed
233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249

	// runtime globals
	uint32_t tck = 0;

	while (1)
	{
		atkport_scan(&atkp0, 2);
		// just... as fast as we can 
 		tck++;
 		// this modulo op is slow AF
 		// that means streamlining atkport_scan without modulos is probably a rad thing
    	if(!(fastModulo(tck, 4096))){
     		pin_toggle(&stlclk);
     	}
	}
}

250
// commutation timer
Jake Read's avatar
Jake Read committed
251
ISR(TCD0_OVF_vect){
Jake Read's avatar
Jake Read committed
252 253 254 255
	// CL 6-step Commutate
	
	// get encoder reading
	ams5047_read(&ams5047, &enc_reading);
256 257 258 259 260
	// reversal if
	enc_reverse ? enc_reading = enc_resolution - enc_reading : (0);
	// target phase expressed in 0 - 2PI, where 2PI in terms of an encoder counts' worth of ticks
	// we add the encoder's resolution here so that we can safely have negative encoder offsets
	phase_target = (enc_resolution + enc_reading + enc_offset + pi_enc) % twoPi_enc;
261
	
262 263
	// we need to compute the respective pwm positions given the phase target 
	pwm_by_sin_duty(phase_target, 0.04);
Jake Read's avatar
Jake Read committed
264 265 266 267 268 269 270 271 272
}

ISR(USARTE1_RXC_vect){
	uart_rxhandler(&up0);
}

ISR(USARTE1_DRE_vect){
	uart_txhandler(&up0);
}