README.md 27 KB
Newer Older
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 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 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 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189
# Programming the BLDC Driver

## Background

Here I'm using the chip I set up for my Networking project, it's an ATSAMS70 with four 'ports' for message passing, and a bells-and-whistles header that I'm thinking will be useful for doing-other-stuff (i.e. this motor controller is one of such things).

The board is documented [here](https://github.com/jakeread/tinynets/tree/master/circuit) and code for the switch (as well as useful bits for some peripheral setup is [here](https://github.com/jakeread/tinynets/tree/master/embedded)). 

## The plan

- do PWM Setup
- do motor turning around at low level, on some set frequency / duty
- do UART interface for Frequency / Duty
- do MODS UART

## PWM Setup

OK, I'm going to start by turning on the PWM peripherals I'll use to control my switches. A few initial spec's:

Channels 
 - 3 channels, 6 outputs: one hi / one low - per phase

Base Frequency
 - In selecting a base frequency for the PWM, a real engineer would look at the time constant of their motor. The coils of a motor are basically big inductors, so when voltage is switched 'hi' to them, it takes some time for the current in the coils to rise. The 'time constant' of a system is a pretty broad way to say on what-timescale-does-stuff-happen. So, in a small motor with relativley low inductance, we'll have a small time constant (say, of a few us[microseconds]). I'm not exactly sure what the units are like here, but I think this is like the time it takes for current to rise to 66% of what a full-out 'on' current level would be. This has something to do with impulse response. 
 - For the motors I'm using (smallish), I know (purely imperical) that a base frequency on the order of 10kHz is what I'm aiming at - a period of 100us. In microcontroller time, this is fairly slow! Additionally, this frequency generates an audible buzz (we hear the coils 'banging' on the mechanical system of the motor at this frequency) so being able to push this frequency outside of the audible range is awesome. 

Deatime Insertion 
 - Because the circuit is setup with a high and low side mosfet, and the fets don't switch instantaneously, if we were to switch the high off and low on instantaneously, there would be a brief moment when the high and low switches were both on, allowing current to 'shoot through' from VCC to GND, and this is bad. To get around this we do something called 'deadtime insertion' - just putting a few us of off-time on both channels, between switching. There is a register for this, and additionally the DRV8302 does this automatically - so I maybe don't need to worry about it off the bat, but if it's straightforward I'll do that.

ADC and PWM Synchronization
 - I'm reading current off of 'current shunts' that are 'below' my low-side mosfet.
 - Current only flows through these shunts when the low-side mosfets are on! Otherwise, they are open to nothing, nothing flows, no measurement etc.
 - I have to coordinate my ADC sampling with the *center* of the on-cycle of the low-side PWM. There are ways to do this with the peripherals, but it's cross-linking peripherals in the event system, so this will be a challenge.
 - I don't need to do current control *yet* so I'm going to save this struggle for later on!

I'm tempted to use the ASF modules for this.. 

However, I didn't! I got through this massive set of registers (holy shit there are PWM options for days) by carefully reading... guess.. the datasheet! Sweet deliverance of knowledge from the Si gods... In the ATSAMS70 Datasheet particularely, bouncing between 47.6.6 (PWM initialization sequence) and 47.6 (Functional Description) and 47.7 (Register Descriptions). I was able to work through it. I have *one* pwm line operational, now I'm looking at how to get the low-side of that signal operating as well... Here's a trace from my logic analyzer, digital and analog signals of the high side of this channel. I have the frequency correct - a good start.

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-alive.jpg)

So my second channel is not enabled because I've made an error in labelling pins! Here's the group prior to my noticing the error:

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/pwm-pins-prior.jpg)

and corrected

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/pwm-pins-shifted.jpg)

OK, so I'm going to have to keep track of this carefully, and I'm probably going to have to jumper wires between boards, rather than using these stacking headers. Welp. This is why we do incremental development.

With this confusion out of the way, here's a complete channel - high and low sides of my gates:

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-fullchannel.jpg)

And a detail on the deadtime insertion:

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-fullchannel-deadtime.jpg)

Here's the code I'm running:

```C

void setuppwm(void){
	PMC->PMC_PCER1 = 1 << 28; // turn on peripheral clock for PWM 1
	// these are pins for channels 1, 2, 3 in lo / hi pairs
	PIOD->PIO_PDR = PIO_PER_P2 | PIO_PER_P3 | PIO_PER_P4 | PIO_PER_P5 | PIO_PER_P6 | PIO_PER_P7; // disable PIO
	PIOD->PIO_ABCDSR[0] = PIO_PER_P2 | PIO_PER_P3 | PIO_PER_P4 | PIO_PER_P5 | PIO_PER_P6 | PIO_PER_P7; // enable peripheral B
	//PIOD->PIO_ABCDSR[1] // already these are 0s, we want that [01] is peripheral B
	
	// PREA Clock is Peripheral Clock / 64
	// PREA Clock supplies Clock A, Clock A is PREA / 24
	// Clock B is off
	PWM1->PWM_CLK = PWM_CLK_PREA(2) | PWM_CLK_DIVA(7) | PWM_CLK_DIVB_CLKB_POFF;
	// Channel 0 uses CLCKA, uses Center Aligned, has dead time, is waveform aligned (with others?)
	PWM1->PWM_CH_NUM[1].PWM_CMR  = PWM_CMR_CPRE_CLKA | PWM_CMR_CES | PWM_CMR_DTE | PWM_CMR_CALG;
	// Channel 0 uses this period
	// (2 * 2^PREA * DIVA * CPRD) / f_peripheralClock
	// so for 10kHz target frequency, we want 1 / 10000, 
	// have (2 * 64 * 12 * x )/ 150000000
	// ~ spreadsheets are your friends ~
	PWM1->PWM_CH_NUM[1].PWM_CPRD = PWM_CPRD_CPRD(255);
	// set duty cycle, initially, to low
	// this is between 0 and CPRD, so it looks like shooting for a big CPRD is (y)
	PWM1->PWM_CH_NUM[1].PWM_CDTY = PWM_CDTY_CDTY(1);
	// configure deadtime generator, between 0 and CPRD - CDTY
	PWM1->PWM_CH_NUM[1].PWM_DT = PWM_DT_DTH(2) | PWM_DT_DTL(2);
	// configure update mode
	// mode 1 is 'manual write of double buffer registers and automatic update of synchronous channels'
	// other modes are for manual write / manual  update and DMA mode
	// also add sync. channels - spec that 0, 1, 2 are ~ on the same wavelength, man ~
	PWM1->PWM_SCM = PWM_SCM_UPDM_MODE1 | PWM_SCM_SYNC0;
	
	// now, boot it up
	PWM1->PWM_ENA = PWM_ENA_CHID1; // just channel 0 for now
	
	// understand: compare? clock? update? duty cycle register? period register?
}

int main (void)
{
	board_init();
	sysclk_init();
	wdt_disable(WDT);
	
	setuppwm();
	
	PMC->PMC_PCER0 = 1 << ID_PIOA;
	
	PIOA->PIO_PER |= PIO_PER_P27;
	PIOA->PIO_OER = PIO_PER_P27;
	
	startupflash();
	
	while(1){
		// to update, must use duty update register, not just 'duty' 
		PWM1->PWM_CH_NUM[1].PWM_CDTYUPD = PWM_CDTY_CDTY(20);
		delay_ms(100);
		PWM1->PWM_CH_NUM[1].PWM_CDTYUPD = PWM_CDTY_CDTY(155);
		delay_ms(50);
	}
}
```

Now I'll set up three channels for my three half-bridges, write a function to do sinusoid open-loop commutation, and maybe even test that out on a motor! I'll also write a function to change the global duty cycle.

Here's my shorthand pin-mapping

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/pwm-pins-mapping.jpg)

I want to push these values to my phases:

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/diagram-three-phase.png)

Where the '0' point will be 1/2 of a full-on pwm phase (so, with my CPRD of 255, 127 - this is where the half bridge is switched hi half of the time and low the other half), -1 is a pwm duty of 0 (where the half bridge is switched low 100% of the time) and 1 is a value of 255 (where the half bridge is switched high 100% of the time). 

Here we go, a sinusoidal PWM:

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-sinusoidal.jpg)

I think I'm ready to check all of my other motor controller IO's - like, do I need to assert, also, an enable pin? I forget. Then I'll plug it in, and flip the switch, ho-ha! Yep - I just have on extra PIO to add, an Enable Pin. Set this up.

## UART

OK, I'm going to bring up the UART. I have a block of code for this from my networks project that I'll try bringing in.

This works, largely. I'm bringin the UART Baudrate way down to 9600... I'm getting characters returning on the line, now I need to setup the interrupt system to handle transmitted characters, and echo them.

Figuring out how the heck the interrupts work was a PITA. Kind of interesting, though - the Interrupt System is technically an ARM-Cortex domain thing, and so Atmel's datasheets barely touch on it... they say things like 'configure the interrupt controller before doing this' - but NOWHERE does it explain how to do that. Heck.

Here's my brief:
 - There are a set # of interrupts, they are enumerated in the software framework, and naming is pretty straightforward, for example, UART1 Interrupt Number is UART1_IRQn ...
 - There are a few globally defined NVIC setup functions, which take these IRQ Numbers as arguments. Their naming convention is also pretty straightforward
  - NVIC_DisableIRQ(IRQn) // disables!
  - NVIC_SetPriority(IRQn, uint8_t priority) // 0-255 priority, 0 being the highest
  - NVIC_EnableIRQ(IRQn) // enables!
 - When one of these interrupts fires, it directs to a pre-defined Handler. For example, the UART1 handler is UART1_Handler(){// code for handler} - Atmel Studio's autocomplete makes this work excellently.
 - Critically, most interrupts require that you clear some register, usually just by reading it.
  - in the UART example, I clear the event by reading the UART data in register. This will vary between peripherals, and it's not documented anywhere, so poke around!

Here's my Setup Code (runs in main())

```C
	NVIC_DisableIRQ(UART1_IRQn);
	NVIC_ClearPendingIRQ(UART1_IRQn);
	NVIC_SetPriority(UART1_IRQn, 9);
	NVIC_EnableIRQ(UART1_IRQn);
```

of course, I'm also setting up the UART peripheral... to see that, you can check out the code in this repo!

Here's the interrupt routine - properly, I would quickly push this data into a ringbuffer (to be implemented) and exit the routine - ISR's (interrupt service routines) should be very small, lest you get a barrage of them... buffering is bueno. Here, I'm just passing the received character back - this is an echo test, a good way to make sure everything is working as it should. It is, hurray!

```C
void UART1_Handler(){
	if(UART1->UART_SR & UART_SR_RXRDY){
		uint8_t data = UART1->UART_RHR; // read the incoming data
		while(!(UART1->UART_SR & UART_SR_TXRDY)); // hang-10 to make sure the UART is ready to send
		UART1->UART_THR = data;
	} else {
		//
	}
}
```

Note - the handler does not distinguish between an RXReady event and the other interrupts - a TXReady, for example, so in this routine I would have to also check which has fired and act accordintly...

Some ARM Documentation is [here](http://infocenter.arm.com/help/index.jsp) - navigate to Cortex-M7 Documentation, the latest revision, and look for NVIC.

Jake Read's avatar
Jake Read committed
190
From here, I brought up the hardware - a satisfying adventure. Go [here](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/tree/master/circuit) for that.
191

192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221
Then I worked (briefly) on implementing a control interface for this using mods. That should be [here](http://fab.cba.mit.edu/classes/863.17/CBA/people/jakeread/) in week 12.

## Closing Loops

Next up, I want to roll an encoder reading into the system, and run simple closed loop sinusoidal commutation. Then I'll try rolling a brief PID loop around that to find positions. Then I'll be satisfied with this spiral.

However, at the moment I'm having an issue where the chip is resetting, and things seem to be getting really hot. I see some smoke, I'm not sure where it's coming from, and it continues to work - so I'm confused. Sounds like I'm maybe smoking out the motor a little bit? Thinking I should maybe chill out to a 12v power supply - these motors have an Rm of only about .1 ohms, so if I accidentaly dump 240v into them for any appreciable time I'll see 240amps after whatever inductive time constant. That's deadly!

Actually my leading theory right now is that I'm just commutating so slowly the phase current is allowed to reach really big numbers, hence the smokey smokey. I'll implement the closed loop commutation to remedy this.

Or perhaps I'm driving the current sense amps way out of range and that's f'ing other things up?

OK actually I solved this earlier - something to do with the ATSAMS70 becoming unhappy and resetting when not enough power on the 3v3 line. *shrugman* - next rev on this board I'll address that. Power electronics: hard.

I'm also going to try booting up the FLIR Camera.

But, for now, SPI.

## SPI AS5047

OK. Long road ahead. Here's my list 
 - Pinouts / Pins / through the header, through the ATSAM
 - Peripheral Wakeup
 - Parity / Clock / etc setup
 - Reading Addresses / tiny abstraction for the AS5047 particularly.
  - parity bit checking important b/c previous experience with this encoder shitting the bed when presented with big EMF environment.
 - fin section

I'm using the ATSAM's SPI Peripheral. I am *really* swimming in the datasheet at the moment. There's a lot to configure!

Jake Read's avatar
Jake Read committed
222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262
Here we go, some code. 

```C
void setupspi(void){
	// using SPI0, PID 21
	// MISO, PD20, Peripheral B
	// MOSI, PD21, Peripheral B
	// SPCK, PD22, Peripheral B
	// NPCS, PA31, Peripheral A
	
	PMC->PMC_PCER0 = 1 << ID_SPI0;
	
	PIOD->PIO_PDR |= PIO_PER_P20 | PIO_PER_P21 | PIO_PER_P22;
	PIOA->PIO_PDR |= PIO_PER_P31;
	
	SPI0->SPI_CR |= SPI_CR_SPIEN;
	
	SPI0->SPI_MR |= SPI_MR_MSTR | SPI_MR_PCS(1);
	
	SPI0->SPI_CSR[1] |= SPI_CSR_BITS_16_BIT | SPI_CSR_NCPHA | SPI_CSR_SCBR(200);
}
```

Turned out to be kind of boring.

So SPI is *really fast* - i.e. I can run this at ~5MBPS no problem, and I'm only doing a 32 bit transfer each time. What's more, SPI is like synchronous duplex, as in during the same time shift out 32 bits, 32 bits come in. This is the major advantage of a clocked / master:slave relationship. This means that for a total of 64 bit transferred, we take only 6.4us. Holy heck. I'm not even going to bother writing non-blocking code.

```C
void readencoder(uint16_t *data){
			
	while(!(SPI0->SPI_SR & SPI_SR_TXEMPTY)); // wait 4 ready
	SPI0->SPI_TDR = SPI_TDR_TD(0xFFFF);// | SPI_TDR_PCS(1);
	while(!(SPI0->SPI_SR & SPI_SR_TXEMPTY)); // wait 4 ready
	SPI0->SPI_TDR = SPI_TDR_TD(0x0000);// | SPI_TDR_PCS(1);
	*data = SPI0->SPI_RDR & SPI_RDR_RD_Msk;
	
	//uint16_t pard = data >> 15;
	*data = *data << 2; // shift parity and error bit out
	*data /= 4;
}
```
263

Jake Read's avatar
Jake Read committed
264 265 266 267
OK - I've got this set up properly. Now I'll try plugging the encoder in an reading some values!

Great, and with a shift in some setting (I was wrong) I'm reading SPI values nicely. Next test is to see if these values get all messed up when I turn the motor on (update: nope, yay), introducing lots of EMF and noise into the system... Then I'll write some open-loop commutation using the encoder values (update: done). Then I'll think about writing a simple PID position loop. If I get through all of that tonight, I'll go to bed (haha).

Jake Read's avatar
Jake Read committed
268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306
OK - I got closed-loop commutation working. This is where I take a position reading on the rotor and use that to advance the sinusoidal voltage I am sending to the phases such that *roughly* I get a magnetic field that is 90deg out of phase from the rotor's magnetic field. This is the max torque scenario.

```C

		uint16_t encoder;
		
		readencoder(&encoder);
		
		if(reverse){
			encoder = resolution - encoder;
		}
		uint16_t elecpos = (encoder + offset) % modulo; // 0 -> 2*PI in rads
		
		// to update, must use duty update register, not just 'duty' 
		if(dir){
			phaseu = elecpos/(modulo / 1024);
			phasev = elecpos/(modulo / 1024) + 341;
			phasew = elecpos/(modulo / 1024) + 682;
		} else {
			phaseu = elecpos/(modulo / 1024) + 682;
			phasev = elecpos/(modulo / 1024) + 341;
			phasew = elecpos/(modulo / 1024);
		}
		
		if (phaseu > 1023){
			phaseu -= 1023;
		}
		if(phasev > 1023){
			phasev -= 1023;
		}
		if(phasew > 1023){
			phasew -= 1023;
		}
```

Pretty slick, the key is adjusting the offset.

[Here](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/video/clcomm.m4v) is a video of the motor turning *nicely* using closed-loop commutation. Not sure why this is upside-down, but that doesn't seem important. Also - you'll hear the 10KHz pwm buzz - this is something I plan on eliminating (it's simple - I just push the PWM frequency out of the audible range) but I have left it just in the audible range so that I can be sure when the gate-drivers are on (I don't want to fry anything!).

Jake Read's avatar
v0.3  
Jake Read committed
307 308
And that's right. Those are some BFC's.

Jake Read's avatar
Jake Read committed
309 310 311
# The EMI Problem

Below, a log of me writing notes when I find this intermittent, horrible, reset condition. 
Jake Read's avatar
Jake Read committed
312 313 314 315 316

Also: a note, I'm having odd reset events occasionally. Related to the earlier issue w/ usb power. I think I need to revisit the 'bfpsu' issue - wherein I want a 24V PSU that will deliver nice consistent power ~ 65 amps (that's about as much juice as is possible to pull out of a wall socket). I may need to build this, as these don't seem readily available. My other thought is to find a big switching PSU, and hook up a *giant* capacitor to the output to smooth it out.

OMG it's the SWITCH is being super intermittent. Don't use a switch rated for 7.5 amps past 7.5 amps. Heck!

Jake Read's avatar
Jake Read committed
317 318 319 320 321 322
OK - like 8th loop on this. I fixed that wiring and was still having a reset issue. Back at it now, added extra capacitance to the 5V output, and a diode that I had previously omitted soldering on. Seems better, but I make no promises to myself.

Another update on this - I've shortened the motor lead wires and this seems to have killed the problem. Again, no expectations - I've been wrong a few times. But I suspect EMI was the issue, and shorter lines (smaller antennas) could be the solution! Hurray, maybe :| . 

![emi](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/emi.jpg)

Jake Read's avatar
Jake Read committed
323 324 325 326 327 328
So this shortened motor wires did seem to really kill the problem. Fin.

# v0.3

We're going to do this on the ATSAMD51, now. I have a v0.3 board here (which will shortly be rev'd to 0.31 due to a few already apparent mistakes, welp) and I can program it. I'm in the process of checking all of the hardware so that I can go forward with a new board order, knowing a bit better that I'll be o-k with that set. 

Jake Read's avatar
Jake Read committed
329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503
I've got the PWM up, and it's running as expected. Nice.

```C

int main(void)
{
    /* Initialize the SAM system */
    SystemInit();
	
	PORT->Group[1].DIRSET.reg |= (uint32_t)(1 << 9);
	PORT->Group[0].DIRSET.reg |= (uint32_t)(1 << 23);
	
	SysTick_Config(5000000);
	
	/* TCC SETUP */
	// from 49.6.2.1
	// a few registers are protected - and can only be updated when
	// TCCn.CTRLA.ENABLE = 0
	// FCTRLA and FCTRLB, WEXCTRL, DRVCTRL, and EVCTRL
	
	// (4) Configure Output Pin with PORT->Group[n].DIRSET.reg
	// PA8 PA9 PA10 PA12, PB10 PB11
	// 32.9.13
	PORT->Group[0].DIRSET.reg |= (uint32_t)(1 << 8) | (uint32_t)(1 << 9) | (uint32_t)(1 << 10) | (uint32_t)(1 << 12);
	PORT->Group[1].DIRSET.reg |= (uint32_t)(1 << 10) | (uint32_t)(1 << 11);
	
	// 1 lo / hi
	PORT->Group[0].PINCFG[10].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[10>>1].reg |= PORT_PMUX_PMUXE(0x5);  // on peripheral F
	PORT->Group[0].PINCFG[12].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[12>>1].reg |= PORT_PMUX_PMUXE(0x5); 
	
	// 2 lo / hi
	PORT->Group[0].PINCFG[9].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[9>>1].reg |= PORT_PMUX_PMUXO(0x5);  // on peripheral F
	PORT->Group[1].PINCFG[11].bit.PMUXEN = 1;
	PORT->Group[1].PMUX[11>>1].reg |= PORT_PMUX_PMUXO(0x5);
	
	// 3 lo / hi
	PORT->Group[0].PINCFG[8].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[8>>1].reg |= PORT_PMUX_PMUXE(0x5);  // on peripheral F
	PORT->Group[1].PINCFG[10].bit.PMUXEN = 1;
	PORT->Group[1].PMUX[10>>1].reg |= PORT_PMUX_PMUXE(0x5);
	
	// (1) enable the TCC Bus Clock - CLK_TCCn_APB
	// https://www.eevblog.com/forum/microcontrollers/atmel-sam-d-tc-and-tcc-(no-asf)/
	
	TCC0->CTRLA.bit.ENABLE = 0;
	
	MCLK->APBBMASK.reg |= MCLK_APBBMASK_TCC0; // at 15.8.9
	
	GCLK->GENCTRL[5].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN;
	while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL5);
	
	GCLK->PCHCTRL[TCC0_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN_GCLK5;
	
	TCC0->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV8 | TCC_CTRLA_PRESCSYNC_PRESC |TCC_CTRLA_RESOLUTION(0);
	
	// (2) Select Waveform Generation operation in the WAVE register WAVE.WAVEGEN
	// we want dual slope pwm
		
	TCC0->WAVE.reg = TCC_WAVE_WAVEGEN_DSBOTH; // 'dual slope both' - updates on both hi and lo of slope ?
		
	// (3) We want OTMX - Output Matrix Channel Pin Routing Configuration - at 0x0
	
	TCC0->WEXCTRL.reg = TCC_WEXCTRL_DTHS(1) | TCC_WEXCTRL_DTLS(1) | 
						TCC_WEXCTRL_DTIEN1 | TCC_WEXCTRL_DTIEN2 | TCC_WEXCTRL_DTIEN3 | TCC_WEXCTRL_DTIEN0 |
						TCC_WEXCTRL_OTMX(0);
						
	TCC0->PER.reg = TCC_PER_PER(256); // 18 bit
	
	TCC0->COUNT.reg = 0;
	
	TCC0->CC[0].reg = 12; // '3'
	TCC0->CC[1].reg = 24; // '2'
	TCC0->CC[2].reg = 48; // '1'
	TCC0->CC[3].reg = 0;
	
	// (4) Enable with CTRLA.ENABLE
	
	TCC0->CTRLA.bit.ENABLE = 1;
	while(TCC0->SYNCBUSY.bit.ENABLE);

    while (1) 
    {
		PORT->Group[1].OUTTGL.reg = (uint32_t)(1 << 9);
    }
}
```

![atsamd51 pwm](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-alive-atsamd51.png)

![atsamd51 pwm](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-alive-atsamd51-picture.jpg)

Next up is the SPI wakeup.

Great, this is running as well.

```C
int main(void)
{
    /* Initialize the SAM system */
    SystemInit();
	
	PORT->Group[1].DIRSET.reg |= (uint32_t)(1 << 9);
	PORT->Group[0].DIRSET.reg |= (uint32_t)(1 << 23);
	
	SysTick_Config(5000000);
	
	/* BEGIN SPI SETUP */
	
	// PA04, SER0-0, SPI_MISO
	// PA05, SER0-1, SPI_SCK
	// PA06, SER0-2, SPI_CSN
	// PA07, SER0-3, SPI_MOSI
	PORT->Group[0].DIRCLR.reg |= (uint32_t)(1 << 4); 
	PORT->Group[0].DIRSET.reg |= (uint32_t)(1 << 5) | (uint32_t)(1 << 6) | (uint32_t)(1 << 7);
	
	PORT->Group[0].PINCFG[4].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[4>>1].reg |= PORT_PMUX_PMUXE(0x3);  // on peripheral D
	PORT->Group[0].PINCFG[5].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[5>>1].reg |= PORT_PMUX_PMUXO(0x3);  // on peripheral D
	PORT->Group[0].PINCFG[6].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[6>>1].reg |= PORT_PMUX_PMUXE(0x3);  // on peripheral D
	PORT->Group[0].PINCFG[7].bit.PMUXEN = 1;
	PORT->Group[0].PMUX[7>>1].reg |= PORT_PMUX_PMUXO(0x3);  // on peripheral D
	
	// setup clocks to sercom
	
	MCLK->APBAMASK.reg |= MCLK_APBAMASK_SERCOM0; // at 15.8.9
	
	GCLK->GENCTRL[6].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN;
	while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL6);
	
	GCLK->PCHCTRL[SERCOM0_GCLK_ID_CORE].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN_GCLK6;
	
	// TCC0_GCLK_ID
	
	// Some registers can't be written unless CTRL.ENABLE = 0:
	// CTRLA, CTRLB, BAD and ADDR
	
	// (1) set to master
	
	SERCOM0->SPI.CTRLA.reg |= SERCOM_SPI_CTRLA_MODE(0x3); // 0x2 or 0x3, slave or master
	
	// SERCOM0->SPI.CTRLA.reg |= SERCOM_SPI_CTRLA_CPHA | SERCOM_SPI_CTRLA_CPOL; // clock phase and polarity
	
	// (2) set pin configurations
	
	SERCOM0->SPI.CTRLA.reg |= SERCOM_SPI_CTRLA_DIPO(0x0) | SERCOM_SPI_CTRLA_DOPO(0x2); // pin selections, see 35.8.1 bits 21:20 and 17:16, pg. 910
	
	// (3) set character size, data direction
	
	//SERCOM0->SPI.CTRLA.reg |= SERCOM_SPI_CTRLA_DORD; // 0 MSB, 1 LSB
	//SERCOM0->SPI.CTRLB.reg |= SERCOM_SPI_CTRLB_CHSIZE(0x0); // 8 bits character - 0x0, so no need to set
	
	// (4) setup baud rate
	// f_baud = f_ref / (2 * (BAUD +1)) so BAUD = f_ref / (2 * f_baud) - 1
	
	SERCOM0->SPI.BAUD.reg |= SERCOM_SPI_BAUD_BAUD(126);
	SERCOM0->SPI.CTRLB.reg |= SERCOM_SPI_CTRLB_MSSEN | SERCOM_SPI_CTRLB_RXEN; // slave select hardware yes
	
	SERCOM0->SPI.CTRLA.reg |= SERCOM_SPI_CTRLA_ENABLE;
	
    while (1) 
    {
		while(!(SERCOM0->SPI.INTFLAG.bit.DRE));
		SERCOM0->SPI.DATA.reg = SERCOM_SPI_DATA_DATA(80);
		PORT->Group[1].OUTTGL.reg = (uint32_t)(1 << 9); // i-v, to check we made it thru setup
    }
}
```

![atsamd51 spi](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-spi-alive-atsamd51.png)

Jake Read's avatar
Jake Read committed
504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519
Now we do v0.31 board, new step board, etc. Go team, big day.

# Waking up v0.31

![pwm-one-channel](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-spi-atsamd51-as5147.png)

Have this SPI running with the encoder in-line and PWM still setup.

I integrated UART / Ringbuffers etc from the mkstepper project. So I have everything I need to start commutating. 

OK! 

## Commutating

Properly, I should do this on a timer. I'm going to do it in the while() loop for now, just to check that I'm having the output on the PWMs that I want.

520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541
Great - I have this running in the open while() loop. It commutates! I suppose I shouldn't be surprised at this stuff anymore.

I set up my logic analyzer so that I can start on this commutation debug cycle. I have three pins on the lo-sides of the PWMs, three on the voltage sense pinse, and two on the current sense pins. 

I'm hearing this 'tick' every so often as the motor commutates. Here's what it looks like on the analyzer:

![pwm-tick](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-pwm-ticking.png)

So I think that one of the PWM registers is occasionally being written to 100% in error. My suspicion is that this has something to do with my fast-and-loose commutation scheme, which I'm about to improve.

While I did notice that this was only occuring while the motor driver's gates where enabled (so, 'stuff was happening'), I tried using the ATSAMD's PWM Capture-Compare Buffer (capture-compare is the value the pwm timer checks to switch-or-not-switch the output). The buffer let's me write into the PWM registers when I'm sure they're not being read by the peripheral. This eliminated the problem. I also pushed the PWM frequency to 22kHZ and it's all silky smooth sounding now.

OK, some current / voltage waveforms:

Channels: Fault, PWM Hi U, Pwm Hi V, Pwm Hi W, Current V, Current W, Voltage V, Voltage W.

![currents](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-currents-olcommutate-1.png)

![currents](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/raw/master/images/programming-currents-olcommutate-2.png)

I set this up to accept a commanded 'torque' (just PWM duty cycle) and direction, so next step here is doing some network integration as well.

Jake Read's avatar
Jake Read committed
542
[Video](https://gitlab.cba.mit.edu/jakeread/mkbldcdriver/blob/master/video/20khz-commutate.mp4)
543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558

Then, a longer list of development:

# Next:

## Firmware
 - Closed-Loop Speed Control (maybe using simple 6-step commutation?)
 - Search for Encoder Offset
 - Closed-Loop Position Control
  - Probably just Sinusoid PWM Commutation
 - The Big Bite: FOC

## Hardware
 - I have a list of incremental improvements... mostly:
 - Go to 2oz copper so that I dont' blow up any traces when the power hits
 - Discrete LEDs and more indication (there's an overcurrent / overtemp warning I want to break out)