Elephant Companion Bot

/* Servo motor with Arduino example code. Position and sweep. More info: https://www.makerguides.com/ */

// Include the servo library:
#include <Servo.h>

// Create a new servo object:
Servo headServo;
Servo rightEarServo;
Servo leftEarServo;

// Define the servo pin:
int headServoPin = 9;
int headPosition = 70;

// Set headpositions for max left and right rotation
int headMaxRightPosition = 50;
int headMaxLeftPosition = 150;

// Define the rightEar servo pin:
int rightEarServoPin = 10;
int rightEarPosition = 90;
int rightEarMaxBackwardPosition = 40;
int rightEarMaxForwardPosition = 100;

int leftEarServoPin = 11;
int leftEarPosition = 87;
int leftEarMaxForwardPosition = 30;
int leftEarMaxBackwardPosition = 120;

/**
 * Attach Pins to Servo Object
 * Set servos to starting position
 */
void setup() {
  headServo.attach(headServoPin);
  setHeadStraight();
  delay(30);
  rightEarServo.attach(rightEarServoPin);
  leftEarServo.attach(leftEarServoPin);
  setEarsStraight();
  delay(1000);
}

void loop() {
  tiltHead();
  delay(1000);
  moveEars();
  delay(1000);
}

/**
 * ########################################
 * # Init Servo positions
 * ########################################
 */
void setHeadStraight() {
  headServo.write(headPosition);
  delay(20);
}

void setEarsStraight() {
  rightEarServo.write(rightEarPosition);
  leftEarServo.write(leftEarPosition);
  delay(20);
}


/**
 * ########################################
 * # Move Head
 * ########################################
 */
void tiltHead() {
  int currentAngle = 0;
  for (int angle = 70; angle <= headMaxLeftPosition; angle += 1) {
    headServo.write(angle);
    currentAngle = angle;
    delay(20);
  }

  for (int angle = currentAngle; angle >= headMaxRightPosition; angle -= 1) {
    headServo.write(angle);
    currentAngle = angle;
    delay(20);
  }
}


/**
 * ########################################
 * # Move Right Ear
 * ########################################
 */
void moveEars() {
  int leftEarAngle = leftEarMaxBackwardPosition;
  int currentAngle = 0;

  
  for (int angle = rightEarMaxBackwardPosition; angle <= rightEarMaxForwardPosition; angle += 1) {
    rightEarServo.write(angle);
    leftEarAngle -= 1;
    leftEarServo.write(leftEarAngle);
    currentAngle = angle;
    delay(20);
  }

  for (int angle = currentAngle; angle >= rightEarMaxBackwardPosition; angle -= 1) {
    rightEarServo.write(angle);
    leftEarAngle += 1;
    leftEarServo.write(leftEarAngle);
    currentAngle = angle;
    delay(10);
  }
  
}