Problem mit Servo

Moin,

ich habe ein Porblem mit einem Servo. Ich benutzte die Servo Library Servo.h. Der Servo bekommt den Wert 90 zugewiesen, aber er fährt jede Sekunde einmal auf vollausschlag und dann zurück. Ich habe in meinem Programm auch schon die delays rausgenommen, hätte ja daran liegen können, und durch while Schleifen mit millis() ersetzt, hat aber nicht geholfen. Aber wo ist der Fehler? Die CalcGPS ist eine Library von mir, die ich für mein aktuelles Projekt schreibe.

#include <CalcGPS.h>
#include <TinyGPS.h>
#include <NewSoftSerial.h>
#include <Servo.h>
#include <Wire.h>


NewSoftSerial nss(2,3);
TinyGPS gps;
CalcGPS calc = CalcGPS();
Servo se;
Servo fahrt;

float wpk1[] = {
  52.308689,10.209555};
float wpk2[] = {
  52.308369,10.210126};
float wpk3[] = {
  52.308297,10.210992};
float wpk4[] = {
  52.308651,10.210582};
int compass_address = 0x42 >> 1;

float flat, flon, distance, qdm;
unsigned long fix_age;
long lat,lon;
int mh;
int new_servo_pos;
unsigned long time1,time2;

boolean wp1=false,wp2=false,wp3=false,wp4=false;

void setup(){
  Serial.begin(115200);
  nss.begin(9600);
  Wire.begin();
  fahrt.attach(6);
  se.attach(4);
  fahrt.write(90);
  se.write(90);
  time1=millis();
  while(time1+3000<=millis()){
  }
  Serial.println("Ready...");
  time1 = millis();
}

void loop(){
  fahrt.write(120);
  
  while(!wp1){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk1[0],wpk1[1],&distance,&qdm);
    new_servo_pos = calc.get_servo_pos((float)mh,qdm);
    se.write(/*new_servo_pos*/90);
    if(distance<1.0){
      wp1=true;
    }
    if(millis()>=time1+2000){
      Serial.print("Welcher Wegpunkt: ");Serial.println("WP1");
      Serial.print("Start Lat: ");printFloat(flat,5);Serial.println("");
      Serial.print("Ziel Lat: ");printFloat(wpk1[0],5);Serial.println("");
      Serial.print("Lon: ");printFloat(flon,5);Serial.println("");
      Serial.print("Ziel Lon: ");printFloat(wpk1[1],5);Serial.println("");
      Serial.print("MH: ");Serial.println(mh);
      Serial.print("QDM: ");Serial.println(qdm);
      Serial.print("Servo: ");Serial.println(new_servo_pos);
      Serial.print("Fahrtregler: ");Serial.println(fahrt.read());
      Serial.print("Distance: ");Serial.println(distance);
      Serial.println("_______________________________________");
      time1 = millis();
    }
  }
  while(!wp2){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk2[0],wpk2[1],&distance,&qdm);
    se.write(calc.get_servo_pos((float)mh,qdm));
    if(distance<1.0){
      wp2=true;
    }
  }
  while(!wp3){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk3[0],wpk3[1],&distance,&qdm);
    se.write(calc.get_servo_pos((float)mh,qdm));
    if(distance<1.0){
      wp3=true;
    }
  }
  while(!wp4){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk4[0],wpk4[1],&distance,&qdm);
    se.write(calc.get_servo_pos((float)mh,qdm));
    if(distance<1.0){
      wp4=true;
      fahrt.write(60);
      delay(3000);
      fahrt.write(90);
    }
  }
}

void get_gps(){
  while(nss.available()){
    int c = nss.read();
    if(gps.encode(c)){
      gps.get_position(&lat,&lon,&fix_age);
      gps.f_get_position(&flat,&flon,&fix_age);
    }
  }
}

void get_mh(){
  Wire.beginTransmission(compass_address);
  Wire.send('A');
  Wire.endTransmission();
  time2=millis();
  while(time2+10<=millis()){}
  Wire.requestFrom(compass_address, 2);

  if(2 <= Wire.available())     
  { 
    mh = Wire.receive();   
    mh = mh << 8;     
    mh += Wire.receive();  
    mh /= 10;
    mh -= 2;
    if(mh==0)
      mh+=360;
  }
}

void printFloat(double number, int digits)
{
  // Handle negative numbers
  if (number < 0.0)
  {
     Serial.print('-');
     number = -number;
  }

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
  
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  Serial.print(int_part);

  // Print the decimal point, but only if there are digits beyond
  if (digits > 0)
    Serial.print("."); 

  // Extract digits from the remainder one at a time
  while (digits-- > 0)
  {
    remainder *= 10.0;
    int toPrint = int(remainder);
    Serial.print(toPrint);
    remainder -= toPrint; 
  } 
}

Ich hoffe ihr könnt mir irgentwie helfen.

MfG
Philipp

Hallo Phillip

Schreibst Du mir bitte das Model und Hersteller des Servos? Danke Uwe