main.c 8.45 KB
Newer Older
Jake Read's avatar
Jake Read committed
1 2 3 4 5 6 7 8 9 10 11 12 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
/*
* 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"

// 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
46
	// startup SPI
Jake Read's avatar
Jake Read committed
47 48
	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
49 50 51
	spi_start(&spiEncoder, 1);
	// startup object 
	ams5047_init(&ams5047, &spiEncoder);
Jake Read's avatar
Jake Read committed
52 53
}

Jake Read's avatar
Jake Read committed
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
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
70 71
}

72 73 74 75 76 77 78 79 80
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);
}

Jake Read's avatar
Jake Read committed
81
void pwm_init(void){
Jake Read's avatar
Jake Read committed
82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
	// 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;
	AWEXC.DTBOTHBUF = 4; // four counts of pwm clock for deadtime
	AWEXC.OUTOVEN = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5);
	
109
	pwm_periods(0, 0, 0);
Jake Read's avatar
Jake Read committed
110 111 112
}

void drv_init(void){
113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134
	// 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
	pin_clear(&drvModeGain); // low for 10v/v, hi for 40v/v current sense gains
	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
135 136 137
}

void tickers_init(void){
Jake Read's avatar
Jake Read committed
138
	// sets up two timers
Jake Read's avatar
Jake Read committed
139 140
	
	// compare and capture at value
Jake Read's avatar
Jake Read committed
141
	uint16_t perStartSpeed = 150; // 75 should be 10kHz for commutation, 150 for 5kHz 
Jake Read's avatar
Jake Read committed
142
	// write low first, bc bussing / xmega 8-bit oddities cc datasheet @ 3.11
143 144
	uint8_t perSl = (uint8_t) perStartSpeed;
	uint8_t perSh = (uint8_t) (perStartSpeed >> 8);
Jake Read's avatar
Jake Read committed
145 146
	
	// turn on TCC0
147 148 149
	TCD0.CTRLA = TC_CLKSEL_DIV64_gc;
	TCD0.PERBUFL = perSl;
	TCD0.PERBUFH = perSh;
Jake Read's avatar
Jake Read committed
150 151
	
	// set cca interrupt on
Jake Read's avatar
Jake Read committed
152
	TCD0.INTCTRLA = TC_OVFINTLVL_HI_gc;
153
	
Jake Read's avatar
Jake Read committed
154
	/*
155 156
	// and a reasonable speed for acceleration ticking
	
157
	uint16_t perAccelRate = 800;
158 159 160
	uint8_t perAl = (uint8_t) perAccelRate;
	uint8_t perAh = (uint8_t) (perAccelRate >> 8);
	
Jake Read's avatar
Jake Read committed
161
	// another ticker to execute accel
162 163 164
	TCD1.CTRLA = TC_CLKSEL_DIV64_gc;
	TCD1.PERBUFL = perAl;
	TCD1.PERBUFH = perAh;
Jake Read's avatar
Jake Read committed
165
	
Jake Read's avatar
Jake Read committed
166
	TCD1.INTCTRLA = TC_OVFINTLVL_HI_gc;
Jake Read's avatar
Jake Read committed
167
	*/
Jake Read's avatar
Jake Read committed
168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184
}

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
185 186
	// startup encoder
	encoder_init();
187
	// start timers for commutation, accel tickers 
Jake Read's avatar
Jake Read committed
188
	tickers_init();
Jake Read's avatar
Jake Read committed
189
	
190
	// start pwm system
Jake Read's avatar
Jake Read committed
191
	pwm_init();
192 193
	// initialize the bldc state structure
	bldc_init(&bldc);
Jake Read's avatar
Jake Read committed
194 195 196
	// on startup, speed (if clcomm) and duty
	//bldc_setTargetSpeed(&bldc, 1000);
	bldc_setDuty(&bldc, 40);
197 198 199 200
	// startup the driver
	drv_init();
	// and enable the gate
	drv_enable();
Jake Read's avatar
Jake Read committed
201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217

	// 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);
     	}
	}
}

218 219 220 221 222 223 224 225 226
int8_t comTable[6][3] = {
	{1,-1,0},
	{1,0,-1},
	{0,1,-1},
	{-1,1,0},
	{-1,0,1},
	{0,-1,1}
};

Jake Read's avatar
Jake Read committed
227 228 229 230 231 232 233 234 235 236 237 238 239 240 241
#define ENC_RESOLUTION 16834

// 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 = 0; // 1 or 0, for reverse op
const static uint16_t enc_modulo = ENC_RESOLUTION / 4; // 2805 in this case: divisor is # poles / 2
const static uint16_t enc_offset = 60; // start at midpoint - adjust -ve or +ve from
const static uint16_t enc_steplength = ENC_RESOLUTION / 4 / 6;
static uint16_t enc_reading = 0;
static uint16_t enc_adjusted = 0;
static uint16_t enc_phase = 0;
static uint16_t enc_target = 0;
// 2^14 = 16,384

242
// commutation timer
Jake Read's avatar
Jake Read committed
243
ISR(TCD0_OVF_vect){
Jake Read's avatar
Jake Read committed
244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268
	// CL 6-step Commutate
	
	// get encoder reading
	ams5047_read(&ams5047, &enc_reading);
	// offset and modulo to bring to in-phase relative location
	if(enc_reverse){
		enc_adjusted = enc_resolution - enc_reading;
	}
	enc_adjusted += enc_offset; // add offset
	enc_adjusted %= enc_resolution; // wrap to resolution
	enc_adjusted %= enc_modulo; // break into phase relative
	// so this should represent the phase (0-5) that we're currently in
	enc_phase = enc_adjusted / enc_steplength;
	// if dir, phase + 2 to go forwards, else + 4, effectively - 2, and wrap past circle
	(bldc.comDir) ? (enc_target = (enc_phase + 2) % 6) : (enc_target = (enc_phase + 4) % 6);
	//uint8_t dout[6] = {6,0,5,254,255,enc_target};
	//uart_sendchars_buffered(&up0, dout, 6);
	// now we can set PWMs accordingly
	pwm_by_offset(	comTable[enc_target][0] * bldc.comDuty,
					comTable[enc_target][1] * bldc.comDuty,
					comTable[enc_target][2] * bldc.comDuty
				 );
				 
	// OL Commutate
	/*
269 270 271 272 273 274 275 276 277
	(bldc.comDir) ? (bldc.comState ++) : (bldc.comState --);
	if(bldc.comState > 5){
		bldc.comState = 0;
	}
	
	pwm_by_offset(	comTable[bldc.comState][0] * bldc.comDuty,
					comTable[bldc.comState][1] * bldc.comDuty,
					comTable[bldc.comState][2] * bldc.comDuty 
				 );
Jake Read's avatar
Jake Read committed
278
	*/
Jake Read's avatar
Jake Read committed
279 280
}

281
// acceleration timer
Jake Read's avatar
Jake Read committed
282
ISR(TCD1_OVF_vect){
Jake Read's avatar
Jake Read committed
283 284 285 286
	// CL Commutate: PID Speed Loop
	
	// OL Commutate: Acceleration to Speed
	/*
287 288 289 290 291 292 293 294 295
	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
296
	*/
Jake Read's avatar
Jake Read committed
297 298 299 300 301 302 303 304 305
}

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

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