trapped lasers disco with distance sensor

trapped_lasers_disco.ino

#include <Servo.h>

Servo myservo;

// servo & lasers
float pos = 0;
#define LA1 10
#define LA2 11

float minDeg = 80;
float maxDeg = 100;
//bool play = false;

// distance sensor
#include "SR04.h"
#define TRIG_PIN 7
#define ECHO_PIN 6
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long dist;
long lastDist;
long maxDist = 40;
long minDist = 2;
long maxMovt = 40;

void setup() {
  Serial.begin(9600);
  myservo.attach(9);
  pinMode(LA1, OUTPUT);
  pinMode(LA2, OUTPUT);
}

void fadeIn(int ledPinNum) {
  for (int fadeValue = 0 ; fadeValue <= 255; fadeValue += 1) {
    analogWrite(ledPinNum, fadeValue);
  }
}
void fadeOut(int ledPinNum) {
  for (int fadeValue = 255 ; fadeValue >= 0; fadeValue -= 1) {
    analogWrite(ledPinNum, fadeValue);
  }
}

void sweepAnim1() {
    for (pos = minDeg; pos <= maxDeg; pos += 5) {
//      Serial.println(pos, DEC);
      digitalWrite(LA1, HIGH);
      digitalWrite(LA2, LOW);
      myservo.write(pos);
      delay(30);
      digitalWrite(LA1, LOW);
      digitalWrite(LA2, HIGH);
      delay(30);
    }

    for (pos = maxDeg; pos >= minDeg; pos -= 5) {
//      Serial.println(pos, DEC);
      digitalWrite(LA1, HIGH);
      myservo.write(pos);
      delay(100);
      digitalWrite(LA1, LOW);
      delay(100);
    }
}

void sweepAnim2() {
    for (pos = minDeg; pos <= maxDeg; pos += 0.1) {
//      Serial.println(pos, DEC);
      digitalWrite(LA1, HIGH);
      digitalWrite(LA2, LOW);
      myservo.write(pos);
      delay(5);
      digitalWrite(LA1, LOW);
      digitalWrite(LA2, HIGH);
      delay(5);
//      minDeg = random(0, 180);
//      maxDeg = random(0, 180);

    }

    for (pos = maxDeg; pos >= minDeg; pos -= 0.5) {
//      Serial.println(pos, DEC);
      digitalWrite(LA1, HIGH);
      myservo.write(pos);
      delay(5);
      digitalWrite(LA1, LOW);
      delay(5);
    }
}


void loop() {
  lastDist = dist;
  dist=sr04.Distance();
  Serial.println(dist, DEC);
  sweepAnim2();

  if(dist > lastDist + maxMovt || dist < lastDist - maxMovt) {
    lastDist = constrain(lastDist, minDist, maxDist + 20);
    dist = ((lastDist * 5) + dist) / 6;
  }
  if(dist > minDist && dist < maxDist) {
    fadeIn(LA1); fadeIn(LA2);
    myservo.write(
      int( ((myservo.read() * 5) + map(dist, minDist, maxDist, maxDeg, minDeg)) / 6)
    );
  } else {
//    sweepAnim1();
//    fadeOut(LA1); fadeOut(LA2);
  }
}