Go Down

Topic: what's wrong with this code [arduino + arduIMU] (Read 806 times) previous topic - next topic

newbie15

i was trying to use arduIMU as gyro for my gimbal camera project.

this is my code
Code: [Select]

#include <Servo.h>

int r=9;
int p=10;
int y=11;
Servo roll;
Servo pitch;
Servo yaw;

String readString;
int x=0,cnt,km;
int ang[3];
void setup() {
Serial.begin(38400);
roll.attach(r);
pitch.attach(p);
yaw.attach(y);
        delay(5);
}

void loop() {
    //if data like !!!AN0:16,AN1:3,AN2:3,AN3:-30,AN4:8,AN5:117,RLL:9.48,PCH:17.80,YAW:-118.35,***
    cnt=0;km=0;
    while (Serial.available()) {
        delay(1);  //small delay to allow input buffer to fill
if (Serial.available() >0) {
            char c = Serial.read();  //gets one byte from serial buffer
            if(c ==','){
                x=0;
                if(km>5 && km<9 && cnt<3){
                  ang[cnt]=stoi(readString);
                  Serial.print("=");
                  Serial.print(ang[cnt]);
                  Serial.print("=ang[");
                  Serial.print(cnt);
                  Serial.print("]=");
                  Serial.print(readString);  //so you can see the captured string
                  Serial.print(" ");
                  cnt++;
                }else if(km==9){
                  //Serial.println("");
                  cnt=0;
                  km=0;
                }
            readString="";
            km++;
    }
            if(x==1){
                readString += c;
    }
    if(c ==':'){
        x=1;
    } //breaks out of capture loop to print readstring
        } //makes the string readString
    }
    Serial.println();
    roll.write(ang[0]);
    pitch.write(ang[1]);
//    yaw.write(ang[2]);
}

int stoi(String x){
    int n;
    char carray[6]; //converting string to number
    x.toCharArray(carray, sizeof(carray));
    n = atoi(carray);
    Serial.print(n);
    return n;
}


when i try input from serial monitor it works fine and servo move well, but when i connect arduIMU with arduino and rotate IMU servo move unstable.

what's wrong with my code..???

Go Up