main.c 8.42 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
	// 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
106
	AWEXC.DTBOTHBUF = 2; // four counts of pwm clock for deadtime
Jake Read's avatar
Jake Read committed
107 108
	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 = 1024; 
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
Jake Read's avatar
Jake Read committed
147
	TCD0.CTRLA = TC_CLKSEL_DIV8_gc;
148 149
	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
	// on startup, speed (if clcomm) and duty
	//bldc_setTargetSpeed(&bldc, 1000);
Jake Read's avatar
Jake Read committed
196
	bldc_setDuty(&bldc, 30);
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
#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
Jake Read's avatar
Jake Read committed
233 234 235
const static uint16_t enc_modulo = ENC_RESOLUTION / 7; // divisor is # poles / 2
const static uint16_t enc_offset = 0; // start at midpoint - adjust -ve or +ve from
const static uint16_t enc_steplength = ENC_RESOLUTION / 7 / 6;
Jake Read's avatar
Jake Read committed
236 237 238 239 240 241
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
	// 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;
Jake Read's avatar
Jake Read committed
251 252
	} else {
		enc_adjusted = enc_reading;
Jake Read's avatar
Jake Read committed
253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270
	}
	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
	/*
271 272 273 274 275 276 277 278 279
	(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
280
	*/
Jake Read's avatar
Jake Read committed
281 282
}

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

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

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