Go Down

Topic: ITG3200 gyro + Servo Motors (Read 918 times) previous topic - next topic

nutonas

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: [Select]
#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);
}
}

MarkT

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.
[ I won't respond to messages, use the forum please ]

nutonas

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

Go Up