Arduino, Electronics and Programming

[C++] Fast Trigonometric Functions

2012-02-18T14:46:00.001Z

Why?

To improve algorithm performance, one direct way is to shorten calculation time. The part that consumes the most computation power would be the trig functions.

The built-in trig functions are generally very good in terms of accuracy. But we don't need that level of precision, so we can sacrifice accuracy to achieve faster speed.

We only use SIN, COS, ACOS and ATAN in our algorithm, so this post will describe only these functions.

SIN and COS

One popular, easy and really fast way is to use look-up table.
Firstly I generated a list of 256 input from 0 to 360, with equal intervals, and get a list of results for each input.

and then use the list of results as our table, and input (in degree) as our index. so let's say we need to get SIN(30), we just need to look up the 30th value in the table.

The reason that I use 1 degree instead of smaller interval, is that not only it saves spaces, but also I won't need that kind of precision anyway. we can obviously do smaller interval like 0.5, or 0.2. just do some index conversion before looking up.

Actually, we don't need the full cycle of SIN, we just need the first quarter cycle, because the whole cycle can be replicated by the first quarter anyway, so we save a lot of memory! All we have to do is to add some if/else statements, and do some tricks on the index system.

Since SIN and COS are very close brothers (or sisters), that COS is just a shifted version of SIN, and we can lookup its value from the same table. The difference is just on the indexing.

Because we are only using integer degree input, and the result for it was computed from the built-in SIN function, so it's guaranteed to be correct!

[Upload Source Code]

ACOS

ACOS is different, because input will be between -1 and 1, if we are going to use look-up table, we will need to do index conversion which will kill the performance. Also the table need to be huge because we can reduce table size like what we did in the SIN function. So might be better to use infinite series stated here.

It should be noted that the more accurate, the higher power term needs to be computed. Power is a terrible performance killer, so I will only keep the first 4 terms.

To verify the accuracy, we can compare the built-in method with the 'polynomial' method in Excel.

As we can see, the error rises exponentially after about 0.3. It is obvious what we have a larger error as we go near 1. Because we are only keeping the first few polynomial terms, as we go nearer to 1, the neglected terms will become significant, thus making the difference obvious.

what we can do here, is to replace the polynomial method with look-up table, let say when input is larger than 0.8, when the error could have reached about 1%.

New method is about 15 times faster! (measured in VC++)

[Upload Source Code]

ATAN

I have been search for a way to do it. But none seems to be faster than C++ ATAN. So I will stick with it for now.

Hexapod Robot with 3 DOF legs (degree of freedom)

2012-01-21T23:04:00.000Z

This is an upgrade of the Hexapod Robot with 2DOF leg here:

http://arduin0.blogspot.com/2012/01/hexapod-robot-with-2-dof-legs-degree-of.html

So far we have:

［31/Jan/2012]

[13/Feb/2012]

I have ordered parts from Lynxmotion, but they don't have them in stock, so I will have to wait for another 1 or 2 months. Before that, I will build a smaller version hexapod robot to implement the algorithms.

Most of the parts in this project would be inherited from the 2DOF version robot, except the base.

I wanted to make a servo interface with the shield I bought off ebay, which would making it so easy to install the servos without making a mess. In theory i could use 48 servos on a Mega board, but I only soldered 20 servo ports, just to keep wires tidy and compact. I need only 18 servos for the legs and possibly 2 for the sensors anyway.

I am leaving some space on the right hand side of the board to put a adjustable voltage regulator in, as I am planning to use 8xAA batteries, or 3 lipo 11V batteries in the future.

===========================================
Update
============================================
28/01/2012

Redesigned and made another base, with smaller diameter and larger thickness. the Previous one was too thin that it actually bends a little when it's standing.

found that when casting float to int, numbers are floor rounded e.g 4.4 = 4, 4.8 also = 4. which would introduce round-off error. need to implement number casting function to resolve this.

int FloatToInt(float input){

// this is an alternative to cast number directly, to avoid floor rounding

int temp = (int)input; // floor-rounded input

float dif = input - (float)temp; //

if (dif < 0.5) return temp;

else return temp+1;

}

Also there is another problem with responding speed. I check the resulted in the C++ code against the simulation excel spreadsheet, found the error margin is quite big, because I was using int for all the calculations. I then change all of them into float. Although it works on the robot, but the responding time increased quite obviously, also the robot movement become unstable. I think it's because of the computational power of the Arduino just isn't good enough to do pure floating point IK cacluation. I will need to think of a way to balance between accuracy and computational load.

I have also place 2 4xAA battery holders in parallel to supply more current, because i found it could draw up to 1.5A when the servos are loaded which causes voltage to drop and fail to supply enough current and lead to legs paralysis.

=======================================================
Update
=======================================================
29/01/2012

Found a coupld of bugs in the algorithm, thanks to my Excel spread sheet simulation! Now the pitch and roll movements finally work properly, although the legs are still quite unstable. I still don't know why that is, maybe the dimension is still not correct, or maybe there are still bugs in the algorithms.

=======================================================
Update
=======================================================
09/02/2012

I have now adapted rotation matrix in Body IK algorithm, instead of estimating coordinate changes, i will be using formula derive from rotation matrix to calculate these changes (detail could be found in my last post, IK implementation).

I have also started to use micro seconds instead of degrees to write servos.

I have also had a major modification on the legs. It's now doubled the length of the original.

Found a problem with pin22 and pin32 when using them as servo pins. These two pins interference with each other, ie, control one of them would control the other one too. Luckily I made 20 pins, so I can just easily move up all the pins by 1, ie, now using 23 - 41.

We had a water tank overflow on the roof tonight, was a bit scary and we spent hours looking at the problem, so didn't do much testing on the new setup. will try tomorrow.

The main problem with it at the moment remains to be leg-end position accuracy. I don't know whether it's the hardware or the software. But since I have move all the calculation into floating point, it should give relatively good numerical results.

I have started looking at Gait, might implement 6 step ripple gait first.

=======================================================
Update
=======================================================
12/02/2012

Last night, I implemented a simple 6 step ripple gait, which I will explain in a minute. Let me explain what's the problem I discovered first with the IK algorithm.

When the legs are meant to move along Z axis, the front and rear legs are actually moving toward Z-X direction, which is not right. So I wrote a test program to test the leg IK algorithm by checking the actually physical change of the leg position, versus the coordinate I entered manually. Again the legs won't move solely along one particular axis (X or Z), but tend to go somewhere in between.

Spent an evening thinking about it, realize the original idea of calculation IK isn't perfect for my type of hexapod robot, which each leg are in a different frame (frame - Axis system), but I have always been assuming they are in the same frame as the center of body. for example, the frame of the front right leg is rotated -30 degree from the frame of the center of body (see below picture), so before we calculate the change of angles from leg IK algorithm, we need to do coordinate transformation first, to convert it from body frame to the coxa frame.

A common way would be using rotation matrix. detail please go to my IK implementation post.

after the modification, the robot can finally walk like one. ; )

=======================================================
Update
=======================================================
19/02/2012

Good news from Robotshop! My parts have finally arrived their warehouse and has been already shipped to me. So I got to hurry up finishing body balance algorithm this week (probably my last implementation on IK)

This weekend I:
1. tidied up the code a lot!
2. implemented fast SIN, COS, ACOS functions (about 10 times faster than standard C trig functions! body rotation is so much smoother now)
3. implemented 7 more gaits (thanks to Jeroen Janssen's source code, I copied most of the stuff over)
(1) Ripple 12 steps
(2) Quadripple 9 steps
(3) Tripod 4 steps

(4) Tripod 6 steps
(5) Tripod 8 steps
(6) Wave 12 steps
(7) Wave 18 steps

4. modified code to avoid using 'delay()', but to get the time using 'millis()', so processor can do other stuff while waiting rather than just freezes there. Also brings a bit more smooth movement there.

All looking great! however because I am getting so many functions and gaits now, I am having trouble with controls... nun-chuck is simple and handy, but it's very limited in terms of control options. I might need to come up with a menu system that I can switch between modes (body rotation, walking mode), and under walking mode i can also switch between gait types... I just love the nunchuck and don't want to replace it!

================================================================

To Do:
~~1. Implement Fast trig functions~~

~~2. Convert Angles in degree/radian to MicroSeconds and use micro seconds to drive servos.~~

~~3. implement first Gait Sequence Generator~~

~~4. implement different Gaits~~

5. Body balancing algorithm

6. Automation, Sensor (IR or ultra sonic), basic AI (object following, avoiding)

7. control remotely (maybe bluetooth, or XBee)

8. Intermediate AI - Responses to outside world environment, possibly movement, or human gesture

9. Terrain Adaption - more sensors on the legs - highly linked with 5, and 8.

Inverse Kinematics IK Implementation for 3DOF Hexapod Robot

2012-01-21T19:33:00.001Z

Nunchuck controlling single leg test:

Excel Spreadsheet simulator

Discussion of IK algorithm can be found in my previous post:

http://arduin0.blogspot.com/2012/01/hexapod-robot-gaits-calculation-inverse.html

Derivation of the equations:

This is quite a simple trigonometry problem.

Notice I have made a mistake on the first drawing about "leg length". We need to solve it on two different planes, the first plane is looking down in the same direction as Y axis, but the second might not aligned to any particluar axis, but could be some where between x and z axis. So the leg length is what we get from the equation.

[Implement body roll (Z axis), pitch (X axis) and rotate (Y axis)]

The principle behind a robot that moves using Inverse kinematics algorithm, is that if we move the centre of the body, it would change the coordinates of the feet end, therefore it will change the servo angles.

for example, if we are moving the robot to the right (centre of robot moving from red to the blue point), the end of feet coordinates would shift to the right of the same distance as the centre of the body. The feet are stationary, so moving the body won't change the absolute positions of the feet, but their relative positions to the centre of body are changed. Therefore we need to take these changes into account, and compute the latest relative coordinates, then using LegIK procedure described above, to calculate new servo angles.

To sum up, according to the change of coordinates of the body, or rotation, we can calculate the change of coordinates of the legs in Body IK algorithm, and pass that result to the Leg IK algorithm to calculate the change of the servo positions. However, I will implement these two in the reverse order.

Here we go.

[Leg IK]

[Body IK]

So we have solved the problem. To convert the angle into usable value, simple add 90 to them before writing to servos.

To make this more efficient we can write our own trigonometric functions using look up table. I have done sine and cosine functions, will do atan and tan function when I have the time.

If you want the source code please contact me.

[To Do]
1. notice when calculating body rotation (roll and pitch), we should not just do 'tan' to estimate change in y axis, but x and/or z will change as well (although it's small, that's why I ignore it there). If we want a more natural rotation, it's best to include that, and I will probably do so in the next update.

=======================================================
Update
=======================================================
03/Feb/2012

We have been working with 3D coordinates (x,z,y) in our IK algorithm, find the change in each dimension, and calculate the change of angles. Notice that each dimension is calculated using very basic trigonometric functions and ideas, which isn't very close to reality, and as you can probably see from the video demonstration the movement isn't very smooth nor natural.

A more advance and complex way to calculate change of coordinates from rotations is to use Rotation Matrix. Please see here for detail. It might seem complicated and make no sense at all at first, check out youtube for some 2D rotation examples, and move on to 3D, it will become more clear.

Anyway, to implement it in programming doesn't require you to understand it. If you are feeling really impatient, just copy and paste my code into yours and you won't need to worry about it ever again. So feel free to explore the field, and I will describe it briefly here as well.

=======================================================
Update
=======================================================
12/Feb/2012

Spent an evening thinking about it, realize the original idea of calculation IK isn't perfect for my type of hexapod robot, which each leg are in a different frame (frame - Axis system), but I have always been assuming they are in the same frame as the center of body. for example, the frame of the front right leg is rotated -30 degree from the frame of the center of body (see below picture), so before we calculate the change of angles from leg IK algorithm, we need to do coordinate transformation first, to convert it from body frame to the coxa frame.

A common way would be using rotation matrix.

====================================================================
Excel Spread sheets Downloads
====================================================================
Hexapod Robot Spreadsheet V1 (no gaits)
Hexapod Robot Spreadsheet V1.1 (modified roation, no giat)
Hexapod Robot Spreadsheet V1.2 (with gait)

Inverse Kinematics And Trigonometry Basics

2012-01-09T21:51:00.001Z

So firstly, why do we need kinematics? What are they?

Motivation

With your robot having legs the position of those legs dictates where its feet are. Where its feet are dictate its point of balance.

As you undoubtedly know balance can be defined as the robot's centre of mass (affectionately referred to as its centre of gravity) being between its centre of pivots (i.e. the edges of where its feet contact the ground).

If the centre of mass is above the centre of pivots and between them the robot will balance (almost an unstable equilibrium, if you're an applied mathemetician).

If the centre of mass is above but outside the centre of pivots (i.e. beyond the edges of his feet) the robot will overbalance and fall. Fast.

If you feel confident about the basics, here is an implementation example of a 3 DOF hexapod robot using IK:
http://arduin0.blogspot.com/2012/01/inverse-kinematics-ik-implementation.html

If you're a little unclear about this start with something basic, a cube is a good start, and imagine that its centre of mass is right in the middle (which it will be if its density is even throughout). When the cube is just sat there it's stable. The centre of mass is above the centre of pivot (the edges) but because it's between them (when viewed from every direction) it will just sit there until you prod it.

Now you prod it and slowly tilt it. As the centre of mass approaches a point directly above one of the edges (our centre of pivot) the cube will feel lighter to your touch and if you can get the centre of mass directly over that centre of pivot it will balance. As soon as you push it past that point, so the centre of mass is the other side of the centre of pivot it will fall.

The robot is exactly the same. This is why the kinematics of the feet are important to you. If you want the robot to balance dynamically you NEED to know where the feet are and where they're going to need to be.Please understand that I'm not going to do all your work for you, so the code or equations I share are not guaranteed on their accuracy but purely a demonstration of how the method is derived and works.

==================================================

Forward and Inverse Kinematics

Forward kinematics is the method for determining the orientation and position of the end effector (x,y,z) coordinates relative to the centre of mass , given the joint angles and link lengths of the robot arm (servo positions). This equation is deterministic. You know absolutely from the servo positions exactly where the foot is.

Inverse kinematics is the opposite of forward kinematics. This is when you have a desired end effector position, but need to know the joint angles required to achieve it. This is harder than FK, and there could be more than one solution.

The FK is not very useful here, because if we are given a change of angle of a servo, only one effector moves in the chain. But if we are given a change of coordinate, the whole chain of effectors (servos) might have to move a certain angle for the end point to reach the desired position. And also the movement tend to be more natural as well!

=====================================================================

There are two approaches to solving inverse kinematics:

Analytical - requires a lot of trigonometry or matrix algebra
Iterative - better if there are lots of links and degrees of freedom.

IK - Analytical approach

If there are only two or three links then it may be possible to solve it analytically. One possibly might be to draw out the arm with the angles shown on it, then solve for the angles using geometry. The problem is that this is not really a very general approach.

Another analytical approach is to represent each links rotation and translation by a matrix. The end point is then given by all these matrixes multiplied together, so we just need to solve this matrix equation. Then find what rotation each matrix represents.

There may be many solutions or there may not be any solutions. In other words there are lots of ways to reach to a given point, or it may be out of reach.

If there are many solutions, then you might need to apply additional constraints. For instance, human joints can only bend within certain limits.

IK - Iterative -approach (only if you are interested reading)

This is a more general approach for programming complex chains.

Start off with the joints in any position, then move each of the joints in turn, so that each movement takes the endpoint toward the target.
Starting with the joint nearest the end point, rotate the joint so that the current end point moves toward the required end point. Then do the same with the next joint toward the base and so on until the base is rotated. Then keep repeating this, until the end point is close enough to the required end point or if further iterations are not moving it closer to the required point.
It may be possible to have a more realistic strategy than this, for instance, if I am using my arm to pick up an object then, if the object is a long way away, I will move the bigger joints in the arm, then as the hand gets closer the smaller joints of the hand are used for the fine adjustments.
The angle of rotation for each joint is found by taking the dot product of the vectors from the joint to the current point and from the joint to the desired end point. Then taking the arcsin of this dot product.
To find the sign of this angle (ie which direction to turn), take the cross product of these vectors and checking the sign of the Z element of the vector.

because we will be mainly dealing with 3DOF hexapod or Quadurped robot legs, Analytical, or trigonometry would do the trick for now.

================================================

Let's get started, write a trigonometric library first!

1. need to do is refresh your memory on trigonometry.
Understand how you can obtain the sine and cosine of an angle, what they mean when converting angles and distances into x/y coordinates.
Also understand how you take a two dimensional coordinate (x,y) and rotate it about a point (say a,b) by a given angle. This is absolutely CRITICAL to understanding kinematics.

2. implement trigonometric functions
Once you've understood all this you need to implement sines, cosines and tangents (and the inverse versions) in your software. Using the library versions of these functions is going to be expensive in space and computation. So, try not to use standard library function, WRITE your own functions!

Scared? Don't be. Writing your own on a microcontroller is actually surprisingly easy. All we need to do is to pre-calculate values and store them in a table, so we can just look it up.

Let's try to write sine and cos functions first and test them. 3D trigonometry is just 2D trigonometry applied in three dimensions. Create yourself some functions that rotate 3D points around each of the three axes.

Now test again. You have to be confident that it works without bugs and with an accuracy suitable to your application.

If you've got this far you've got trig working in your microcontroller. You've probably also written a whole bunch of supporting functions (e.g. printf() debugging, square root functions etc). You're probably pretty damn proud of yourself.

Now, the fun starts from here.

============================

15/12/2012

To get this turned into a program, you'll have to remember that the angles are the unknowns, and proceed from there.

So, first thing is going to be turn this from a 3D problem into a 2D one, by solving for gamma. I would recommend thinking about theta in terms of the tangent function, X and Y. There is a common function called atan2 which is typically used to find gamma , given X and Y. So, a single function gets you gamma (sweet!).

Now that you have gamma, you have two links and two joints left to place (and they are in the same plane), which are your red leg lines:

You would typically then find OB using OB^2 = (L1^2 + L2^2). OB, L1, and L2 form a plane, which all our calculations are now on.
Now, we notice that the same atan2 function could be used to find alpha (based on Zb and the length of the horizontal projection of OB).
You can then solve for (alpha + delta), because you have all three sides of your triangle -- and then subtract the alpha value you just determined to find delta.
And now, you have a function to describe phi in your sheet (as acos of a really nasty set of numbers).

At that point, you have your values for your servos!

===========================================================

So now we have IK, the next step would be on 'How to use IK to form Gaits'.

the idea is, to make a step, we will have to tell the servo to move to a set of positions. the more positions, the more smooth the motion would be. and that's how gaits are generated.

The best way to study gaits generation, is to go through people's code. Here are some good code we could study:

Best way to learn with those topic is to download the PowerPod program from Lynxmotion's shopsite:

http://www.lynxmotion.com/images/files/firmware/ppod102_x44.zip

Generate a basic code, and try to follow the code line by line and understand what its doing (not necessarily focusing on how), then once you figure out the flow of the code, you can go back into the details of the how by following some basic trig.

It took quite a while for me to figure out what it's doing.

The code is basically doing inverse-kinematics. Kinematics is giving the servo an angle and calculating where it's supposed to land given the length of the arm/apendage. Inverse is giving it a target location and figuring out the angle the servo needs to move in order to accomplish the movement [of the tip of the appendage] to that location. But that's a very high level understanding.

For me, I printed out the awesome explanation of the code by Luis Hidalgo, the code generated by Powerpod, and also the IK-diagram, and just kept studying it over and over until it started to click.

I'm no expert at it, but I'm still on the path to getting to know it more. You should do some google search on this topic (inverse kinematics) and there are several websites out that explains this quite readily. Some useful ones I found are:

==========================================================

Here are some good sites to explore more of the IK field:

http://freespace.virgin.net/hugo.elias/models/m_ik.htm

http://freespace.virgin.net/hugo.elias/models/m_ik2.htm

http://www.lynxmotion.net/viewtopic.php?t=2534
http://www.lynxmotion.net/viewtopic.php?t=3361

Control Arduino Through Serial Connection (USB) VB Visual Basic program

2012-01-02T02:24:00.000Z

The complete Serial Control library is still under development and unavailable. I will publish the source code and program once it's done.

However you might still find the updates and code blocks useful.

LED Example:

Hexapod Example:

Why?

When we are debugging and testing our circuits and codings, most of the times we reset the Arduino board and upload the new program. But the thing is everything has a finite lifetime, and by doing harmful things to the body would even reduce life quicker.

Just like smoking could kill you, repetitively uploading could kill your arduino too! I still remember I read from somewhere, and someone said an Arduino board has an average uploading limit of about 1000 times... I don't know if this is true, but if we could avoid doing something that could harm the arduino, then why not?

So here we are, I was looking for a way to test and develop new moves and gaits for my hexapod robot. But frequently uploading new codes really cost me time and risking killing the arduino, so I thought it would be nice to test it just by sending the arduino a command through USB connection (Serial communication), therefore the possibility of program uploading could be minimized.

This is also useful when we use bluetooth to control arduino.

How?

Arduino has already provided a serial communication class, and there are built in examples of how to use them. Here is the official doc:

http://arduino.cc/en/Reference/Serial

I will first try to establish a basic class and working Arduino program in the Arduino development IDE, and then move on to write a VB program to provide better and more user friendly interface. I will also publish my source code in the near future.

=======================================================

1/1/2012

Happy new year!

I will divide this tutorial into 2 sections: Receiver and Sender ends.

Receiver side coding:

So the command I designed would look like this.

MxPxDx/

x - arbitrary number

M - mode (0 - write, 1 - read, 2 - servo control)

P - pin (0 - 13), or servo number (specified in arduino program)

D - data (0 - LOW, 1 - HIGH, or (600 to 2400) - servo position)

/ - indicate end of command (to execute)

for example,

M0P2D0/ - means to make pin 2 LOW

M2P1D900/ - means to make servo number 1 to 900 ms (1500 = 90 degree)

The idea is that the command get sent to the arduino as a array of charaters one by one, which has delay between each charaters.

notice that the function Serial.read() can only read one byte (one charater) at a time. So if you write something like this:

a = Serial.read()

b = Serial.read()

you can get the first character, but the second one might not be read as it hasn't arrived at the serial receiving buffer yet.

So I use this statement:

void loop(){

if (if (Serial.available() > 0) {

//...Read one byte and store in array...

//...if the last character read was '/' then...

//...execute the command...

//... reset the command array, ...

//...keep listening to serial port...

}

Serial.available() indicates how many bytes are available to be read. So the arduino just keeps running, and reads data when it becomes available and that's how commands are received. Here is how the command is decoded and executed:

1. When I received a character 'M', I remember all the char after 'M', before I encounter 'P', and store that in variable 'mode'

2. same, store characters after 'P' and before 'D', in variable 'pin'

3. store characters before '/', in variable 'data'

Then convert these char into int type (you can also do it during above process), by subtracting '0', which has an ASCII value of 48.

So now we have 3 variables, which is basically the commands. To execute the command, we can write something like this:

switch (mode){
      case 0:
        digitalWrite(pin,data);
        break;

      case 1:
//...Read digital pin...

// ... Read analogue pin...
break;

case 2:

// ...Control Servo...

// ... set position, read position so on...
break;

}

To execute read and servo commands, we will need more complicated coding, but that's roughly how it would work.

Sender side Coding:

To use this serial command system, no coding is needed when using arduino serial monitor. just type in the command and it just works!

But that isn't even 5% of the amazing things it could achieve! If you could write a program and introduce graphical interface, controlling the arduino would be so intuitive and clear! also command can be sent more rapidly through computer programs, especially useful when testing robot moves, or LED lighting effects!

I think the most popular choices for serial programming would be Python, C/C++, or Basic. I know C/C++ the best here, but the thing is C/C++ is not easy to write the graphical interface. So I went for Basic, which I realized its simplicity and fast development time. I spent 2 hours learning the basics, and got my program working 3,4 hours later. Surely most of us could do better.

In the example video (the LED control program), when the ON/OFF button is pressed, it goes to a 'switch - case' statement, depending on what LED is selected, corresponding LED is switched on or off. The Display function is even easier. it sends out a command, delay for sometime, before sends out another one. So the LED flashes and creates some kind of fabulous visual effect.

That's it for the basics. I will carry on completing the code, especially the servo control for my hexapod robot. Finally will put it in a header file.

=====================================================
02/01/2012

Servo Control coding:

Receiver Side (arduino):

command example:
M2P0D1500/ --------- set servo number 1 to middle position (1500 ms).

This command works the best with my way of updating servos' positions (described here, search for "26/12/2011" update. )

Basically what it does is, there is a variable for the position of each servo, when command is received and executed, the variable will be updated rather than writing the servo position straight away. The position is only written at the end of the loop process (or when the timer has reach certain point if you want delay).

for example:

void loop(){

// ... command receiving
// ... command executing (update servo position and store in variable 'pos')

servo.writeMicroseconds(pos);

}

So in the execution coding block, we add:

// ============ Execute command ======
    switch (mode){
      case 0:
        digitalWrite(pin,data);
        break;

      case 1:
        digitalRead(pin);
        break;

      case 2:
        switch (pin){
          case 0:
            pos0 = data; //
            break;
          case 1:
            //pos1 = data;
            break;
          case 2:
            //pos2 = data;
            break;

        }
        break;

    }

Sender side:

When we are testing using arduino serial monitor, no coding is needed.

In VB (basic), it's best to use track bar.

Create events when trackbar is scrolled, this is an example

Private Sub TrackBar1_Scroll(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles TrackBar1.Scroll

Dim valueInStr As String = CStr(TrackBar1.Value)

Dim command As String = "M2P0D" + valueInStr + "/"

SerialPort1.Write(command)

LB_ServoPos.Text = valueInStr

System.Threading.Thread.Sleep(100)

End Sub

it works when you only have one servo, if there are more servos need to be deal with, has to write more codes to determine 'P' (which servo to control).

================================================================

03/01/2012

Tested the serial command system, one big drawback is that between each command, the delay is quite obvious. When I intend to move 2 servo positions at the at same, one servo is actually moving before the other.

That's because it uses this algorithm:

(waiting and listening to USB) ---> (receive command 1) --> (Execute command 1) --> (waiting and listening to USB) ---> (receive command 2) ---> (Execute command 2) --->.......

Although command 1 and 2 are intended to be executed at the same time (sent at the same time), there will be a delay.

I re-consider the system, decided to fix this synchronization problem by adding an additional command:

SxCa...SyCb/

which contains all the servos and changes of position at the same time, and execute them all together, so delay could be reduced between servo moves.

The length of this command is indefinite, but can be determined by how many servos wants to be controlled. E.g. max length of command = (1+2+1+4)*number of servos

assuming we control 18 servos at most, the command buffer length would be 8*18 = 144 (this can be modified easily when needed)

for example:

S0C-200S4C600/ --- would turn Servo 0 backward 200us, Servo 4 forward 600us.

You might notice, when 2 or more servos are moved at the same time, if the changes of positions are different, it will also result in a different duration (where some servo reach destination faster than others).

To address this, I added one more string of data at the end of the new command,

Lxxx,

which means Loops, or steps, required to finish transition. So ideally, when we have a command like the example above, "S0C-200S4C600L50/" Servo 0 will have to spend 50 loops in the arduino program to finish the transition, which divides servo 0 to move -4 each loop, and servo 4 to move 12.

You might think it's unnecessary, but one big good thing it brings is smoothness! let image you want to change a servo position from 1500 to 800, you could do it at once (within 1 loop), but it introduces instability to your robot and circuit (draw of current). Also when motion gets too big, it won't look pretty. with additional loop control, big motion can be broken down into small, digestible pieces.

Now we have the final template servo command:

SxCa...SyCbLc/

--------------------------------------------------

As we have a new command, additional algorithm in the arduino programm will be needed to code with it. What I will do is:

1. create a function that generate a new position for each servo at each loop.

2. make arduino to talk back! Sending the computer a signal, when we have complete a command, and available for another one. (so we can have a collection of commands waiting on the PC side, and are sent when arduino finishes the last one)

====================================================
05/01/2012

This will be the final update on this topic.

new idea of the command analysis process:

first of all we need to have a fixed length of command, by making unused its zero, and use '1' to represent negative in servo command e.g.

M00P05D0235/ --- write pin 5 a PWM value of 235

S10C00300S02C10700L0100/ --- turn servo 10 300um, turn servo 2 -700um after 100loops

in which case, instead of having a switch, we can now just loop through the command buffer char by char, to store the variables and data.

I want to do this is because

1. cleaner code block

2. less likely to have bug.

3. commands are easier to handle when have fixed length.

==============================================================

Source Code:

VB (visual basic) program and command files:
http://dl.dropbox.com/u/457167/Hexapod%20Controller.rar

Arduino Main program:

// Oscar's Project

#include <Servo.h>

#include "serial_command.h"

// ========== Position Table ===========

SerialCommand command;

// ========= Servo ===================

// 12 servos

// first 6 - legs last 6 - sholders

Servo servo[12];

int servoPos[12];

// ========== Pins ============

// first 6 legs, last 6 sholder

const byte pin[12] = {8,9,10,11,12,13,2,3,4,5,6,7};

void setup()

{

Serial.begin(9600);

// == Setup Servos ==

for (int i=0; i<12; i++){

servo[i].attach(pin[i]);

servoPos[i] = 1400;

servo[i].writeMicroseconds(servoPos[i]);

delay(100);

}

delay(2000);

command.Reset();

}

void loop()

{

if (command.ReceiveCommand()){

if (command.DecodeCommand()){

if (command.ExecuteCommand()) {

for(int curPos=0; curPos<command.loops; curPos++){

for (int i=0; i<12; i++){

// UPDATE POSITIONS

servoPos[i] += command.servoPosAdjust[i];

servoPos[i] = constrain(servoPos[i], 600, 2400);

// WRITE POSITIONS

servo[i].writeMicroseconds(servoPos[i]);

}

delay(40);

}

// Finish, send signal to PC

Serial.write('1');

}

else

Serial.println("execution failed"); // execution failed

}

else

Serial.println("decoding failed"); // decoding failed

//if (command.executed && command.listening)

command.Reset();

}

Command class:

#include "WProgram.h"

class SerialCommand{

private:

static const int MAX_SERVO_NUM = 18;

static const int SINGLE_COMMAND_LENGTH = 3+5;

static const int MAX_COMMAND_LENGTH = MAX_SERVO_NUM * SINGLE_COMMAND_LENGTH + 5;

int index;

int command[MAX_COMMAND_LENGTH]; // longest command is 12 char long

public:

boolean executed; // true - ready for new command

boolean listening; // true - recieved part of the command but not '/' yet

// false - still listening to command

int pin; // pin number, OR servo number

int mode; // 0 - write, 1 - read, 2 - servo control

int data;

// Servo data

int loops;

int servoPos[MAX_SERVO_NUM];

int servoPos_LastUpdated;

int servoPosChange[MAX_SERVO_NUM];

float servoPosAdjust[MAX_SERVO_NUM];

// ============ Functions ===============

private:

long GetSerialByte();

int Char2Int(char chr);

public:

SerialCommand();

void Reset();

boolean ReceiveCommand();

boolean DecodeCommand();

boolean ExecuteCommand();

};

#include "serial_command.h"

SerialCommand::SerialCommand(){

servoPos_LastUpdated = 1500;

for (int i=0; i<MAX_SERVO_NUM; i++)

servoPos[i] = 1500; // longest command is 12 char long

Reset();

}

long SerialCommand::GetSerialByte(){

return Serial.read();

}

int SerialCommand::Char2Int(char chr) {

if ((chr < '0') || (chr > '9'))

return -1;

else

return chr - '0';

}

// =========================================================

// ===================== Main functions ====================

// =========================================================

boolean SerialCommand::ReceiveCommand(){

if (Serial.available() > 0) {

// determine

if (index >= MAX_COMMAND_LENGTH) return false;

// Receiving

command[index] = GetSerialByte();

listening = false;

if (command[index] == '/') return true;

listening = true;

index++;

}

// no signal available

return false;

}

boolean SerialCommand::DecodeCommand(){

if (command[index] != '/') return false;

boolean negative = false;

int indexCur = 0;

// Decoding

// differentiate type of command

if (command[indexCur] == 'M') {

// pin mode

// ... more code...

}

else if (command[indexCur] == 'S'){

// servo mode

while (indexCur < index) {

int tempServoNum = 0;

int tempServoPos = 0;

// extracting servo number (2 integers)

for(int i=0; i<2; i++){

indexCur++;

int temp = Char2Int(command[indexCur]);

if (temp < 0) return false;

tempServoNum = tempServoNum*10 + temp;

}

// extracting pos number

indexCur++;

if (command[indexCur] != 'C') return false;

indexCur++;

boolean negative = Char2Int(command[indexCur]);

for (int i=0; i<4; i++) {

indexCur++;

int temp = Char2Int(command[indexCur]);

if (temp < 0) return false;

tempServoPos = tempServoPos*10 + temp;

}

if (negative) tempServoPos = -tempServoPos;

servoPosChange[tempServoNum] = tempServoPos;

// extracting loop number

indexCur++;

if (command[indexCur] == 'L'){

loops = 0;

for (int i=0; i<4; i++){

indexCur++;

int temp = Char2Int(command[indexCur]);

if (temp < 0) return false;

loops = loops*10 + temp;

}

for (int i=0; i<MAX_SERVO_NUM; i++)

servoPosAdjust[i] = (float)servoPosChange[i]/(float)loops;

indexCur++;

}

} // end of while indexCur < index

return true;

} // end of if 'S'

else {

;// unknown command

// ... more code...

}

boolean SerialCommand::ExecuteCommand(){

if ((pin < 0) || (mode < 0) || (data < 0))

return false;

switch (mode){

case 0:

// do noting.

break;

case 1:

// ... more code...

digitalWrite(pin,data);

break;

case 2:

// ... more code...

digitalRead(pin);

break;

case 3:

// ... more code...

servoPos[pin] = data;

servoPos_LastUpdated = data;

break;

}

executed = true;

return true;

}

void SerialCommand::Reset(){

// initialize variables and parameters

executed = false;

listening = false;

index = 0;

//loops = 0;

mode = 0;

pin = 0;

data = 0;

for (int i=0; i<MAX_COMMAND_LENGTH; i++){

command[i] = 0;

}

for (int i=0; i<MAX_SERVO_NUM; i++){

servoPosChange[i] = 0;

servoPosAdjust[i] = 0;

}

Hexapod Robot with 2 DOF legs (degree of freedom)

2012-01-01T12:50:00.000Z

First Motion, Legs flexibility:

stalker is dead... but the body and components are preserved well, so I decided to reuse them and therefore Mini is born.

Lastest update (12/01/2012)

===================================================================
31/12/2011

I just love working with Styrene plastic when building small robots... so flexible and shapable!

although it could take a while to cut and get good at it, but it's definitely a very good choice for people with small budget!

Here is a very good site selling them, it's the cheapest I have found so far (let me know if you know anywhere else cheaper ; )
Styrene Sheet (Plasticard) from Station Road Baseboards)

In this one, I basically reused all the servos and parts from the Stalker, the only things I need to rebuild was the base and the legs..

I hope I can work out some more gaits and moves soon and get it uploaded on youtube.
In the mean time, I will be working on the serial communication program to talk to the robot from the PC ; )

Ciao!

==================================================
07/01/2012

Mini can be now controlled by a VB program via USB connection, check here
http://arduin0.blogspot.com/2012/01/control-arduino-through-serial.html

==================================================
11/01/2012

further modification to the leg:

it's now more stable as the legs are tougher and lighter, more importantly it looks cooler!

====================================================
12/01/2012

Wrote a few more commands files today, works quite well ; )

See 2nd video from the top.

That's it! no more 2DOF hexpad, I will modify it into 3DOF per leg, and start to think about IK and trigonometry!

Sayonara!

How to use Ultra Sonic sensor (HC-SR04), demonstration with LCD.

2011-12-27T03:16:00.000Z

Result:

the HC-SR04 is different from most of those commonly seen on the market, which have 4 pins compared to 3 pins. But the operation principle is the same, transmit signal, and depends on when the signal is received to estimate the distance.

HC-SR04 not only inexpensive but also practical, use a sample AVR development boar –Arduino , with a LCD screen , you can make a easy rangefinder device , which can ranging 2cm to 400cm . (although i couldn't get it work more than 2 meters :-(

Please don't ask me how to use LCD, as it's been explained really well on the Arduino official site, please check here:

http://arduino.cc/en/Tutorial/LiquidCrystal

Now first connect the circuit as below :

Physical wiring:

Download the code below :

#include <LiquidCrystal.h>

LiquidCrystal lcd(8, 7, 6, 5, 4, 3, 2);

int pingPin = 13;
int inPin = 12;
 
void setup() {
  
  // set up the LCD's number of columns and rows: 
  lcd.begin(16, 2);
  
}
 
void loop(){
  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, inches, cm;
 
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(pingPin, LOW);
 
  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(inPin, INPUT);
  duration = pulseIn(inPin, HIGH);
 
  // convert the time into a human readable distance
  cm = microsecondsToCentimeters(duration);
  
  // if distance is too large (probably not being measured properly)
  // will discard the result.
  if (cm > 400){
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Nothing detected");
  }
  
  else {
  
  // clear lcd content
  lcd.clear();
  // set the cursor to column 0, line 0
  // (note: line 1 is the second row, since counting begins with 0):
  lcd.setCursor(0, 0);
  lcd.print("Object detected ");
  lcd.setCursor(0, 1);
  lcd.print(cm);
  lcd.print("cm away!");
  }
 
  delay(500);
  
}
 
long microsecondsToInches(long microseconds){
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  return microseconds / 74 / 2;}
 
long microsecondsToCentimeters(long microseconds){
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;}

Reset the Arduino , then you can see the distance of object in front on the LCD.

Add to Reddit

"Stalker" -- Quadruped Robot Project

2011-12-21T14:07:00.000Z

Quadruped Robot Project -- The Stalker

Inspired by the fighting unit "Stalker" in Starcraft.

What I need:

1. Arduino Mega
2. 12 - 14 Servos
3. Wii Nunchuck controller
4. IR sensor or Ultra sonic sensor
5. a few styrene sheet (skeleton)
6. 4xAA batteries

Plan:

1. design the skeleton of the robot, build with styrene plastic sheet.

2. Build a single leg with 3 servos, and write a class for single-leg motion.

3. figure out a way to control the leg (probably using buttons) without reprogramming it every time.

4. Assembly the whole thing.

-------------

5. learn how to use a Wii Nunchuck.

6. use it on the Stalker.

-------------

7. learn how to use wireless module, and use it on Wii Nunchuck controller. (probably need a extra Arduino to be the transmitter)

8. use it on the Stalker.

-------------

9. IR/ Ultra-Sonic automation

========================================================================

1. Design of the Skeleton

Body

Legs

Hips

Single leg Test:

===================================================

24/12/2011

And here we have all the legs and the base:

Now everything is ready, but the 'brain' of the stalker to arrive from China.

I will use UNO for testing and write some code in order to save time.

This one demonstrate the simplest thing it can do, "breathing":

This uses sine wave multiplying factors:

realise the increase of the leg angles has to be proportional to the hip position. (first order derivative?)
To calculate the angles that the servo needs to turn, we can do calculation shown below, and it's got a fancy name "inverse kinematics" (that's only the basic of the theory, need to dig deeper when developing the gaits)

Code template so far:

void setup(){

     calculate parameters;
     attach servos;

}

void loop(){

     calculate position;
     update position;
     delay;

}

===================================================

26/12/2011

So the stalker are currently being controlled by UNO, only the legs and hips servos are used (8 in total).

It works with the "Breathing" coding. But it has a very very obvious disadvantage: The servos are not powerful enough (as they are only $3 each). Well, that's a great lesson to learn. Also, the plastic legs and base isn't strong enough as it kind of bends and shakes a bit when it's moving. One thing I can do is to weld some more plastic pieces to back it up.

However, occasionally when I send out varying PWM signals (trying to change the positions), the legs just folder up, and stopped moving.

When I disconnected at least 4 servos, and re-uploading the code, everything seems to be normal again. I am wondering if that's something to do with the timers. Let's have a look at how to control multiple servos with timers:

http://arduino.cc/forum/index.php/topic,10666.0.html

Maybe I should write my own servo lib to make them more effiicient, and workable.

Also, to move servo position, i should probably compute all the values (positions) and store in an array, and drive the servos from data.

Some tips about updating the positions of servo: don't use delay as it is a block execution, but milli(), to keep track of past time, so loop can still continue and do other stuff.

===================================================
27/12/2011

This is the updated code template:

parameters declaration;

void setup(){

     calculate parameters;
     calculate positions and store in array;
     attach servos;

}

void loop(){

     int curTime = millis()

     if (curTime >= servoTime + servoDelay){
          servoTime = curTime;
          update positions from array;
     }

     if (curTime >= otherTime + otherDelay){

          do other things;
          ...

     }

}

26/12/2011

So the stalker are currently being controlled by UNO, only the legs and hips servos are used (8 in total).

It works with the "Breathing" coding. But it has a very very obvious disadvantage: The servos are not powerful enough. Well, that's a great lesson to learn. Also, the plastic legs and base isn't strong enough as it kind of bends and shakes a bit when it's moving. One thing I can do is to weld some more plastic pieces to back it up.

However, occasionally when I send out varying PWM signals (trying to change the positions), the legs just folder up, and stopped moving.

http://arduino.cc/forum/index.php/topic,10666.0.html

Maybe I should write my own servo lib to make them more effiicient, and workable.

Also, to move servo position, i should probably compute all the values (positions) and store in an array, and drive the servos from data.

Some tips about updating the positions of servo: don't use delay as it is a block execution, but milli(), to keep track of past time, so loop can still continue and do other stuff.

parameters declaration;

void setup(){

     calculate parameters;
     calculate positions and store in array;
     attach servos;

}

void loop(){

     int curTime = millis()

     if (curTime >= servoTime + servoDelay){
          servoTime = curTime;
          update position from array;
     }

     if (curTime >= otherTime + otherDelay){

          do other things;
          ...

     }

}

So, realized that the servo is definitely not strong enough. To do a gait smoothly, I will have to let the robot balance on only 3 legs. I have been thinking to get some more powerful servos, but they seem to be so expensive. If I was going to get some better ones, I will definitely need some with torque of 7kg/cm or above, but that would cost me £15 each at least!!! oh my gosh.

As it's turn out to be a cash burning project, I will first try to reduce the moment (physics) by reducing the distance of the force from the mass center, see if that would help balancing the body better. If not, I will get some new servos, and at the meantime look around for aluminum legs and body. ; )

done a bit of servo research:

Hitec HS-5645 MG (£30)

Dimensions (mm): 20.0 x 41.0 x 38.0
Weight (grams): 60.0
Speed (sec): 0.23
Torque (Kg.cm): 10.30
Ball Raced: Yes

TowerPro Servo MG-995 (£10)

Size: 40.6mm x 19.8mm x 37.8mm
Weight: 62g
Operating Speed: 0.13sec/60degree (6.0V) 0.17sec/60degree (4.8V)
Stall torque:15.0kg/cm (6.0V) 13.kg/cm (4.8V)

(But not a very good, accurate servo, in most cases, it overshoots, and not centering properly.

see reviews for details:
http://www.rcgroups.com/forums/showthread.php?t=442171
also this
http://www.rcuniverse.com/forum/m_8500712/anchors_8500712/mpage_1/key_/anchor/tm.htm#8500712

Hitec - HS-645MG (£23)

BearingSpeed (4.8V/6.0V):0.24 / 0.20Torque oz./in. (4.8V/6.0V):107 / 133
Torque kg./cm. (4.8V/6.0V):8.0 / 10.0
Size in Inches:1.59 x 0.77 x 1.48S
ize in Millimeters:40.39 x 19.56 x 37.59
Weight ounces:1.94Weight grams:55

These three seem to be the reasonable ones. MG995 has good torque compared to the price, but with the reviews on the Internet, as what people say "I would rather spend some more money on realiability now than saving a few quits, which will pay off in the long run".

So I will probably go for the HS 645MG... asked one of the buyers on eBay, see if I can get a deal if I order a dozen.

===================================================
30/12/2011

So this is what i did to try to reduce the force on the servos when it's standing:
1. shortened the legs, enhance strength of the legs.
2. re-built body and brings legs closer to the center.
3. made body with double layer plastic (was only single)

I think it actually looks better personally, and the robot can do more moves without falling down and twitching as often. But still, the servos are just too weak and can't do the walking. It's so frustrating and I have decided to only focus on the coding and look for new servos and parts (possibly aluminium rather than plastic).

This video is probably the last one I will upload showing the 9g Servos.... .v.

====================================================
31/12/2011

Update: decided to delay the purchase of new servos and the quadruped robot. will use 12 9g servos to build a hexapod first, and write a VB program to control arduino from the PC through serial port.

I will write a new post about the lastest work soon.