Servorevers, Servowegbeschränkung

Author: Dipl.-Ing. Wilfried Klaas Board: Arduino Duemilanove, Arduino Leonardo

Ein beliebtes Problem ist immer die Servoreversierung bzw. die Servowegbeschränkung. Hier meine Lösung dafür: (Wie immer mit der RCReceiver Bibliothek)

Servo.ino
#include <RCReceive.h>
#include <debug.h>
#include <makros.h>
#include <Servo.h>
 
 
/*
 Servorevers und Servowegbegrenzung. Der Empfänger wird vom Pin 2.
*/
 
// Hardwareanbindung für Arduino Hardware, Empfängerkanäle
const byte PIN_RC = 2;
 
// Ausgänge
const byte SERVO = 9; // PWM Kanal 
const byte LED = 13; // LED auf dem Board
 
// Wir halbieren mal den Servoweg...
const byte MAX_SERVO = 45;
const byte MIN_SERVO = 135;
 
// und wir reversieren oder auch nicht
const boolean REVERS = true;
 
// ------------------------------------------------------------
// AB HIER BITTE NIX MEHR ÄNDERN
// ------------------------------------------------------------
 
RCReceive rcReceiver;
 
Servo servo;
 
void setup() {
  // LED Kanal auf Ausgang, und dann deaktivieren
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);
 
  // Eingang für RC
  rcReceiver.attach(PIN_RC);
 
  // Servo definieren
  servo.attach(SERVO);
}
 
void loop() {
 
  // und gleich auch für's Steuerservo
  rcReceiver.poll();
 
  // Nullpunktsbestimmung ?
  if (rcReceiver.hasNP()) {
    doWork();
  }
}
 
/*
Modus Soft berechnung, Werte werden jeweils halbiert und dann addiert.
 */
void doWork() {
  // Werte holen
  int servo1 = rcReceiver.getValue();;
 
  // Nullpunkte festlegen
  servo1 = servo1 - rcReceiver.getNP();
 
  // die eigentliche Berechnung
  if (REVERS) {
    servo1 = -1 * servo1;
  }
 
  // Und damit das nicht aus dem Ruder läuft, etwas begrenzen
  servo1 = constrain(servo1, -128, 128);
 
  // Nullpunkte wieder hinzu
  servo1 = servo1 + 128;
 
  // Auf den Servo umsetzen und begrenzen
  servo1 = map(servo1, 0, 255, MIN_SERVO, MAX_SERVO);
 
  // und einstellen
  servo.write(servo1);
}

dyn. Servowegbeschränkung

Wer, von euch RC Auto Spezies, kennt das Problem nicht? Fährt man langsam, kommt man gut mit den Servowegen klar. Man kommt gut um die Kurve, alles ist wunderbar. Sobald man aber mal etwas Gas gibt, ist der Steuerweg für die Lenkung einfach zu groß. Driften macht zwar Spass, aber manchmal wünscht man sich dann doch, das man etwas mehr Steuerweg an der Fernbedienung hat und der Servo nicht ganz so weit ausschlägt. Sowas nenn ich eine dynamische Servowegbegrenzung.
Die dynamische Servowegbegrenzung koppelt den Servoweg des 2. Kanales mit dem Weg des 1. Kanals. Das heißt, ist Kanal 1 in Mittelstellung, kann man den vollen Servoweg an Kanal 2 nutzen. Fährt man aber Vollgas, so wird der Servoweg dynamisch verkleinert, d.h. ein Vollausschlag am „Lenkrad“ macht nur noch z.B. den halben Servoweg.
Die ganze Berechnung ist etwas Mathematik. S ist unser Steuerservo. E ist das Signal unseres Fahrtregler. C ist der Control, also der Einfluss auf den Steuerservo. 1 bedeutet kein Einfluss, 2 bedeutet halber Steuerweg, 3 nur noch ein drittel…
Die Formel dafür sieht so aus:
S = S / (1 + (C - 1) * (abs(E) /128))
(Noch ein kleiner Tip, läßt man das abs() weg, dann hat man eine automatische Servowegumkehrung bei Rückwärtsfahrt…)
Hier ist die Lösung als Programm (Achja, basiert auf meiner RCReciver Bibliothek):

dyna_servo.ino
#include <debug.h>
#include <makros.h>
 
#include <Servo.h>
#include <RCReceive.h>
 
/*
 dynamische Servowegbegrenzung. Kanal 1 ist ESC, Kanal 2 ist Steuerservo.
 Der Empfänger wird vom Pin 2 und 3 gelesen, 
 */
 
// Hardwareanbindung für Arduino Hardware, Empfängerkanäle
const byte PIN_RC_THR = 2; // das ist INT 0 aber Pin 2!!!!
const byte PIN_RC_STE = 3; // das ist INT 1 aber Pin 3!!!!
 
// Ausgänge
const byte SERVO_1 = 9; // PWM Kanal 
const byte LED = 13; // LED auf dem Board
 
// Stärke der Fahreinflusses auf die Steuerung
const byte control = 2;
 
// ------------------------------------------------------------
// AB HIER BITTE NIX MEHR ÄNDERN
// ------------------------------------------------------------
const byte ctrlM1 = control - 1;
 
// Laufvariable des Ringspeichers
RCReceive escReceiver;
RCReceive servoReceiver;
 
Servo a1;
 
void setup() {
  // Kanäle auf Ausgang, und dann deaktivieren
  pinMode(SERVO_1, OUTPUT);
 
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);
 
  // Eingang für RC
  pinMode(PIN_RC_THR, INPUT_PULLUP); 
  pinMode(PIN_RC_STE, INPUT_PULLUP);     
 
  escReceiver.attach(PIN_RC_THR);
  servoReceiver.attach(PIN_RC_STE);
 
  // Servos definieren
  a1.attach(SERVO_1);
 
#ifndef __AVR_ATtinyX4__ 
#ifdef debug
  Serial.begin(57600); 
  Serial.flush();
  Serial.println("dynaServo");
  delay(100);
#endif
#endif
}
 
void loop() {
 
  // Aktuellen RC-Wert Gas lesen
  escReceiver.poll();
 
  // und gleich auch für's Steuerservo
  servoReceiver.poll();
 
  // Nullpunktsbestimmung ?
  if (escReceiver.hasNP() || servoReceiver.hasNP()) {
    doWork();
  } else {
    dbgOutLn("no NP");
  }
#ifdef debug
  delay(100);
#endif
}
 
/*
Modus Soft berechnung, Werte werden jeweils halbiert und dann addiert.
 */
void doWork() {
  // Werte holen
  int escValue = escReceiver.getValue();;
  int servoValue = servoReceiver.getValue();;
 
  // Nullpunkte festlegen
  escValue = escValue - escReceiver.getNP();
  servoValue = servoValue - servoReceiver.getNP();
  dbgOut("E:");
  dbgOut(escValue);
  dbgOut(",S:");
  dbgOut(servoValue);
 
  // die eigentliche Berechnung
  int servo1 = servoValue ;
 
  // Und damit das nicht aus dem Ruder läuft, etwas begrenzen
  servo1 = constrain(servo1, -128, 128);
 
  float divisor  = 1 + (ctrlM1 * (abs(escValue) / 128.0));
  dbgOut(",D:");
  dbgOut(divisor);
 
  servo1 = servo1 / divisor;  
  dbgOut(",C:");
  dbgOut(servo1);
 
  // Nullpunkte wieder hinzu
  servo1 = servo1 + 128;
 
  // Auf den Servo umsetzen
  servo1 = map(servo1,0, 255,0,180);
 
  dbgOut(",SV:");
  dbgOutLn(servo1);
  // und einstellen
  a1.write(servo1);
}