FPGA-based Controller for Kinematic Systems

[wuselfuzz]

Testing tomorrow.

[wuselfuzz]

I just took another photo, because grabbing a second fpga board from the lab to test the cam resulted in this idea:

[Matt Denton]

This looks pretty interesting stuff! I want to add a video feed to a wifi matchport, but have no idea how to grab data from the camera to feed back.. maybe you can advise?

You need to make your photos a little smaller, on small screens, such as 1024x800 the images are cropped. I found 640x480 is best for forum posts. (don't ask me why I'm currently using a screen that size, I don't usually )

[Mark H]

Hmm, i use a pretty small browser window at work (1030x699) so i can still see things behind - didnt even notice the crop. Why does the forum crop it? couldnt it put scrollbars on it?

The FPGA work is looking really good, I'm quite impressed!

[Matt Denton]

Why does the forum crop it? couldnt it put scrollbars on it?

I guess it crops so that you can still see the message psot details colum on the right? but it is a little anoying.. I guess its a "feature"

The FPGA work is looking really good, I'm quite impressed!

I agree.. really impressive!

[wuselfuzz]

Hm. I didn't scale the photos, because I took them with the DV cam and so they don't have an insane resolution anyways. Usually I scale to 800x600 for the web.

About grabbing data from the cam: I'll try to figure that out today. I suppose that the CMOS sensor is clocked and transmits 8 bit pixel values with each clock cycle. I'll use a few blocks of dual ported ram and a row and column counter to save this data to on-chip ram. For a first test, I'll also use a VGA image generator (I already made one quite a while ago for something else) connected to the second port of that ram to a produce VGA signal of the caputured image at the VGA output of the board.

When this works, I'll connect that video ram to my CPU and can access video data in realtime.

Future ideas: Feeding three lines of video through a hardware multiplier/adder engine, so simple transformations using a 3x3 convolution matrix would be possible in real time, too. Or even simpler stuff like a threshold algorithm.

[wuselfuzz]

Workspace cam, currently online: http://www.date.upb.de/~mgrieger/cam/

I'm also on IRC, IRCNet (irc.ircnet.org) and QuakeNet (irc.quakenet.org), I made a channel on the first one, #hexapodrobot. Link might work or not. Clients: kvirc, xchat, mIRC, Colloquy, irssi. Google them. Or use the webchat at http://www.mibbit.com.

[Matt Denton]

Workspace cam, currently online: http://www.date.upb.de/~mgrieger/cam/

I'm also on IRC, IRCNet (irc.ircnet.org) and QuakeNet (irc.quakenet.org), I made a channel on the first one, #hexapodrobot. Link might work or not. Clients: kvirc, xchat, mIRC, Colloquy, irssi. Google them. Or use the webchat at http://www.mibbit.com.

Loving the botcam , link didn't work.

[wuselfuzz]

Loving the botcam , link didn't work.

That needs an irc client installed then. I added the log output from minicom to the page now.

[wuselfuzz]

Update: Spent almost the whole day coding and speeding up my IK engine.

Interesting things:

Code: Select all

volatile unsigned int *_counter=(unsigned int *)0x20000060;

#define COUNTER (*_counter)
#define FRAME_LENGTH 500000 // 50 Hz

#define NUM_SERVOS 18
#define NUM_LEGS NUM_SERVOS/3

struct offset {
   int x;
   int y;
   int z;
}; 

struct orientation {
   int rx;
   int ry;   
   int rz;
};

struct leg {
   struct offset        origin;
   struct offset        position;
   int                  angle[3];
};

struct bot {
   struct offset        position;
   struct orientation   orientation;
   
   struct leg           leg[NUM_LEGS];
};


static int f_sqrt[65536]; // integer square roots from 0 to 65536
static int f_sin[8192];  // sin(x)=f_sin(8192*(x/(2*PI)))/0x10000
static int f_cos[8192];

void
ik_init() {
   int i;
   for(i=0;i<1024;i++) {
      f_acos[i]=acos((i-512)/512.0)*5215.18;
   }

   for(i=0;i<65536;i++) f_sqrt[i]=sqrt(i);

   for(i=0;i<8192;i++) {
      f_sin[i]=sin(2.0*M_PI*i/8192.0)*0x10000;
      f_cos[(i+2048)&0x1fff]=f_sin[i];
   }
}

void
ik(struct bot *b) {
   int i;
//   int j;
   int r;
   int f2,t2,r2z2,sr2z2;
   int acos_1,acos_2,acos_3;

   int x,y,z;

   int rx_x,rx_y,rx_z;
   int ry_x,ry_y,ry_z;
   int org_x,org_y,org_z;

   const int c=20;
   const int f=80;
   const int t=130;

   for(i=0;i<NUM_LEGS;i++) {

      rx_x=b->leg[i].origin.x;
      rx_y=(b->leg[i].origin.y*f_cos[(b->orientation.rx>>2)&0x1fff] - b->leg[i].origin.z*f_sin[(b->orientation.rx>>2)&0x1fff])>>16;
      rx_z=(b->leg[i].origin.y*f_sin[(b->orientation.rx>>2)&0x1fff] + b->leg[i].origin.z*f_cos[(b->orientation.rx>>2)&0x1fff])>>16;

      ry_x=(rx_x*f_cos[(b->orientation.ry>>2)&0x1fff]+rx_z*f_sin[(b->orientation.ry>>2)&0x1fff])>>16;
      ry_y=rx_y;
      ry_z=-(rx_x*f_sin[(b->orientation.ry>>2)&0x1fff]+rx_z*f_cos[(b->orientation.ry>>2)&0x1fff])>>16;

      org_x=(ry_x*f_cos[(b->orientation.rz>>2)&0x1fff] - ry_y*f_sin[(b->orientation.rz>>2)&0x1fff])>>16;
      org_y=(ry_x*f_sin[(b->orientation.rz>>2)&0x1fff] + ry_y*f_cos[(b->orientation.rz>>2)&0x1fff])>>16;
      org_z=ry_z;

      if(i<(NUM_LEGS/2)) {
         x=(b->leg[i].position.x)-(org_x)+(b->position.x);
         y=(b->leg[i].position.y)-(org_y)+(b->position.y);;
      } else {
         x=-((b->leg[i].position.x)-(org_x)+(b->position.x));
         y=-((b->leg[i].position.y)-(org_y)+(b->position.y));
      }
      z=(b->leg[i].position.z)-(org_z)+(b->position.z);

      r=f_sqrt[x*x+y*y]-c;
      f2=f*f;
      t2=t*t;

      r2z2=r*r+z*z;
      sr2z2=f_sqrt[r2z2];

      b->leg[i].angle[0]=-atan2(x,y)*5215;
      acos_1=(512*(r2z2+f2-t2)/(2*f*sr2z2))+512;
      acos_2=(512*r/sr2z2)+512;
      acos_3=(512*(r2z2-f2-t2)/(2*f*t))+512;
      b->leg[i].angle[1]=-f_acos[acos_1] + f_acos[acos_2];
      b->leg[i].angle[2]=-f_acos[acos_3]+8192;
   }
}

void
move_to(struct bot *current_position, struct bot *target_position, int num_frames) {
   struct bot frame_position;
   unsigned int wait_until;
   int frame;
   int leg,i,j;
   memcpy(&frame_position,current_position,sizeof(struct bot));

   for(frame=1;frame<=num_frames;frame++) {
      wait_until=COUNTER+FRAME_LENGTH;
      frame_position.position.x=current_position->position.x+frame*(target_position->position.x-current_position->position.x)/num_frames;
      frame_position.position.y=current_position->position.y+frame*(target_position->position.y-current_position->position.y)/num_frames;
      frame_position.position.z=current_position->position.z+frame*(target_position->position.z-current_position->position.z)/num_frames;
      frame_position.orientation.rx=current_position->orientation.rx+frame*(target_position->orientation.rx-current_position->orientation.rx)/num_frames;
      frame_position.orientation.ry=current_position->orientation.ry+frame*(target_position->orientation.ry-current_position->orientation.ry)/num_frames;
      frame_position.orientation.rz=current_position->orientation.rz+frame*(target_position->orientation.rz-current_position->orientation.rz)/num_frames;
      for(leg=0;leg<NUM_LEGS;leg++) {
         frame_position.leg[leg].position.x=current_position->leg[leg].position.x+frame*(target_position->leg[leg].position.x-current_position->leg[leg].position.x)/num_frames;
         frame_position.leg[leg].position.y=current_position->leg[leg].position.y+frame*(target_position->leg[leg].position.y-current_position->leg[leg].position.y)/num_frames;
         frame_position.leg[leg].position.z=current_position->leg[leg].position.z+frame*(target_position->leg[leg].position.z-current_position->leg[leg].position.z)/num_frames;
      }
      ik(&frame_position);

      if(COUNTER>wait_until) {
         uart_write_hex16(COUNTER-wait_until);
         uart_write(" slowdown\r\n");
      }

      while(COUNTER<wait_until) ;
      for(i=0;i<NUM_LEGS;i++) {
         for(j=0;j<3;j++) {
            servo_set_angle(&servo[i*3+j],frame_position.leg[i].angle[j]);
         }
      }
   }

   memcpy(current_position,target_position,sizeof(struct bot));
}

void
move_to_parabolic(struct bot *current_position, struct bot *target_position, int num_frames, int dx, int dy, int dz, int a, int leg_mask) {
   struct bot frame_position;
   unsigned int wait_until;
   int frame;   
   int leg,i,j;

   int t,d;
   
   memcpy(&frame_position,current_position,sizeof(struct bot));

   for(frame=1;frame<=num_frames;frame++) {
      wait_until=COUNTER+FRAME_LENGTH;
     
      t=((0x100*2*frame)/num_frames)-0x100;
      d=a-(a*t*t)/0x10000;

      frame_position.position.x=current_position->position.x+frame*(target_position->position.x-current_position->position.x)/num_frames;
      frame_position.position.y=current_position->position.y+frame*(target_position->position.y-current_position->position.y)/num_frames;
      frame_position.position.z=current_position->position.z+frame*(target_position->position.z-current_position->position.z)/num_frames;

      for(leg=0;leg<NUM_LEGS;leg++) {
         if(!((1<<leg)&leg_mask)) {
            frame_position.leg[leg].position.x=current_position->leg[leg].position.x+frame*(target_position->leg[leg].position.x-current_position->leg[leg].position.x)/num_frames;
            frame_position.leg[leg].position.y=current_position->leg[leg].position.y+frame*(target_position->leg[leg].position.y-current_position->leg[leg].position.y)/num_frames;
            frame_position.leg[leg].position.z=current_position->leg[leg].position.z+frame*(target_position->leg[leg].position.z-current_position->leg[leg].position.z)/num_frames;
         } else {
            frame_position.leg[leg].position.x=dx*d + current_position->leg[leg].position.x + frame*(target_position->leg[leg].position.x - current_position->leg[leg].position.x)/num_frames;
            frame_position.leg[leg].position.y=dy*d + current_position->leg[leg].position.y + frame*(target_position->leg[leg].position.y - current_position->leg[leg].position.y)/num_frames;
            frame_position.leg[leg].position.z=dz*d + current_position->leg[leg].position.z + frame*(target_position->leg[leg].position.z - current_position->leg[leg].position.z)/num_frames;

         }
      }
      ik(&frame_position);

      if(COUNTER>wait_until) {
         uart_write_hex16(COUNTER-wait_until);
         uart_write("slowdown\r\n");
      }

      while(COUNTER<wait_until) ;
      for(i=0;i<NUM_LEGS;i++) {
         for(j=0;j<3;j++) {
            servo_set_angle(&servo[i*3+j],frame_position.leg[i].angle[j]);
         } 
      }
   }

   memcpy(current_position,target_position,sizeof(struct bot));
}



Full source here: http://fnord.terror.de/f-cks-tools.tgz

The IK engine on my bot is running at 50 Hz and supports body translation and rotation and leg end effector translation (rotation not possible at 3DOF).

It's using an init routine that precalculates square roots, sines, cosines and arccosines in a bunch of tables using ~80kb RAM (but I have a megabyte onboard).