Lego Wedo Semaphore

After building the Semaphore robot, I was clearing up the Wedo Speed trap stuff, and wondered if we could make a semaphore robot from Lego?

The easiest way to do it would be using EV3 motors. We can run Sniff on the EV3, and the Mindstorm, and in fact EV3 motors are perfect for this task. We weren't able to use regular motors for the Arduino version, as normal motors just turn, and you can't figure out where they are (so we used stepper motors). EV3 Motors have an extra sensor built into them so you can position them exactly. That means they can easily turn the required angle, and move to specific positions. However setting up the EV3 is quite a bit of hassle.

That's one of the reasons I like the wedo so much. It's just quick, plug and play fun, without the "bigness" of EV3.

The Wedo supports motors by default, but it can also use Power Functions components like lights. One of the coolest Power Functions components is the Servo, which can move to a precise (ish) position over 180 degrees. It's not supported by the Official Wedo software but we can operate it from Sniff.

Check back with the previous post on semaphore for the bulk of the code and background on handling semaphore. We'll concentrate on the Wedo bit. Most of that is taken care of by a script called "update":

when update

.forever

..set wedoConnector to 1

..set wedoValue to -2*((target1+1)-3)/6

..tell wedo to "setMotorSpeed"

..set wedoConnector to 2

..set wedoValue to -2*((target2-3)-3)/6

..tell wedo to "setMotorSpeed"

This maps semaphore positions in the "target" variables to value between -1 and +1. However we need to be a bit careful. The Lego Servo is an odd machine, which only handles a fixed set of positions. It can move to centre (0) and 6 positions each side (even though the Lego website says 7!). A value to 1 means 90 degrees, and we can move in 15 degree increments. If you try and set a value between those increments strange things happen - sometimes the servo skips, or doesn't move, and sometimes it makes whining noises. For best operation we need to move the servo value in 1/6ths

 

For the left motor we have to support semaphore values between -1 and 5 so lets add 1 to it, so now its between 0 and 6. Now subtract 3 so its centred around 0 (ish), then divide by 6 so we get  nice reliable steps.

But each step is only 15 degrees. We want each step to be 45 degrees. We also need a travel of 270 degrees, which the Servo can't do... That's why we used a stepper last time. But now we're using Lego, we can easily build a gearing system. A Lego small cog has 8 teeth, and a large one has 24, so if we use the servo to drive a large cog, it will turn a small cog 45 degrees, and we will have a potential travel of 540 degree!

That sort of works, but neither the servo or the gearing is that precise - there's a lot of play in both, which means that it isn't accurate enough to clearly signal. Instead we multiply by 2, so each semaphore position is two steps separate (30 degrees). Now we need a gearing ration of 1.5, which turns out to be quite hard to make. However if you have Extra large cogs they have 40 teeth, so if we drive a large cog from an extra large cog we get a ration of about 1.6 which is close enough.

The minus sign just flips the direction, as the gearing reverses the direction of rotation and we need to compensate. We have a similar mapping for the right hand servo which is on Wedo connector 2.

The rest of the changes from last time are just about tweaking the code to work right with the specifics of the build, and evolution of the code. Left and right ware swapped just because of the way I connected it, and target is now specified in semaphore positions rather than stepper positions. The timings change a bit just because of the speed of the motors and we have no way of knowing when they arrive in position (thought the lego servo is much faster than the steppers).

make theLetter string

make theLeftPos number

make theRightPos number

when getPositionsForLetter

.make counter number

.repeat length of key using counter

..if letter counter of key=theLetter

...set theLeftPos to item counter of leftFlag

...set theRightPos to item counter of rightFlag

...stop script

.set theLeftPos to 0

.set theRightPos to 8

when attention

.repeat 3

..set target1 to 1

..set target2 to 7

..wait 1 secs

..set target1 to 3

..set target2 to 5

..wait 1 secs

.set target1 to 0

.set target2 to 8

.wait 2 secs

when start

.make counter number

.

.set target1 to 0

.set target2 to 8

.

.broadcast update

.broadcast load and wait

.forever

..ask "What's the message?" and wait

..broadcast attention and wait

..

..repeat length of answer using counter

...set theLetter to letter counter of answer

...broadcast getPositionsForLetter and wait

...say join [theLeftPos] join ":" [theRightPos]

...set target1 to theLeftPos

...set target2 to theRightPos

...wait 2 secs

..

..set target1 to 0

..set target2 to 8

And here's the "finished" version:

Unfortunatly now I have to admit, I couldn't finish the project - I've only got 1 PF Servo... They cost £20.99 + postage and my lego budget has to cover a lot of other things! Even worse on Ebay there are buy it nows from £28 to £60!!!!! (EV3 medium motors are £19.99 on lego.co.uk but £34.99 on eBay!) I've noticed this before - ebay is some kind of crazy place to buy Lego. Sometimes you get bargains, but on newer stuff you can pay more than buying direct.

Rant Warning - you can skip the next few paragraphs...

To make things worse, Lego just obsoleted Power Functions. PF isn't officially dead (yet), but the new Wedo 2.0 uses a completly different connector. According to Lego PR, the Bluetooth connection between the computer and 2.0 hub means they "needed" to change the cable between the hub and motors... and the tilt sensor now has a "shake" mode... so they needed to change the cable! This is obviously untrue. Previously Wedo used standard PF components, but now the Wedo 2.0 has it's own connector and won't plug into anything else (whereas the old version plugged together like... you know... Lego?). The explanation is that PF is about to be replaced by PF 2.0 (there's a reference here: https://education.lego.com/en-gb/lesi/elementary/wedo-2/faqs) . There's no backward compatibility so throw out all your old sensors and motors!!

It's really about time Lego got its act together on this... three generations of Mindstorms used different cabling (with some backward compatibility at least), Power Functions was introduced to replace the old 9V technics motor system (and versions before that!), with the claim it allowed for future expansion - which only materialised as Wedo. Technics never got any sensor features, and Wedo was kept secret from the public. Each generation of Lego engineers decides that the old plug system is too limiting. Wedo 2.0 delivers nothing in terms of new sensors (yet) and in fact we loose lights and servo's, but they're asking me to throw out about £500 of my own motors sensors and hubs, just to avoid the cable to the computer.

Going Bluetooth has benefits (and disadvantages if you have a lot of them in the same room and group 1's computer pairs to group 2's build - accidentally or deliberately!),  and if I could buy a Bluetooth hub to use with my current motors and sensors and ADD to my wedo system I'd be first in the queue, but starting over with a system that's less capable...

With that rant out of the way, now might not be a great time to invest in Power Functions components! At some point there's going to be some tough decisions whether to invest more in established (but obsolete) 1.0 tech or throw out a lot of expensive stuff, and  start over.

And we're back...

Anyway I was able to test and get both arms working - just not at the same time! It's less precise that the stepper version, and but it works really well. Cost wise its a bit crazy - the hub and two servos would cost about £80, whereas the Arduino version cost less than £10. On the other hand, the lego version demonstrated the power of Lego - I was able fabricate the supporting structure, including gearing way more easily -  building a gearbox for arduino and servo's isn't something I could do. That's where the Wedo really pays off - just being able to combine building and coding so easily...

Stepper Motor Semaphore

For some reason I can't quite remember I stumbled across some web pages on semaphore, and thought wouldn't it be good to write some code to convert messages to semaphore. 

In Semaphore, each letter is represented by the position of two flags. There are 8 flag positions, numbered clockwise from 0 at the bottom, through 4 (straight up) to 7 bottom right. Of course we could just draw them on the screen but really we want to to actually wave some flags around. 

There are basically three options for physically moving stuff under computer control. For robots we just use DC motors, but all you can do is turn these on and off. Then they spin fast and you've no which orientation they're in. You can add a sensor to track the rotation, but that's hard work. The second option is Servos. These would be perfect - you send them a timing signal which represents a position, and they move to that position. However there's a gotcha - servos typically have a limited rotation of about 180 degrees. That would be Ok if each arm just had to move on its own side, but if you look at the chart, you'll see that sometimes both flags are on the same side - for example the left flag naturally moves from 0 (down) to 4 (up) on the left side (as we see it), but sometimes it needs to move to 5 or 7 - a total angle of 270 degrees. While there are servos that can do this (and we could add gearing to solve the problem) we need to consider a third option.

Stepper Motors have a magnet in the middle, and a set of coils (typically 4) around the outside. 

When we turn on one of the coils the motor turns to align the magnet with the field of the coil. If we turn on each coil in turn, then we can make the motor rotate. However its under our exact control - each time we turn on the next coil the motor turns by exactly one "step". Depending on how the motor is built, each step will be a known angle, so we can move the motor very precisely - with one proviso - we can move it precisely by any required angle, but we've no way of knowing where it is!

We bought two motors and controllers for under £5 on eBay. You can get them cheaper, but we wanted them now! Hook up is simple - two power pins, and 1 pin for each of the four coils. Now we can code this up:

make stepper number

make coilA digital output D4

make coilB digital output D5

make coilC digital output D6

make coilD digital output D7

when fullStep

.forever

..if stepper > 4

...change stepper by -4

..if stepper < 1

...change stepper by 4

..

..set coilA to (stepper=1)

..set coilB to (stepper=2)

..set coilC to (stepper=3)

..set coilD to (stepper=4)

when start

..broadcast fullStep

.

.set stepper to 1

.repeat 2048

..change stepper by 1

..wait 2000 microsecs

.repeat 2048

..change stepper by -1

..wait 2000 microsecs

The four coils are connected to digital outputs, and we run the "fullStep" script all the time in the background. The variable stepper represents which coil is currently turned on. In the main script we increment stepper 2048 times - as the motors I bought take 2048 steps to complete 1 revolution. We then then decrement it to rotate back. We have a pause of 2milliseconds between each step - these are physical things and take time to respond, so if you move them too fast they'll just sit there and jitter. This was as fast as the ones I bought could go.

when fullStep

.forever

..if stepper > 4

...change stepper by -4

..if stepper < 1

...change stepper by 4

..

..set coilA to (stepper=1 or stepper=2)

..set coilB to (stepper=2 or stepper=3)

..set coilC to (stepper=3 or stepper=4)

..set coilD to (stepper=4 or stepper=1)

The original version works fine, but we can get a little better performance out of it by turning on two coils at a time instead of one. When stepper is 2, both coils A and B are turned on, then when it changes to 3, A turns off and C turns on. The magnet now lines up half way between the two active coils.

when halfStep

.forever

..if phase > 8

...change phase by -8

..if phase < 1

...change phase by 8

..

..set coilA to phase = 8 or phase=1 or phase=2

..set coilB to phase = 2 or phase=3 or phase=4

..set coilC to phase = 4 or phase=5 or phase=6

..set coilD to phase = 6 or phase=7 or phase=8

If we combine these two ideas we can "half step" the motor - alternately lining it up with a single could and half way between coils. The sequence is on A, AB, B, BC, C, CD, D, DA... It's now taken 8 half steps to move the same distance as 4 full steps. Movement is a little smoother, and we have finer control over position.

Now its time to wrap all  that up in something we can use in our Semaphore code:

make position number

make target number

make stepsPerTurn number 2048

when halfStep

.make phase number

.

.forever

..if position<target

...change phase by 1

...change position by 0.5

..if position>target

...change phase by -1

...change position by -0.5

..

..if phase > 8

...change phase by -8

..if phase < 1

...change phase by 8

..

..set coilA to phase = 8 or phase=1 or phase=2

..set coilB to phase = 2 or phase=3 or phase=4

..set coilC to phase = 4 or phase=5 or phase=6

..set coilD to phase = 6 or phase=7 or phase=8

..

..wait 1000 microsecs

This improved halfStep script, uses three variables to keep track of the motor: position (where it currently is), target (where we'd like it to be), and phase (where it is in the step sequence). It then uses the half step sequence to move the motor towards the desired target. You'll note that now we're half stepping we can run twice as fast (delaying 1mS).

With that in place we can turn to the problem of semaphore. To do that we need to control 2 flags, and in a "real" programming language we'd do that with an array and a data structure containing the outputs and the positions. Unfortunatly Sniff can't do that - it is after all Scratch, and data structures are the main limitation of a language designed for at primary school children. Instead we need to duplicate the halfStep script and make it operate on a second set of coils.

make leftFlag list of numbers

make rightFlag list of numbers

make key string "abcdefghijklmnopqrstuvwxyz"

when load

.#A

.add 1 to leftFlag

.add 8 to rightFlag

.

.#B

.add 2 to leftFlag

.add 8 to rightFlag

.

.#C

.add 3 to leftFlag

.add 8 to rightFlag

.

.....

.

.#X

.add 5 to leftFlag

.add 7 to rightFlag

.

.#Y

.add 3 to leftFlag

.add 6 to rightFlag

.

.#Z

.add -1 to leftFlag

.add 6 to rightFlag

We add the flag positions to two lists - one for left and right (as we face them). However there's one trick to this - down is position 0, so when we move to position 4 from there we move clockwise, but for the right flag this would hit the left flag as it rotates. We want the right flag to move from the down position to its target position anticlockwise, so instead of recording it as 0 we record it as 8. Simarly when we want the left flag to move to the 7 position, we call it -1

Now we can write some code to look up letters in these lists:

make theLetter string

make theLeftPos number

make theRightPos number

when getPositionsForLetter

.make counter number

.repeat length of key using counter

..if letter counter of key=theLetter

...set theLeftPos to item counter of leftFlag

...set theRightPos to item counter of rightFlag

...stop script

.set theLeftPos to 0

.set theRightPos to 8

Finally we just need to initialise everything, get a message from the user, then step through the letters of the message broadcasting each in turn:

when start

.make counter number

.

.set position1 to stepsPerTurn

.set position2 to 0

.set target1 to position1

.set target2 to position2

.

.broadcast halfStep

.broadcast load and wait

.

.ask "What's the message?" and wait

.broadcast attention and wait

.

.repeat length of answer using counter

..set theLetter to letter counter of answer

..broadcast getPositionsForLetter and wait

..say join [theLeftPos] join ":" [theRightPos]

..set target1 to theRightPos*stepsPerTurn/8

..set target2 to theLeftPos*stepsPerTurn/8

..wait until position1 = target1 and position2 = target2

..wait 0.5 secs

.

.set target1 to stepsPerTurn

.set target2 to 0

And here's the robot actually running:

(idea for next project - use GSM shield to actually receive SMS messages and show them as semaphore!!!).

Download the source code.