Wishful Coding

Over here in the Netherlands we usually have just a few days or weeks of snow per year. When the time comes, there are three things that absolutely have to happen.

1. Go sledging
2. Go ice skating
3. Build a robot to drive in the snow

I wont bother you with the first two items, but I thought you might like number three.

I was surprised that I couldn’t find a single video of  a snow plow, plowing real snow. There are robots pushing LEGO around, and static models of a snow plow. This is what I made.

The robot is a regular half-track, but uses a triangular tread configuration to keep the motor out of the snow. It also has a rotation sensor on the unpowered front wheels, to detect the robot isn’t moving.

I’m not sure if you are interested in the software or building instructions, because it uses the RCX with parts from Ultimate Accessories and Ultimate Builders sets, as well as miscellaneous red parts. Are you?

After my success with the Cyber arm, I had to try my luck at the Sciento one.

This time I was out of luck with Dutch or English documentation, but my brother found a Spanish PDF, from which I was able to get the needed information using Google Translate.

Unlike the Cyber, the Sciento is controlled by sending actual ASCII commands, rather than stepping the motors directly.

It is a lot smarter than the other arm, as it can remember its position. A list of commands:

• “Z” Sets the reference point
• “N” Go to the reference position
• “C”/”O” Close/open the claw
• “M100,0,-250,0,0,0” Move the motors for the specified steps.
• “H7” Save the current position in the specified register(1-100)
• “P5,500,200,-5,0,0,0” Save the specified position in the specified register(first number, 1-100)
• “G1” Move to the location in the specified register
• “S5” Set the motor speed(1-5)
• “D1” Wait for x seconds. Useless?
• “L1” Check the limits of the motors. Recommended for testing, off by default.

Note that numbers are represented as ASCII text and that commands are upper-case. All commands are terminated by a carriage return.

A downside to this arm is that in comparison to the one-byte commands for the Cyber, it takes ages to send an ASCII command across, making this arm unsuitable for smooth control with a gamepad.

I programmed a coarse and ugly controller, with buttons for storing positions and moving to them. In the video I’m just stepping through the motions I stored earlier.

The controller is a real mess, based on the Cyber code, and not very interesting.

The code for the arm itself is nice though. Note the use of the actual strobe line, as opposed to the strobe bit in the Cyber code.

import parallel
from time import sleep

p = parallel.Parallel()

def write(s):
    bs = map(ord, s)
    bs.append(13)
    for b in bs:
        p.setData(b)
        p.setDataStrobe(1)
        p.setDataStrobe(0)
        sleep(0.001)

def command(cmd, *params):
    write(cmd+",".join(map(str, params)))

def move(m1=0, m2=0, m3=0, m4=0, m5=0, m6=0):
    "Move a number of steps"
    command("M", m1, m2, m3, m4, m5, m6)

def here(n):
    "Save current position in slot n 1-100"
    command("H", n)

def position(n, m1, m2, m3, m4, m5, m6):
    "Save the specified position in slot n 1-100"
    command("P", n, m1, m2, m3, m4, m5, m6)

def zero():
    "Set the reference point"
    command("Z")

def goto(n):
    "Go to position in slot n 1-100"
    command("G", n)

def home():
    "Return to the reference"
    command("N")

def open():
    "open the claw"
    command("O")

def close():
    "Close the claw"
    command("C")

def speed(n):
    "Set the speed 1-5"
    command("S", n)

def delay(n):
    "Wait for n seconds"
    command("D", n)

def limit(b):
    "enable/disable limits"
    commands("L", b)

We got 2 robotic arms from a school lab that did them away. After we got them, they had been accumulating dust for ages in our shed, with one failed attempt at making them work in between.

This was my second attempt. As you can imagine, there aren’t nearly as many people writing about robotic arms as there are writing about Clojure or Node.js, so documentation is sparse.

The most useful (English!) resource I found was this collection, which has original manual pages he got from a museum(!). I contacted him for a couple more pages, which where very helpful.

I can only say it was very hard to find out how easy it was to control this arm.

Basically, you have one pin for every motor, and 2 control pins. To send a command you just set the motor pins, and then turn the corresponding toggle pin on and off.

To reverse motor 2:

01000000
01000001
01000000

To turn motor 3 one step:

00100000
00100010
00100000

Once I figured that out, I probably spent most of my time installing stuff on Windows, then installing Ubuntu and installing there.

To make PyParallel work, I had to rmmod lp and to run as root.

The code, might you have a dusty Cyber 310 in your shed:

import parallel
from time import sleep

base = 1
shoulder = 2
elbow = 4
leftwrist = 8
rightwrist = 16
grip = 32
prf = 64
strobe = 128

p = parallel.Parallel()

def direction(m):
    p.setData(m)
    p.setData(m+strobe)
    p.setData(m)

def step(m):
    p.setData(m)
    p.setData(m+prf)

def move(steps, *motors):
    dir_pattern = abs(sum([m for m in motors if m < 0]))
    step_pattern = sum(map(abs, motors))
    
    direction(dir_pattern)
    for i in range(steps):
        step(step_pattern)
        sleep(0.01)

import os, pygame, cyber
from time import sleep

os.environ["SDL_VIDEODRIVER"] = "dummy"
pygame.display.init()
pygame.init()
j = pygame.joystick.Joystick(1)
j.init()

#axes = {cyber.base:0, cyber.shoulder:1, cyber.elbow:2, cyber.leftwrist:3, cyber.rightwrist:4, cyber.grip:5}
axes = {cyber.base:0, -cyber.shoulder:1, -cyber.elbow:4}

def dead_int(real, deadzone=0.5):
    if real > deadzone:
        return 1
    elif real < -deadzone:
        return -1
    else:
        return 0

print 'Initialized Joystick : %s' % j.get_name()
try:
    while True:
        pygame.event.pump()
        #sleep(0.01)
        movements = [k*dead_int(j.get_axis(v)) for k, v in axes.iteritems()]

	x, y = j.get_hat(0)
	if x == 0 and y != 0:
		wr = (cyber.leftwrist+cyber.rightwrist) * y
		movements.append(wr)
	elif x != 0 and y ==0:
		movements.append(cyber.leftwrist * x)
		movements.append(cyber.rightwrist * -x)

	if j.get_button(0):
		movements.append(cyber.grip)
	elif j.get_button(1):
		movements.append(-cyber.grip)	

        cyber.move(1, *movements)
        
except KeyboardInterrupt:
    j.quit()