Difference between revisions of "Twee servo's met correctie"

Latest revision as of 13:22, 1 February 2018

int servoPin1=9;   // servo1 op pin digitale IO pin 9
int potPin1=0;     // potmeter1 op analoge pin A0
int servoPin2=10;  // servo2 op pin digitale IO pin 10
int potPin2=1;     // potmeter2 op analoge pin A1
int cyclus=20000;  // microsecondes van de hele PWM cyclus => 50Hz
int minPulse=50;   // microsecondes hoog voor minimale arm-stand
int maxPulse=2500; // microsecondes hoog voor maximale arm-stand
float correctie1=-0.041;
float correctie2=0.00;

void setServoPulses(float arm_stand1, float arm_stand2)
// arm-stand 0 => helemaal links
// arm-stand 1 => helemaal rechts
{
  int pulseBreedte1 = (int)minPulse + arm_stand1*(maxPulse-minPulse);
  int pulseBreedte2 = (int)minPulse + arm_stand2*(maxPulse-minPulse);
  // pulse 1
  digitalWrite(servoPin1,HIGH);
  delayMicroseconds(pulseBreedte1);
  digitalWrite(servoPin1,LOW);
  // pulse 2
  digitalWrite(servoPin2,HIGH);
  delayMicroseconds(pulseBreedte2);
  digitalWrite(servoPin2,LOW);
  // resttijd cyclus
  delayMicroseconds(cyclus-pulseBreedte1-pulseBreedte2);
}

void setup()
{
  pinMode(servoPin1,OUTPUT);
  pinMode(servoPin2,OUTPUT);
}

float oldVal1 = -1; // -1 => waarde is zeker veranderd
float oldVal2 = -1;
unsigned long last = 0;

void loop()
{
  float potVal1=(float)analogRead(potPin1)/1024+correctie1;
  float potVal2=(float)analogRead(potPin2)/1024+correctie2;
  float diff = max(abs(oldVal1-potVal1),abs(oldVal2-potVal2));
  // alleen als een stand voldoende is veranderd, en elke seconde
  if ((diff>0.02) || (millis() - last>1000)) {
    for (int i=0;i<=10;i++)  // blijft 10 stapjes
    {
      setServoPulses(potVal1,potVal2);
    }
    // vertraagde aanpassing 'oldVal' => snelle reactie
    oldVal1 = oldVal1 + 0.15*(potVal1-oldVal1);
    oldVal2 = oldVal2 + 0.15*(potVal2-oldVal2);
    last = millis();
  }  
}