Control code for Chartreuse the moving robot

Chartreuse.ino

// SERVO 
#include <Servo.h>

//NEOPIXELS
#include <Adafruit_NeoPixel.h>

//STEPPER
#include "Stepper_28BYJ_48.h"

#ifdef __AVR__
  #include <avr/power.h>
#endif

//set out signal pin
#define PIN 9

Adafruit_NeoPixel strip = Adafruit_NeoPixel(2, PIN, NEO_GRB + NEO_KHZ800);

const int echo = 2;
const int trig = 3;
const int ultrasonicServoPin = 5; 
const int ledStatusPin = 7;
const int eyeServoPin = 6;
const int ledPixelPin = 9;
Stepper_28BYJ_48 stepper(10,11,12,13);

Servo ultServo;
Servo eyeServo;

int ultAngle = 90;
int eyeAngle = 90;
int stepperAngle = 90;

long dur;
int dist;

long lastSeen;

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT); 
  pinMode(ledStatusPin, OUTPUT);
  
  ultServo.attach(ultrasonicServoPin); //48deg -> 120
  eyeServo.attach(eyeServoPin); //0 -> 180
  ultServo.write(90);
  eyeServo.write(90);
  
  strip.begin();
  strip.show();
  
  Serial.begin(9600); // Starts the serial communication
}

void loop() {
  /*
  REMOTE CONTROL
  int tester = analogRead(0);
  int mapper = map(tester,0,1023,0,3);
  setEyes(mapper);
  
  if(digitalRead(1) == LOW){
    stepper.step(1);
  }
  if(digitalRead(2) == LOW){
    stepper.step(-1);
  }
  */
  
  //MOVE SCANNER -> MOVE STEPPER -> MOVE EYES
  
  int lastAngle = ultAngle;
  
  // //ULTRASONIC TRACKING
  getDistance();
  //Serial.println(dist);
  if (dist > 10 && dist < 30){ //30 for testing
    //Serial.println("Distance: " + String(dist) + " Angle: " + String(ultAngle));
    digitalWrite(ledStatusPin, HIGH);
    if (ultAngle < 60){
      ultAngle = 60;
    }else{
      ultAngle-=6;
    }
  }else{
    digitalWrite(ledStatusPin, LOW);
    if (ultAngle > 120){
      ultAngle = 120;
    }else{
      ultAngle++;
    }
  }
  if (ultAngle > 50 && ultAngle < 108){
    lastSeen = millis();
  }
  if (lastAngle != ultAngle){
    ultServo.write(ultAngle);
  }
  Serial.println(ultAngle);

  delay(1);
  
  
  //STEPPER ------------------------------------------
  stepToDeg(ultAngle);
  delay(1);
  
  //EYES ---------------------------------------------
  long lastContact = (millis()-lastSeen);
  //Serial.println(lastContact);
  if (lastContact > 3000 && lastContact < 6000){
    setEyes(2);
  }else if (lastContact <= 3000){
    setEyes(1);
  }else{
    setEyes(0);
  }
}

// -----------------------------------------------------------------------------------------

void stepToDeg(int stepAngle){
  if (ultAngle < 121){
    int difference = stepperAngle - ultAngle;
    int absDifference = abs(difference);
    int negDif = -difference*7;
    stepper.step(negDif);
    stepperAngle = ultAngle;
  }
}

void getDistance(){ //TAKES AN ULTRASONIC MEASUREMENT AND RETURNS THE COMPUTED DISTANCE IN CM
  // Clears the trigPin
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  
  //Sends pulse
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  dur = pulseIn(echo, HIGH);
    
  dist = dur*0.034/2;
}

void setEyes(int mode){ //SETS THE ANGLE OF THE EYE SERVO FOR VARIOUS EXPRESSIONS
  switch (mode) { //DEFAULT:NEUTRAL 1:HAPPY 2:SAD
    case 1: //HAPPY
      eyeServo.write(179);
      for(uint16_t i=0; i<strip.numPixels(); i++){
        strip.setPixelColor(i,255,140,0);
        strip.show();
      }
      break;
    case 2: //SAD
      eyeServo.write(1);
      for(uint16_t i=0; i<strip.numPixels(); i++){
        strip.setPixelColor(i,90,90,255);
        strip.show();
      }
      break;
    default: //NEUTRAL
      eyeServo.write(99);
      for(uint16_t i=0; i<strip.numPixels(); i++){
        strip.setPixelColor(i,255,200,180);
        strip.show();
      }
      break;
  }
}