Differences

This shows you the differences between two versions of the page.

--- projects:farmrobot:hinderniserkennung_mit_sonar [2021/02/21 12:02]
leon-david created
+++ projects:farmrobot:hinderniserkennung_mit_sonar [2021/02/24 20:44] (current)
tobias [Aufbau]
@@ Line 1: / Line 1: @@
-Hinderniserkennung mit Sonar
+====== Sonar ======
+Um zu verhindern das der Farm Robot mit voller Kraft in Objekte oder sogar Personen fährt, wurden Sonarsensoren angebracht, welche den Roboter stoppen, falls er sich etwas nährt.
+==== Aufbau ====
+Es sind acht [[https://elektro.turanis.de/html/prj121/index.html | HC-SR04]] Sensoren mit einem Arduino Mega verbunden, zwei für jede Seite.
+{{:projects:farmrobot:sensoren_angeschlossen.jpg?400|}}
+Die Sensoren sind mithilfe von Mounts am Farm Robot angebracht, welche mit Fusion 360 erstellt und anschließend vom 3D-Drucker ausgedruckt wurden.
+{{:projects:farmrobot:sensor_farm_robot.jpg?400|}}
+Der Arduino ist Seriell mit dem Telem1 Port des Pixhawks verbunden und wird auch vom Pixhawk mit Strom versorgt.
+{{:projects:farmrobot:arduino_pixhawk.png?400|}}
+Zusätzlich müssen in der Full Parameter List zwei Eigenschaften des Telem1 Ports eingestellt werden.
+^ Command           ^ Value  ^
+| SERIAL1_Baud      | 57     |
+| SERIAL1_PROTOCOL  | 2      |
+==== Code ====
+Der Code wertet die Daten der Sensoren aus und sendet mithilfe von [MAVLink](https://mavlink.io/en/) ein disarm Befehl, sobald etwas zu nah kommt.
+Der Code verwendet eine [[https://github.com/mavlink/mavlink | MAVLink library ]], welche zuvor [[https://discuss.ardupilot.org/uploads/default/original/2X/0/064748b43308485aa9bd0d86fb91d74e17ed8c2a.zip | gedownloadet]] und eingebunden werden muss.
+Auf dem Arudino läuft folgende Code:
+<code>
+/*
+    Ultrasonic sensor Pins:
+        VCC: +5VDC
+        Trig : Trigger (INPUT)
+        Echo: Echo (OUTPUT)
+        GND: GND
+ */
+#include <mavlink.h>
+int echoPin0 = 22;    // Echo
+int trigPin0 = 23;    // Trigger
+int echoPin1 = 24;    // Echo
+int trigPin1 = 25;    // Trigger
+int echoPin2 = 26;    // Echo
+int trigPin2 = 27;    // Trigger
+int echoPin3 = 28;    // Echo
+int trigPin3 = 29;    // Trigger
+int echoPin4 = 30;    // Echo
+int trigPin4 = 31;    // Trigger
+int echoPin5 = 32;    // Echo
+int trigPin5 = 33;    // Trigger
+int echoPin6 = 34;    // Echo
+int trigPin6 = 35;    // Trigger
+int echoPin7 = 36;    // Echo
+int trigPin7 = 37;    // Trigger
+int outPin = 1;
+long distance = 95; // in cm
+long duration0, cm0;
+long duration1, cm1;
+long duration2, cm2;
+long duration3, cm3;
+long duration4, cm4;
+long duration5, cm5;
+long duration6, cm6;
+long duration7, cm7;
+// MAVLink
+mavlink_message_t msg;
+uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+uint16_t len;
+void setup() {
+  //Serial Port begin
+  Serial.begin (57600);
+  Serial1.begin (57600);
+  //Define inputs and outputs
+  pinMode(echoPin0, INPUT);
+  pinMode(trigPin0, OUTPUT);
+  pinMode(echoPin1, INPUT);
+  pinMode(trigPin1, OUTPUT);
+  pinMode(echoPin2, INPUT);
+  pinMode(trigPin2, OUTPUT);
+  pinMode(echoPin3, INPUT);
+  pinMode(trigPin3, OUTPUT);
+  pinMode(echoPin4, INPUT);
+  pinMode(trigPin4, OUTPUT);
+  pinMode(echoPin5, INPUT);
+  pinMode(trigPin5, OUTPUT);
+  pinMode(echoPin6, INPUT);
+  pinMode(trigPin6, OUTPUT);
+  pinMode(echoPin7, INPUT);
+  pinMode(trigPin7, OUTPUT);
+  pinMode(outPin, OUTPUT);
+}
+void loop() {
+  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
+  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
+  // 0
+  digitalWrite(trigPin0, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin0, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin0, LOW);
+  // Read the signal from the sensor: a HIGH pulse whose
+  // duration is the time (in microseconds) from the sending
+  // of the ping to the reception of its echo off of an object.
+  // 0
+  pinMode(echoPin0, INPUT);
+  duration0 = pulseIn(echoPin0, HIGH);
+  // 1
+  digitalWrite(trigPin1, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin1, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin1, LOW);
+  pinMode(echoPin1, INPUT);
+  duration1 = pulseIn(echoPin1, HIGH);
+  // 2
+  digitalWrite(trigPin2, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin2, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin2, LOW);
+  pinMode(echoPin2, INPUT);
+  duration2 = pulseIn(echoPin2, HIGH);
+  // 3
+  digitalWrite(trigPin3, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin3, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin3, LOW);
+  pinMode(echoPin3, INPUT);
+  duration3 = pulseIn(echoPin3, HIGH);
+  // 4
+  digitalWrite(trigPin4, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin4, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin4, LOW);
+  pinMode(echoPin4, INPUT);
+  duration4 = pulseIn(echoPin4, HIGH);
+  // 5
+  digitalWrite(trigPin5, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin5, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin5, LOW);
+  pinMode(echoPin5, INPUT);
+  duration5 = pulseIn(echoPin5, HIGH);
+  // 6
+  digitalWrite(trigPin6, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin6, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin6, LOW);
+  pinMode(echoPin6, INPUT);
+  duration6 = pulseIn(echoPin6, HIGH);
+  // 7
+  digitalWrite(trigPin7, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPin7, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin7, LOW);
+  pinMode(echoPin7, INPUT);
+  duration7 = pulseIn(echoPin7, HIGH);
+  // Convert the time into a distance
+  // 0
+  cm0 = (duration0/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 1
+  cm1 = (duration1/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 2
+  cm2 = (duration2/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 3
+  cm3 = (duration3/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 4
+  cm4 = (duration4/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 5
+  cm5 = (duration5/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 6
+  cm6 = (duration6/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // 7
+  cm7 = (duration7/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  // Check if something gets to close
+  if(cm0 <= distance || cm1 <= distance || cm2 <= distance || cm3 <= distance || cm4 <= distance || cm5 <= distance || cm6 <= distance || cm7 <= distance){
+    mavlink_msg_command_long_pack(255, 0, &msg, 1, 0, MAV_CMD_COMPONENT_ARM_DISARM, 0, 0, 0, 0, 0, 0, 0, 0);
+    // mavlink_msg_command_long_pack(0xFF, 0xBE, &msg, 1, 1, 400, 1,0.0,0,0,0,0,0,0);
+    len = mavlink_msg_to_send_buffer(buf, &msg);
+    // Send the message (.write sends as bytes)
+    Serial.write(buf, len);
+    Serial.println("Too close!");
+    Serial1.write(buf, len);
+  }else{
+    Serial.println("Nothing close!");
+  }
+}
+</code>

FabLab Kamp-Lintfort

User Tools

Site Tools

Differences

Page Tools