Kiwibot

The Kiwibot is a recently released robot design for VEX IQ. It comes with an autonomous program, but it's written in RobotC. So let's port it to Robot Mesh Python for VEX, and see if we learn anything interesting.

The Kiwibot has six TouchLEDs, 3 bumper switches, and 3 motors, which fills I/O port on a VEX IQ Robot Brain. In autonomous mode, it drives around mostly-randomly, switching drive modes every time it bumps into something.

The six TouchLEDs indicate which drivemode the Kiwibot is currently in, and when touched will throttle the motors or shut them off.

So, first things first, let's write the TouchLED code.

Normally, to check the state of six touch sensors in a single if expression, you'd have to do something like:

if FR_touch.is_touch() or MR_touch.is_touch() or BR_touch.is_touch() or FL_touch.is_touch() or ML_touch.is_touch() or BL_touch.is_touch():
    doSomething()

This has a number of disadvantages:

  1. We end up having to type is_touch() over and over, which is annoying. (Don't Repeat Yourself, he repeated.)
  2. Very long lines are hard to parse, and therefore impair readability. (Code is hard enough to read already, we don't want to do anything to make it worse.)
  3. If the end of a line hangs off the edge of the screen, then bugs can hide where you won't see them at a casual glance. If we had forgotten the () on the very last is_touch then the if expression would always evaluate as True[1], and we would be very confused when the robot acted like the TouchLEDs were constantly being pressed.
  4. It's ugly and I hate it.

So let's write something a little nicer.

for touch_sensor in (FR_touch, MR_touch, BR_touch, FL_touch, ML_touch, BL_touch):
    if touch_sensor.is_touch():

This just loops through the TouchLEDs one at a time, checking to see if they've been pressed. Here's the code in context:

def CheckTouchSensors():
    global normalmode
    global slowmode
    global stopmode
    for touch_sensor in (FR_touch, MR_touch, BR_touch, FL_touch, ML_touch, BL_touch):
        if touch_sensor.is_touch():
            if normalmode:
                normalmode = False
                slowmode   = True
            elif slowmode:
                slowmode   = False
                stopmode   = True
            elif stopmode:
                stopmode   = False
                normalmode = True
            sys.sleep(0.07)
            break

In Python, you can read from a variable outside the local scope, but if you want to write to it, you have to use the global keyword. If you forget to do that, then Python helpfully creates a locally scoped variable of the same name, then throws it away when you exit the function, which can produce mystifying bugs:

x = "Global"

def Foo():
    x = "Local"
    
Foo()

print x # This prints "Global" to console.

The interpreter will not produce any warnings or errors when it does this.

This is because Python would really rather you use function arguments to pass variables into functions, and return values to get the output. This has quite a few benefits, and broadly tends to make your code easier to read, but is unavoidably slower than straightforward imperative code, since the interpreter is spending more time copying things around behind the scenes. For maximum speed you have to change global state quite a lot. And so, of course, while porting the program for the Kiwibot I ended up shooting myself in the foot over and over by forgetting to put in globals where they were needed. That's the price you pay

if normalmode:
    normalmode = False
    slowmode = True
elif slowmode:
    slowmode = False
    stopmode = True
elif stopmode:
    stopmode = False
    normalmode = True
sys.sleep(0.07)
break

This switches between the normal, slow and stopped states when any of the TouchLEDs are pressed. After setting one of the flags we sleep for 70 milliseconds to give the user time to lift their finger off the touch sensor; otherwise the robot brain is fast enough that a single press can change the running flag several times. Once we're done sleeping we break out of the for loop, since there's no need to check the rest of the touch sensors once one's been touched. (This is a mostly pointless microptimization that I left in because I thought it was fun.)

Next, let's check to see if any of the bumper switches are pressed, written in a straightforward imperative style.

if F_bump.is_pressed():
    FR_touch.named_color(1) #Turn on the LEDs on the side that bumped into something
    MR_touch.named_color(0)
    BR_touch.named_color(0)
    FL_touch.named_color(1)
    ML_touch.named_color(0)
    BL_touch.named_color(0)
    FL_motor.run(-100) # Drive away from whatever we hit
    FR_motor.run(100)
    B_motor.off() # B_motor might have been running when we hit something
    sys.sleep(.2)
    SpinRandom() # SpinRandom chooses the next drivemode
elif BL_bump.is_pressed():
    FR_touch.named_color(0)
    MR_touch.named_color(0)
    BR_touch.named_color(0)
    FL_touch.named_color(0)
    ML_touch.named_color(1)
    BL_touch.named_color(1)
    FL_motor.run(100)
    FR_motor.off()
    B_motor.run(-100)
    sys.sleep(.2)
    SpinRandom()
elif BR_bump.is_pressed():
    FR_touch.named_color(0)
    MR_touch.named_color(1)
    BR_touch.named_color(1)
    FL_touch.named_color(0)
    ML_touch.named_color(0)
    BL_touch.named_color(0)
    FL_motor.off()
    FR_motor.run(-100)
    B_motor.run(100)
    sys.sleep(.2)
    SpinRandom()

This works, but gosh it's repetitive. We end up calling the same TouchLEDs in the same order, again and again. This functionality is a good candidate for splitting off into its own helper function:

def NamedColorLEDs(FR, MR, BR, FL, ML, BL):
    FR_touch.named_color(FR)
    MR_touch.named_color(MR)
    BR_touch.named_color(BR)
    FL_touch.named_color(FL)
    ML_touch.named_color(ML)
    BL_touch.named_color(BL)

if F_bump.is_pressed():
    NamedColorLEDs(1, 0, 0, 1, 0, 0) 
    FL_motor.run(-100) # Drive away from whatever we hit
    FR_motor.run(100)
    B_motor.off() # B_motor might have been running when we hit something
    sys.sleep(.2)
    SpinRandom() # SpinRandom chooses the next drivemode
elif BL_bump.is_pressed():
    NamedColorLEDs(0, 0, 0, 0, 1, 1)
    FL_motor.run(100)
    FR_motor.off()
    B_motor.run(-100)
    sys.sleep(.2)
    SpinRandom()
elif BR_bump.is_pressed():
    NamedColorLEDs(0, 1, 1, 0, 0, 0)
    FL_motor.off()
    FR_motor.run(-100)
    B_motor.run(100)
    sys.sleep(.2)
    SpinRandom()

There, 7 fewer lines, 19% less code. The total savings is better than that, since we work with the TouchLEDs a lot in this program, so we'll be using NamedColorLEDs() frequently.

There's still improvements to make, though. We could make a helper function for the motors too, but the savings wouldn't be nearly as great, and I don't feel like it would be worth having the remember the order of arguments.

Really, what we want is an abstraction that hides the stuff we don't care about (motor power, order of arguments) and provides an interface to what we do care about, telling the robot in which direction to drive.

Time for trigonometry!

Sine wave

Basically what we're doing here is adding a bunch of force vectors. I don't have room here to go into a full explanation, so I'll just give you an idea of why it works.

For any given motor, if it wants to go forward, it wants 100 power, backwards, -100 power, and for the two angles parallel to its axle, 0 power.

The sine functions have the handy property of taking angles and outputting a smoothly varying curve between -1 and 1, crossing 0 twice for every 360 degrees. So, we just poke the output of sine right into run. Motor FR and FL are 120 and -120 degrees away from motor B, respectively, so we just adjust the angle we're giving them. We also flip the input angle 180 degrees so that angle 0 is at the front of the robot.

def SinD(degrees):
    return math.sin(math.radians(degrees))

def KiwiDrive(angle):
    x = angle + 180
    FR_motor.run(SinD(x + 120) * 100)
    B_motor.run(SinD(x) * 100)
    FL_motor.run(SinD(x - 120) * 100)

(Unhelpfully, the standard Python trig functions take radians, so we have to use a wrapper function to get a sine that takes degrees.)

While we're at it, the bumper switch if statements are all pretty repetitive, differing only in the parameters passed to them. Why not simplify things there too?

def SinD(degrees):
    return math.sin(math.radians(degrees))

def NamedColorLEDs(FR, MR, BR, FL, ML, BL):
    FR_touch.named_color(FR)
    MR_touch.named_color(MR)
    BR_touch.named_color(BR)
    FL_touch.named_color(FL)
    ML_touch.named_color(ML)
    BL_touch.named_color(BL)

def KiwiDrive(angle):
    x = angle + 180
    FR_motor.run(SinD(x + 120) * 100)
    B_motor.run(SinD(x) * 100)
    FL_motor.run(SinD(x - 120) * 100)

def Bump(angle):
    if angle == 0:
        NamedColorLEDs(1, 0, 0, 1, 0, 0) 
    elif angle == 120:
        NamedColorLEDs(0, 1, 1, 0, 0, 0)
    elif angle == 240:
        NamedColorLEDs(0, 0, 0, 0, 1, 1)
    KiwiDrive(angle + 180)
    sys.sleep(.5)
    SpinRandom()

while True:
    if F_bump.is_pressed():
        Bump(0)
    elif BR_bump.is_pressed():
        Bump(120)
    elif BL_bump.is_pressed():
        Bump(240)

This block of mostly-functional code is only a single line shorter than the imperative code it replaced, but almost all of the core logic is broken out into reusable functions. There's only six lines of imperative-style code left.

The last thing Bump() calls before exiting is SpinRandom().

def SpinRandom():
    global drivemode
    x = random.randint(100, 260)
    x = 3 * x
    FL_motor.run(100, x)
    FR_motor.run(100, x)
    B_motor.run(100, x)
    while not FL_motor.reached_target():
        pass
    lastmode = drivemode
    drivemode = random.randint(1, 7)
    if drivemode == lastmode: # Don't want to use the same drive mode, to avoid getting stuck in corners
        drivemode += 1 # Add one to the drive mode
        if drivemode > 7:
            drivemode = 1

It (approximately) spins the robot between 100 and 260 degrees, to avoid running into the thing it just ran into again, and chooses a new drivemode randomly, avoiding the drivemode it just used, to somewhat mitigate its tendency to get stuck in corners.

It has seven drivemodes, which I won't go through in detail, since this post is already long enough. DriveForward is the simpliest:

def DriveForward():
    KiwiDrive(0)
    if normalmode:
        if loopcounter % 120 == 0:
            NamedColorLEDs(0, 0, 12, 0, 0, 12)
        elif loopcounter % 80 == 0:
            NamedColorLEDs(0, 12, 0, 0, 12, 0)
        elif loopcounter % 40 == 0:
            NamedColorLEDs(12, 0, 0, 12, 0, 0)

loopcounter is incremented on every trip through the main loop. Every 40 ticks we switch TouchLED states, using the modulo operator. From Wikipedia:

If the time is 7:00 now, then 8 hours later it will be 3:00. Usual addition would suggest that the later time should be 7 + 8 = 15, but this is not the answer because clock time "wraps around" every 12 hours; in 12-hour time, there is no "15 o'clock". Likewise, if the clock starts at 12:00 (noon) and 21 hours elapse, then the time will be 9:00 the next day, rather than 33:00. Because the hour number starts over after it reaches 12, this is arithmetic modulo 12.

An easy way to play with modulo math is repl.it. The example script:

for x in range (1, 11):
    print x, x % 5, x % 10

Outputs:

1 1 1
2 2 2
3 3 3
4 4 4
5 0 5
6 1 6
7 2 7
8 3 8
9 4 9
10 0 0

In DriveForward we have to put the % 120 if-case before % 80, because for the number 240, both % 80 and % 120 would equal 0. (240 / 80 = 4, with no remainder) This is a classic pitfall of modulo math, as exploited in the fizz buzz test.

And that's it! To check out the rest of the drivemodes, or to play with the code yourself, take a look at the project page.


1: Evaluating the name of a variable or a function always returns True because that's how you test if a variable is defined. The only exception is if you assign False to a variable, in which case it'll do what you expect. ^