Pages: [1]   Go Down
Author Topic: arduino servo library updating only 5 times / sec?  (Read 460 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

OK guys, I have an arduino trying to control 4 speed controllers on a quadrotor.  The speed controllers can be controlled as standard servos, so I am using the arduino servo library.  I am using the writeMicroseconds() command to send the signals to the speed controllers.  The signals are read from an R/C receiver, stored in an array and written to the speed controllers.

Everything in the loop seems to be working well, just not fast enough.  The loop takes between 107ms and 110ms to run.  By this measurement, the values sent to the speed controllers should be updated 10 times per second.  The arduino can read the values from the receiver at this rate, and even write them to the serial port at this rate, but the values sent to the speed controllers changes only about 5 times per second (This, I tested by using a standard servo in place of the speed controller for simplicity.)

Any solutions?  I tried changing the frame rate in the servo library and this just caused the servo to freak out, for lack of a better technical term.  Any advice is appreciated.  Code will be posted below.

Code:
#include <Servo.h>

Servo speedController[4];

int timers[4];
long time = millis();

int NORTH = 0;
int SOUTH = 1;
int EAST = 2;
int WEST = 3;

int throttle[4];

int channel_1 = 42;

int xCoefficient = 100;
int yCoefficient = 100;
int zCoefficient = 100;
int throttleCoefficient = 700;

int coefficient[] = {yCoefficient, xCoefficient, throttleCoefficient, zCoefficient};

int ppm_bottom = 20140;
int ppm_top = 21320;

int servo_ppm[4];
long raw_ppm[4];

char Sbuffer[21]; //so strange. need to bring this one to the forums.;

int bottom = 1000;
int zero = 1292;
int top = 2000;

int mainThrottle = zero;

long refTime = millis();

int decodeThrottle(int start){
    
    int throttle = 0;
    int power = 1000;
    
    for(int i = start; i < start + 4; i ++){
        
        throttle += (int(Sbuffer[i]) - 48) * power;
        
        power /= 10;
        
    }
    
    return throttle;
    
}

void PANIC(){
    
    setAll(1000);
    
}

void ResetRefTime(){
    refTime = micros();
}

void set0(int SIGNAL){
    speedController[0].writeMicroseconds(SIGNAL);
}

void set1(int SIGNAL){
    speedController[1].writeMicroseconds(SIGNAL);
}

void set2(int SIGNAL){
    speedController[2].writeMicroseconds(SIGNAL);
}

void set3(int SIGNAL){
    speedController[3].writeMicroseconds(SIGNAL);
}

void setThrottles(int throttle[]){
    for(int i = 0; i < 4; i ++){
        speedController[i].writeMicroseconds(throttle[i]);
    }
}

void setAll(int SIGNAL){
    for(int i = 0; i < 4; i ++){
        speedController[i].writeMicroseconds(SIGNAL);
    }
}

void serial2In(){
    
    long local_time = millis();
    boolean flag = true;
    int i = 0;
    char in;
    
    char buffer[22]; //very very strange. if problems arise with buffer, come here first.;
    
    in = Serial2.read();
    
    //reset the flag time;
    time = millis();
    
    while(in != '*' &&
          flag){
      
      //wait for Serial2 data, but don't wait too long;
      while((!Serial2.available()) &&
          millis() - time < 300){}
      //if more than 1/3 of a second passes without receiving
      //Serial2 data, break the loop;
      if(millis() - time >=300){
        flag = false;
      }
      
      //add the character, move in and i forward in sequence;
      buffer[i] = in;
      in = Serial2.read();
      i ++;
      time = millis();
      
    }
    
    if(flag){
      
      //add signature and end character to message;
      buffer[21] = 'A';
      buffer[22] = '*';
      
      //set global variable Sbuffer equal to local variable buffer;
      for(int i = 0; i < 23; i ++){
        
        Sbuffer[i] = buffer[i];
        
      }
      
    }if(!flag){
      
      Serial2.println("Serial2 data corrupt.");
      Serial2.flush();
      
    }
    
    time = millis();
    
}

void arm(){

    char prompt1[] = "Waiting...";
    char prompt2[] = "Ready.";

    Serial2.println(top);
    setAll(top);
    Serial2.println(prompt1);
    delay(3000);

    Serial2.println();
    Serial2.println(bottom);
    setAll(bottom);
    Serial2.println(prompt1);
    delay(3500);

    Serial2.println();
    Serial2.println(mainThrottle);
    setAll(mainThrottle);
    Serial2.println(prompt2);

}

void inData(){

    servo_ppm[0] = map(pulseIn(channel_1 + 0, LOW), ppm_bottom, ppm_top, -1 * coefficient[0], coefficient[0]);
    servo_ppm[1] = map(pulseIn(channel_1 + 1, LOW), ppm_bottom, ppm_top, -1 * coefficient[1], coefficient[1]);
    servo_ppm[2] = map(pulseIn(channel_1 + 2, LOW), ppm_bottom, ppm_top, 0, coefficient[2]);
    servo_ppm[3] = map(pulseIn(channel_1 + 3, LOW), ppm_bottom, ppm_top, -1 * coefficient[3], coefficient[3]);

}

void outData(){
  
  Serial2.print("$");
  for(int i = 0; i < 4; i ++){
    
    Serial2.print(throttle[i]);
    Serial2.print(".");
    
  }Serial2.println("A*");
  
}

void setThrottles(){
  
  throttle[0] = 1000 + servo_ppm[2] + servo_ppm[1];
  throttle[1] = 1000 + servo_ppm[2] - servo_ppm[1];
  throttle[2] = 1000 + servo_ppm[2] + servo_ppm[0];
  throttle[3] = 1000 + servo_ppm[2] - servo_ppm[0];
  
  for(int i = 0; i < 4; i ++){
      
      speedController[i].writeMicroseconds(throttle[i]);
      
  }
  
}

void digitalWriteThrottles(){
    
    long ref = millis();
    
    time = micros();
    
    for(int i = 0; i < 4; i ++){
        
        digitalWrite(i + 30, HIGH);
        
    }
    
    while(millis() - ref > 200){

        for(int i = 0; i < 4; i ++){

            if(throttle[i] >= micros() - time){

                digitalWrite(i + 30, LOW);

            }

        }
    }
}

void setup(){

    Serial.begin(9600);

    for(int i = 0; i < 4; i ++){
        speedController[i].attach(30 + i);
    }

}

void loop(){
    
    inData();
    setThrottles();
    
}
Logged

Norway@Oslo
Offline Offline
Edison Member
*
Karma: 12
Posts: 2033
loveArduino(true);
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

The same code (presumably) 'minified':
Code:
#include <Servo.h>

Servo speedController[4];

int servo_ppm[4];
int throttle[4];

int ppm_bottom = 20140;
int ppm_top = 21320;

int xCoefficient = 100;
int yCoefficient = 100;
int zCoefficient = 100;
int throttleCoefficient = 700;

int channel_1 = 42;

void setup(){
    Serial.begin(9600);

    for(int i = 0; i < 4; i ++){
        speedController[i].attach(30 + i); //NOTABENE!!! magic number 30
    }
}

void loop(){
    inData();
    setThrottles();
}


void inData(){
    servo_ppm[0] = map(pulseIn(channel_1 + 0, LOW), ppm_bottom, ppm_top, -1 * yCoefficient, yCoefficient);
    servo_ppm[1] = map(pulseIn(channel_1 + 1, LOW), ppm_bottom, ppm_top, -1 * xCoefficient, xCoefficient);
    servo_ppm[2] = map(pulseIn(channel_1 + 2, LOW), ppm_bottom, ppm_top, 0, throttleCoefficient);
    servo_ppm[3] = map(pulseIn(channel_1 + 3, LOW), ppm_bottom, ppm_top, -1 * zCoefficient, zCoefficient);
}

void setThrottles(){
  throttle[0] = 1000 + servo_ppm[2] + servo_ppm[1];
  throttle[1] = 1000 + servo_ppm[2] - servo_ppm[1];
  throttle[2] = 1000 + servo_ppm[2] + servo_ppm[0];
  throttle[3] = 1000 + servo_ppm[2] - servo_ppm[0];

  for(int i = 0; i < 4; i ++){
      speedController[i].writeMicroseconds(throttle[i]);
  }
}

About 80% superfluous code removed.
« Last Edit: November 27, 2010, 03:31:40 pm by AlphaBeta » Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Right, well I've been modifying the same code for a while and just posted it all. Thanks for cleaning it up.
Logged

Pages: [1]   Go Up
Jump to: