Go Down

Topic: ITG3200 gyro + Servo Motors (Read 997 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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy