Golf Ball Dispenser

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// ------------------ LCD Setup ------------------
LiquidCrystal_I2C lcd(0x20, 20, 4);  // Initialize 20x4 I2C LCD

// ------------------ Motor Setup ------------------
const int pwmPin = 5;         // PWM pin for motor speed control
const int in1 = 6;            // Motor direction pin 1
const int in2 = 7;            // Motor direction pin 2
const int breakBeamPin = 22;  // Break-beam sensor for ball position   
                              // (HIGH=clear, LOW=broken)

const int liftMotorSpeed = 100;              // Motor speed (PWM)
const unsigned long motorRunOnTime = 200;    // Motor continues running after 
                                             // beam breaks (ms)
unsigned long lastBeamClearTime = 0;         // Timestamp of last clear beam

// ------------------ Servo Setup ------------------
Servo releaseServo;
const int servoPin = 9;       // PWM pin for servo
const int servoLow = 10;      // Minimum servo angle
const int servoHigh = 120;    // Maximum servo angle
int speed = 50;               // Sweep speed (1=slow, 100=fast)

volatile int servoPos = servoLow;     // Current servo position
volatile int servoTarget = servoHigh; // Target servo position
volatile bool sweeping = false;       // Flag for active sweep
volatile int servoDirection = 1;      // Sweep direction (1=forward, -1=backward)
volatile int servoTickCount = 0;      // Tick counter for servo timing

// ------------------ IR Sensor Setup ------------------
const int irPin = A0;                   // Analog pin for IR distance sensor
const float minDistanceIn = 5.0;        // Minimum valid wave distance (inches)
const float maxDistanceIn = 13.0;       // Maximum valid wave distance (inches)
const unsigned long debounceTime = 500; // Minimum time needed (ms)

bool lastDetected = false;              // Last IR detection state
unsigned long detectionStart = 0;       // Timestamp when detection started

// ----- DISPENSING STATES -----
enum DispenseState { IDLE, DISPENSING, DONE }; // FSM states
DispenseState dispenseState = IDLE;
unsigned long dispenseStart = 0;             // Start time of dispensing cycle
const unsigned long maxDispenseTime = 20000; // Max dispense duration (ms)

// ------------------ Swing-Speed Sensor Setup ------------------
const int beamStartPin = 2;                 // Start break-beam sensor
const int beamStopPin  = 3;                 // Stop break-beam sensor
const float beamDistance_m = 1.75 * 0.0254; // Distance between beams in meters

volatile unsigned long startTime = 0;    // Timestamp from start beam
volatile unsigned long stopTime  = 0;    // Timestamp from stop beam
volatile bool measurementReady  = false; // Flag for swing measurement complete
volatile bool armed             = true;  // Flag to arm ISR for new measurement

float swingSpeed_mph = 0;                // Calculated swing speed

// ------------------ Helper Functions ------------------
int speedDelay()
{
  return map(speed, 1, 100, 15, 1);    // Map sweep speed to ISR delay
}

float getDistanceCM(int analogValue)   // Convert IR analog reading to cm
{
  if (analogValue < 30) return 80;
  float distance = 4800.0 / (analogValue - 20);
  if (distance > 80) distance = 80;
  if (distance < 10) distance = 10;
  return distance;
}

// ------------------ Sweep Servo Control ------------------
void startServoSweep()                 // Initialize servo sweep
{
  sweeping = true;
  servoDirection = 1;
  servoTarget = servoHigh;
  servoTickCount = 0;
}

// ------------------ Swing-Speed ISR ------------------
void startBeamISR()                    // ISR triggered by start beam
{
  if (armed)
  {
    startTime = micros();
    armed = false;
  }
}

void stopBeamISR()                     // ISR triggered by stop beam
{
  if (!armed)
  {
    stopTime = micros();
    measurementReady = true;
    armed = true;
  }
}

// ------------------ Setup ------------------
void setup()
{
  lcd.init();                            // Initialize LCD
  lcd.backlight();
  lcd.clear();

  // Initial LCD messages
  lcd.setCursor(0,0); 
  lcd.print("Wave to release ball");
  lcd.setCursor(0,1); 
  lcd.print("Speed:              ");
  lcd.setCursor(0,2); 
  lcd.print("Distance:           ");
  lcd.setCursor(0,3); 
  lcd.print("Jesse Stockebrand   ");

  // Motor pins
  pinMode(pwmPin, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(breakBeamPin, INPUT_PULLUP);

  // Swing-speed sensor pins
  pinMode(beamStartPin, INPUT_PULLUP);
  pinMode(beamStopPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(beamStartPin), startBeamISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(beamStopPin), stopBeamISR, FALLING);

  // Servo setup
  releaseServo.attach(servoPin);
  releaseServo.write(servoLow);
  servoPos = servoLow;

  // ------------------ Timer2 Setup for Non-Blocking Servo ------------------
  noInterrupts();
  TCCR2A = 0;
  TCCR2B = 0;
  TCNT2  = 0;
  OCR2A = 249;
  TCCR2A |= (1 << WGM21);
  TCCR2B |= (1 << CS22);
  TIMSK2 |= (1 << OCIE2A);
  interrupts();

  Serial.begin(115200);                 // Serial for debugging
}

// ------------------ Timer2 ISR for Servo ------------------
ISR(TIMER2_COMPA_vect)                 // Non-blocking servo sweep ISR
{
  if (sweeping)
  {
    servoTickCount++;
    if (servoTickCount >= speedDelay())
    {
      servoTickCount = 0;
      servoPos += servoDirection;
      releaseServo.write(servoPos);

      // Reverse sweep direction at target positions
      if (servoDirection > 0 && servoPos >= servoTarget)
      {
        servoDirection = -1;
        servoTarget = servoLow;
      }
      else if (servoDirection < 0 && servoPos <= servoTarget)
      {
        sweeping = false;
      }
    }
  }
}

// ------------------ Main Loop ------------------
void loop()
{
  // ------------------ Motor Control ------------------
  int beamState = digitalRead(breakBeamPin);

  if (beamState == HIGH)               // Beam clear: run motor
  {
    lastBeamClearTime = millis();
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    analogWrite(pwmPin, liftMotorSpeed);
  }
  else                                  // Beam broken: check run-on
  {
    if (millis() - lastBeamClearTime < motorRunOnTime)
    {
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      analogWrite(pwmPin, liftMotorSpeed);
    }
    else                                // Stop motor
    {
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);
      analogWrite(pwmPin, 0);
    }
  }

  // ------------------ IR Sensor ------------------
  long sum = 0;
  for (int i = 0; i < 20; i++)
  {
    sum += analogRead(irPin);           // Average multiple samples
  }
  float distanceCM = getDistanceCM(sum / 20);
  int distanceIn = (int)(distanceCM * 0.393701 + 0.5);
  bool waveDetected = (distanceIn >= 5 && distanceIn <= 9); // Detection range

  // ------------------ Dispensing FSM ------------------
  switch(dispenseState)
  {
    case IDLE:
      lcd.setCursor(0,0); 
      lcd.print("Wave to release ball");
      if (waveDetected)
      {
        dispenseState = DISPENSING;
        dispenseStart = millis();
        lastDetected = false;
      }
      break;

    case DISPENSING:
      if (waveDetected)
      {
        if (!lastDetected) 
        {
          detectionStart = millis();          // Start debounce timer
        }
        lastDetected = true;

        if (millis() - detectionStart >= 500) // Debounce completed
        {
          lcd.setCursor(0,0); 
          lcd.print("RELEASING           ");
          startServoSweep();                  // Trigger servo
        }
      }
      else
      {
        lastDetected = false;
      }

      if (measurementReady || millis() - dispenseStart >= maxDispenseTime)
      {
        dispenseState = DONE;             // Move to DONE state
        lcd.setCursor(0,0); 
        lcd.print("Wave to release ball");
      }
      break;

    case DONE:
      if (!waveDetected) 
      {
        dispenseState = IDLE;             // Reset to IDLE when user leaves
      }
      break;
  }

  // ------------------ LCD Updates ------------------
  lcd.setCursor(0,2);
  lcd.print("Distance:           ");
  lcd.setCursor(10,2);
  lcd.print(distanceIn);
  lcd.print(" in   ");

  // ------------------ Swing-Speed Measurement ------------------
  if (measurementReady)
  {
    noInterrupts();                      // Temporarily disable interrupts
    unsigned long t1 = startTime;
    unsigned long t2 = stopTime;
    measurementReady = false;
    interrupts();

    unsigned long delta_us = t2 - t1;
    if (delta_us >= 700 && delta_us <= 3000) // Valid measurement window
    {
      float speed_mps = beamDistance_m / (delta_us / 1e6);
      swingSpeed_mph  = speed_mps * 2.237;   // Convert to mph

      lcd.setCursor(7,1);
      lcd.print(swingSpeed_mph,1);
      lcd.print(" mph   ");

      dispenseState = DONE;
      lcd.setCursor(0,0);
      lcd.print("Wave to release ball");
    }
  }

  delay(50);
}