FPGA-based Controller for Kinematic Systems

[wuselfuzz]

Hi.

As I already mentioned in my introductionary thread, I started making a Hexapod controller on an FPGA after scrapping the atmega-project.I'm currently using a 50 MHz clocked Xilinx 3S200 Spartan 3 FPGA on a Digilent Spartan 3 Starter-Kit board.

The first version of the PWM module for servo output is already done and tested. The length of the whole PWM pulse is internally stored in a 10 bit value (0-2047). This leaves a 9 bit resolution for the interesting range between 1000 and 2000 usec. If neccessary, the design could be modified to add another couple of bits to that resolution. Seeing the PWM pulse incrementing on the scope in 0.1 usec steps in a test run was a bit amazing.

Some copypasta with four of those PWM modules needed less than 1% of the available chip area, so I suppose there will be plenty of space left for a MIPS CPU core and a UART.

Here's a link to the current code, VHDL probably looks like moonspeak to programmers who never were involved in hardware design: http://fnord.terror.de/f-cks.vhd.txt

Today I picked up my MSR-H01 kit from the post office.

So the next step is assembling it (I don't have all servos yet, though) and trying to fit the FPGA board on top. The current board is pretty large, 100x135mm, but if I got a working prototype running, I'll get a smaller one with a larger FPGA (probably a Zefant LC3E using a Spartan 3E, physical dimensions about 100x58mm or the sparkfun.com Spartan 3 board).

*rushes off, grabbing a screwdriver*

[wuselfuzz]

An update with pics and C-code:





I piggybacked the FPGA-board to a breadboard via a 40 pin connector. The on-chip MIPS CPU and PWM modules are already running, the bootloader takes a gcc compiled MIPS binary via RS232.

The code below already compiles and runs and lets you control the leg servos via a terminal program by sending a,z,s,x,d,c. hex.h contains constant definitions and #defines for COUNTER, UART, SERVO0. etc.

Servo registers take the PWM pulse time in microseconds. The actual output is limited to a range of 1000 to 2047 usecs in the VHDL hardware design, so it's impossible to set too short or too long pulses in software and damage the servos. COUNTER is a 25 MHz counter supplied by the plasma system.

I'm currently working on getting float variables working (gcc -msoft-float, the onboard CPU doesn't have an FPU) and linking my code properly to newlib for math functions and sprintf. And my gf started coding a sim using qt, vtk and opengl to display an simple model of a hexapod. Code for the bot can then be linked against the simulator and the servo motions are shown in a simple 3d model.

/edit: a few links:

Plasma project page: http://www.opencores.org/projects.cgi/web/mips/overview
Xilinx (FPGA vendor) home page: http://www.xilinx.com/ (you can get the free ISE WebPack FPGA IDE here)
Guide for setting up a cross-gcc: http://www.ifp.uiuc.edu/~nakazato/tips/xgcc.html

Code: Select all

#include "hex.h"

char* banner="\r\n\nHi there!\r\n\n";

void uart_write(char *);
void uart_write_hex(char);

int main() {
   unsigned int last_counter;
   unsigned int frame;

   unsigned int servos[3] = { 1500,1500,1500 };
   char received_char;

   uart_write(banner);

   last_counter=COUNTER;
   for(;;) {
      if(UART_DATA_AVAILABLE) {
         received_char=UART;
         switch(received_char) {
            case 'a':
               servos[0]+=10;
               break;
            case 'z':
               servos[0]-=10;
               break;
            case 's':
               servos[1]+=10;
               break;
            case 'x':
               servos[1]-=10;
               break;
            case 'd':
               servos[2]+=10;
               break;
            case 'c':
               servos[2]-=10;
               break;
         }
         SERVO0=servos[0];
         SERVO1=servos[1];
         SERVO2=servos[2];

         for(i=0;i<3;i++) {
            uart_write("servo ");
            uart_write_hex(i);
            uart_write(": ");
            uart_write_hex((servos[i]&0xFF00)>>8);
            uart_write_hex(servos[i]&0xFF);
            uart_write("\r\n");
         }
         uart_write("\r\n");
      while(COUNTER<(last_counter+500000));
      frame++;
      last_counter=COUNTER;
   }

   return 0;
}

void
uart_write(char *s) {
   while(*s) {
      UART=*s;
      s++;
   }
}

void
uart_write_hex(char b) {
   char b1, b2;
   b1=(b&0xf0)>>4;
   b2=b&0x0f;
   if(b1<10) UART=b1+48; else UART=b1+55;
   if(b2<10) UART=b2+48; else UART=b2+55;
}



[Matt Denton]

Wow!, this is coming along fast!

I loving the fisher technik influence.. haven't seen that stuff since I left school (whihc was some toe ago!)

Are you aware the tibia servo is installed the opposite way around to normal? This may cause you problems when trying to get your hexapod to power down, as the tibia length will be too long for the body to reach the floor.

[wuselfuzz]

Are you aware the tibia servo is installed the opposite way around to normal? This may cause you problems when trying to get your hexapod to power down, as the tibia length will be too long for the body to reach the floor.

Oops, I messed that up then.

In other news, today I solved the problems with the C-compiler and coded a small tool that lets you control the servos via rs232. Math support functions and double variables are working now, too. Currently implemented commands and some code fragments below. Next will be a "run" command that starts calling a main loop function every 20ms and updates servo angles in real time. I'll try to move the tibia tip in a square motion from the neutral position (0,c+f,t) to (10,c+f,t) to (10,c+f+10,t) to (0,c+f,t) back to the start. (c: coxa length, the shortest distance from the coxa axis to the femur axis, 25mm if I measured correctly, f: femur length, 85mm and t: tibia length, 150mm)

With a pen attached, this should draw a square with 1cm edge length.

Code: Select all

d - dump state: shows servo pulse lengths and angles
aXXYY - set servo XX (in hex) to angle YY (in hex, 00=0, FF=2*PI)
pXXYYYY - set servo XX pulse length to YYYY (in hex, microseconds, clipped to 1000-2047)
 i - interactive mode, lets you increment and decrement servo angles in ~1 degree steps with azsxdc
k - inverse kinematic interactive mode, lets you increment and decrement x,y,z coordinates with azsxdc


Example for a dump:

Code: Select all

> ?

Help
-----
? - shows help
d - dump state
a<num><angle> - set servo angle
p<num><pulse> - set servo pulse
p - call pulses()
> d

servo   pulse   angle
00      059A    0.4549142066 (26.06466408)
01      05D4    0.1638070956 (9.385455232)
02      05F9    0.2067959094 (11.84853283)
03      05DC    0 (0)
04      05DC    0 (0)
05      05DC    0 (0)
06      05DC    0 (0)
07      05DC    0 (0)
08      05DC    0 (0)
09      05DC    0 (0)
0A      05DC    0 (0)
0B      05DC    0 (0)
0C      05DC    0 (0)
0D      05DC    0 (0)
0E      05DC    0 (0)
0F      05DC    0 (0)
10      05DC    0 (0)
11      05DC    0 (0)


Servo calibration is still hard coded:

Code: Select all

   servo_state.angle[0]=0;
   servo_state.calibration_value[0]=(862*2)/(M_PI);
   servo_state.calibration_offset[0]=0x4a1;

   servo_state.angle[1]=0;
   servo_state.calibration_value[1]=(0x752-0x572)*4/(M_PI);
   servo_state.calibration_offset[1]=0x570;

   servo_state.angle[2]=0;
   servo_state.calibration_value[2]=(0x754-0x57d)*4/(M_PI);
   servo_state.calibration_offset[2]=0x57d;


Inverse kinematics C-code:

Code: Select all

void
ik(double x,double y,double z) {
   double r;
   double f2,t2;
   double r2z2,sr2z2;
   double c,f,t;

   c=25;
   f=85;
   t=150;

   r=sqrt(x*x+y*y)-c;

   f2=f*f;
   t2=t*t;

   r2z2=r*r+z*z;
   sr2z2=sqrt(r2z2);

   ik_result.phi1=atan2(x,y);
   ik_result.phi2=acos((r2z2+f2-t2)/(2.0*f*sr2z2)) - acos(r/sr2z2);
   ik_result.phi3=acos((r2z2-f2-t2)/(2.0*f*t))-(M_PI/2);

}


Conversion of angles to pulse lengths:

Code: Select all

void
to_pulses() {
   int i;
   for(i=0;i<NUM_SERVOS;i++) {
      servo_state.pulse[i]=(servo_state.angle[i]*servo_state.calibration_value[i])+servo_state.calibration_offset[i];
      if(servo_state.pulse[i]>2047) servo_state.pulse[i]=2047;
      if(servo_state.pulse[i]<1000) servo_state.pulse[i]=1000;
   }
}


[wuselfuzz]

Not dead yet.
Videos:

http://fnord.terror.de/walk.avi (3.8MB)

This was the first version of my walk algorithm. There's a small delay at the start, because the IK routines were too slow and this version calculated a table with all servo values for a whole walk loop which then just got played back.

http://fnord.terror.de/botfun.avi (37 MB)

After IK optimizations, lots of code cleanups (and mess ups ) and all that, now movements are calculated on-the-fly.

C-Code for the on-chip CPU:

http://fnord.terror.de/current.tgz

VHDL-Code for the FPGA, including the Plasma MIPS CPU from opencores.org and my memory-mapped PWM module for 18 channel PWM (I got the resolution down to ~2-3usec which is enough):

http://fnord.terror.de/vhdl-current.tgz

ASCII block schematic:

Code: Select all

+---------------------------------------------------+
|           +-----------------+      Spartan 3S200  | 
|           |                 |                     |
|           | Plasma MIPS CPU |        +----------+ |
|           |                 |        | Boot ROM | |
|           +-----------------+        +----------+ |
|                    |A/D bus                |      |
|       +------------+-----------+-----------+      | 
|       |            |           |           |      |
|   +------+    +-----------+    |       +------+   |
|   | UART |    | 18 ch PWM |    |       | SPI  |   |
|   +------+    +-----------+    |       +------+   |
+-------|---------|||||||||------|-----------|------+
        |         |||||||||      |           |
    Notebook       Servos     1 MB RAM   Nintendo DS



The top part is all inside the Spartan 3 FPGA. A bunch of pre-initialized block RAMs are used as a ROM for the bootloader, containing the original Plasma bootloader at memory address 0x00000000. This allows to inspect memory locations and transfer and run binary data via the UART. The FPGA board also has 1MB SRAM which is directly connected to the FPGA and mapped to memory locations 0x10000000-0x100fffff.

The UART and PWM registers are mapped to other memory locations. The SPI module is still WIP, here's a small teaser. The Nintendo DS has a 66 MHz ARM9, a 33 MHz ARM7, 4 MB RAM, two screens, wifi and an own battery. My SuperCard Lite adds another 32 MB RAM and a 2 GB MicroSD card. It also has a coprocessor for fixed-point math and 3D hardware that might allow to offload forward kinematic calculations from the CPU. But currently, after optimizing the code for the MIPS CPU went so well, I'll stick with that for a while and will probably use the DS for higher level behaviour.

[Matt Denton]

Looks like you have been very busy since your last post.. and good to see the project hasn't died and I think you have the award for the tallest MSR-H01, especially wit the inverse mounted tibia servos

I look forward to see what you do with the DS, are they easy to hack / write code for? I would like to see a separate thread on this if you fancy sharing your hack tips / code?

Great work!

[wuselfuzz]

Looks like you have been very busy since your last post.. and good to see the project hasn't died and I think you have the award for the tallest MSR-H01, especially wit the inverse mounted tibia servos

inverse mounted.... uhm.

At least my IK calculations have parameters for that.

They also are not 100% correct yet. In the botfun vid, when the leg is lifted from the DVD box, suddenly a jump occurs when crossing z=0. Also, there must be some case differentiation missing, because in my code, z is squared and so the sign gets lost.

I look forward to see what you do with the DS, are they easy to hack / write code for? I would like to see a separate thread on this if you fancy sharing your hack tips / code?

Currently, I went back to code more for the onboard CPU. It has some drawbacks (e.g. no FPU), but when you work around them, it seems to be fast enough. This is actually pretty similar to the Playstation 1 CPU which was a 33 MHz MIPS, the plasma is running at 25 MHz, but the missing FPU is quite an impact for IK calculations.

Anyways, programming that CPU or the DS or any other embedded CPU is always pretty similar.

Before going into detail about DS stuff now, it seems I actually have to start a bunch of new threads, because there's a bit in every catergory.



Small update about the project itself:

Today I made a smaller board for distributing servo power and signals. It's the same size as a pBrain and mounted inside the body of the bot, with servo connectors on the edges (front left, front right, left, right, back left, back right 4 each = 6x4 = 24 total).

Power pins of the servo are connected in a circle and to a connector for the power supply. I'm using an old PC ATX PSU, 35 A on the 5V rail should be more than enough. Additionally, a 2200uF cap is connected to servo power to buffer short load spikes (I'm not sure if this actually makes sense, but it can't hurt).

Servo signal lines are connected to a 24 pin DIL socket in the center of the board. Only 22 servo signals are wired, pin 12 is GND and pin 24 will be +5V (not connected now). Currently, another 24 pin DIL socket is plugged into that, with 23 wires going to a dual row pin header which plugs into the FPGA board I have (expansion connector B1, I might have linked the data sheet of that board in my first post).

The FPGA board is strapped to a sheet of plastic as an insulator and then mounted flat on the body of the robot.

Of course, I could have used some other kind of connector, a 24 pin DIL socket seems a bit unusual. But the idea is to replace the FPGA board with this: http://shop.trenz-electronic.de/catalog ... 200_t1.jpg

That's the same FPGA I'm using on the Digilent board, with 512kbyte external ram fitting into a 24 pil DIL socket. Besides the I/O pins at the DIL socket, it also has a pin header with 8 additional I/Os. This could then connect to the Nintendo DS.

However, the nice thing about FPGAs is, that you can interface almost any hardware with direct 1:1 connections and limiting the number of pins to 22+8 is pretty bad. For comparison, after hooking up 18 servos and the DS (this requires 4 wires + GND) and 8 onboard LEDs and 8 onboard buttons and switches and the UART and the onboard 16 bit RAM, there are still over 30 unused I/O pins left at the expansion connectors.

And I definitely plan on adding a camera. It might be possible to implement USB on the FPGA and use a cheap webcam.

[wuselfuzz]

New pictures (after fixing the tibia servos XD): http://fnord.terror.de/images/botpix090317/

New vid: http://fnord.terror.de/botfun2.avi

[TXBDan]

Nice work. It seems to move fast and be very responsive.

[wuselfuzz]

So I spent the time today (it's a slow week at work, almost everyone except me is out of the house) disassembling a USB webcam, removing the USB controller chip and soldering some wires to those traces leading directly to the CMOS chip.







Next step will be a simple frame grabber that displays the image to a VGA monitor (the FPGA board has 3 bit RGB out, 1 bit per color, enough to display some stuff).