Hallo liebe Community,
ich habe ein Problem bei folgender Aufgabenstellung:
Ein Ultraschallsensor (HC-SR04) wird durch einen Servo kontinuierlich bewegt (von 0Grad bis 180Grad, und dann wieder zurück), um den vorliegenden Bereich "abzuscannen".
Die Messergebnisse werden auf einem Display (1.8" TFT) ebenfalls kontinuierlich, übereinstimmend mit der Servoposition (Winkel) angezeigt.
Das Ganze wird mit einem Arduino Nano (oder aktueller Versuch Nano Every) realisiert.
Folgender Code läuft soweit auch, nur leider sehr langsam. Dies bedeutet, der Servo benötigt ca. eine Minute um sich von 0 bis 180° zu drehen, und läuft auch nur "ruckelnd".
Meine Frage wäre nun ob der "Nano" einfach mit dieser Aufgabe überfordert ist, oder ob es möglich wäre den Code "schlanker" oder einfach intelligenter zu schreiben?
#include<Servo.h> //Library für Servomotor
#include <SPI.h> //Library Schnittstellen
#include <TFT.h> //Library Display
#include "Ucglib.h" //Library Display
#define trigPin 8 //Pin für den sendenden Ultraschallsensor
#define echoPin 9 //Pin für den empfangenden Ultraschallsensor
#define TFT_CD 6 //Pin command/data Display
#define TFT_CS 10 //Pin Chip select Display
#define TFT_RST 2 //Pin reset Display
Servo myServo; //Erstellung Servomotor
Ucglib_ST7735_18x128x160_HWSPI ucg(TFT_CD, TFT_CS, TFT_RST); //Zuweisung Pins von Display
int x; //Variable x für Schleifen
int p = 5; //Pin Servo
int distance; //berechnete Entfernung Hindernis, Sensor schafft 65cm
int Ymax = 128; //Bildschirmhöhe
int Xmax = 160; //Bildschirmbreite
int pos = 8; //Höhe des Radars auf dem Bildschirm
int deg; //Variable Grad
int val = 200; //Länge Linien Radar
int k; //Variable Umwandlung x auf Display
int previous_k; //grüne Linie mit Verzögerung löschen
long duration; //Dauer Ultraschallmessung
float theta; //Bogenmaß berechnen
float scaled_distance;//Verhältnis echte Entfernung zu Display
float m; //X-Koordinate Hindernis auf Display
float n; //Y-Koordinate Hindernis auf Display
char buffer[3]; //benötigt für korrekte Grad-Anzeige bei Rechtslauf
float ring1 = 0.30; //Beschriftungen für Halbkreise auf Display; zeigen grobe Entfernung an
float ring2 = 0.45;
float ring3 = 0.60;
void setup(void) {
myServo.write(0); //Servo startet in Position 90°
pinMode(trigPin,OUTPUT); //Zuweisung Trigger als Sender
pinMode(echoPin,INPUT); //Zuweisung Echo als Empfänger
myServo.attach(p); //Servo auf Pin 5
Serial.begin(19200); //Einstellung Baud 9600
ucg.begin(UCG_FONT_MODE_SOLID); //Schrift Display
ucg.setFont(ucg_font_6x10_tr); //Schrift Display
ucg.clearScreen(); //Bildschirm leeren
ucg.setRotate270(); //Rotation des Bildes um 90°
}
void loop(void) {
fix();
for ( x=0; x <= 180; x++){
delay(5); // 5
myServo.write(x);
distance = calculateDistance(); //Entfernung zu Hindernis durch Funktion calculateDistance()
ucg.setPrintPos(10,0); //Anzeige Entfernung Hindernis
ucg.print(distance);
k = map(x, 0, 180, 88, 2); //Anpassen Grad des Servos an Displaygröße
if (distance < 65) { //Bis max. 65cm Entfernung
scaled_distance = distance / 65.0; //Verhältnis Gesamtentfernung zu Displaygröße
theta = x * (PI / 180); //Grad umrechnen in Bogenmaß
m = (Xmax / 2) - (scaled_distance * (Xmax / 2) * -cos(theta)); //X-Koordinate des Hindernisses auf Display
n = (scaled_distance * Ymax * sin(theta)); //Y-Koordinate des Hindernisses auf Display
ucg.setColor(255, 0, 0); //rot
ucg.drawPixel(m, n + pos); //Zeichnet Pixel auf Display
}
else {
ucg.setColor(0, 207, 0); //grün
ucg.drawLine(Xmax / 2, pos + 3, Xmax / 2 + (-val * cos(radians(k * 2))), pos + (val * sin(radians(k * 2)))); //Zeichnet grüne Linie falls kein Hindernis
}
previous_k = map(x - 10, 0, 180, 88, 2); //löscht grüne Linie mit Verzögerung, Anpassung von 180° an Display
ucg.setColor(0, 0, 0);
ucg.drawLine(Xmax / 2, pos + 3, Xmax / 2 + (-val * cos(radians(previous_k * 2))), pos + (val * sin(radians(previous_k * 2))));
ucg.setColor(255, 0, 0); //Gradanzeige links unten
ucg.setPrintPos(160,0);
ucg.setPrintDir(2);
ucg.print("Deg :");
deg = x;
ucg.setPrintPos(120,0);
ucg.setPrintDir(2);
ucg.print(deg);
ucg.setColor(0, 0, 255); //Zahlenanzeige Kreisraster
ucg.setPrintPos(90,38);
ucg.setPrintDir(2);
ucg.print(ring1);
ucg.setPrintPos(90,70);
ucg.print(ring2);
ucg.setPrintPos(90,110);
ucg.print(ring3);
}
ucg.clearScreen();
fix();
for ( x=180; x >= 0; x--){
delay(5);
myServo.write(x); //Rechtslauf durch Hochzählen von x in der Schleife
distance = calculateDistance(); //Entfernung zu Hindernis durch Funktion calculateDistance()
ucg.setPrintPos(10,0);
ucg.print(distance); //Ausgabe Entfernung
k = map(x, 0, 180, 2, 88); //Anpassen des Grad-Bereichs des Servos an Display
if (distance < 65) { //Bis max. 65cm Entfernung
scaled_distance = distance / 65.0; //Verhältnis Gesamtentfernung zu Displaygröße
theta = x * (PI / 180); //Umrechnen Grad in Bogenmaß
m = (Xmax / 2) - (scaled_distance * (Xmax / 2) * -cos(theta)); //X-Koordinate
n = (scaled_distance * Ymax * sin(theta)); //Y-Koordinate
ucg.setColor(255, 0, 0); //rot
ucg.drawPixel(m, n + pos); //Zeichnet Hindernis als Pixel
}
else {
ucg.setColor(0, 207, 0);
ucg.drawLine(Xmax / 2, pos + 3, Xmax / 2 + (-val * -cos(radians(k * 2))), pos + (val * sin(radians(k * 2)))); //zeichnet grüne Linie, wenn kein Hindernis erkannt
}
previous_k = map(x + 10, 0, 180, 2, 88);
ucg.setColor(0, 0, 0);
ucg.drawLine(Xmax / 2, pos + 3, Xmax / 2 + (-val * -cos(radians(previous_k * 2))), pos + (val * sin(radians(previous_k * 2)))); //löscht grüne Linie mit Verzögerung
ucg.setColor(255, 0, 0); //Grad-Anzeige unten links
ucg.setPrintPos(160,0);
ucg.setPrintDir(2);
ucg.print("Deg :");
sprintf(buffer, "%03d", x); //korrekte Anzeige, sonst 990... anstatt 99...
ucg.setPrintPos(120,0);
ucg.setPrintDir(2);
ucg.print(buffer);
ucg.setColor(0, 0, 255);
ucg.setPrintPos(90,38);
ucg.setPrintDir(2);
ucg.print(ring1); //Zahlenanzeige Kreisraster
ucg.setPrintPos(90,70);
ucg.print(ring2);
ucg.setPrintPos(90,110);
ucg.print(ring3);
}
ucg.clearScreen();
}
int calculateDistance(){ //Implementierung Funktion zur Berechnung der Entfernung mithilfe des Sensors
digitalWrite(trigPin, HIGH); //Sender anschalten
delayMicroseconds(10); //Sendezeit 10 Mikrosekunden
digitalWrite(trigPin, LOW); //Sender ausschalten
duration = pulseIn(echoPin,HIGH,10000); //Reisezeit ist der empfangene Puls am Echopin (wenn dieser HIGH ist); der Sensor wartet 10 ms
if (duration == 0) {
distance = 999; //wenn der Sensor nach 10 ms nichts empfangen hat setzt er die Entfernung auf 999
}
else {
distance = duration*0.034/2;} //Berechnung der Entfernung durch die Schallgeschwindigkeit (340 m/s) durch Hin- und Rücklauf /2 , da Einzelstrecke benötigt
if (distance > 65) { //wenn Entfernung größer 65cm, dann Ausgabe 999
distance = 999;}
return distance;
}
void fix() { //Hintergrunddesign mit Halbkreisen
ucg.setColor(255, 0, 0);
ucg.drawDisc(Xmax / 2, pos, 5, UCG_DRAW_LOWER_RIGHT);
ucg.drawDisc(Xmax / 2, pos, 5, UCG_DRAW_LOWER_LEFT);
ucg.setColor(225, 255, 50);
ucg.drawCircle(80, pos, 115, UCG_DRAW_LOWER_RIGHT);
ucg.drawCircle(80, pos, 115, UCG_DRAW_LOWER_LEFT);
ucg.drawCircle(80, pos, 78, UCG_DRAW_LOWER_RIGHT);
ucg.drawCircle(80, pos, 78, UCG_DRAW_LOWER_LEFT);
ucg.drawCircle(80, pos, 40, UCG_DRAW_LOWER_RIGHT);
ucg.drawCircle(80, pos, 40, UCG_DRAW_LOWER_LEFT);
ucg.drawLine(0, pos, 160, pos);
ucg.setColor(0, 0, 0);
ucg.drawBox(100, 0, 30, 8);
}
Ich bin über jegliche Hilfe dankbar,
vielen herzlichen Dank!
