next topic »

[Reply #210 on:]

Why not just use a ordinary Servo then?
Thank you for the kinds words

Regards
Lauszus

Hey Lauszuz,

Sorry for the long time till i replay i was away.

An ordinary Servo wont give me the Power i need, and i need 360 degrees and not 180, and i have size capabilities.

I think i will get the IMU that you use, will it give me 100% accuuracy ?

[Reply #211 on:]

Okay It is actually possible to get a servo that can rotate 360 degrees. I found this after some googling: http://www.pololu.com/catalog/product/522 , that might do the job.

The IMU I used can only get 360 degrees resolution, if you only use either pitch or roll, but it can not be used at the same time...
But if you only need one of them, the IMU will work just fine

Regards Lauszus

[Reply #212 on:]

Okay It is actually possible to get a servo that can rotate 360 degrees. I found this after some googling: http://www.pololu.com/catalog/product/522 , that might do the job.

The IMU I used can only get 360 degrees resolution, if you only use either pitch or roll, but it can not be used at the same time...
But if you only need one of them, the IMU will work just fine

Regards Lauszus

Hey, Thanks !

I actually need a really strong Servo, i need something like 70kg/cm
I found this http://www.hobbyking.com/hobbyking/store/uh_viewItem.asp?idProduct=18742 and i will add him another gear to make it 100gk/cm

If i get the IMU that you use in your example and use the Kalman Filtering, will i get 100% accuracy on 360 rotation ?

Thanks,
Dan

[Reply #213 on:]

If you have a servo you dont need the sensor to know the angle, you just tell the servo which angle you want with the servo library. If you need an even bigger motor then you could buy a motor decoder some of them give you the exact position of the motor i saw one once and it wasperfect i cant seem to find a link sorry.

@ Lauszus sorry i always interfere in your post it was of so much help and want to help as much as i can xD

I have a question im using different sensors the i2c ones Im able to get all the angles now X Y Z but when i go past 90 it starts going down like 87 88 89 89 87 or -87 -89 -89 -87 how could i get the full 180... Pythagoras was never my friend maybe you have some good ideas.

PS the Z axis is working fine it goes 187 188 189 -189 -188 -187 if i could get the same result on the other 2 angles it would be perfect

Thanks

[Reply #214 on:]

Hi

what do i have to change to the program to use it with a 5 d-o-f imu?

I'm using a idg500/adxl335 IMU and an arduino uno

[Reply #215 on:]

You only need to change the sensitivity for the gyro and accelerometer, since the gyro for the yaw axis is not used, you can still get the same result as shown in the demonstration video with a 5DOF

Regards
Lauszus

[Reply #216 on:]

Thank you for your reply, Lauszus.

we changed the values of the sensitivity as you recommended, but it doesn't seem to be working.
 I'm not sure what does
    float Q_angleY  =  0.3;
    float Q_gyroY   =  0.002; 
    float R_angleY  =  0.03;
refer to..

do they mean
gyro
accelerometer
force vector?

Code:

//made by Kristian Lauszus - see http://arduino.cc/forum/index.php/topic,58048.0.html for information
#define gX A0
#define gY A1

#define aX A3
#define aY A4
#define aZ A5

#define RAD_TO_DEG 57.295779513082320876798154814105

//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;

int gyroZeroY;//y-axis
float gyroYadc;
float gyroYrate;
float gyroYangle;

//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;

int accZeroY;//y-axis
float accYadc;
float accYval;
float accYangle;

int accZeroZ;//z-axis
float accZadc;
float accZval;
float accZangle;

//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;

float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;

//This is pretty simple. It takes 100 readings and calculate the average.

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;

int inputGyroY[100];//y-axis
long resultGyroY;

//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

int inputAccY[100];//y-axis
long resultAccY;

int inputAccZ[100];//z-axis
long resultAccZ;

//gyros
int calibrateGyroX()
{
  for(int i=0;i<100;i++)
  {
    inputGyroX[i] = analogRead(gX);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroX += inputGyroX[i];
  }
  resultGyroX = resultGyroX/100;
  return resultGyroX;
}

int calibrateGyroY()
{
  for(int i=0;i<100;i++)
  {
    inputGyroY[i] = analogRead(gY);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroY += inputGyroY[i];
  }
  resultGyroY = resultGyroY/100;
  return resultGyroY;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}
 
int calibrateAccY()
{
  for(int i=0;i<100;i++)
  {
    inputAccY[i] = analogRead(aY);
  }
  for(int i=0;i<100;i++)
  {
    resultAccY += inputAccY[i];
  }
  resultAccY = resultAccY/100;
  return resultAccY;
}

int calibrateAccZ()
{
  for(int i=0;i<100;i++)
  {
    inputAccZ[i] = analogRead(aZ);
  }
  for(int i=0;i<100;i++)
  {
    resultAccZ += inputAccZ[i];
  }
  resultAccZ = resultAccZ/100;
  return resultAccZ;
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.3;
    float Q_gyroX   =  0.002; 
    float R_angleX  =  0.03; 

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
   
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
   
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
   
    return x_angle;
  }

 // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.3;
    float Q_gyroY   =  0.002; 
    float R_angleY  =  0.03; 

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
   
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
   
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
   
    return y_angle;
  }

 //print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
  Serial.print(gyroYangle);Serial.print("\t");
 
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t"); 
 
  Serial.print(compAngleX);Serial.print("\t"); 
  Serial.print(compAngleY); Serial.print("\t");
 
  Serial.print(xAngle);Serial.print("\t");
  Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
}



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
 
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY(); 
 
  accZeroX = calibrateAccX();
  accZeroY = calibrateAccY();
  accZeroZ = calibrateAccZ();
 
  timer=millis();//start timing
}

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter
 
  gyroYadc = analogRead(gY);
  gyroYrate = (gyroYadc-gyroZeroY)/1.0323;//(gyroYadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroYangle=gyroYangle+gyroYrate*dtime/1000;//Without any filter
 
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
 
  accYadc = analogRead(aY);
  accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
 
  accZadc = analogRead(aZ);
  accZval = (accZadc-accZeroZ)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  accZval++;//1g in horizontal position
 
  R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  //accZangle = acos(accZval/R)*RAD_TO_DEG;
 
/*  //used for debugging
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");

  Serial.print(gyroXangle,0);Serial.print("\t");
  Serial.print(gyroYangle,0);Serial.print("\t");
  Serial.print(gyroZangle,0);Serial.print("\t");

  Serial.print(accXval,2);Serial.print("\t");
  Serial.print(accYval,2);Serial.print("\t");
  Serial.print(accZval,2);Serial.print("\t");

  Serial.print(accXangle,0);Serial.print("\t");
  Serial.print(accYangle,0);Serial.print("\t");
  //Serial.print(accZangle,0);Serial.print("\t");
*/
 
  dtime = millis()-timer;
  timer = millis();
 
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
  yAngle = kalmanCalculateY(accYangle, gyroYrate, dtime);

  /*Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print(compAngleY,0);Serial.print("\t");
  Serial.print(xAngle,0);Serial.print("\t");
  Serial.print(yAngle,0);Serial.print("\t");
  Serial.println("");*/
 
  processing(); 
}

[Reply #217 on:]

That's the process and measurement noise constant used in the Kalman Filter. Do not change it!! At least until you got everything working. I recommend changing them back to there originals values:

Code:

float Q_angleX  =  0.001;
float Q_gyroX   =  0.003; 
float R_angleX  =  0.03; 

And then wait until you got it working.
See these links for more information: http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf.

You have to change the set the sensitivity in the code. Right now you are not setting the constant. For example in this line:

Code:

accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3

Send me a link to the datasheets and I will figure the sensitivity out for you

Regards
Lauszus

[Reply #218 on:]

Hi,
I will respond to your personal messages here, so everyone else can learn by your mistakes
First you wrote:

Thank you for your reply.

How should i program it so? How do i integrate both the gyro and accelerometer into one program? i'm relatively new to arduino and have just started this project, i have totally no idea how to do it. I tried the gyro program in the arduino playground but the values do not seem to stop dropping..

your reply is very appreciated!  sorry for my bad english

Just grab my program and change the sensitivity (see the next respond). The reason why you experience a drop, it's what called a drift. All gyros do that!! But the Kalman filter will take care of this

The second message:

Hi,

I just saw your reply to my question in the post, here you go
idg500 http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
adxl335 http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf

your help is really much appreciated

If your are using 3.3 volt, then your sensitivity would be:

gyroSensitivity: 0.0091/3.3*1023=2.821 (4.5 times output) else 0.002/3.3*1023=0.62 (normal output)
accelerometerSensitivity: 0.300/3.3*1023=93

Hope it make more sense to you now

- Lauszus

[Reply #219 on:]

Thank you Lauszus for your prompt reply,

Code:

#define gX A0

#define aX A3

#define RAD_TO_DEG 57.295779513082320876798154814105

//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;

//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;

//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;

float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;

//This is pretty simple. It takes 100 readings and calculate the average.

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;

//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

//gyros
int calibrateGyroX()
{
  for(int i=0;i<100;i++)
  {
    inputGyroX[i] = analogRead(gX);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroX += inputGyroX[i];
  }
  resultGyroX = resultGyroX/100;
  return resultGyroX;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.001;
    float Q_gyroX   =  0.003; 
    float R_angleX  =  0.03; 

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
   
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
   
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
   
    return x_angle;
  }

 // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.001;
    float Q_gyroY   =  0.003; 
    float R_angleY  =  0.03; 

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
   
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
   
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
   
    return y_angle;
  }

 //print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
   
  Serial.print(accXangle);Serial.print("\t");
 
  //Serial.print(compAngleX);Serial.print("\t"); 
  //Serial.print(compAngleY); Serial.print("\t");
 
  Serial.print(xAngle);Serial.print("\t");
  //Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
}



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
 
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
 
  accZeroX = calibrateAccX();
 
  timer=millis();//start timing
}

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/0.62;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter

 
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/93;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
 
 
  //R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  //accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  //accZangle = acos(accZval/R)*RAD_TO_DEG;
 
/*  //used for debugging
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");

  Serial.print(gyroXangle,0);Serial.print("\t");
  Serial.print(gyroYangle,0);Serial.print("\t");
  Serial.print(gyroZangle,0);Serial.print("\t");

  Serial.print(accXval,2);Serial.print("\t");
  Serial.print(accYval,2);Serial.print("\t");
  Serial.print(accZval,2);Serial.print("\t");

  Serial.print(accXangle,0);Serial.print("\t");
  Serial.print(accYangle,0);Serial.print("\t");
  //Serial.print(accZangle,0);Serial.print("\t");
*/
 
  dtime = millis()-timer;
  timer = millis();
 
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  //compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
  //yAngle = kalmanCalculateY(accYangle, gyroYrate, dtime);

  /*Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print(compAngleY,0);Serial.print("\t");
  Serial.print(xAngle,0);Serial.print("\t");
  Serial.print(yAngle,0);Serial.print("\t");
  Serial.println("");*/
 
  processing(); 
}

we require vertical tilt detection, does it matter which accelerometer we use? we are currently using the x gyro and x accelerometer...
and after we turn the gyro 360 degrees it only shows 180 degrees rotation, i think we are doing something wrong, could you please advice

your reply is very much appreciated!

[Reply #220 on:]

First of all. You can't calculate the force vector with only 1 accelerometer axis. You need 3! You are dividing with a unset variable.
Instead your should write:

Code:

accXangle = asin(accXval)*RAD_TO_DEG;

But if you use all 3-axis, then you will get better precision, so I recommended that!!
So connect all three axis and calculate all the values as so:

Code:

accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/93;

accYadc = analogRead(aY);
accYval = (accYadc-accZeroY)/93;

accZadc = analogRead(aZ);
accZval = (accZadc-accZeroZ)/93;
 
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
accXangle = acos(accXval/R)*RAD_TO_DEG-90;

You can only use the x and y axis.

You can get 360 resolution by using the z-axis. Take a look in the version 3 of the code. This is what I did:

Code:

  if(accZval < 0)//360 degrees resolution
  {
    if(accXangle < 0)
    {
      accXangle = -180-accXangle;
    }
    else
    {
      accXangle = 180-accXangle;
    }       
  }

- Lauszus

[Reply #221 on:]

Lauszus thank you for your help, i've got stable readings for the gyro, it still drifts but not as much as before. thanks again! 

[Reply #222 on:]

Hy guys, i would use this IMU http://www.robot-italy.com/product_info.php?cPath=15_138&products_id=1688. Can  I use your code? you think the same? how to connect Arduino? I would also  connect to arduino 2 servant http://www.robot-italy.com/product_info.php?cPath=85_28&products_id=1826 , according to you is enough for me an Arduino? I do not know if enough analog inputs

[Reply #223 on:]

#batista1987
Hi,
Actually I already helped another person with the exact requirements as you. Please see the following reply and onward:
http://arduino.cc/forum/index.php/topic,58048.msg543286.html#msg543286
Feel free to write again if you need more help or something you don't understand

Regarding the servo, it's very easy to use it with a Arduino. Look at the playground: http://www.arduino.cc/playground/ComponentLib/Servo or just use google as there is a ton of information on how to hook up a servo to a Arduino.

As with the analog inputs, you only need two (4 and 5) for the I2C communication - if you don't use a Arduino Mega. The servo(s) can use any pwm compatible pin. See the hardware page for the Arduino Uno: http://www.arduino.cc/en/Main/arduinoBoardUno at "Input and Output - PWM".

Regards
Lauszus

[Reply #224 on:]

#Lauszus
Hi Lauszus, the output of your code is not correctly. the program output are all 0