This shows you the differences between two versions of the page.
Next revision | Previous revision Last revision Both sides next revision | ||
projects:farmrobot:hinderniserkennung_mit_sonar [2021/02/21 12:02] leon-david created |
projects:farmrobot:hinderniserkennung_mit_sonar [2021/02/24 20:43] tobias [Aufbau] |
||
---|---|---|---|
Line 1: | Line 1: | ||
- | Hinderniserkennung mit 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:// | ||
+ | |||
+ | {{: | ||
+ | |||
+ | Die Sensoren sind mithilfe von Mounts am Farm Robot angebracht, welche mit Fusion 360 erstellt und anschließend vom 3D-Drucker ausgedruckt wurden. | ||
+ | |||
+ | {{: | ||
+ | |||
+ | Der Arduino ist Seriell mit dem Telem1 Port des Pixhawks verbunden und wird auch vom Pixhawk mit Strom versorgt. | ||
+ | |||
+ | {{: | ||
+ | |||
+ | Damit das Funktioniert müssen in der Full Parameter List zwei Eigenschaften des Telem1 Ports eingestellt werden. | ||
+ | |||
+ | ^ Command | ||
+ | | SERIAL1_Baud | ||
+ | | SERIAL1_PROTOCOL | ||
+ | |||
+ | |||
+ | ==== Code ==== | ||
+ | |||
+ | Der Code wertet die Daten der Sensoren aus und sendet mithilfe von [MAVLink](https:// | ||
+ | Der Code verwendet eine [[https:// | ||
+ | |||
+ | |||
+ | Auf dem Arudino läuft folgende Code: | ||
+ | < | ||
+ | /* | ||
+ | Ultrasonic sensor Pins: | ||
+ | VCC: +5VDC | ||
+ | Trig : Trigger (INPUT) | ||
+ | Echo: Echo (OUTPUT) | ||
+ | GND: GND | ||
+ | */ | ||
+ | #include < | ||
+ | |||
+ | 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, | ||
+ | pinMode(trigPin0, | ||
+ | pinMode(echoPin1, | ||
+ | pinMode(trigPin1, | ||
+ | pinMode(echoPin2, | ||
+ | pinMode(trigPin2, | ||
+ | pinMode(echoPin3, | ||
+ | pinMode(trigPin3, | ||
+ | pinMode(echoPin4, | ||
+ | pinMode(trigPin4, | ||
+ | pinMode(echoPin5, | ||
+ | pinMode(trigPin5, | ||
+ | pinMode(echoPin6, | ||
+ | pinMode(trigPin6, | ||
+ | pinMode(echoPin7, | ||
+ | pinMode(trigPin7, | ||
+ | |||
+ | pinMode(outPin, | ||
+ | } | ||
+ | |||
+ | 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, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin0, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin0, | ||
+ | // 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, | ||
+ | duration0 = pulseIn(echoPin0, | ||
+ | // 1 | ||
+ | digitalWrite(trigPin1, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin1, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin1, | ||
+ | |||
+ | pinMode(echoPin1, | ||
+ | duration1 = pulseIn(echoPin1, | ||
+ | // 2 | ||
+ | digitalWrite(trigPin2, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin2, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin2, | ||
+ | |||
+ | pinMode(echoPin2, | ||
+ | duration2 = pulseIn(echoPin2, | ||
+ | // 3 | ||
+ | digitalWrite(trigPin3, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin3, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin3, | ||
+ | |||
+ | pinMode(echoPin3, | ||
+ | duration3 = pulseIn(echoPin3, | ||
+ | // 4 | ||
+ | digitalWrite(trigPin4, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin4, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin4, | ||
+ | |||
+ | pinMode(echoPin4, | ||
+ | duration4 = pulseIn(echoPin4, | ||
+ | // 5 | ||
+ | digitalWrite(trigPin5, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin5, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin5, | ||
+ | |||
+ | pinMode(echoPin5, | ||
+ | duration5 = pulseIn(echoPin5, | ||
+ | // 6 | ||
+ | digitalWrite(trigPin6, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin6, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin6, | ||
+ | |||
+ | pinMode(echoPin6, | ||
+ | duration6 = pulseIn(echoPin6, | ||
+ | // 7 | ||
+ | digitalWrite(trigPin7, | ||
+ | delayMicroseconds(5); | ||
+ | digitalWrite(trigPin7, | ||
+ | delayMicroseconds(10); | ||
+ | digitalWrite(trigPin7, | ||
+ | |||
+ | pinMode(echoPin7, | ||
+ | duration7 = pulseIn(echoPin7, | ||
+ | |||
+ | // Convert the time into a distance | ||
+ | // 0 | ||
+ | cm0 = (duration0/ | ||
+ | // 1 | ||
+ | cm1 = (duration1/ | ||
+ | // 2 | ||
+ | cm2 = (duration2/ | ||
+ | // 3 | ||
+ | cm3 = (duration3/ | ||
+ | // 4 | ||
+ | cm4 = (duration4/ | ||
+ | // 5 | ||
+ | cm5 = (duration5/ | ||
+ | // 6 | ||
+ | cm6 = (duration6/ | ||
+ | // 7 | ||
+ | cm7 = (duration7/ | ||
+ | |||
+ | // 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, | ||
+ | // mavlink_msg_command_long_pack(0xFF, | ||
+ | len = mavlink_msg_to_send_buffer(buf, | ||
+ | |||
+ | // Send the message (.write sends as bytes) | ||
+ | Serial.write(buf, | ||
+ | Serial.println(" | ||
+ | Serial1.write(buf, | ||
+ | }else{ | ||
+ | Serial.println(" | ||
+ | } | ||
+ | } | ||
+ | </ |