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.

Es sind acht HC-SR04 Sensoren mit einem Arduino Mega verbunden, zwei für jede Seite.

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 Pixhawk und wird auch von diesem mit Strom versorgt.

Der Code wertet die Daten der Sensoren aus und sendet mithilfe von [MAVLink](https://mavlink.io/en/) ein Stopp Signal, sobald etwas zu nah kommt.

Auf dem Arudino läuft folgende 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!");
  }
}