Instructables

Quadroped Robot - (NIOSII Assembly)

Featured
First of all, thanks to my partner  in this project; Yerusha Nuh. We had to work a few long nights, but we got it done :)

Before you get into this instructable, a word of caution. The lego construction part of this instructable is not quite complete; the parts lists are not finished and some pictures are missing. I am not in the habit of putting up half-finished work, but in this case, I'm in Austria (away from my robot in Toronto) for 4 months, so I figured I'd give you all a taste of the project (and a full layout of the code), so that when I get home in September I can quickly finish the parts list and you can build one for yourself WITHOUT just tinkering around. Though I'm sure you'll be able to figure it out. We instrutable folks like a challenge!


This instructable outlines the construction of my quadroped robot. It was originally for a school project, but I figured I could break it down into some easy to follow steps. Makers should note: this project uses a lot of LEGO, and some special components, that can be substituted for others. All materials below are the ones that I used. Well, here we go!

First of all, you'll need some materials (I'll go into greater detail in later steps)
- An Altera DE2 board (This is an FPGA board with a built in 50Mhz Clock, Memory, and other connections)
- A LEGO breakout box
- A LEGO control board (plugs into JP1 of the DE2)
- Two LEGO low RPM motors
- Two LEGO touch sensors (I'd definitely use range sensors or sonar, but we had limited resources for the project)
- A TON of LEGO to build the robot

To keep things simple, I'm going to divide this instructable into two sections: Robot Building (Starts on Step 1), and Coding (Starts on Step 6).

To get you interested, I mounted a camera to the finished robot, and took a video of him walking around. With a little work, this little guys could definitely become a spy robot.

 
Remove these adsRemove these ads by Signing Up

Step 1: Robot Building - Getting Started

Picture of Robot Building - Getting Started
Alright! So this quadroped robot is based on the same model Theo Jansen uses in his Strandobeasts. Don't  worry if you have no idea what that is, you'll see a video of it later.

Basically, there will be three parts to this robot: The left legs, the right legs, and the body. The leg structures are mirrored copies of one another, and the body is a simple structure that holds the two low RPM motors in place.

Why low RPM you ask? Because with the LEGO high RPM motors, the robot doesn't have the strength to lift itself, and fails to make a single step. (Though, when it's off the ground, it runs like a cheeta!)

To get the basic idea of how the legs will work, watch the following video (someone put this on youtube, and it is great)




So that's the basic motion of the legs. Traditionally, this kind of walker would have at least 3 sets of legs on either side, but because of limited parts, I could only make two. This makes the robot's gait a little awkward, but if the motors are strong enough, it doesn't matter much, and appears all the more alive (I've gotten the comment that it moves an awful lot like a cat. It slinks quite nicely :P ).

To do this we're obviously going to need a LOT of LEGO. Keep in mind, you can definitely use other materials for the structure of the robot.

The parts you'll need explicitly are (for each set of legs):
- 8x 6pt bar
- 8x

.
...


...



 

Step 2: Robot Building - Building A Leg

Picture of Robot Building - Building A Leg
Here's where we start having fun. I'll put instructions below, but I'll also add a "how to" video later, in case they're difficult to follow.

1. Take one of your 6pt axels, and put a half bushing on one end, (keeping another ready)
2. Thread (in this order) the last hole of the following bars onto the axel: 8pt,6pt,10pt,6pt,8pt, and cap the other end with a half bushing
3. Thread an 12pt axel through the last hole of the two 6pt bars you just added, and add another 3 6pt bars between them, and on either side. Remember to add half bushings to either end, but this time slide them in until they almost touch the  bars added.
4. take the middle 6pt bar, and move it so that it sits between the 8pt bars.
5. Thread a 6pt axel through the 8pt bars' last hole, adding two more 6pt bars, and making sure the 6pt bar from step 4 is pinned in the middle. Half bushings on either end to hold it in place.
6. Just as a check, you should now have a triangle (rigid) with 4 6pt bars hanging down from it, and a 10pt bar hanging off it's top.
7. Grab another 10pt bar, and put it between the two 6pt bars hanging off the back (the side with the existing 10pt bar)
8. Thread an 8pt axel through, and add a 16pt bar to either side, a 6pt bar in the middle, and be sure to get the new 10pt bar in the middle as well. Cap with half bushings.
9. From the front dangling 6pt bars, put the back hanging 6pt bar between them.
10. Take an 6pt axel, and thread it through the ends of  these 6pt bars (with the back-attached one in the midddle). Add a 8 pt bar to either end, and cap with bushings.
11. Almost there, take these two 8 pt. bars, and bring them to the two 16pt bars. Using an 8pt axel, pin them in the 5th hole down from the top of the 16pt bars. Cap with bushings.
12. Alright! Now we should have a single leg with two hanging 10pt bars! (If not, you should really see the video below)
13. Lastly, add an 8pt axel between the last hole in the two 10pt bars to pin there ends together. Slide a half bushing up either side until the two bars are pinned next to each other. (This will come in handy later)


Step 3: Robot Building - Putting a Leg Set Together

So now that you've built at least 2 legs (if you haven't, you really should :P ) we're going to put them together to form one set. In our final robot, there will be two sets that mirror each other in construction. Don't worry though, the legs are all identical, so you won't need to do anything too complicated to switch from right to left.

First, get your two legs together.
Second, make sure to have at least the following parts:
- 4x 16pt bars
- 4x 1by8 flat pieces
- 7x 24tooth gears (at least 4 with places for axles)
- 1x 12tooth gear
- 6x plug/axel
- 1x 6pt axel
- 4x 2pt axel
- 4x hook thing (you'll see in a sec.)
- 4x half bushing
- some extra lego (preferably bars, they give you something to work with)

So, again, step by step instructions, and then I'll upload a video later, in case they are unclear.

1. Take two 16pt bars, and attach them together long-wise with a 1by8 flat piece above and below. Make two of these.
2. Remember that really long (12pt) axel you put at the bottom of the triangle in the leg? Well add another bushing to the longer side (you choose), and then slide it into the second last hole of one of the looong bars you just created. Cap it on the other side  with another half bushing.
3. Set up the gears as follows: on one of the center holes, use a 6pt axel to pin a 12tooth gear on one side, and a 24tooth gear on the other side. (The 12tooth gear will be our working side)
4. Add two 24tooth gears to either side of the 12tooth gear, so that they mesh nicely.
5. Add a 2pt axel to the last gear on either side. To this axel, add a hook thing pointing outward, and into the angle of rotation.
6. Take the axel pinning the two 10pt bars of your leg together, and pin it to the first hole in the hook thing with a half bushing. (Don't expect this to be easy :P )
7. Congratulations! You have one working leg. If you spin the gears, you should see your leg take a step!
8. Do the same on the opposing side of the same looong bar. Pin the leg to wherever it needs to be to match the first with the gears. Remember, the legs are supposed to be facing away from each other.
9. Add two gears to the unused loooong bar, in the same places as the final gears of the leg set you've built. Add the 2pt axel, and hook thing to each one, to mirror what you've built.
10. Attach this to the rest of your build, and be sure to cap the connection to the leg with bushings to that the gears on either side of the leg match nicely.
11. Finally, attach both looong bars together with some spare parts (making sure that these connections don't interrupt the movement of the legs or the hook things)
12. Huzzah! You've just built a set of legs! Be sure that the hook things are always pointing the same direction for optimal stride.



Remember, you need two of these, so be sure to build a second one that mirrors the first (ie. when it's done, the gears on the outside face each other.

Step 4: Robot Building - Putting it all Together

The last  thing to do for the body of the robot is to attach the motors. There are many ways to do this, but I found that the following worked very well:

Get about 8xlong (12-16pt) bars, 4x10pt bars, 8xpins, 2x12tooth gears, and your 2 low RPM motors. Also, make sure you have some extra pieces to re-enforce your structure.

1. Create two parallel cross bars that are attached to the underside of the leg sets. They should be attached such that there is at least 10pts of space left between the leg sets.
2. Remember those single gears on the outer side of each set of legs? Those will be pointing inward (toward each other). Mount the motors (with the 12tooth gears attached) to the crossbars, and make sure the teeth of the gears mesh.
3. Create another two cross bars on the top of the leg structures, and motors.
4. Lastly, taking your pins and 10 pt bars, and add them vertically to the high/low crossbars on each side. You should note that these will most likely not be straight, but will be slanted, and that is fine.

Step 5: Robot Building - Marvel at your Creation

SAM_3456.JPG
SAM_3470.JPG
SAM_3471.JPG
Congratulations! You've build a quadroped! 

Keep in mind, as this robot  only has four legs, as it walks it puts a fair bit of strain on the cross bars, and their connections to the leg sets. It may be wise to re-enforce the structure at  those points.

Also, often motors are not perfectly identical. In my build, one of the motors was notably slower than the other, and caused the robot to lose sync with  its walking every once and a while. This can be compensated for with code (timing the motors) but it is a lot simpler  if you have good motors.



Step 6: Coding - If I Only had a Brain

Picture of Coding - If I Only had a Brain
So we've built a robot, but as yet, it really doesn't do much more than sit there. Lets get to work on giving  it some way to interact with the world around it.

Makers should note: the following sections may contain gratuitous amounts of assembly code. (And minimal use of C).
Also note: I'm using an FPGA (field programmable gate array) for my NIOSII processor, and it is on a specialized board.

Before we get started with the code, let's list out the functions we would like to give our Quadroped.

- Walk forward
- Walk backward
- Stop walking
- Turn right
- Turn left
- automatically turn away from walls

Sound good? Let's get started.

Oh, a quick warning to those skilled in assembly: I'm still learning, so I'm sorry in advance for redundancies/inefficiencies in the code.

Also, for convenience, I've included documentation of the lego controller which can be downloaded as the pdf's below:


Step 7: Coding - The Motor Functions

First of all, create a new .s file for your motor functions. We'll be referencing these functions in our later code, so it's nice to put them all in one place. Also - remember that in assembly input values to functions are passed in r4,r5,r6,r7, and then on the stack. We will be assuming that the motor control address will be passed in with r4.

To save space, I'll just refer to all popping and pushing from the stack with #pop  &   #push   respectively. To clarify for anyone new to assembly, popping and pushing refers to saving register data to memory so that the registers can be used for something else, and putting the values back when that something else is done :)


.global motors_forward             # REMEMBER THE .global ! If you don't, you'll wonder why you can't use your functions.
motors_forward:
        #push
        movia r9,0x004000008
        stwio r9,0(r4)
        #pop
        ret

.global motors_forward
motors_reverse:
        #push
        movia r9,0x00400000a
        stwio r9,0(r4)
        #pop
        ret

.global motors_forward
motors_stop:
        #push
        movia r9,0x004000ff
        stwio r9,0(r4)
        #pop
        ret

.global motors_forward
motors_turn_left:
        #push
        movia r9,0x0040000b
        stwio r9,0(r4)
        #pop
        ret

.global motors_forward
motors_turn_right:
        #push
        movia r9,0x0040000c
        stwio r9,0(r4)
        #pop
        ret


Now, anyone who knows these motors inside and out will mention now that we're only using one leg set when turning, and that this is inefficient. I agree, and originally we had both legs assist in the turning. Unfortunately our legs suffered from a lack of rigidity, and on high friction surfaces (classically good for walking robots), Hecaton nearly tore himself apart. So, we stuck with a one sided turn. Feel free to modify this, or add to this as you see fit.

For convenience, I've included the motors.s file we wrote for our robot below:


Step 8: Coding - Interrupts you Say?

Now that we have our motor functions finished, we need a way to control them, and tell the system to execute them when we want. We are going to do this using interrupts.

For those unfamiliar with assembly, an interrupt is simply a component informing the system that it has been triggered (push button, keyboard, sensor, etc.), and the system halting whatever it's doing, and dealing with the interrupt appropriately.

So, our main program will have the following structure:

.data
        # Character stuff in case we want to print out later
.section .exceptions, "ax"
        # push
        # check to see who interrupted
        # handle keyboard inputs
        # handle sensor interrupts
        # pop
.section .text
        # setup
        # main loop

Let's start with the .text section, because it is definitely the simplest.

Again, for the sake of convenience, I've included the main.s file below. Note: This is NOT the highest level file, so I apologize to all of the serious coders out there who are cursing my misuse of a file name.
main.s11 KB

Step 9: Coding - Setup & The Run Loop

We will now code the initial setup of the system, and then put it into a simple run loop.

The setup involves allowing all of the devices to trigger interrupts, and initially setting the motors to OFF.

I should note that this code is preceded by the .data section, and the handler functions. Additionally, the following statements have already been made:

.equ TIMER, 0x10002000
.equ LEGO, 0x10000060
.equ LEGO_IRQ11, 0x800
.equ LEGO_IRQ8, 0x100
.equ LEGO_Edge, 0x1000006C
.equ JTAG_UART, 0x10001000
.equ LEDG, 0x10000010
.equ LEDR, 0x10000000


So, let's get started.


.section .text
.global pokemonmaster        # lol. When you write the code for your robot, call your functions whatever you want.
pokemonmaster:
    movia r27,0x800000              /* Initialize the stack  (aligned to 4) */
    movia r8,TIMER
    movia r9,0x6500             /* Set's time to countdown 10 second  500000000 = 1DCD6500 */
    stwio r9,8(r8)
    movia r9,0x1DCD
    stwio r9,12(r8)
    movia r9,0x0                /* Ensure that the timer is clean */
    stwio r9,0(r8)
    ldwio r9,0(r8)              /* Just to see what's going on here */

    movia r11, LEGO                     /* The base address of the GPIO is now in r11 */
    movia r9, 0x07f557ff                /* Lego control board's initialization in r9 */
    stwio r9, 4(r11)                    /* Stores into the Direction Register */
    movia r9, 0xFFFFFFFF
    stwio r9, 0(r11)
                                                                       # E:load:value
    movia r9,0xff3ffbff         /* Sensor 0, Threshold is HEX E  1111|1-111|0-0-1-1-11-11-11-11-10-11-11-11-11-11 */
    stwio r9,0(r11)
                                                                       # E:load:value
    movia r9,0xff3fefff        /* Sensor 1, Threshold is HEX E   1111|1-111|0-0-1-1-11-11-11-10-11-11-11-11-11-11 */
    stwio r9,0(r11)

    movia r9,0xffbfffff        /* Disable sensors for loading */
    stwio r9,0(r11)

    movia r9,0xffdfffff         /* Sets to state mode */
    stwio r9,0(r11)

    movia r9,0x18000000     /******************************************************************/
    stwio r9,8(r11)



    movia r8, JTAG_UART /* r8 now contains the JTAG_UART base address */
                        /* Tell the UART to request interrupts when characters are received */
addi  r9, r0, 0x1  /* set bit 0 (REI) of the CSR register to 1 */
stwio r9, 4(r8)


    addi  r9, r0, 0x900  /* set bits 11 and 8 of ctl3 to 1 */
wrctl ctl3, r9

    movia r9,1
    wrctl ctl0,r9

    movia r9,LEGO_Edge      /* Clear the lego sensor edge trigger */
    stwio r0,0(r9)




run_loop:
    movia r9,LEDG
    movia r20,LEGO

    ldwio r19,0(r20)
    srli r19,r19,26
    stwio r19,0(r9)
    movia r9,LEGO_Edge
    stwio r0,0(r9)
    br run_loop





.global wall_escape_left
wall_escape_left:               /* Turns left a certain distance to avoid a wall */
    movia r8,TIMER
    stw r0,0(r8)

    movia r9,0x4                /* Starts the timer but not continuous (needs intentional restart) */
    stw r9,4(r8)                /* Start the timer for a 1 period run */
    movia r4,LEGO                /* Moves the address of the motor controller into r4 for the function */
    addi sp,sp,-16
    stw r11,12(sp)
    stw r9,8(sp)
    stw r8,4(sp)
    stw ra,0(sp)
    call motors_turn_left
    ldw ra,0(sp)
    ldw r8,4(sp)
    ldw r9,8(sp)
    ldw r11,12(sp)
    addi sp,sp,16
LEFT_LOOP:
    ldw r9,0(r8)                /* Find out if the flag has gone up (a period has passed) */
    movia r10,0x00000001
    and r9,r9,r10
    beq r9,r10,LEFT_CLEAR       /* If the timer's timed out...*/
    br LEFT_LOOP
LEFT_CLEAR:
    stw r0,0(r8)               /* Clear the flag */
    addi sp,sp,-16
    stw r11,12(sp)
    stw r9,8(sp)
    stw r8,4(sp)
    stw ra,0(sp)
    movia r4,LEGO
    call motors_forward         /* Start walking forward again */
    ldw ra,0(sp)                /* Bring back the return address */
    ldw r8,4(sp)
    ldw r9,8(sp)
    ldw r11,12(sp)
    addi sp,sp,16                /* Shrink the stack back down */
    ret



.global wall_escape_right
wall_escape_right:
    movia r8,TIMER
    stw r0,0(r8)

    movia r9,0x4
    stw r9,4(r8)
    movia r4,LEGO                /* Moves the address of the motor controller into r4 for the function */
    addi sp,sp,-16
    stw r11,12(sp)
    stw r9,8(sp)
    stw r8,4(sp)
    stw ra,0(sp)
    call motors_turn_right
    ldw ra,0(sp)
    ldw r8,4(sp)
    ldw r9,8(sp)
    ldw r11,12(sp)
    addi sp,sp,16
RIGHT_LOOP:
    ldw r9,0(r8)
    movia r10,0x00000001
    and r9,r9,r10
    beq r9,r10,RIGHT_CLEAR
    br RIGHT_LOOP
RIGHT_CLEAR:
    stw r0,0(r8)
    addi sp,sp,-16
    stw r11,12(sp)
    stw r9,8(sp)
    stw r8,4(sp)
    stw ra,0(sp)
    movia r4,LEGO
    call motors_forward
    ldw ra,0(sp)
    ldw r8,4(sp)
    ldw r9,8(sp)
    ldw r11,12(sp)
    addi sp,sp,16
    ret
       

If that was way too confusing, I'm totally happy to answer question. Just ask them below.

Step 10: Be Like Kanye - INTERRUPT!

So now we'll write what is arguably the most important part of the code; the interrupts.

The interrupt handler will take care of the 'reaction' to external inputs. In this case, the external inputs are key commands (forward, turn, etc.) and the touch sensors. We want the robot to move forward when it detects a certain key, backwards for another, and so on.

So, without further ado, the interrupt handler:

.data
thechar:    .byte ' '               # the character that will be sent. It is read by our program and overwritten by the interrupt handler.


/* This is the main function of the Walker robot. */

.section .exceptions, "ax"
.align 2

handler:
addi sp, sp, -12  #save ea, et, ctl1 on stack
stw et, 0(sp)
rdctl et, ctl1
stw et, 4(sp)
stw ea, 8(sp)

    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)

rdctl et, ctl4        /* check the interrupt pending register (ctl4) */
andi et, et, LEGO_IRQ11      /* check if the pending interrupt is from GPIO JP1 */
bne et, r0, handle_sensor

rdctl et, ctl4        /* check the interrupt pending register (ctl4) */
andi et, et, LEGO_IRQ8      /* check if the pending interrupt is from JTAG UART */
bne et, r0, handle_JTAG

br exit_handler

handle_sensor:
/* Check for false interrupt */

movia r2, LEGO    #_IRQ11            /* load GPIO JP1 into r2 */
ldwio r4, 0(r2)
srli r4, r4, 27
andi r4, r4, 0x01f
cmpeqi r5, r4, 0x01f

bne r5, r0, exit_handler    /* false interrupt */

cmpeqi r5, r4, 0x01e                 /* check sensor 0 */
bne r5, r0, handle_sensor_left

cmpeqi r5, r4, 0x01d             /* check sensor 1 */
bne r5, r0, handle_sensor_right

br exit_handler



handle_JTAG:
    movia r7, JTAG_UART
    ldwio r2, 0(r7)
    andi r13, r2, 0x8000 # check if bit 15 is 1 (read valid)
    beq r13, r0, exit_handler
    andi r2, r2, 0xff #mask lower byte


/* check for 'w' */
movia r12, 'w'      #'w'
beq r2, r12, handle_forward

/* check for 's' */
movia r12, 's'      #'s'
beq r2, r12, handle_stop

/* check for 'x' */
movia r12, 'x'      #'x'
beq r2, r12, handle_reverse

/* check for 'a' */
movia r12, 'a'      #'a'
beq r2, r12, handle_left

/* check for 'd' */
movia r12, 'd'      #'d'
beq r2, r12, handle_right


br exit_handler

handle_sensor_left:
    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)
    movia r4,LEDR
call lightup

    movia r4,LEGO                /* move LEGO into r4 so that motor functions can reference */

call wall_escape_right

movia r4,LEDR
call lightdown
ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32
    movi et,0x1                     /* re-enable interrupts */
    wrctl ctl0,et                   /* sets PIE bit pack to 1 (enable inerrupts) */
br exit_handler

handle_sensor_right:
    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)
    movia r4,LEDR
call lightup

    movia r4,LEGO                /* move LEGO into r4 so that motor functions can reference */

call wall_escape_left

movia r4,LEDR
call lightdown
    ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32

    movi et,0x1                     /* re-enable interrupts */
    wrctl ctl0,et                   /* sets PIE bit pack to 1 (enable inerrupts) */
br exit_handler


handle_forward:
    movia r7,JTAG_UART
    ldwio r7,0(r7)

    movi et,0x1                     /* re-enable interrupts */
    wrctl ctl0,et                   /* sets PIE bit pack to 1 (enable inerrupts) */

    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)

    movia r4,LEGO                /* move LEGO into r4 so that motor functions can reference */
call motors_forward
    ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32
br exit_handler


handle_reverse:
    movia r7,JTAG_UART
    ldwio r7,0(r7)

    movi et,0x1
    wrctl ctl0,et

    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)

    movia r4,LEGO
call motors_backward
    ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32
br exit_handler



handle_left:
    movia r7,JTAG_UART
    ldwio r7,0(r7)

    movi et,0x1
    wrctl ctl0,et

    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)

    movia r4,LEGO
call motors_turn_left
    ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32
br exit_handler


handle_right:
    movia r7,JTAG_UART
    ldwio r7,0(r7)

    movi et,0x1
    wrctl ctl0,et

    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)

    movia r4,LEGO
call motors_turn_right
    ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32
br exit_handler


handle_stop:
    movia r7, JTAG_UART
    ldwio r7,0(r7)

    movi et,0x1                     /* re-enable interrupts */
    wrctl ctl0,et                   /* sets PIE bit pack to 1 (enable inerrupts) */

    addi sp,sp,-32
    stw r15,28(sp)
    stw r14,24(sp)
    stw r13,20(sp)
    stw r12,16(sp)
    stw r11,12(sp)
    stw r10,8(sp)
    stw r9,4(sp)
    stw r8,0(sp)
    movia r4,LEGO

call motors_stop           /* set all motors off and disable all sensors */
stwio  r9, 0(r2)
    ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32
br exit_handler

exit_handler:
    movia r9,LEGO_Edge      /* Clear the lego sensor edge trigger */
    stwio r0,0(r9)

ldw r8,0(sp)
ldw r9,4(sp)
ldw r10,8(sp)
ldw r11,12(sp)
ldw r12,16(sp)
ldw r13,20(sp)
ldw r14,24(sp)
ldw r15,30(sp)
addi sp,sp,32

ldw et, 0(sp)
    ldw et, 4(sp)           # restore ctl1, ea, et
wrctl ctl1, et
ldw ea, 8(sp)
addi sp, sp, 12   # restore stack pointer
subi  ea, ea, 4   # make sure we execute the instruction that was interrupted. Ea/r29 points to the instruction after it
eret     # return from interrupt (this restores ctl0 to it?s previous state that was saved in ctl1 and does pc = ea)


Just to be clear, this code in included in the main.s file downloadable on the previous page.

Now that the interrupt has been taken care of, let's add some flashing LED's, and a top function in C code to wrap it all up!

Step 11: LEDS and main code in C

"Wait!" I hear you exclaim. "Zach, this is ridiculous! Why would you switch languages mid instructable!"

I have two answers for you.
1. Because it is important to understand how assembly can be linked with C code
2. Because our assignment required a mixture of assembly and C code for a good mark :P

So, regardless of reason, this is the next step in our process, and fortunately, by far the easiest.

We're going to write a main function in C that will call the pokemonmaster function in assembly.
We are also going to write a simple C function that turns on, and turns off some LEDS when a sensor interrupt has been triggered.

And here is the code:

#include <stdio.h>

void lightup(int *LEDS);


int main(){
    pokemonmaster();
    return 0;
}


void lightup(int *LEDS){
    *LEDS = 10;
    return;
}

void lightdown(int *LEDS){
    *LEDS = 0;
    return;
}



And you thought it would be difficult :P
The real trick here is to be sure to put the address for the LEDS you want into register 4 (r4) BEFORE calling the lightup and lightdown functions. As I said before, variables are passed in r4-r7, then the stack. This is a standard, and C was written to expect it.

That's it! If you've managed to follow my confusing instructions, you will have some pretty nifty code!

If you've had any trouble at all with these coding instructions, please don't hesitate to ask me a question. I'll do my best to clarify anything. 

Now, let's put everything together and get our robot walking!

Step 12: Putting it all together...

breakout_box_0to4nofig.jpg
Terminal.png
monitor1.png
monitor3.png
monitor4.png
Assuming you've made it this far unscathed, you've both done better then me, and are ready to get this thing walking!

A brief intro to setting up your robot
1. Take DE2 board, and connect to computer & power
2. Take Lego controller and connect to DE2 and power (innermost 32pins)
3. Take Lego breakout box and connect to Lego controller
4. Connect motors and sensors on robot to breakout box
5. done.


Setting up ALTERA Monitor Program:
1. Open program
2. Click 'File' and 'New Project...'
3. In the first window that pops up, pick a location for your project (should be an empty folder), and give it an awesome name. Click 'Next'.
4. Second window, in 'Select a System' drop down, click 'DE2 Media Computer'. Click 'Next'.
5. Third window, in top drop down, choose 'C Program'. Click 'Next'.
6. Fourth window, click 'Add...' and add your 3 code files. Be sure that the browser is showing ALL FILES, and not just ALL C Files, as you will not be able to see your assembly code files. Once you've added all three, click 'Next'.
7. Fifth window is useless to you. Click 'Next'.
8. Sixth and final window, in the text boxes labelled 'Start offset in device (hex)' enter the value 600. Click 'Finish'.
9. Your project has been created!
10. Click on 'Load and Compile' (found under 'Action'), and watch magic happen.

Below are photos explaining the Monitor Setup.

Step 13: It walks. It Walks. IT WALKS!

Now I'll bet you're itching to see some action. Let's take care of that.

Once your program has been loaded and compiled (assuming you do not have any errors to debug), a number of new options will open for you on the monitor program. One of them looks like a play button. Click this to start the program.

BAM! Nothing happened. It's ok, it's not supposed to automatically.
In the monitor program, click on the bottom left window labeled Terminal.

Now you're ready to rock.

Tap the 'w' key on your computer, and the robot should begin to walk forward.
Tap the 's' key to stop him.
Tap the 'x' key to walk backward.
Tap the 'a' key to turn left.
Tap the 'd' key to turn right.

SUCCESS!

Congratulations! (assuming everything worked) You've just built and coded a working quadroped!

Below is a video of Hecaton (ours) doing a demo of his entire range of movement on a slightly slippery floor.

 


If you have any questions or confusions, don't hesitate to send a message. I'll do my best to clarify anything.

Enjoy your robot! 
snoop9112 years ago
Looks great! Can you say more about the Lego Control board? Is that a commercially available board like the mindstorm or something you made?

Also, I don't have a DE2, but do have an Altera BeMicro board... would that work?? assuming I can emulate whatever interface is on JP1?

(I've never used the 'altera monitor program', does that just use jtag on any nios system?)

ZachFejes (author)  snoop9112 years ago
Thank you!

Sadly, we were provided the Lego control board by our faculty, and I'm not sure if it's commercially available. I can ask, and get back to you on that when I get an answer from the school :P

Technically, you could use any microcontroller for this project, although a few notable things would be different. First of all, if you don't use NIOSII, my assembly code may not work for you at all. Given a chance, I'd want to try this again with my arduino :P

As per the Altera BeMicro (awesome! I've never seen one before), I see no reason that it wouldn't work, so long as you could emulate JP1. If I'm not mistaken, it is still a good sized FPGA (which mirrors the DE2 FPGA I was using) and shouldn't have a problem handling a NIOSII structure. From there the only trick would, again, be hooking it up to the Lego Control board.
ZachFejes (author)  ZachFejes2 years ago
Upon further research I've discovered that the Lego Control board is made exclusively for my university (University of Toronto, Faculty of Applied Science and Engineering). Although this is unfortunate, I'll give you a rough layout of what it does, and I'm sure you could figure a work around.

The Lego Controller basically acts as an interpreter for the NIOSII from the FPGA (through the 40pin JP1 GPIO), and with the breakout box, it forms a way to link to the lego circuitry, a motor controller, and a sensor interpreter.

I would imagine that you could imitate this with a classic lego mindstorm controller, so long as you could somehow interpret signals from the FPGA.
RoboTable2 years ago
Cool!
I voted for you in the 'Make it Real' contest. I would vote for you in the robot challenge, but I sorta, entered it. If I didn't enter I would vote for you in the robot challenge.
ZachFejes (author)  RoboTable2 years ago
Thanks! And I appreciate the thought. Good luck in the contest, I'll definitely check out your entry.