Etiqueta: Power Functions

ev3dev – using IRLink with python

I got myself a HiTechnic IRLink sensor.

As of today (August 2016) ev3dev already recognizes the IRLink as a nxt-i2c sensor but there’s no language support for it. David Lechner suggested me using the “direct” attribute to communicate directly with the IRLink at I2C level.

Last time I wrote something mildly related to I2C was about 20 years ago for a Microchip PIC project but well… why not?

So after lots of trial and error, reading several times the LEGO Power Functions RC Protocol and shamelessly copying code from Mike Hatton (“Parax”), Xander Soldaat and Lawrie Griffiths I found on GitHub, RobotC forum and LeJOS forum I fanally managed to control a PF motor in ComboPWM mode.

In the following video, I’m increasing the motor speed (all 7 steps) then decreasing it again until it stops:

This is the python script running in the EV3:

#!/usr/bin/python

#
# based mainly on RobotC code from Mike Hatton ("Parax") and Xander Soldaat
# but also on LeJOS code from Lawrie Griffiths
#

# assumes IRLink at Input 1 as sensor0

import sys
from time import sleep

# channel: 0..3
# motorA, motorB: 0..7

channel = 0
for motorA in (1,1,2,2,3,3,4,4,5,5,6,6,7,7,6,6,5,5,4,4,3,3,2,2,1,1,0,0):

  motorB = motorA

  iBufferSize=2
  iBuffer = bytearray(iBufferSize)

  iBuffer[0] = ((0x04 | channel) << 4) | motorB
  iBuffer[1] = motorA << 4
  check = 0xF ^ (0x04 | channel) ^ motorB ^ motorA
  iBuffer[1] = iBuffer[1] | check

  oBufferSize=14
  oBuffer = bytearray(oBufferSize)

  # clear all positions
  for i in range (0,oBufferSize):
    oBuffer[i]=0

  oBuffer[0]=0x80    # Start Bit

  oBufferIdx = 0

  for iBufferByte in range (0,2):
    for iBufferIdx in range (0,8):
      oBuffer[1 + (oBufferIdx / 8)] |= (0x80 >> (oBufferIdx % 8) )
      if ( ( ( iBuffer[iBufferByte] ) & (0x80 >> (iBufferIdx % 8) ) ) != 0 ) :
        oBufferIdx = oBufferIdx + 6
      else:
        oBufferIdx = oBufferIdx + 3

# Stop bit
  oBuffer[1+ (oBufferIdx / 8)] |= (0x80 >> (oBufferIdx % 8) )

  tailIdx = 1 + (oBufferIdx / 8) + 1

  # Tail


  if (tailIdx == 10):
    oBuffer[tailIdx]= 0x10 # IRLink message payload length
    register = 0x43
  else:
    oBuffer[tailIdx]= 0x11
    register = 0x42

  oBuffer[tailIdx+1]= 0x02 # IRLink in Power Functions Mode
  oBuffer[tailIdx+2]= 0x01 # IRLInk Start transmission 


# clear IRLink (not sure if correct but seems to improve)

  fd = open("/sys/class/lego-sensor/sensor0/direct", 'wb',0)
  fd.seek(0x41)
  fd.write(chr(0x46))
  fd.write(chr(0x44))
  fd.write(chr(0x4C))
  fd.write(chr(0x50))
  fd.close()
  sleep(0.1)

  for i in range(0,5):
    fd = open("/sys/class/lego-sensor/sensor0/direct", 'wb',0)
    fd.seek(register)
    for oBufferIdx in range (0,oBufferSize):
      fd.write(chr(oBuffer[oBufferIdx]))
    fd.close()

    # Power Functions timings (for a 5-command burst)
    if (i==1):
      sleep(0.064)
    elif (i==5):
      sleep(0.096)
    else:
      sleep(0.080)

 

SBrick Remote Control

A fast incursion in Python and Tkinter (a very easy GUI library for those like me who abuse from Google-Copy-Paste) so I can play with SBrick from my Ubuntu laptop without the command line:

SBrick-RemoteControl

#!/usr/bin/env python 

from Tkinter import *
from time import sleep
from subprocess import call

def quit():
  global Window
  Window.destroy()

  return;

def STOP():

  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0002",shell=True)
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0003",shell=True)
  return

def NORTH():

  time=scale.get()
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010301FF",shell=True)
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010200FF",shell=True)
  sleep(time)
  STOP()
  return

def SOUTH():

  time=scale.get()
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010300FF",shell=True)
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010201FF",shell=True)
  print("Backward")
  sleep(time)
  STOP()
  return

def EAST():

  time=scale.get()
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010301FF",shell=True)
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010201FF",shell=True)
  print("RIGHT")
  sleep(time)
  STOP()
  return

def WEST():

  time=scale.get()
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010200FF",shell=True)
  call("gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010300FF",shell=True)
  sleep(time)
  STOP()
  return


Window = Tk()
Window.title("SBrick Remote Control v0.1")

B_North = Button(text = "N", command = NORTH)
B_North.grid(row=0, column=1)

B_West = Button(text = "W", command = WEST)
B_West.grid(row=1, column=0)

B_STOP = Button(text = "STOP", command = STOP)
B_STOP.grid(row=1, column=1)

B_East = Button(text = "E", command = EAST)
B_East.grid(row=1, column=2)

B_South = Button(text = "S", command = SOUTH)
B_South.grid(row=2, column=1)

scale = Scale(Window, from_=0.125, to=2.5, digits=3, resolution=0.125, orient=HORIZONTAL, length=250, label="Time")
scale.grid(row=3,column=0,columnspan=3)

B_QUIT = Button(text = "QUIT", command = quit)
B_QUIT.grid(row=4, column=1, )

mainloop()