Build a DIY brushless motor controller in a day (ish)- Part 2, the codez
Here's part two of my tutorial-ish thing on building a brushless motor controller. Part one, discussing the hardware, is here. The pic above is our brushless robot base after a few tweaks- bigger batteries, slightly more reliable packaging, and just the right number of clip leads. Oh, and an iAMdriver from AndyMark so we can drive it with an iPhone!
So we have a power section and a couple of motors. Let's make some stuff spin, shall we?
Of course, there's a chunk missing here. There needs to be something to take the input from the hall sensors, figure out what to do with it, and spit out some signals to drive the inputs on the power section. And that, as it turns out, is exactly the sort of job a microprocessor excels at. I'm thinking about this with digital halls in mind, but the ideas can be generalized to any commutation scheme.
The digital halls sense the magnetic field of the rotor. There are three of them, one for each phase, and they're spaced so that they coincide with the sine waves produced by the back EMF of the motor. Now, the thing to know about this (and something that I didn't pick up on right away) is that they coincide with the back EMF between phases. So, if one were to put the ground of a scope probe on one of the wires coming out of a motor, and the probe itself on one of the other wires, the sine wave produced when the motor is spun by hand would correspond to the output of one of the halls. Essentially, the zero points of the sine waves correspond to the state changes of the halls.
This is different from what I thought at first- after custom building a motor, I had measured the back EMF with respect to the common point to all of the windings. There is a bit of a phasing difference between this and what is measured between phases. It turns out that this phasing difference is one of the things that makes trapezoidal commutation really easy.
When you look at all three of these halls, they spit out what is essentially a Gray code. This can be tied to lookup tables for each phase, which will contain multipliers for the output duty cycles. What we want to create, in the end, is an approximation of a sine wave. What we lose in this approximation is the 30 degrees of the sine wave that does the least work- not really a big deal.
Anyway, I feel like my explanation of this is just getting confusing and everyone involved here will be much better served if I just cut to the chase and show you some code. This code will take a servo pulse (the 1-2 ms pulse hobby servos use to determine position or speed) and converts it into a direction and speed. It's written in PICBASIC PRO for the PIC18F4431 processor I used on the FroBoard. One is used for each motor, since the hardware PWM module only has 4 duty cycle generators- not enough for two motors. A processor with a better interrupt structure and some good low-level programming could handle two motors, but I wanted to keep things simple for this project. So, here's the meat of it:
Main:
if PORTA.1 = 0 then pulsin PORTA.1,1,PulseWidth ;Get pulse
if PulseWidth = 0 then goto Commutation ;PULSIN timed out, carry on
if PulseWidth >= 760 then ;Was pulse greater than 1.5ms?
Dir = 1 ;If so, go forward
DutyCycle = 33*(PulseWidth - 760) ;Scale the duty cycle for later
PORTD.6 = 1 ;Do something meaningful with the LEDs
PORTD.7 = 0
else
if PulseWidth <= 740 then ;Was pulse less than 1.5ms?
DIR = 0 ;If so, go backward
DutyCycle = 33*(740 - PulseWidth) ;Scale duty cycle
PORTD.6 = 0 ;Do something meaningful with the LEDs
PORTD.7 = 1
else
DutyCycle = 0 ;There are 40us of deadband between directions
PORTD.6 = 0 ;Do something meaningful with the LEDs
PORTD.7 = 0
endif
endif
Commutation:
;Assemble Gray code from hall inputs on PORTE
;These bits can be reassigned to find correct phasing
if Dir = 1 then
HallValue.0 = PORTE.0
HallValue.1 = PORTE.1
HallValue.2 = PORTE.2
lookup HallValue,[0,1,2,2,0,0,1],DC0 ;Read duty cycle multiplier
lookup HallValue,[0,2,0,1,1,2,0],DC1 ;for each phase
lookup HallValue,[0,0,1,0,2,1,2],DC2
else ;In reverse, swap two power outputs and two hall inputs
HallValue.0 = PORTE.1
HallValue.1 = PORTE.0
HallValue.2 = PORTE.2
lookup HallValue,[0,1,2,2,0,0,1],DC2
lookup HallValue,[0,2,0,1,1,2,0],DC1
lookup HallValue,[0,0,1,0,2,1,2],DC0
endif
DC0 = DC0*DutyCycle ;Multiply by duty cycle for final value
DC1 = DC1*DutyCycle
DC2 = DC2*DutyCycle
PDC0H = DC0.highbyte ;Finally, set output duty cycles
PDC0L = DC0.lowbyte
PDC1H = DC1.highbyte
PDC1L = DC1.lowbyte
PDC2H = DC2.highbyte
PDC2L = DC2.lowbyte
goto Main
Not too bad, right? There are a couple things to know about what's going on here. The code under the Commutation label does the real motor control stuff. To avoid any nasty floating point stuff, the value held in the DutyCycle variable is scaled to half of what we want the final duty cycle to be. We then multiply by the values in the lookup tables- 0, 1, or 2. So in the AC world, 0 would be full negative, 1 would be neutral, and 2 would be full positive. These are the only states we worry about in trapezoidal commutation. A picture is worth a thousand words, so here's my sketch on the whiteboard of what the commutation scheme really looks like:
So anyway, look at the picture, and then look at the code, and then back at the picture, and back at the code. It should start to come together.
All of the stuff above the Commutation label in the main loop is acquiring the servo pulse and calculating a duty cycle. This can be changed to get a duty cycle from anywhere- I've used RS-232 in the past, as well as a potentiometer or a parallel bus coming from a master FroBoard. Here at spingarage, we hooked up the dual FroBoards to an iAMdriver and within a few seconds we were driving the robot around using an iPhone. Since the iAMdriver outputs servo pulses, it was just a matter of plugging in the servo cables. Lots of fun.
The full code is down at the bottom of this post- it's kind of long and Posterous hasn't given me an easy way to upload code files like this yet. I included the configurations for the RS-232 on the FroBoard and the analog to digital converter module built in to the PIC, so all that's left is to enable the modules and write a few lines of code to get talking to them.
As far as actually wiring this thing up- it's pretty straightforward. The power section needs to be grounded, of course. 6 of the remaining 12 logic-level inputs (on the connector labeled "Input" on the schematic) get connected to the PWM outputs on the PIC. The outputs on the PIC are in complementary pairs- PORTB.0 should go to the low-side transistor in a half bridge, PORTB.1 should go to the high-side, and so on. The other 6 inputs will go to the other processor in the same fashion.
The hall sensors coming out of the motor are attached to PORTE. These might be open-collector outputs, so it doesn't hurt to put a 10K pullup resistor between the output and +5V.
The three wires coming out of each motor go to the outputs of the half bridges. Just keep track of which processor is hooked up to which half bridges and which hall sensors, and you'll be fine. If the order is wrong, it's easy to either tweak this in code or switch wires around until it works right. Don't run for longer than necessary with the wires in an incorrect order; stuff can start to heat up if there isn't any back EMF to hold back the battery voltage.
Well, I think that's about it! If you're new to motor controller, I'm sure this is all a bit fuzzy. Tinkering with the code and hardware really helps, so give it a shot! I've let out plenty of magic smoke working on these things, it's all part of the process. If you have any questions, feel free to ask in the comments- I'll do my best to answer them. Good luck!
//AGA
p.s. As promised, here is the full code, including configs and what not:
'****************************************************************
'* spingarage LLC *
'* Name : FroBoard_Servo.BAS *
'* Author : Andrew Angellotti *
'* Notice : Copyright (c) 2011 spingarage LLC *
'* : All Rights Reserved *
'* Date : 5/25/2011 *
'****************************************************************
; This code is intended to use the spingarage FroBoard to drive a brushless
; motor, commutated with digital hall sensors, in forward and reverse based
; on a servo input signal. Connections are as follows:
;
; Hall 1: PORTE.2
; Hall 2: PORTE.1
; Hall 3: PORTE.0
; Servo input: PORTA.1
;
; Logic output to power section is driven by PWM0:2, as follows:
; L0: PORTB.0
; H0: PORTB.1
; L1: PORTB.2
; H1: PORTB.3
; L2: PORTB.4
; H2: PORTB.5
; Configs
;***************************************************************asm
__CONFIG _CONFIG1H, _OSC_HS_1H ;HS Oscillator
__CONFIG _CONFIG2H, _WDTEN_OFF_2H ;No watchdog for now
__CONFIG _CONFIG4L, _LVP_OFF_4L ;No low voltage programming
endasm;***************************************************************
; Defines
;***************************************************************define OSC 20 ;Set oscillator at 20 MHz for PBP
define PULSIN_MAX 1000 ;Set PULSIN timeout to 2ms;***************************************************************
; Variable declarations
;***************************************************************PulseWidth var word
DutyCycle var Word
Dir var bit
DC0 var word
DC1 var word
DC2 var word
HallValue var byte
Temp var word;***************************************************************
; Initialize peripheral modules
;***************************************************************;Initialize I/O
TRISA = %11111111 ;PORTA is input
PORTB = %00000000 ;Initialize PORTB to 0 to ensure half bridges stay low
TRISB = %00000000 ;PORTB is output
TRISC = %11111111 ;PORTC is input
PORTD = %00000000 ;Initialize PORTD to 0
TRISD = %00000000 ;PORTD is output;Initialize EUSART to 9600 8N1
TRISC.6 = 1
TRISC.7 = 1
BAUDCTL = 0 ;8-bit BRG
SPBRG = 31 ;9600 Baud @ 20 MHz
TXSTA = 32 ;Tx enabled
RCSTA = 144 ;Serial port and Rx enabled;Initialize ADC
ADCON0 = %00110100 ;Continuous loop, multi-channel, SEQM2
ADCON1 = %00011000 ;Use AVdd and AVss for Vrefs, use FIFO
ADCON2 = %11111110 ;Right justified result, 64 Tad delay, clock is Fosc/64
ADCON3 = %11000000 ;No interrupts, no triggers
ANSEL0 = %00000000 ;AN7:0 are digital I/O
ANSEL1 = %00000000 ;AN8 is digital I/O (A/D not used)
ADCON0.0 = 0 ;A/D on bit is cleared (A/D disabled);Initialize Power-Control PWM
;Initialize duty cycles to 0 (low side transistors will all be active)
PDC0H = 0
PDC0L = 0
PDC1H = 0
PDC1L = 0
PDC2H = 0
PDC2L = 0
PTCON0 = %00000000 ;1:1 post and prescale, free-running
PTCON1 = %10000000 ;Turn on PWM time base, count up
PWMCON0 = %01000000 ;PWM0:5 enabled in complementary mode
PWMCON1 = %00000000 ;Special event configs (not implemented)
DTCON = %11111111 ;Dead time clock source is Fosc/16- dead time set to max
OVDCOND = %11111111 ;No overrides
OVDCONS = %00000000 ;Output inactive when override bit is cleared
FLTCONFIG = %00000000 ;Faults disabled
PTPERL = %00000000
PTPERH = %00001111 ;This ends up being ~1KHz. Not necessarily optimal.;***************************************************************
; Mainloop
;***************************************************************
Main:
if PORTA.1 = 0 then pulsin PORTA.1,1,PulseWidth ;Get pulse
if PulseWidth = 0 then goto Commutation ;PULSIN timed out, carry on
if PulseWidth >= 760 then ;Was pulse greater than 1.5ms?
Dir = 1 ;If so, go forward
DutyCycle = 33*(PulseWidth - 760) ;Scale the duty cycle for later
PORTD.6 = 1 ;Do something meaningful with the LEDs
PORTD.7 = 0
else
if PulseWidth <= 740 then ;Was pulse less than 1.5ms?
DIR = 0 ;If so, go backward
DutyCycle = 33*(740 - PulseWidth) ;Scale duty cycle
PORTD.6 = 0 ;Do something meaningful with the LEDs
PORTD.7 = 1
else
DutyCycle = 0 ;There are 40us of deadband between directions
PORTD.6 = 0 ;Do something meaningful with the LEDs
PORTD.7 = 0
endif
endif
Commutation:
;Assemble Gray code from hall inputs on PORTE
;These bits can be reassigned to find correct phasing
if Dir = 1 then
HallValue.0 = PORTE.0
HallValue.1 = PORTE.1
HallValue.2 = PORTE.2
lookup HallValue,[0,1,2,2,0,0,1],DC0 ;Read duty cycle multiplier
lookup HallValue,[0,2,0,1,1,2,0],DC1 ;for each phase
lookup HallValue,[0,0,1,0,2,1,2],DC2
else ;In reverse, swap two power outputs and two hall inputs
HallValue.0 = PORTE.1
HallValue.1 = PORTE.0
HallValue.2 = PORTE.2
lookup HallValue,[0,1,2,2,0,0,1],DC2
lookup HallValue,[0,2,0,1,1,2,0],DC1
lookup HallValue,[0,0,1,0,2,1,2],DC0
endif
DC0 = DC0*DutyCycle ;Multiply by duty cycle for final value
DC1 = DC1*DutyCycle
DC2 = DC2*DutyCycle
PDC0H = DC0.highbyte ;Finally, set output duty cycles
PDC0L = DC0.lowbyte
PDC1H = DC1.highbyte
PDC1L = DC1.lowbyte
PDC2H = DC2.highbyte
PDC2L = DC2.lowbyte
goto Main
