/* Servotester mit LCD Anzeige 4x20
2x Messung
2x Ausgang für Servosteller
LCD Anzeige:
* LCD RS pin to digital pin 12
* LCD Enable pin to digital pin 11
* LCD D4 pin to digital pin 7
* LCD D5 pin to digital pin 8
* LCD D6 pin to digital pin 9
* LCD D7 pin to digital pin 10
* LCD R/W pin to ground
* 10K resistor:
* ends to +5V and ground
* wiper to LCD VO pin (pin 3)
*/
#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 7, 8, 9, 10); //12=RS, 11= Enable, 7=D4, 8=D5; 9=D6, 10=D7
const byte PIN_RC1 = 2; // das ist INT 0 aber Pin 2 wo das Empfängersignal eingelesen wird
const byte PIN_RC2 = 3; // das ist INT 1 aber Pin 3 wo das Empfängersignal eingelesen wird
// alles was im Inteerupt läuft muss als volatile deklariert werden, direkt aus dem RAM
volatile int ReceivedSpeedValues1[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // Array mit 8 Werten für die gemessenen Werte
volatile int Summe1 = 12000; // Summe aus dem Array 8x1500, als Anfangswert
volatile int Mittelwert1 =0; // Mittelwert aus der Summe
volatile int Zeiger1 =0; // Zeiger für das Array, fängt mit 0 an
volatile long LastSpeedChange1 =0;
volatile long nMicros =0;
volatile long nDifference1 =0;
volatile byte errorcount1=0;
volatile int ReceivedSpeedValues2[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // Array mit 8 Werten für die gemessenen Werte
volatile int Summe2 = 12000; // Summe aus dem Array 8x1500, als Anfangswert
volatile int Mittelwert2 =0; // Mittelwert aus der Summe
volatile int Zeiger2 =0; // Zeiger für das Array, fängt mit 0 an
volatile long LastSpeedChange2 =0;
volatile long nDifference2 =0;
volatile byte errorcount2=0;
#include <Servo.h>
const byte SERVO_1 = 5; // Servo 1 an Pin 5
const byte SERVO_2 = 6; // Servo 2 an Pin 6
const byte POTI_1 = 0; // analoger Eingang auf Pin 23
const byte POTI_2 = 1; // analoger Eingang auf Pin 24
int value_1= 0; //analoger Wert von Servosteller 1
int value_2 =2; //analoger Wert von Servosteller 2
Servo servo_1; // Servoobjekt zur Ansteuerung
Servo servo_2; // Servoobjekt zur Ansteuerung
void setup()
{
pinMode(PIN_RC1, INPUT); // Eingang für Empfängersignal an Pin 2
digitalWrite(PIN_RC1, HIGH); // Pull Up Widerstand aktivieren
pinMode(PIN_RC2, INPUT); // Eingang für Empfängersignal an Pin 3
digitalWrite(PIN_RC2, HIGH); // Pull Up Widerstand aktivieren
attachInterrupt( 0, SpeedPositionInterrupt1, CHANGE); // 0=Interrupt 0,also Pin 2 / NAME der Interruptroutine / springe zum Interrupt wenn der Wert an Pin 2 sich ändert
attachInterrupt( 1, SpeedPositionInterrupt2, CHANGE); // 1=Interrupt 1,also Pin 3 / NAME der Interruptroutine / springe zum Interrupt wenn der Wert an Pin 3 sich ändert
lcd.begin(20, 4); //LCD Display mit 20 Spalten und 4 Zeilen
analogReference(DEFAULT); //Anlogereferenz auf 5V legen
servo_1.attach(SERVO_1); // Zuweisung des Servos an PWM Pin 5
servo_2.attach(SERVO_2); // Zuweisung des Servos an PWM Pin 6
}
void SpeedPositionInterrupt1()
{
nMicros = micros(); // die aktuelle Prozessorzeit wird in der variablen nMicros abgespeichert, dann geht der Zugriff schneller
nDifference1 = (nMicros - LastSpeedChange1); // hier wird geprüft wie lange der letzte Pegelwechsel her ist
if ( (nDifference1 > 700 ) && ( nDifference1 < 2200)) // liegt die Zeitdifferenz zwischen 0,9 und 2,1 ms, so ist es ein HIGH Pegal
{
Summe1 -= ReceivedSpeedValues1[Zeiger1]; // Summe = Summe - ReceivedSpeedValues[Zeiger];// substrahiere letzte Messung aus dem Array
ReceivedSpeedValues1[Zeiger1] = nDifference1; // Array mit 8 Werten für die gemessenen Wert
Summe1 += nDifference1; //Summe = Summe + ReceivedSpeedValues[Zeiger]; // addiere Wert zur Summe
Zeiger1 = ( ( Zeiger1 + 1 ) & 0x07 ); // Index erhoehen und ggf. von 8 auf 0 springen
Mittelwert1 = ( Summe1 >> 3 ); //der Befehl >>3 ist bit shift left um drei bit, das ist wie durch 8 teilen
errorcount1 =0; //setzt der Fehlerzähler zurück wenn ein gültiger Wert geladen wurde
}
LastSpeedChange1 = nMicros;
}
void SpeedPositionInterrupt2()
{
nMicros = micros(); // die aktuelle Prozessorzeit wird in der variablen nMicros abgespeichert, dann geht der Zugriff schneller
nDifference2 = (nMicros - LastSpeedChange2); // hier wird geprüft wie lange der letzte Pegelwechsel her ist
if ( (nDifference2 > 700 ) && ( nDifference2 < 2200)) // liegt die Zeitdifferenz zwischen 0,9 und 2,1 ms, so ist es ein HIGH Pegal
{
Summe2 -= ReceivedSpeedValues2[Zeiger2]; // Summe = Summe - ReceivedSpeedValues[Zeiger];// substrahiere letzte Messung aus dem Array
ReceivedSpeedValues2[Zeiger2] = nDifference2; // Array mit 8 Werten für die gemessenen Wert
Summe2 += nDifference2; //Summe = Summe + ReceivedSpeedValues[Zeiger]; // addiere Wert zur Summe
Zeiger2 = ( ( Zeiger2 + 1 ) & 0x07 ); // Index erhoehen und ggf. von 8 auf 0 springen
Mittelwert2 = ( Summe2 >> 3 ); //der Befehl >>3 ist bit shift left um drei bit, das ist wie durch 8 teilen
errorcount2 =0; //setzt der Fehlerzähler zurück wenn ein gültiger Wert geladen wurde
}
LastSpeedChange2 = nMicros;
}
void loop()
{
// Ansteuerung der Servos
value_1 = analogRead(POTI_1); // Liest den Analogwert vom Pin 23 zwischen 0 und 1023)
value_1 = map(value_1, 0, 1023, 800, 2100); // Skaliert den analogen Eingangswert auf 0,8-2,1 ms
servo_1.writeMicroseconds(value_1); // Schreibt den Sollwert vom Servo
value_2 = analogRead(POTI_2); // Liest den Analogwert vom Pin 24 zwischen 0 und 1023)
value_2 = map(value_2, 0, 1023, 800, 2100); // Skaliert den analogen Eingangswert auf 0,8-2,1 ms
servo_2.writeMicroseconds(value_2); // Schreibt den Sollwert vom Servo
// LCD Anzeige
lcd.setCursor(0, 0); // Zeile 0 für Messung 1
lcd.print("Impuls 1: ");
lcd.setCursor(10, 0); // 10= Stelle, 0=Zeile
float displ_1 = Mittelwert1/1000.0; // Wert umrechnen in Nachkommazahl
lcd.print(displ_1, 3); // Ausgabe mit 3 Nachkommastellen
lcd.setCursor(16, 0); // 16=Stelle, 0=Zeile
lcd.print("ms");
errorcount1++; //Fehlerzähler
if (errorcount1 >=10) //nach 10 Fehlerzählern wird ---- ausgegeben
{
lcd.setCursor(10, 0); // 10= Stelle, 0=Zeile
lcd.print("-----");
}
lcd.setCursor(0, 1); // Zeile 1 für Messung 2
lcd.print("Impuls 2: ");
lcd.setCursor(10, 1); // 10= Stelle, 1=Zeile
float displ_2 = Mittelwert2/1000.0; // Wert umrechnen in Nachkommazahl
lcd.print(displ_2, 3); // Ausgabe mit 3 Nachkommastellen
lcd.setCursor(16, 1); // 16=Stelle, 1=Zeile
lcd.print("ms");
errorcount2++; //Fehlerzähler
if (errorcount2 >=10)//nach 10 Fehlerzählern wird ---- ausgegeben
{
lcd.setCursor(10, 1); // 10= Stelle, 0=Zeile
lcd.print("-----");
}
lcd.setCursor(0, 2); // Zeile 2 für Ausgabe Servo 1
lcd.print("Servo 1: ");
lcd.setCursor(10, 2); // 10= Stelle, 2=Zeile
float displ_3 = value_1/1000.0; // Wert umrechnen in Nachkommazahl
lcd.print(displ_3, 3); // Ausgabe mit 3 Nachkommastellen
lcd.setCursor(16, 2); // 16=Stelle, 2=Zeile
lcd.print("ms");
lcd.setCursor(0, 3); // Zeile 3 für Ausgabe Servo 2
lcd.print("Servo 2: ");
lcd.setCursor(10, 3); // 10= Stelle, 3=Zeile
float displ_4 = value_2/1000.0; // Wert umrechnen in Nachkommazahl
lcd.print(displ_4, 3); // Ausgabe mit 3 Nachkommastellen
lcd.setCursor(16, 3); // 16=Stelle, 3=Zeile
lcd.print("ms");
delay (50); //50 ms delay
}
für diesen Thread.
