I M Building Test-phase Prototype Of Capstone Co

#include <Arduino.h>
#include <EEPROM.h>

/*
  One-lane conveyor dispenser test prototype — Arduino Uno
  Serial: 115200 baud. Commands: D=dispense, R=returned package,
  C=calibrate, S=status, T=self-test.

  THEORETICAL STARTING STEP CALCULATION:
  28BYJ-48 output (half-step) ≈ 4096 steps/revolution.
  Roller circumference = PI × driven roller diameter.
  Steps per package spacing = 4096 × package spacing / roller circumference.
  Example: 20 mm spacing, 20 mm belt-contact roller diameter:
  4096 × 20 / (PI × 20) = about 1304 half-steps.
  Use the belt-only test to tune STEPS_PER_SPACING. Belt slip and roller
  tolerance mean measured movement is more important than this estimate.
*/

// ---------- EASY-TO-TUNE MECHANICAL AND DECISION CONSTANTS ----------

struct CalibrationData {
  uint16_t magic;
  float fullDistanceMm;
  float emptyDistanceMm;
  float expectedIncreaseMm;
};


// Forward declarations
void logEvent(const char *eventName, long steps, float distanceMm, const char *status);
void setMotorPattern(int index);
void releaseMotor();
void moveMotor(long requestedSteps);
float oneDistanceReadingMm();
float medianDistanceMm();
bool calibrationValid();
bool isEmptyDistance(float distanceMm);
void printStatus();
void runCalibration();
void dispenseOne();
void recordReturn();
void selfTest();

const float PACKAGE_SPACING_MM = 20.0;       // Replace with actual fixed package spacing.
const float DRIVEN_ROLLER_DIAMETER_MM = 20.0; // Belt-contact diameter, not motor shaft diameter.
const long MOTOR_STEPS_PER_OUTPUT_REV = 4096; // 28BYJ-48 half-step approximation.
const long STEPS_PER_SPACING = 1304;          // Tune after belt measurement.
const long MAX_STEPS_PER_DISPENSE = 2608;     // One move plus exactly one retry.
const byte STARTING_PACKAGE_COUNT = 10;       // Full-lane count used during calibration.
const byte LOW_STOCK_THRESHOLD = 3;
const float DISTANCE_TOLERANCE_MM = 7.0;      // Allowed error around expected increase.
const float EMPTY_TOLERANCE_MM = 12.0;        // Near-empty calibration distance allowance.
const unsigned int STEP_DELAY_US = 1800;      // Increase if the motor buzzes or skips.
const byte MEDIAN_SAMPLES = 7;
const unsigned long ECHO_TIMEOUT_US = 30000UL;
const long SELF_TEST_STEPS = 32;

// ---------- PIN DEFINITIONS: REVIEWED AGAINST THE WIRING DIAGRAM (connector verified) ----------
const byte TRIG_PIN = 2;
const byte ECHO_PIN = 3;
const byte IN1_PIN = 8;
const byte IN2_PIN = 9;
const byte IN3_PIN = 10;
const byte IN4_PIN = 11;



const uint16_t EEPROM_MAGIC = 0xC48D;
CalibrationData calibration;
int countEstimate = STARTING_PACKAGE_COUNT;
long totalCommandedSteps = 0;
int stepIndex = 0;

const byte halfStepPattern[8][4] = {
  {1, 0, 0, 0}, {1, 1, 0, 0}, {0, 1, 0, 0}, {0, 1, 1, 0},
  {0, 0, 1, 0}, {0, 0, 1, 1}, {0, 0, 0, 1}, {1, 0, 0, 1}
};

void logEvent(const char *eventName, long steps, float distanceMm, const char *status) {
  Serial.print(millis()); Serial.print(',');
  Serial.print(eventName); Serial.print(',');
  Serial.print(steps); Serial.print(',');
  if (distanceMm < 0) Serial.print("NA"); else Serial.print(distanceMm, 1);
  Serial.print(','); Serial.print(countEstimate); Serial.print(',');
  Serial.println(status);
}

void setMotorPattern(int index) {
  digitalWrite(IN1_PIN, halfStepPattern[index][0]);
  digitalWrite(IN2_PIN, halfStepPattern[index][1]);
  digitalWrite(IN3_PIN, halfStepPattern[index][2]);
  digitalWrite(IN4_PIN, halfStepPattern[index][3]);
}

void releaseMotor() {
  digitalWrite(IN1_PIN, LOW); digitalWrite(IN2_PIN, LOW);
  digitalWrite(IN3_PIN, LOW); digitalWrite(IN4_PIN, LOW);
}

void moveMotor(long requestedSteps) {
  for (long i = 0; i < requestedSteps; i++) {
    stepIndex = (stepIndex + 1) & 7;
    setMotorPattern(stepIndex);
    delayMicroseconds(STEP_DELAY_US);
  }
  totalCommandedSteps += requestedSteps;
  releaseMotor(); // Prevents unnecessary heating while stopped.
}

float oneDistanceReadingMm() {
  digitalWrite(TRIG_PIN, LOW); delayMicroseconds(3);
  digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long duration = pulseIn(ECHO_PIN, HIGH, ECHO_TIMEOUT_US);
  if (duration == 0) return -1.0;
  return duration * 0.343f / 2.0f;
}

float medianDistanceMm() {
  float readings[MEDIAN_SAMPLES];
  byte valid = 0;
  for (byte i = 0; i < MEDIAN_SAMPLES; i++) {
    float reading = oneDistanceReadingMm();
    if (reading >= 20.0 && reading <= 4000.0) readings[valid++] = reading;
    delay(45); // Wait so echoes from one sample do not affect the next.
  }
  if (valid < (MEDIAN_SAMPLES / 2 + 1)) return -1.0;
  for (byte i = 0; i < valid; i++) {
    for (byte j = i + 1; j < valid; j++) {
      if (readings[j] < readings[i]) {
        float swapValue = readings[i]; readings[i] = readings[j]; readings[j] = swapValue;
      }
    }
  }
  return readings[valid / 2];
}

bool calibrationValid() {
  return calibration.magic == EEPROM_MAGIC &&
         calibration.emptyDistanceMm > calibration.fullDistanceMm &&
         calibration.expectedIncreaseMm > 0.5;
}

bool isEmptyDistance(float distanceMm) {
  return calibrationValid() && distanceMm >= calibration.emptyDistanceMm - EMPTY_TOLERANCE_MM;
}

const char *stockStatus(float distanceMm) {
  if (countEstimate <= 0 || isEmptyDistance(distanceMm)) return "EMPTY";
  if (countEstimate <= LOW_STOCK_THRESHOLD) return "LOW_STOCK";
  return "OK";
}

void printStatus() {
  float distance = medianDistanceMm();
  logEvent("STATUS", totalCommandedSteps, distance,
           calibrationValid() ? stockStatus(distance) : "UNCALIBRATED");
}

void runCalibration() {
  Serial.println("# CALIBRATION: install a full row of exactly STARTING_PACKAGE_COUNT packages. Send F.");
  while (!Serial.available()) delay(10);
  char input = toupper(Serial.read());
  if (input != 'F') {
    logEvent("CALIBRATION_ABORTED", 0, -1, "EXPECTED_F");
    return;
  }

  float full = medianDistanceMm();
  if (full < 0) {
    logEvent("CALIBRATION_FAILED", 0, full, "FULL_SENSOR_ERROR");
    return;
  }

  Serial.println("# CALIBRATION: remove every package; leave the normal empty belt. Send E.");
  while (!Serial.available()) delay(10);
  input = toupper(Serial.read());
  if (input != 'E') {
    logEvent("CALIBRATION_ABORTED", 0, -1, "EXPECTED_E");
    return;
  }

  float empty = medianDistanceMm();
  if (empty < 0 || empty <= full + 1.0) {
    logEvent("CALIBRATION_FAILED", 0, empty, "EMPTY_READING_INVALID");
    return;
  }

  calibration.magic = EEPROM_MAGIC;
  calibration.fullDistanceMm = full;
  calibration.emptyDistanceMm = empty;
  calibration.expectedIncreaseMm = (empty - full) / STARTING_PACKAGE_COUNT;
  EEPROM.put(0, calibration);
  countEstimate = STARTING_PACKAGE_COUNT;
  logEvent("CALIBRATION_OK", 0, full, "CALIBRATED");
  Serial.print("# full_mm="); Serial.print(full, 1);
  Serial.print(" empty_mm="); Serial.print(empty, 1);
  Serial.print(" expected_increase_mm="); Serial.println(calibration.expectedIncreaseMm, 2);
}

void dispenseOne() {
  if (!calibrationValid()) {
    logEvent("DISPENSE_REJECTED", 0, -1, "CALIBRATE_FIRST");
    return;
  }

  float before = medianDistanceMm();
  if (before < 0) {
    logEvent("DISPENSE_FAILED", 0, before, "SENSOR_ERROR_BEFORE");
    return;
  }
  if (isEmptyDistance(before) || countEstimate <= 0) {
    countEstimate = 0;
    logEvent("DISPENSE_REJECTED", 0, before, "EMPTY");
    return;
  }

  long sentThisCommand = 0;
  for (byte attempt = 0; attempt < 2; attempt++) {
    if (sentThisCommand + STEPS_PER_SPACING > MAX_STEPS_PER_DISPENSE) {
      logEvent("DISPENSE_FAILED", sentThisCommand, before, "MAX_STEP_LIMIT");
      return;
    }

    moveMotor(STEPS_PER_SPACING);
    sentThisCommand += STEPS_PER_SPACING;
    delay(150);

    float after = medianDistanceMm();
    if (after < 0) {
      logEvent("DISPENSE_FAILED", sentThisCommand, after, "SENSOR_ERROR_AFTER");
      return;
    }

    float increase = after - before;
    if (fabs(increase - calibration.expectedIncreaseMm) <= DISTANCE_TOLERANCE_MM) {
      if (countEstimate > 0) countEstimate--;
      logEvent(attempt == 0 ? "DISPENSE_SUCCESS" : "DISPENSE_SUCCESS_RETRY",
               sentThisCommand, after, stockStatus(after));
      Serial.print("# distance_before_mm="); Serial.print(before, 1);
      Serial.print(" actual_increase_mm="); Serial.println(increase, 1);
      return;
    }
  }

  float finalDistance = medianDistanceMm();
  logEvent("DISPENSE_FAILED", sentThisCommand, finalDistance,
           "NO_EXPECTED_DISTANCE_CHANGE");
}

void recordReturn() {
  if (countEstimate < STARTING_PACKAGE_COUNT) countEstimate++;
  float distance = medianDistanceMm();
  logEvent("RETURN_RECORDED", 0, distance,
           calibrationValid() ? stockStatus(distance) : "UNCALIBRATED");
}

void selfTest() {
  float distance = medianDistanceMm();
  logEvent("SELFTEST_SENSOR", 0, distance, distance >= 20.0 ? "PASS" : "FAIL");
  moveMotor(SELF_TEST_STEPS);
  // No encoder exists: confirm actual belt movement with your eyes.
  logEvent("SELFTEST_MOTOR_PULSE", SELF_TEST_STEPS, distance,
           "PASS_VISUALLY_VERIFY_BELT_MOVED");
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT);
  pinMode(IN1_PIN, OUTPUT); pinMode(IN2_PIN, OUTPUT);
  pinMode(IN3_PIN, OUTPUT); pinMode(IN4_PIN, OUTPUT);
  releaseMotor();
  Serial.begin(115200);
  EEPROM.get(0, calibration);
  Serial.println("timestamp_ms,event,steps_commanded,distance_mm,count_estimate,status");
  logEvent("BOOT", 0, -1, calibrationValid() ? "CALIBRATED" : "UNCALIBRATED_SEND_C");
}

void loop() {
  if (!Serial.available()) return;
  char command = toupper(Serial.read());
  if (command == '\n' || command == '\r' || command == ' ') return;

  switch (command) {
    case 'D': dispenseOne(); break;
    case 'R': recordReturn(); break;
    case 'C': runCalibration(); break;
    case 'S': printStatus(); break;
    case 'T': selfTest(); break;
    default: logEvent("UNKNOWN_COMMAND", 0, -1, "USE_D_R_C_S_T"); break;
  }
}