next topic »

[on:]

Hi everyone, im playing with ITG3200 gyro and with that gyro i want to controll my servos but there is a problem... When im uploading my code to ATMega168 my 4 servos just twitches... So i dont know where is problem.. Wiring are good..

Code:

#include <Wire.h>
#include <ITG3200.h>
#include <Servo.h>
ITG3200 gyro = ITG3200();
float  x,y,z;
float angleServas1=0;
float angleServas2=0;
float angleServas3=0;
float angleServas4=0;
int ix, iy, iz;
unsigned long time_1=0;
unsigned long time_2=0;
Servo servas1;
Servo servas2;
Servo servas3;
Servo servas4;
int ServoPositionServo1;
int ServoPositionServo2;
int ServoPositionServo3;
int ServoPositionServo4;
void setup(void) {
  Serial.begin(9600);
  servas1.attach(8);
  servas2.attach(9);
  servas3.attach(10);
  servas4.attach(11);
  Wire.begin();      
  delay(100);
  servas1.write(0);
  servas2.write(0);
  servas3.write(0);
  servas4.write(0);
  gyro.init(ITG3200_ADDR_AD0_HIGH);
  
  Serial.print("zeroCalibrating...");
  gyro.zeroCalibrate(2500, 2);//(number of mesaurments, milliseconds between them)
  Serial.println("done.");
}

void loop(void) {
    while (gyro.isRawDataReady()) {
   // Reads calibrated values in deg/sec    
    gyro.readGyro(&x,&y,&z);
    time_1=time_2;
    time_2 = micros();
    if (time_1!=0){
 
angleServas4=angleServas4+2*y*((float)time_2-(float)time_1)/10000;
//angleServas1=angleServas1+2*y*((float)time_2-(float)time_1)/10000;
//angleServas2=angleServas2+2*y*((float)time_2-(float)time_1)/10000;
//angleServas3=angleServas3+2*y*((float)time_2-(float)time_1)/10000;

        angleServas2=angleServas2+2*y*((float)time_2-(float)time_1)/1000000;

        angleServas1=angleServas1+2*x*((float)time_2-(float)time_1)/1000000;
        angleServas3=angleServas3+2*x*((float)time_2-(float)time_1)/1000000;
    }
   ServoPositionServo1=90-angleServas1;
   ServoPositionServo2=90-angleServas2;
   ServoPositionServo3=90-angleServas3;
   ServoPositionServo4=90-angleServas4;
  
   servas1.write(ServoPositionServo1);
  
   servas2.write(ServoPositionServo2);
  
   servas3.write(ServoPositionServo3);
  
   servas4.write(ServoPositionServo4);
}
}

[Reply #1 on:]

You are not limiting the angles to valid ranges.  Try printing out the values supplied to the servo.write(...)  calls.

Your code can simplied a lot - firstly calculate time2-time1 once only, secondly use an array of 4 servo objects.

[Reply #2 on:]

Can you write example? Couse im new in arduino..