My Overly Complicated Hexapod Brain

Hexapod Robots

My Overly Complicated Hexapod Brain

Postby dr.angryrahvin » Sun Jun 26, 2011 12:36 pm

Arduino168 + Arduino328 + Arduino1280 + uM-FPU3.1 + SSC32

OK, wait, let me explain :?

So I ordered a hexapod kit, since I'm waiting for it to arrive I thought I'd start planning / researching the software control.

My goals are:
Slowly evolve the IK until I'm happy with it ( never happen :D )
Be able to control movement remotely using my RC aircraft transmitter.
Learn a bunch while I'm at it.

As my research progressed for suitable onboard hardware it looked like an arduino mega1280 was the go, cheap, easy to use etc etc. So I ordered one and thought that would do. Then as I started to draw up the workflow and did more reading on the IK maths it started to look obvious that the arduino wont cut it for the smoothness of movement I would like. I'm shooting for 20 IK frames per second, 5+ would probably be more than enough though.

First of all, my RC receiver output servos pulses every 22ms, it's not a problem to catch and time the pulses on the arduino, but it can't do anything else while waiting for the pulse, and they occur simultaneously on 6 channels, so sequentially that's 130ms minimum just for command input.

Solution, another, smaller arduino328 which will catch the receiver pulses, and on request via interrupt send the last 6 channel reading via serial to the main arduino.

Second the arduino has no onboard floating point unit, and my research suggest they can be quite slow. I've read a number of hexapod related forum posts from people with processing speed issues. So the solution is a uM-FPU, a 32bit FPU connected via ICP to the main controller. I can save IO time by pre-flashing the uM with the IK functions, and simply sending the values to be computed, and returning only the required joint angles.

I had the idea that since there is a processing delay on the uM, this would be the ideal time to have the main arduino do such things as retrieve RC commands for the next frame and do the simple integer maths to shift the feet coordinates.

To simplify programming and save cpu time he servo positions will be outputted via serial to an SSC32 servo controller. I can measure the per frame IK processing time and the SSC can do the maths to make the servo movements match up to the current frame rate.

Lastly, I wanted a live processing time count displayed on the bot somewhere. I ran out of IO on the arduino328 for driving a couple 7 segment led displays, so single AT168P with the arduino bootloader will count each interrupt request from the 1280 to the 328 and drive some leds.

So, since this is my first hexapod, has anybody tried the multiple processor approach before and what was your experience? Is this just plain insane?
dr.angryrahvin
 
Posts: 9
Joined: Sun Jun 19, 2011 11:15 am

Re: My Overly Complicated Hexapod Brain

Postby dr.angryrahvin » Mon Jun 27, 2011 9:21 pm

Update:

I got some of my gear in the mail yesterday, no hex parts but I did get some arduino's, ssc and fpu, so I started bashing out and testing some of the basic functionality.

Reading the rc receiver output pulses and outputting them via serial takes 87ms per 6 channel cycle, so with a goal of an overall 20Hz program cycle I would already be well over my 50ms budget.

I have also written my first leg IK function and flashed the FPU, but in the process I found that for the optimal sequential write of data to the fpu registers I have to rearrange the order I used registers in the IK function.

I'll post an update with code once I have done that and tested integration with fpu / arduino.

Hoping to get my hex parts in the next few days, so I'll get some pics up as the build progresses.
dr.angryrahvin
 
Posts: 9
Joined: Sun Jun 19, 2011 11:15 am

Re: My Overly Complicated Hexapod Brain

Postby TemplarGFX » Mon Jun 27, 2011 11:57 pm

I must say, this is a very impressive project! you must keep this thread updated with your progress. It sounds like you are building the first Skynet LOL
Hexapod Pet Project
Vision Based Object Avoidance with Roborealm
TemplarGFX
 
Posts: 76
Joined: Thu Mar 17, 2011 10:32 am

Re: My Overly Complicated Hexapod Brain

Postby dr.angryrahvin » Tue Jun 28, 2011 8:33 am

Thanks! Although with my programming "skills" Skynet will never happen. ;)

Update on the rc receiver component: In my earlier test I forgot to include the code that would take the servo pulses from the rc receiver and turn it into data usable by the IK engine, I was just dumping the raw values into serial. It made for a nice h4ax0r-esque looking terminal window but was otherwise useless. Suddenly the cycle time went from 87ms to 355! What the... so I had a look at my code to see if I could optimise it. I replaced a constrain() function used to ensure the measured pulse is within a valid range with some if's and simple integer math, suddenly it's back down to 88ms! I don't know what the math behind the constrain() function is, but it sure is slow. The if statement produces the same result but 133 times faster.

Moral of the story: anyone struggling with cycle time, get rid of constraints! With a caveat of "I'm not a programmer and probably implemented it wrong!"

The origional constraint driven code:

Code: Select all
 rxinput = constrain( (pulseIn(modepin, HIGH, 25000) / 10) , 100, 200); //get value and limit travel
  if ( rxinput < 101 || rxinput > 199) //check its valid
  {
    modeswitch = 150;
  }
  else
  {
    modeswitch = rxinput;
  }


My code for the entire shebang. I haven't programmed since "Intro to C" back at uni and that was a decade ago! So if there any any obvious WTF's I would appreciate the feedback.

Code: Select all
#include <SoftwareSerial.h>

#define rxpin 9    //set up pins for software serial used on LED display
#define txpin 10
#define ledpin 13
SoftwareSerial serial2 = SoftwareSerial(rxpin, txpin);
byte pinstate = 0;

int modeswitch = 0; //rx values
int bodyz = 0; 
int roll = 0; 
int pitch = 0; 
int yaw = 0; 
int throttle = 0;

int rxinput = 0; //temporary value in case interrupt is called when rx input is invalid

byte modepin = 3; //rx pin numbers
byte bodyzpin = 4;
byte rollpin = 5;
byte pitchpin = 6;
byte yawpin = 7;
byte throttlepin = 8;

byte irqpin = 2; //set interrupt pin number

volatile int irqcall = RISING; //interrupt variable

int fps = 0;
unsigned long timer = 0;
unsigned long irqlast = 0;

void setup()
{
  Serial.begin (115200); //start serial for spektrum receiver data
 
  pinMode(rxpin, INPUT);
  pinMode(txpin, OUTPUT);
  pinMode(ledpin, OUTPUT);
  serial2.begin(9600);
 
  pinMode (modepin, INPUT);
  pinMode (bodyzpin, INPUT);
  pinMode (rollpin, INPUT);
  pinMode (pitchpin, INPUT);
  pinMode (yawpin, INPUT);
  pinMode (throttlepin, INPUT);
 
  pinMode (irqpin, INPUT);
 
  attachInterrupt(0, serialout, RISING);

}

void loop()
{
  getdata();
 
  noInterrupts(); 
  serial2.print("v"); //clear LED display
    if (millis() > (irqlast + 2000)) // display to read 0 if interrupts stop coming, NOT WORKING
  {
    fps = 0;
  }
  serial2.print(fps);
  serial2.print("F");
  serial2.print("5");
  interrupts();
 
}

void serialout() //print collected values to serial, spaces & finshtime used for debugging to be removed in final build
{
  noInterrupts();
  Serial.print(modeswitch);
  Serial.print(" ");
  Serial.print(bodyz);
  Serial.print(" ");
  Serial.print(roll);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.print(yaw);
  Serial.print(" ");
  Serial.print(throttle);
  Serial.print(" ");
 
  timer = millis() - irqlast;
  irqlast = millis();
  fps = 1000 / timer;
  delay(5); //wait for main board to turn off interrupt
  interrupts();
}

void getdata() //measure PWM from spektrumRX
{
  rxinput = pulseIn(modepin, HIGH, 25000); //get value and limit travel
  if ( rxinput < 1000 || rxinput > 2000) //check its valid
  {
    modeswitch = 150;
  }
  else
  {
    modeswitch = rxinput / 10;
  }
 
  rxinput = pulseIn(bodyzpin, HIGH, 25000); //get value and limit travel
  if ( rxinput < 1000 || rxinput > 2000) //check its valid
  {
    bodyz = 150;
  }
  else
  {
    bodyz = rxinput /10;
  }
 
  rxinput = pulseIn(rollpin, HIGH, 25000); //get value and limit travel
  if ( rxinput < 1000 || rxinput > 2000) //check its valid
  {
    roll = 150;
  }
  else
  {
    roll = rxinput / 10;
  }
 
  rxinput = pulseIn(pitchpin, HIGH, 25000); //get value and limit travel
  if ( rxinput < 1000 || rxinput > 2000) //check its valid
  {
    pitch = 150;
  }
  else
  {
    pitch = rxinput / 10;
  }
 
  rxinput = pulseIn(yawpin, HIGH, 25000); //get value and limit travel
  if ( rxinput < 1000 || rxinput > 2000) //check its valid
  {
    yaw = 150;
  }
  else
  {
    yaw = rxinput / 10;
  }
 
  rxinput = pulseIn(throttlepin, HIGH, 25000); //get value and limit travel
  if ( rxinput < 1000 || rxinput > 2000) //check its valid
  {
    throttle = 150;
  }
  else
  {
    throttle = rxinput / 10;
  }
}
dr.angryrahvin
 
Posts: 9
Joined: Sun Jun 19, 2011 11:15 am

Re: My Overly Complicated Hexapod Brain

Postby dr.angryrahvin » Wed Jun 29, 2011 7:11 am

Spent some time with the FPU today, it's now talks to the arduino (and that took an embarrasingly long time...). The first revision of the leg IK takes 76ms to compute a single leg, but I have yet to implement 'block' data writing to and from the FPU so I think a good chunk of that is IO between arduino and FPU. Also the maths in the IK function could probably be made more efficient.

On a side note, my Ubug arrived today, now I'm just waiting for the servos to arrive and this thing will really get underway.
dr.angryrahvin
 
Posts: 9
Joined: Sun Jun 19, 2011 11:15 am

Re: My Overly Complicated Hexapod Brain

Postby Matt Denton » Fri Jul 01, 2011 7:26 am

This is a full-on project ;) I don't have any experience of the arduino chips, and I'm not sure what frame rate they run at, but the reason I use the dsPIC33 series of processor for the HexEngine is due to the speed it can crunch numbers at and the onbaord output compare registers for harware PWM generation. The HexEngine runs at 16Mips, which is well below the chips maximum of 40Mips. At this speed It will run 50Fps IK and have plenty of overhead for serial comms PWM etc, although you do neeed to take care about how you nest hardware interrupts so as not to loose serial data.

Good luck with the project, sounds like your haveing fun! And post some pictures when you can :)
Matt Denton
AKA: Winchy_Matt

micromagic systems ltd
Matt Denton
Site Admin
 
Posts: 1622
Joined: Tue May 20, 2008 9:15 pm
Location: Winchester UK

Re: My Overly Complicated Hexapod Brain

Postby dr.angryrahvin » Fri Jul 01, 2011 9:25 am

Thanks Matt,

After a very long day with the FPU chip I just can't get it to run faster than the arduino, the minimum I can get it down to is 80ms per leg! Where the arduino does all six in 7ms. Maybe the I/O time is whats causing it, but either way the arduino alone seems fast enough.

Anywho, I've been testing my leg IK and it seems to work great, the letdown is the non-linearity of the cheap servos I'm using to test it with (my digitals still haven't arrived in the mail)

I also switched from using rs232 serial for comms between the rc receiver and main board to using the I2C bus. It's much simpler to program for, and seems faster too.

Have yet to do the IK for body rotations & translations, or any gait or trajectory programming, but now the rc receiver and leg IK are done I will start on that.

It's not real impressive right now but here is a pic of the various controllers all wired up for testing.Image
dr.angryrahvin
 
Posts: 9
Joined: Sun Jun 19, 2011 11:15 am

Re: My Overly Complicated Hexapod Brain

Postby Matt Denton » Fri Jul 01, 2011 9:32 am

Yeah its odd the FPU doesn't cope better than the Arduino, but as yu say maybe its a comms bottleneck? Looks like a fun project though, thanks for the pic!
Matt Denton
AKA: Winchy_Matt

micromagic systems ltd
Matt Denton
Site Admin
 
Posts: 1622
Joined: Tue May 20, 2008 9:15 pm
Location: Winchester UK

Re: My Overly Complicated Hexapod Brain

Postby dr.angryrahvin » Sun Jul 10, 2011 6:29 am

OK, uBug received, assembled, dissassembled, junk $4 servos thrown in fire, re-assembled with HS65's and now programmed.

The Arduino is doing a fairly good job on the IK speed-wise. I have noticed a number of problems with accuracy of floating point calcs though. Arduino is only an 8bit microcontroller and it has to do some fairly horrible rounding to make ends meet.

I was thinking for my MSH01 I may spruik for a Maple, an arduino compatible dev board that uses a 32bit ARM processor @ 72MHz. It's actually slightly cheaper than an Arduino.

At any rate, here is a short vid of my first run at body translations, still working on getting leg sequencing right for walking.

http://www.youtube.com/watch?v=AtE3WNs-Zks
dr.angryrahvin
 
Posts: 9
Joined: Sun Jun 19, 2011 11:15 am

Re: My Overly Complicated Hexapod Brain

Postby Zenta » Sun Jul 10, 2011 8:20 am

Hi,
You are doing great progress on your code! Very smooth movements, that's what we like ;)
Zenta
 
Posts: 92
Joined: Fri Sep 26, 2008 5:23 pm

Next

Return to Hexapods

Who is online

Users browsing this forum: No registered users and 1 guest

cron