EV3 Sumo Robot

After a long period of silence, I finally made some things with my EV3. Last time the tools where not really ready, but now everything works great.

I used ev3dev and python-ev3 to build this bulldozer. Watching sumo matches at LEGO World, I saw a few things that do and don’t work.

A lot of people seem to go for blades and hammers, but unless you actually want to destroy the robot, they are useless. Treads sound like a good idea, but wheels have more grip in practice. Make sure the wheels don’t fall off. Encased wheels are the best.

The goal is to push the other robot off the table, so the more powered rubber the better. Four wheel drive is better than rear wheel drive, but rear wheel drive is still much better than front wheel drive.

It’s all about pushing. The only useful thing besides pushing forward is pushing up or sideways. Pushing up is the easiest, because you have the floor to back you up. It also pushes the opponent’s wheels off the ground. This lead me to the following design.

I used two pairs of driven rear wheels on a pivot, so even if I’m lifted, all four remain in contact with the ground. They are geared down, which means I’m slow, but powerful. All the weight of the motors and EV3 is directly above the wheel, for maximum grip.

On the front there is a lift arm, using a worm wheel and a very solid construction. I went through several iterations, improving the strength each time. On the first iteration, it was to weak, but its wedge shape still lifted my opponent a little.

The code is quite simple. Push forward. When you reach the edge, turn around. When something hits the bumper, raise the arm.

from ev3 import lego
import time
import select
import os

# Utility for waiting on the motor.
p = select.poll()
callbacks = {}

def poll():
    for fd, mask in p.poll(0):
        os.lseek(fd, 0, os.SEEK_SET)
        state = os.read(fd, 16)
        if state == "idle\n":
            p.unregister(fd)
            os.close(fd)
            callbacks.pop(fd)()

def on_completion(motor, callback):
    fd = os.open(motor.sys_path + '/state', os.O_RDONLY)
    callbacks[fd] = callback
    os.read(fd, 16)
    p.register(fd, select.POLLPRI)

# Initiate all sensors and motors
l = lego.LargeMotor("A")
r = lego.LargeMotor("D")
lift = lego.MediumMotor()

dist = lego.InfraredSensor()
line = lego.ColorSensor()
touch = lego.TouchSensor()

# reset the motors
l.reset()
r.reset()
lift.reset()

try:
    # run forwards
    l.run_forever(100, regulation_mode=False)
    r.run_forever(100, regulation_mode=False)
    # main loop
    while True:
        poll()
        if touch.is_pushed and lift.state == "idle":
            # we hit something, raise the arm!
            lift.run_position_limited(-3000, 1000)
            on_completion(lift, lambda: lift.run_position_limited(0, 1000))
        elif line.reflect < 20 and l.duty_cycle > 0 and r.duty_cycle > 0:
            # we reached the edge, back up and scan for opponent
            l.run_time_limited(1000, -100)
            on_completion(l, lambda: l.run_forever(100))
            r.run_forever(-100)
        elif dist.prox < 50:
            # found opponent, charge!!!
            r.run_forever(100)
            l.run_forever(100)
finally:
    # stop motors and lower arm
    l.stop()
    r.stop()
    lift.run_position_limited(0, 1000)

Generic programming tutorial

I have wanted to teach you some programming since I started this blog, but I refrained from doing so, because there are so many ways to program, and I want to use them all.

What I’m going to attempt, is not to teach you a programming language, but teach about programming itself. This may seem useless to you, but my experience is that when you understand programming, the language does not really mater, and you can program in anything that is not excessively weird.

For practical reasons, any code examples will be written in NBC, Python, NXT-G and Clojure.

A computer

To understand programming, you first need to understand what a computer is. For our purpose, it suffices to think of a computer as a processor, a block of storage, and a block of memory.

This is true for desktops, smartphones, the NXT, and anything else with these components. Of course there are some other components involved, which can be divided in inputs(keyboard, sensor) and outputs(screen, motor).

What a computer does, is read instructions from the storage, execute them on the processor, which modifies the memory and reads/writes to the in- and outputs.

To give you an idea, you could put instructions in the storage that tell the computer to store the number 2 in memory, copy it, and multiply the two numbers, saving the result in memory.

A more useful set of instructions could put the input from the keyboard to the screen, or read a sensor, do some math and control the motor.

A compiler

When I said instructions, I did not mean instructions in plain English. Processor instructions are not easy to read and write for humans, that is why we let computers translate them for us.

In its most basic form, a compiler is a set of instructions which converts words like “add” and “read” to stuff that a computer understands.

More advanced compilers also allow you to define new words, such as “turn left”, in terms of other words, such as “motor on” and “motor off”.

A language

A language is a set of instructions, as understood by a specific compiler. A language consists of a few things.

A syntax

This is like grammar for compilers. A few examples of adding a number:

  • Python: 1 + 1
  • Clojure: (+ 1 1)
  • NBC: add 1 1 result
Which are all the same thing, except that NBC is a statement instead of an expression, more about that later.

An API

This is the hardest part of programming. But let me tell you, even good programmers don’t remember all APIs, you simply google them.

An API is the set of words at your disposal to express your problem. This API is different for every language.

In NBC there is a word called “OnFwd” which can be used with an output, like “OnFwd(OUT_A)”, but in Python, there is no such thing. Python doesn’t even know what a motor is, or what forward means.

However, people have already defined words to talk about the NXT in Python. To use words already defined elsewhere, we can say “from nxt.motor import Motor, PORT_A” in Python. Now we can say “Motor(my_nxt, PORT_A).run()”

Expressions

Expression have a value. The value of (+ 1 1) is 2, so we can also say (* 2 (+ 1 1)), which has the value 4.

People commonly say expressions ‘return’ a value, which is what you do when you define an expression in Python:

def expressions():
    return 2

Statements

Unlike expressions, statements do not have a value. What would be the value of “while True:”(the start of a loop in Python)?

Note that not all languages have expressions and statements.

NBC has only statements, which is why you write “add 1 1 result”, so that the result of the addition gets saved in memory.

Clojure has only expressions. if something has no useful value, it returns nil.

Python is a mixed bag.

References

So far I have talked about that block of memory as an abstract thing where you save and retrieve values. In reality, it is very useful to label the box you put it in.

For example, in Python you can say

x = 1
y = x + 1

These are statements that store the value of the expression. ‘x’ now references the value of 1, which is 1. ‘y’ is now a reference to the value of adding the value of x(1) to the value of 1(1).

Collections

So far we used numbers as values, but what if you want to talk about a collection of things?

languages usually provide means of defining a collection of things, and for doing things to the elements or the whole collection, like sorting it, or getting/setting elements.

An array of 5 integers in NBC:

dseg segment
  int reference[5]
dseg ends

Functions

We talked earlier about the words that make up an API. In most cases, words are also just references to values.

These values are usually called functions, and consist of a collection of other functions.

Not all languages have functions as values. In NBC, functions are statements, which don’t have a value. Clojure, however…

(def x 2)
(def square (fn [n] (* n n)))
(square x)

Here, I defined ‘x’ a reference to the value 2, and ‘square’ a reference to a function that multiplies a number by itself. The, I called the value of ‘square’ with the value of ‘x’, resulting in the value 4.

Learning a programming language

I hope to have given you a good understanding of how a programming language works. To actually start using a language like NBC, you need to find out a few things:

  • How do I use words/call functions?
  • How do I define references to values in memory?
  • How do I define new words?
  • What existing words do I have at my disposal?

A good starting place is usually a beginners tutorial. Google for “<language> tutorial” and click the first result.

After a section or two, they usually start to talk about how to do things. You might continue, or stop here, and get your hands dirty. If you run into trouble, simply google for “<language> how to <problem>” or find the function reference by searching for “<language> function reference>”.

If you are really in deep trouble, Stack Overflow is a great website for asking questions.

I hope this is enough preparation for you to start learning, and for me to focus on explaining how to do things in any language needed, like plotting a picture with the NXT 😉

Explorer robot without sensors

Usually the first robot you make when you get the NXT is the wheelbase with a bumper, you know, make it run into a wall, turn back and repeat.

To really get my point about saving sensors across, I made a robot like that without any sensors.

It works by turning the motors in regulated mode(constant speed, varying power) and measuring the actual power applied. If the robot runs into a wall, the firmware will apply extra power to the motors to keep them turning. With some tweaking, you can even detect which wheel hit the wall.

The commented code:

// define 2 variables for containing the actual speed
dseg segment
  aaspeed byte
  caspeed byte
dseg ends

thread main
Start:
  // turn the motors on, regulated
  OnFwdReg(OUT_AC, 50, OUT_REGMODE_SPEED)
  // wait for the robot to accelerate
  // it will apply full power here
  wait 1000
Forever:
  // get the actual power used
  getout aaspeed OUT_A ActualSpeedField
  getout caspeed OUT_C ActualSpeedField

  // print the power to the screen
  NumOut(0, LCD_LINE1, aaspeed)
  NumOut(0, LCD_LINE2, caspeed)

  // if one of the motors uses more than 75 power
  // jump to either LResistance or RResistance
  brcmp GT LResistance aaspeed 75
  brcmp GT RResistance caspeed 75

  // repeat forever
  jmp Forever

LResistance:
  // reverse, turn right, jump to start
  OnRevReg(OUT_AC, 50, OUT_REGMODE_SPEED)
  wait 2000
  OnFwdReg(OUT_A, 50, OUT_REGMODE_SPEED)
  wait 500
  jmp Start

RResistance:
  // reverse, turn left, jump to start
  OnRevReg(OUT_AC, 50, OUT_REGMODE_SPEED)
  wait 2000
  OnFwdReg(OUT_C, 50, OUT_REGMODE_SPEED)
  wait 500
  jmp Start
endt

How fast line following works

When using Robotic Invention System, or NXT-G for programming a robot, line following is usually done like this:

If the light is more than 50, turn left, else turn right.

This results in a slow scanning motion. It works fine for a first time, but soon, you’ll want to go faster.

I used to think that you just needed 2 light sensors, one on both sides of the line, so that you could go straight if both where white, and turn towards the one that becomes back. There is a better way.

When the light sensor is on the edge of the line, does it see black or white? In fact it sees a bit of both, so you get something in between. The trick is to think of the line as a gradient, like so.

If you put the NXT in the gray area, you can have a proportional steering function. Light gray means just a bit left, while dark gray means just a bit right.

Proportional, you say? Yes, we can just apply good old PID again!

// Define to which ports the sensor and motors are connected
#define LIGHTSENSOR IN_1
#define LEFT OUT_C
#define RIGHT OUT_A

// Define constants to tweak the algorithm
#define kp 100
#define ki 5
#define kd 30
// And another one to scale the final value
#define scale 10

dseg segment

// Light sensor reading
light word

// target light
target word
high word
low word

// The current error
err sdword
// The previous error
errold sdword
// The integral, all accumulated errors
errint sdword
// The deriviate, the expected next error
errdiff sdword

// Final pid value
pid sdword

// Temporary variable for calculations
temp sdword
temp2 sdword

// power to the motors
leftpower sdword
rightpower sdword

dseg ends

thread main
  // Initialize the light sensor
  SetSensorColorRed(LIGHTSENSOR)

  // Get the time and start turning around
  gettick temp
  add temp temp 3000
  OnFwd(LEFT, 50)
  OnRev(RIGHT, 50)

  // get light sensor reading
  getin light LIGHTSENSOR ScaledValue

  // set high and low to that reading
  mov low light
  mov high light

Circle:
  // Get the light reading
  // if it is more than high, jump to Higher
  // if it is lower than low, jump to Lower
  getin light LIGHTSENSOR ScaledValue
  brcmp LT Lower light low
  brcmp GT Higher light high

  // else check if the time has passed
  // Jump to Done, else go back to Circle
  gettick temp2
  brcmp LT Done temp temp2
  jmp Circle

// set light to the new low
// jump back to Circle
Lower:
  mov low light
  jmp Circle

// set light to the new high
// jump back to Circle
Higher:
  mov high light
  jmp Circle

Done:
  // we now have the max and min light value found
  // calculate the center value
  sub target, high, low
  div target target 2
  add target target low

Forever:
  // Read the sensor and store it in light
  getin light LIGHTSENSOR ScaledValue

  // Substract the actual distance from the target for the current error
  sub err target light // Proportional

  // Add the error to the integral
  add errint errint err // Integral
  mul errint errint 0.8 // multiply by 0.8 to dampen it

  // Sunstract the previous error from error
  // so that we get the speed at which the error changes
  sub errdiff err errold // Derivative
  mov errold err // set the current error as he old error

  mul pid err kp // Apply proportional parameter

  mul temp errint ki // Apply integral parameter
  add pid pid temp

  mul temp errdiff kd // Apply derivative parameter
  add pid pid temp

  div pid, pid, scale       // Apply scale

  NumOut(0,0,target)
  NumOut(0,8,light)

  // saturate over 100 and under -100
  brcmp LT, under100, pid, 100
  mov pid, 100
under100:
  brcmp GT, overMin100, pid, -100
  mov pid, -100
overMin100:

  // subtract pid from one of the motors
  brtst LT, Negative, pid
  OnFwd(LEFT, 100)
  sub rightpower 100 pid
  OnFwd(RIGHT, rightpower)

  jmp Run
Negative:
  OnFwd(RIGHT, 100)
  add leftpower 100 pid
  OnFwd(LEFT, rightpower)
Run:

  jmp Forever
endt

Did you know that even the motors of the NXT use PID themselves to provide accurate control?

About PID control

I found this video on the blog of Xander Soldaat:

Unfortunately, he does not show how to actually implement a PID controller, or how to tweak the values of the algorithm, so I thought I’d show you how it’s done.

For my robot, I chose the trike base by HiTechnic, because it is simple, and usable for my next program. The result:

If you are new to NBC, the main thing to remember that an action consist of a line, starting with the action, usually followed by the variable to store the result in, followed by other parameters.

add result 1 2

Another important concept are comments, which start with //. These are my notes about what the code does, to help you understand it.

If you want to know more about NBC, read this tutorial.

// Define to which ports the sensor
// and motors are connected
#define ULTRASONICSENSOR IN_4
#define motors OUT_AC

// Define constants to tweak the algorithm
#define kp 50
#define ki 12
#define kd 2
// And another one to scale the final value
#define scale 10

// target distance in cm
#define target 30

// From here to dseg ends are variable declarations
dseg segment

// Ultrasonic sensor reading
distance word

// The current error
err sdword
// The previous error
errold sdword
// The integral, all accumulated errors
errint sdword
// The deriviate, the expected next error
errdiff sdword

// Final pid value
pid sdword

// Temporary variable for calculations
temp sdword

dseg ends

// This is where the actual code starts
thread main
  // Initialize the ultrasonic sensor
  SetSensorUltrasonic(ULTRASONICSENSOR)

Forever:
  // Read the sensor and store it in distance
  ReadSensorUS(ULTRASONICSENSOR, distance)

  // Substract the actual distance
  // from the target for the current error
  sub err target distance // Proportional

  // Add the error to the integral
  add errint errint err // Integral
  mul errint errint 0.8 // multiply by 0.8 for damping

  // Sunstract the previous error from error
  // so that we get the speed
  // at which the error changes
  sub errdiff err errold // Derivative
  // set the current error as he old error
  mov errold err

  mul pid err kp // Apply proportional parameter

  mul temp errint ki // Apply integral parameter
  add pid pid temp

  mul temp errdiff kd // Apply derivative parameter
  add pid pid temp

  div pid, pid, scale       // Apply scale

  ClearScreen()
  NumOut(0,0,pid)
  NumOut(0,16,distance)

  // saturate over 100 and under -100
  brcmp LT, under100, pid, 100
  mov pid, 100
under100:
  brcmp GT, overMin100, pid, -100
  mov pid, -100
overMin100:

  // Turn the motors according to the scaled PID value.
  OnRev(motors, pid)
  jmp Forever
endt

If you have built a robot, and written the PID controller, the last thing you need to do is tweak the parameters on the lines that start with #define.

kp is multiplied by the proportial, this is where you start. Set the other two to zero, and this one to any value.

If the robot does not move, increase it. If the robot oscillates wildly, decrease it. Do this until it until it oscillates just a bit.

Now divide kp roughly in half, so that it does not oscillate, but stops to early. Now increase ki until it reaches the target as fast as needed. It will overshoot its target.

Finally, increase kd until it stops on target with as little oscillation as possible. You might need to go back and tweak the other parameters a bit.

Leave a comment if you have any questions.

Gamepad remote control

I had this pincer bot that I had not yet programmed, but using software remotes proved disappointing. With a few lines of code, I was able to use any gamepad or joystick to control the robot.

To use this code, you need to know how to execute commands on your computer. Then, do the following.

  1. Install Python if you don’t already have it.
  2. Install PyGame for interfacing with the gamepad.
  3. Install NXT-python for interfacing with the NXT.
  4. Make sure your NXT and gamepad are connected and working.
  5. Run
    python nxtjoy.py

    in the directory where you’ve downloaded the code below.

The code assumes you have the pincer bot in the video, for which I’ll give you instructions later. Moving around should work with most tank-steered robots.
import pygame
from nxt import locator, motor
from time import sleep

# edit this to reflect your joystick axis and buttons
axis = {'x':0, 'y':1}

b = locator.find_one_brick()

left = motor.Motor(b, motor.PORT_B)
right = motor.Motor(b, motor.PORT_A)
action = motor.Motor(b, motor.PORT_C)

closed = False

def limit(nr):
    if nr > 50 or nr < -50:
        return min(127, max(-128, nr))
    else:
        return 0

def move(fwd=0, turn=0):
    lp = int((fwd - turn) * -100)
    rp = int((fwd + turn) * -100)
    left.run(limit(lp))
    right.run(limit(rp))

def pincer(button):
    global closed
    try:
        if button and not closed:
            closed = True
            action.turn(-40, 70, emulate=False)
        elif not button and closed:
            closed = False
            action.turn(30, 70, emulate=False, brake=False)
    except motor.BlockedException:
        print action.get_tacho()

pygame.init()
j = pygame.joystick.Joystick(0) # first joystick
j.init()
print 'Initialized Joystick : %s' % j.get_name()
try:
    while True:
        pygame.event.pump()
        sleep(0.1)

        # get_axis returns a value between -1 and 1
        move(j.get_axis(axis['y']), j.get_axis(axis['x']))
        pincer(j.get_button(0))

except KeyboardInterrupt:
    j.quit()