Software Project 2 Assignment

#include <Servo.h>

// === Pin Assignment ===
#define PIN_IR    A0        // IR sensor input pin
#define PIN_LED   9         // LED pin
#define PIN_SERVO 10        // Servo control pin

// === Servo Duty (in microseconds) ===
// Adjust these values if servo doesn't move correctly
#define _DUTY_MIN 500       // servo at 0° (clockwise limit)
#define _DUTY_NEU 1500      // servo at 90° (middle)
#define _DUTY_MAX 2500      // servo at 180° (counter-clockwise limit)

// === Distance range (mm) ===
#define _DIST_MIN  100.0    // 10 cm
#define _DIST_MAX  250.0    // 25 cm

// === Filter and Loop ===
#define EMA_ALPHA  0.2      // EMA filter smoothing (0.1~0.3 typical)
#define LOOP_INTERVAL  20   // Loop interval in milliseconds

Servo myservo;
unsigned long last_loop_time = 0;

float dist_prev = _DIST_MIN;
float dist_ema  = _DIST_MIN;

void setup() {
  pinMode(PIN_LED, OUTPUT);
  myservo.attach(PIN_SERVO);
  myservo.writeMicroseconds(_DUTY_NEU);

  Serial.begin(1000000); // Fast serial for plotter
}

void loop() {
  unsigned long time_curr = millis();
  if (time_curr < (last_loop_time + LOOP_INTERVAL))
    return;
  last_loop_time = time_curr;

  // === Read IR sensor ===
  int a_value = analogRead(PIN_IR);
  float dist_raw = ((6762.0 / (a_value - 9.0)) - 4.0) * 10.0;  // mm

  // === Range Filter ===
  bool in_range = (dist_raw >= _DIST_MIN && dist_raw <= _DIST_MAX);

  if (in_range) {l
    digitalWrite(PIN_LED, HIGH);  // Turn LED ON
  } else {
    digitalWrite(PIN_LED, LOW);   // Turn LED OFF
  }

  // === EMA Filter ===
  dist_ema = EMA_ALPHA * dist_raw + (1.0 - EMA_ALPHA) * dist_prev;
  dist_prev = dist_ema;

  // === Servo Control ===
  float duty;
  if (in_range) {
    // Linear mapping from distance to duty (without using map())
    duty = _DUTY_MIN + (dist_ema - _DIST_MIN) * (_DUTY_MAX - _DUTY_MIN) / (_DIST_MAX - _DIST_MIN);
  } else if (dist_raw < _DIST_MIN) {
    duty = _DUTY_MIN;
  } else {
    duty = _DUTY_MAX;
  }

  myservo.writeMicroseconds((int)duty);

  // === Serial Output (for Serial Plotter) ===
  Serial.print("IR:");        Serial.print(a_value);
  Serial.print(",dist_raw:"); Serial.print(dist_raw);
  Serial.print(",ema:");      Serial.print(dist_ema);
  Serial.print(",servo:");    Serial.print(duty);
  Serial.println("");
}