main.c 9.49 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

// first setup all the pins
15
// want six step commutation, or sinpwm on encoder reading?
Jake Read's avatar
Jake Read committed
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

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
	spi_start(&spiEncoder, 1);
51
	// startup object
Jake Read's avatar
Jake Read committed
52
	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

69
void pwm_sintype_periods(uint16_t peru, uint16_t perv, uint16_t perw){
Jake Read's avatar
Jake Read committed
70 71 72 73 74 75 76 77 78 79 80 81 82 83 84
	// 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
void pwm_sintype_by_offset(int16_t ofu, int16_t ofv, int16_t ofw){
88 89 90 91 92
	// +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
93
	pwm_sintype_periods(peru, perv, perw);
94 95
}

96
void pwm_sintype_by_sin_duty(uint16_t phase, float duty){
97 98 99 100 101 102 103 104 105 106 107 108 109
	// 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;
	
110
	pwm_sintype_periods(peru, perv, perw);
111 112
}

113 114 115 116 117 118 119 120 121 122 123 124 125 126 127
void pwm_sixstep(uint16_t lo1, uint16_t hi1, uint16_t lo2, uint16_t hi2, uint16_t lo3, uint16_t hi3){
	TCC1.CCABUFL = (uint8_t) lo1;
	TCC1.CCABUFH = (uint8_t) (lo1 >> 8);
	TCC1.CCBBUFL = (uint8_t) hi1;
	TCC1.CCBBUFH = (uint8_t) (hi1 >> 8);
	
	TCC0.CCCBUFL = (uint8_t) lo2;
	TCC0.CCCBUFH = (uint8_t) (lo2 >> 8);
	TCC0.CCDBUFL = (uint8_t) hi2;
	TCC0.CCDBUFH = (uint8_t) (hi2 >> 8);
	
	TCC0.CCABUFL = (uint8_t) lo3;
	TCC0.CCABUFH = (uint8_t) (lo3 >> 8);
	TCC0.CCBBUFL = (uint8_t) hi3;
	TCC0.CCBBUFH = (uint8_t) (hi3 >> 8);
Jake Read's avatar
Jake Read committed
128 129
}

Jake Read's avatar
Jake Read committed
130
void pwm_init(void){
Jake Read's avatar
Jake Read committed
131 132
	// setup awex etc
	
133 134 135 136 137 138 139 140
	pin_init(&lo1, &PORTC, PIN4_bm, 4, 1); // TCC1 OC1A
	pin_init(&hi1, &PORTC, PIN5_bm, 5, 1); // TCC1 OC1B
	
	pin_init(&lo2, &PORTC, PIN2_bm, 2, 1); // TCC0 OC0C
	pin_init(&hi2, &PORTC, PIN3_bm, 3, 1); // TCC0 OC0D
	
	pin_init(&lo3, &PORTC, PIN0_bm, 0, 1); // TCC0 OC0A
	pin_init(&hi3, &PORTC, PIN1_bm, 1, 1); // TCC0 OC0B 
Jake Read's avatar
Jake Read committed
141 142 143 144 145 146 147
	
	// 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);
	
148 149
	// PWM for 6-step commutation

Jake Read's avatar
Jake Read committed
150
	TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
151 152
	TCC1.CTRLA = TC_CLKSEL_DIV1_gc;
	
Jake Read's avatar
Jake Read committed
153 154 155
	TCC0.PERBUFL = perl;
	TCC0.PERBUFH = perh;
	
156 157 158 159 160 161 162 163
	TCC1.PERBUFL = perl;
	TCC1.PERBUFH = perh;
	
	TCC0.CTRLB = TC_WGMODE_DS_T_gc | (1 << 7) | (1 << 6) | (1 << 5) | (1 << 4);
	TCC1.CTRLB = TC_WGMODE_DS_T_gc | (1 << 5) | (1 << 4);
	
	/*
	// PWM for three complimentary pairs:
Jake Read's avatar
Jake Read committed
164 165 166 167 168
	// 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
169
	AWEXC.DTBOTHBUF = 2; // four counts of pwm clock for deadtime
Jake Read's avatar
Jake Read committed
170 171
	AWEXC.OUTOVEN = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5);
	
172
	pwm_periods(0, 0, 0);
173
	*/ 
Jake Read's avatar
Jake Read committed
174 175 176
}

void drv_init(void){
177 178 179 180 181 182 183 184 185 186 187
	// 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
188
	pin_set(&drvModeGain); // low for 10v/v, hi for 40v/v current sense gains
189
	pin_clear(&drvDcCal); // turn DC cal off, we turn this high to set midpoint on amps
190
	pin_clear(&drvEnPin); // disable the gate driver, to start. also broken by no/go hardware switch
191 192 193 194 195 196 197 198
}

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

void drv_disable(void){
	pin_clear(&drvEnPin);
Jake Read's avatar
Jake Read committed
199 200 201
}

void tickers_init(void){
Jake Read's avatar
Jake Read committed
202
	// sets up two timers
Jake Read's avatar
Jake Read committed
203 204
	
	// compare and capture at value
205
	uint16_t perStartSpeed = 1024;
Jake Read's avatar
Jake Read committed
206
	// write low first, bc bussing / xmega 8-bit oddities cc datasheet @ 3.11
207 208
	uint8_t perSl = (uint8_t) perStartSpeed;
	uint8_t perSh = (uint8_t) (perStartSpeed >> 8);
Jake Read's avatar
Jake Read committed
209 210
	
	// turn on TCC0
Jake Read's avatar
Jake Read committed
211
	TCD0.CTRLA = TC_CLKSEL_DIV8_gc;
212 213
	TCD0.PERBUFL = perSl;
	TCD0.PERBUFH = perSh;
Jake Read's avatar
Jake Read committed
214 215
	
	// set cca interrupt on
Jake Read's avatar
Jake Read committed
216
	TCD0.INTCTRLA = TC_OVFINTLVL_HI_gc;
217
	
Jake Read's avatar
Jake Read committed
218
	// another timer runs 'acceleration'
Jake Read's avatar
Jake Read committed
219
	uint16_t perAccelRate = 2048;
220 221 222 223 224 225
	uint8_t perAl = (uint8_t) perAccelRate;
	uint8_t perAh = (uint8_t) (perAccelRate >> 8);
	
	TCD1.CTRLA = TC_CLKSEL_DIV64_gc;
	TCD1.PERBUFL = perAl;
	TCD1.PERBUFH = perAh;
Jake Read's avatar
Jake Read committed
226
	
Jake Read's avatar
Jake Read committed
227
	TCD1.INTCTRLA = TC_OVFINTLVL_HI_gc;
Jake Read's avatar
Jake Read committed
228 229 230 231
}

int main(void)
{
Jake Read's avatar
Jake Read committed
232
	// boilerplate atk things
Jake Read's avatar
Jake Read committed
233 234 235 236 237 238 239 240 241 242 243 244 245
	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
246 247
	// startup encoder (currently borked)
	// encoder_init();
Jake Read's avatar
Jake Read committed
248
	
249
	// start pwm system
Jake Read's avatar
Jake Read committed
250
	pwm_init();
Jake Read's avatar
Jake Read committed
251
	
252 253
	// initialize the bldc state structure
	bldc_init(&bldc);
254
	bldc_setTargetSpeed(&bldc, 800);
Jake Read's avatar
Jake Read committed
255
	bldc_setSpeed(&bldc, 100);
256 257 258
	bldc.currentSpeed = 100;
	bldc_setDuty(&bldc, 52); // MAX 512, dangerous above ~ 120

259 260 261 262
	// startup the driver
	drv_init();
	// and enable the gate
	drv_enable();
Jake Read's avatar
Jake Read committed
263

264 265 266
	// start the timers that do work
	tickers_init();

Jake Read's avatar
Jake Read committed
267 268 269 270 271 272
	// runtime globals
	uint32_t tck = 0;

	while (1)
	{
		atkport_scan(&atkp0, 2);
273 274 275 276 277
		// just... as fast as we can
		tck++;
		if(!(fastModulo(tck, 4096))){
			pin_toggle(&stlclk);
		}
Jake Read's avatar
Jake Read committed
278 279 280
	}
}

281
// commutation timer
Jake Read's avatar
Jake Read committed
282
ISR(TCD0_OVF_vect){
Jake Read's avatar
Jake Read committed
283
	
284
	// open-loop commutation
Jake Read's avatar
Jake Read committed
285 286 287 288 289 290
	
	(bldc.comDir) ? (bldc.comState ++) : (bldc.comState --);
	if(bldc.comState > 5){
		bldc.comState = 0;
	}
	
291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312
	uint8_t cs = bldc.comState;
	uint16_t dt = bldc.comDuty;
	
	uint16_t clo1 = ct[cs][0];
	clo1 *= dt;
	uint16_t chi1 = ct[cs][1];
	chi1 *= dt;
	uint16_t clo2 = ct[cs][2];
	clo2 *= dt;
	uint16_t chi2 = ct[cs][3];
	chi2 *= dt;
	uint16_t clo3 = ct[cs][4];
	clo3 *= dt;
	uint16_t chi3 = ct[cs][5];
	chi3 *= dt;
	
	// ct is comtable 
	pwm_sixstep(clo1, chi1, clo2, chi2, clo3, chi3);
	
	/*
	// here is PWM for sinusoid-type pwm setup
	pwm_by_offset(comTable[bldc.comState][0] * bldc.comDuty,
Jake Read's avatar
Jake Read committed
313 314 315
	comTable[bldc.comState][1] * bldc.comDuty,
	comTable[bldc.comState][2] * bldc.comDuty
	);
316
	*/
Jake Read's avatar
Jake Read committed
317 318 319
	
	/*
	// CL 6-step Commutate
Jake Read's avatar
Jake Read committed
320 321
	// get encoder reading
	ams5047_read(&ams5047, &enc_reading);
322 323 324 325 326
	// 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;
327
	// we need to compute the respective pwm positions given the phase target
Jake Read's avatar
Jake Read committed
328 329
	pwm_by_sin_duty(phase_target, 0.75);
	*/
Jake Read's avatar
Jake Read committed
330 331 332 333 334
}

static uint16_t thisRead;

ISR(TCD1_OVF_vect){
Jake Read's avatar
Jake Read committed
335 336 337 338 339 340 341 342 343
	if(bldc.currentSpeed != bldc.targetSpeed){
		if(bldc.currentSpeed < bldc.targetSpeed){
			bldc.currentSpeed ++;
			bldc_setSpeed(&bldc, bldc.currentSpeed);
			} else {
			bldc.currentSpeed --;
			bldc_setSpeed(&bldc, bldc.currentSpeed);
		}
	}
Jake Read's avatar
Jake Read committed
344 345 346 347 348 349 350 351 352
}

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

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