NXT Lego Motor Driver
Following on from my h-bridge motor driver skin, I encountered the 'god of engineering' trying to put it to use - dc motor's are high-speed but low-torque, gearing is required, shoddy gears have a lot of friction. Hence this; a driver for Lego NXT (v2.0?) motors which have reasonable in-built gearing. It again requires a TI754410 quad h-bridge (albeit I've only used half of it) and has the advantage (more later) of having an inbuilt rotary encoder that you can read and (hope) to use to determine actual, rather than set, speed.
This one;
- again needs my pwm module, for the moment and a TI754110 or similar
- and an external 9v battery to provide enough juice for the motor
- and preferably an NXT cable socket (in my case de-soldered from another Kickstarter - the BrickPi)
- and my library is setup for X/Y orientation of my particular skin, but should be trivial to change pins to suit any configuration
I again built this on a bit of 12x8 perfboard, this time with the TI754410 right-way-up, added some diodes to reduce the chances of me burning things out with wrong-ways battery connections, and (because there was room) added in a little bitty 3mm led for flashy-indicatory-kind-of-thingies.
TI SN 754410 NE
Quad H-bridge
-----------------------
|1 1,2EN +ve 16| -> Vin
|2 1A 4A 15| -> Pin Y11
|3 1Y 4Y 14| -> NXT Mtr / Black Wire / Motor Pole 1
Gnd & -ve Bat via diode <- |4 Gnd Gnd 13| -> Gnd & -ve Bat via diode
Gnd & -ve Bat via diode <- |5 Gnd Gnd 12| -> Gnd & -ve Bat via diode
|6 2Y 3Y 11| -> NXT Mtr / White Wire / Motor Pole 2
|7 2A 3A 10| -> Pin Y10
+ve Bat <- diode <- |8 +ve Bat 3,4EN 9| -> Pin Y9
-----------------------
Lego NXT cable termination block
-------
| + -> Black Wire / Motor Pole 1 -> H-Bridge Pin 14 (Y4)
| + -> White Wire / Motor Pole 2 -> H-Bridge Pin 11 (3Y)
| + -> Green Wire / Power for sensor / pyboard 3.3v Pin
| + -> Red Wire / Gnd for sensro / to Gnd
| + -> Blue Wire / Rotary Encoder Tack 0 / Pin Y8
| + -> Yellow Wire / Rotary Encoder Tack 1 / Pin Y7
-------
LED
Pin Y12 -> Led -> Gnd (no current limiting resistor, living dangerous!)
known limitations
- inherits the limitations of my PWM module in terms of which pins can be used for enable on the h-bridge
- whilst there's built-in speed calibration, it turns out motor speed is non-linear to pwm 'voltage' ... so, whilst I've tried to match speed sensor output to pwm input range, it doesn't really work and will require mathematics that are beyond me
- only wired for one motor (there's not a lot of room on a 12x8 bit of perfboard once you've got the chip and an NXT connector on it - especially if you're soldering iron has a tip the size of a Volkswagen Beetle)
- it seems to crash the pyboard occasionally
usage
basic
>>> import nxtmotor
>>> nxt=nxtmotor.NXTMOTOR(nxtmotor.Y) #instance the class for skin in Y position
>>> nxt.forwards() #full steam ahead
>>> nxt.backwards() #full steam backwards
>>> nxt.stop() #full stop
>>> nxt.forwards(255/2) #half steam ahead
>>> nxt.stop() #full stop
>>> nxt.state(255/2) #half speed ahead
>>> nxt.state(-255/2) #half speed backwards
>>> nxt.state() #what speed have we set anyway?
-127
>>> nxt.stop() #full stop
>>> nxt.state()
0
calibration
Your motor, power supply and what-not will differ from mine. I've set some defaults (the constants CALIB_HIGH and CALIB_LOW, which you can change) there is a method to run a simple test, calculate and set suitable values, you probably want to run it without the motor being under load;
>>> nxt.calibrate()
Spinning up to maximum speed ...
Capturing 1000 samples at max speed, 10ms intervals, please wait ...
Spinning down to lowest speed ...
Capturing 1000 samples at lowest speed, 10ms intervals, please wait ...
Calibration high: 3944
Calibration low: 6782
>>> nxt.calib_high #the high set-point is now set
3944
>>> nxt.calib_low #the low set-point is also now set
6782
These calibration values are used to scale the actual numbers that come out of the rotary encoder to try and match them to the pwm range of 0-255 ... but, as noted above; it doesn't work very well ... motor behaviour appears to have a non-linear response to input pwm duty/voltage.
reading the speed via the rotary encoder (or trying to)
>>> nxt.state(50)
>>> nxt.speed()
54.6535
>>> nxt.speed()
54.2636
>>> nxt.state(255)
>>> nxt.speed()
271.432
>>> nxt.speed()
254.876
>>> nxt.speed()
252.651
looks promising!!!, but then again;
>>> nxt.state(100)
>>> nxt.speed()
168.561
>>> nxt.speed()
169.405
>>> nxt.state(200)
>>> nxt.speed()
231.582
>>> nxt.speed()
229.006
curse the real world, why is nothing simple? ;o) ... if you're smarter than me you can no-doubt improve the calibration routine and speed calcs; self.__pulsetime (time since last rising edge on monitored encoder tack) and self._pulsedir (direction the encoder thinks it's spinning) are the key
But to make up for all that silliness, at least you can turn the led on and off eh?
>>> nxt.led(True) #on - fantastic
>>> nxt.led(False) #off - not so fantastic
the module
nxtmotor.py
import pyb, pwm, gc
#these values relate to pwm range, don't change without changing pwm.py
FORWARDS = 255
BACKWARDS = -FORWARDS
STOP = 0
#the lowest speed my motor will actually turn at is 35
#behaviour of speed measurements is put off by v.low values
LOW_SPEED = 50
#led statuses
LED_ON = 1
LED_OFF = 0
#change these values to the average output of .calibrate()
#for your motor(s) - these are mine, not yours!
CALIB_HIGH = 3875
CALIB_LOW = 6600
#possible nxt motor shield configurations, depends on skin orientation
Y = { 'enable': pyb.Pin.board.Y9, \
'control0': pyb.Pin.board.Y10, \
'control1': pyb.Pin.board.Y11, \
'led': pyb.Pin.board.Y12, \
'tack0': pyb.Pin.board.Y8, \
'tack1': pyb.Pin.board.Y7 \
}
X = { 'enable': pyb.Pin.board.X9, \
'control0': pyb.Pin.board.X10, \
'control1': pyb.Pin.board.X11, \
'led': pyb.Pin.board.X12, \
'tack0': pyb.Pin.board.X8, \
'tack1': pyb.Pin.board.X7 \
}
class NXTMOTOR:
"""dirty NXT Motor class"""
def __init__(self, pins_dict, reverse_pols=False, calib_high=CALIB_HIGH, calib_low=CALIB_LOW):
self.__enable = pwm.PWM(pins_dict['enable'])
self.__control0 = pins_dict['control0'] if not reverse_pols else pins_dict['control1']
self.__control0.init(pyb.Pin.OUT_PP)
self.__control0.low()
self.__control1 = pins_dict['control1'] if not reverse_pols else pins_dict['control0']
self.__control1.init(pyb.Pin.OUT_PP)
self.__control1.low()
self.__led = pins_dict['led']
self.__led.init(pyb.Pin.OUT_PP)
self.__tack0 = pins_dict['tack0'] if not reverse_pols else pins_dict['tack1']
self.__tack1 = pins_dict['tack1'] if not reverse_pols else pins_dict['tack0']
self.__tack1.init(pyb.Pin.IN)
self.calib_high = calib_high
self.calib_low = calib_low
#start a very fast timer, with a high period (pyb.millis doesn't give enough resolution)
self.__ucounter = pyb.Timer(6, prescaler=83, period=0x7fffffff)
self.__pulsetime = 0 #this is measured speed ... we are at rest
self.__pulsedir = -1 #invalid __pulsedir value
#register interrupt on tack0
self.__extint = pyb.ExtInt(self.__tack0, pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_UP, self.__ISR)
@micropython.native
def __ISR(self, line):
self.__pulsetime = self.__ucounter.counter()
self.__ucounter.counter(0)
self.__pulsedir = self.__tack1.value()
def speed(self):
if self.__pulsetime > self.calib_low+2000:
#it is safe to assume we've stopped ...
return 0
else:
sign = +1 if self.__pulsedir else -1 #account for direction
return sign*((((1/self.__pulsetime) - (1/self.calib_low)) * ((FORWARDS-LOW_SPEED) / ((1/self.calib_high)-(1/self.calib_low))))+LOW_SPEED)
def calibrate(self, num_samples=1000):
samples_high = []
samples_low = []
print('Spinning up to maximum speed ...')
self.state(FORWARDS)
pyb.delay(1000) #let the motor get up to speed
print('Capturing 1000 samples at max speed, 10ms intervals, please wait ...')
for i in range(num_samples):
samples_high.append(self.__pulsetime)
pyb.delay(10)
print('Spinning down to lowest speed ...')
self.state(LOW_SPEED) #on my rig
pyb.delay(3000) #let the motor get up to speed
print('Capturing 1000 samples at lowest speed, 10ms intervals, please wait ...')
for i in range(num_samples):
samples_low.append(self.__pulsetime)
pyb.delay(10)
self.stop()
self.calib_high = int(sum(samples_high)/len(samples_high))
self.calib_low = int(sum(samples_low)/len(samples_low))
print('Calibration high: {0}\nCalibration low: {1}'.format(self.calib_high, self.calib_low))
gc.collect()
def state(self, value=None):
"""get or set motor state as -ve|0|+ve as backwards|stop|forwards"""
if value == None:
if self.__enable.duty() > 0:
if self.__control0.value() and not self.__control1.value():
return -self.__enable.duty()
elif not self.__control0.value() and self.__control1.value():
return self.__enable.duty()
else:
return ValueError('Inconsistent state')
else:
return 0
elif value < 0:
self.__control0.high()
self.__control1.low()
self.__enable.duty(abs(value))
elif value > 0:
self.__control0.low()
self.__control1.high()
self.__enable.duty(abs(value))
elif value == 0:
self.__enable.duty(0)
else:
raise ValueError('Invalid state value')
def backwards(self, value=-BACKWARDS):
self.state(-value)
def forwards(self, value=FORWARDS):
self.state(value)
def stop(self):
self.state(STOP)
def led(self, value=None):
if value == None:
return self.__led.value()
else:
self.__led.value(value)