Pages: [1]   Go Down
Author Topic: ITG3200 gyro + Servo Motors  (Read 750 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 5
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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);
}
}
Logged

0
Offline Offline
Shannon Member
****
Karma: 162
Posts: 10512
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

[ I won't respond to messages, use the forum please ]

Offline Offline
Newbie
*
Karma: 0
Posts: 5
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Pages: [1]   Go Up
Jump to: