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.

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 Telem1 Port des Pixhawks verbunden und wird auch vom Pixhawk mit Strom versorgt.

Zusätzlich müssen in der Full Parameter List zwei Eigenschaften des Telem1 Ports eingestellt werden.

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 MAVLink library , welche zuvor gedownloadet und eingebunden werden muss.

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!");
  }
}