BHIL2023 - Otto Avoid Competition Winner

otto_avoid.ino

#include <Otto.h>
#include <Servo.h> 
#include <EEPROM.h>
Otto Otto;

#define LeftLeg 2 // left leg pin
#define RightLeg 3 // right leg pin
#define LeftFoot 4 // left foot pin
#define RightFoot 5 // right foot pin
#define Buzzer 13 //buzzer pin
#define Trigger 10 // ultrasonic sensor trigger pin
#define Echo 11 // ultrasonic sensor echo pin

long duration;
int distance;

long ultrasound() {
   long duration, distance;
   //digitalWrite(Trigger,LOW);
   //delayMicroseconds(2000);
   //digitalWrite(Trigger, HIGH);
   //delayMicroseconds(100);
   //digitalWrite(Trigger, LOW);
   //duration = pulseIn(Echo, HIGH);
   //distance = duration/58;
   //return distance;
  // Clears the trigPin
  digitalWrite(Trigger, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trigger, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(Echo, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2;
  // Prints the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);
  return distance;
}

void setup() {
   Serial.begin(250000); 
   Otto.init(LeftLeg, RightLeg, LeftFoot, RightFoot, true, Buzzer);
   pinMode(Trigger, OUTPUT); 
   pinMode(Echo, INPUT); 
   Otto.home();
   //delay(100);
   //Otto.sing(S_OhOoh);
}
void loop(){
  Serial.println("Ultra:");
  Serial.println(ultrasound());

  int sample;
  sample = 0;
  sample = ultrasound();
  delay(40);
  sample += ultrasound();
  delay(40);
  sample += ultrasound();
  delay(40);
  sample += ultrasound();

  Serial.println(sample);

  if (((sample)/4) < 8) {
    Otto.sing(S_fart1);
    Otto.walk(2,800,-1); // BACKWARD x2
    Otto.turn(3,1000,-1); // LEFT x3
  }
  
  Otto.walk(4, 1000, 1);
  //delay(400);
}