Bei Sketchübertragung bewegt sich der Servomotor

International Deutsch
12

theev:
ich habe mir mal ServoOutput ausgeben lassen und die ersten beiden Werte sind tatsächlich:

0.00
0.00
20.00
20.00
20.00
...

Dann sollte man die ersten beiden Stellwerte ignorieren, ein Versuch (ungetestet):

#include <Servo.h>
#include <PID_v1.h>

double Setpoint, Input, Output, ServoOutput;

byte doubleshot = 2;
int last_reading = 0;

const int ServoPin = 7;
const int trigger = 10;
const int echo = 11;

float Kp = 5;
float Ki = 5;
float Kd = 1;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
Servo myServo;

void setup()
{
  Serial.begin (9600);
  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT);
  myServo.attach(ServoPin);
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-5, 5);
  Input = readPosition();
}


void loop()
{
  Setpoint = 4;
  Input = readPosition();
  myPID.Compute();
  last_reading = Input;
  ServoOutput = 25 + Output;
  if (doubleshot) {
    doubleshot--;
  } else {
    myServo.write(ServoOutput);
  }
}

float readPosition() {
  long dauer = 0;
  long entfernung = 0;
  unsigned long now = millis();
  delay(40);
  digitalWrite(trigger, LOW);
  delay(5);
  digitalWrite(trigger, HIGH);
  delay(10);
  digitalWrite(trigger, LOW);
  dauer = pulseIn(echo, HIGH);
  entfernung = (dauer / 2) * 0.03432;
  if (entfernung > 30)
  {
    entfernung = 30;
  }
  Serial.print(entfernung);
  Serial.println();
  return entfernung;

}