Let's Make Robots!

Homer, our robot, gets a little intelligence!


Finally got a little time this week - weekend to work on our robot project. This is a video of steering stabilization for our tracked robot. We're using compass angle as the source for set-point and input for moving correction. After lots of experimentation with localization possibilities, I felt the ability to maintain a stable heading with a track robot was an absolute necessity for localization based on dead reckoning. So here's our solution. This is our working draft code. We tried to make it as simple and fast as possible.

I'll try to pretty the code up a bit later. The code and video are on our site, Appiphania.com.

Thanks,

Jim 

 

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Zero and 360 deg crossing in movement make it difficult to use a compass for course /heading stabilization. This is why you'll not see many (if any) references on google. However, it's not impossible as the video illistrates.

Keep in mind, this is still a work in progress, that's why I've not posted complete code. I would also recommend setting this up on a test Arduino will a compass (double sided taped) so you can debug with the Serial Monitor. If you use this in a project, please acknowledge this as my code and if you modify it, please share it with me.

 

My approach was to sample heading whenever I change state to take a heading (setpoint), then as the robot moves repeatedly read it's updated heading and take the absolute difference to setpoint. This error is multipled by some factor to create a correction signal. I then figure out if the difference in heading is above or below the setpoint and apply this to the motor left and right speed respectively.

So ...basically... this should work...

the first routine is just to sample heading. 

long getheading()

{

        // sample heading angle

        hmc6352.wake();

        sensVal = hmc6352.getHeading();

        hmc6352.sleep();

  return sensVal;

}  

Next while running (polling);

 

void setheading()

/*-----------------------------------------------------

SetHeading Routine

by Jim Winburn 

Appiphania.com

 9/2/2011

*/------------------------------------------------------

{

  Input = getheading(); // I sample heading again

  c = 180 - abs(180- abs(Setpoint -Input)); //take the absolute difference

  d = c*10; // apply multiplier to achive desired correction response (you can replace this with a proper PID algorithm if needed)

  e = (maxspeed-d); // apply correction

  if (e<= 0) //if correction < 0 (negative), make it zero

  {

    e=0;

  }  

 

  if (Setpoint - Input > 180) //test if the difference is above or below the setopint, compensating for zero and 360 crossing

    {

    Input +=360;

    }

    else if (Setpoint - Input < -180)

    {

      Input -=360;

    }

   b = Setpoint - Input; 

 

  if (b < 0) // = negative ... moving to the right ... correct respective motor speed  based on state 

  {

    if (anfwd == 1)   //forward         

    {

    leftspeed = e;

    rightspeed = maxspeed;

    }

    else if (anrev == 1) //reverse

    {

    leftspeed = maxspeed;

    rightspeed = e;

    }  

 

  }  

  else if (b > 0) //= positive ... moving to the left ... correct respective motor speed  based on state 

  {

     if (anfwd == 1)            

     {

     leftspeed = maxspeed;

     rightspeed = e;

     }

     else if (anrev == 1)

     {

     leftspeed = e;

     rightspeed = maxspeed;

     }

   } 

 }

i will be glad to give you my code as soon as i can get it to do what i need like your self work in progress having a few issues getting to correct to the left i thinking it's me getting things a bit backwards im going to use the maths for the most part. 

thank you very much for your help  

 

I'm working on a set of algorithms for precision turns by degree as well as routines for dead-reckoning based on compass angle and odometry. I'm thinking I may eventually create a "CompassPilot" library based on this work for Arduino. 

Good luck with your project,

Jim