FinalCodeInternshipArduino.ino

#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Adafruit_DotStar.h>
#include <SPI.h>

// ---------------------------------------------------------
// CONFIG - edit these to match hardware
// ---------------------------------------------------------

const int X_STEP_PIN   = 7;
const int X_DIR_PIN     = 8;
const int Y_STEP_PIN    = 10;
const int Y_DIR_PIN     = 12;
const int ENABLE_PIN    = 6;   // shared enable, active LOW on DRV8825

// E-STOP CONFIG
const int ESTOP_PIN     = 2;   // MUST be Pin 2 or 3 on Uno/Nano for interrupts
volatile bool isEstoppped = false; // volatile because it changes inside an interrupt

const bool X_INVERT = false;
const bool Y_INVERT = false;

// Trigger Button (Starts the pattern sequence)
const int BUTTON_PIN = 4; 
const unsigned long DEBOUNCE_DELAY_MS = 50;
int lastButtonReading = HIGH;
int buttonState = HIGH;
unsigned long lastDebounceTime = 0;

const float MOTOR_STEPS_PER_REV = 200.0;
const float MICROSTEPS          = 16.0;
const float X_MM_PER_REV        = 40.0;
const float Y_MM_PER_REV        = 40.0;

const float X_STEPS_PER_MM = (MOTOR_STEPS_PER_REV * MICROSTEPS) / X_MM_PER_REV;
const float Y_STEPS_PER_MM = (MOTOR_STEPS_PER_REV * MICROSTEPS) / Y_MM_PER_REV;

const float MAX_SPEED_MM_S = 15.0;
const float ACCEL_MM_S2    = 30.0;

const int MATRIX_WIDTH  = 8;
const int MATRIX_HEIGHT = 8;
const int NUM_PIXELS    = MATRIX_WIDTH * MATRIX_HEIGHT;
const uint8_t MATRIX_BRIGHTNESS = 40;

Adafruit_DotStar matrix(NUM_PIXELS, DOTSTAR_BRG);

AccelStepper motorX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper motorY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
MultiStepper steppersXY;

// Forward declarations
void collectsample();
void drawZigZag(float startX, float startY, float totalWidth, float totalHeight, int steps);
void parseAndExecute(String cmd);
float getFloatArg(String cmd, char key);
void fillMatrix(uint8_t r, uint8_t g, uint8_t b);
void clearMatrix();
void drawRectangle(float x, float y, float width, float height);
void moveTo(float xMM, float yMM);
void emergencyStop();
bool checkEmergencyState();
void Pause(int ms);
void enableDrivers(bool enable);

// ---------------------------------------------------------
// SETUP
// ---------------------------------------------------------

void setup() {
  Serial.begin(9600);
  
  pinMode(ENABLE_PIN, OUTPUT);
  enableDrivers(false); // Start with motor drivers DISABLED on boot

  // Configure E-Stop pin
  pinMode(ESTOP_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), emergencyStop, FALLING);

  // Configure the hardware trigger button
  pinMode(BUTTON_PIN, INPUT_PULLUP); 

  motorX.setPinsInverted(X_INVERT, false, false);
  motorY.setPinsInverted(Y_INVERT, false, false);

  motorX.setMaxSpeed(MAX_SPEED_MM_S * X_STEPS_PER_MM);
  motorX.setAcceleration(ACCEL_MM_S2 * X_STEPS_PER_MM);
  motorY.setMaxSpeed(MAX_SPEED_MM_S * Y_STEPS_PER_MM);
  motorY.setAcceleration(ACCEL_MM_S2 * Y_STEPS_PER_MM);

  steppersXY.addStepper(motorX);
  steppersXY.addStepper(motorY);

  motorX.setCurrentPosition(0);
  motorY.setCurrentPosition(0);

  matrix.begin();
  matrix.setBrightness(MATRIX_BRIGHTNESS);
  matrix.clear();
  matrix.show();

  Serial.println("System Ready (Motors are OFF).");
  Serial.println("- Press Pin 4 button or type 'S' to run the 1cm sweep.");
  Serial.println("- Type 'E' to E-STOP.");
}

// ---------------------------------------------------------
// MAIN LOOP
// ---------------------------------------------------------

void loop() {
  // 1. SERIAL COMMAND CHECK
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    if (input.length() > 0) {
      if (input.equalsIgnoreCase("E")) {
        emergencyStop();
        checkEmergencyState();
      } 
      else if (input.equalsIgnoreCase("RESET")) {
        isEstoppped = false;
        enableDrivers(false); // Ensure they stay off after a reset
        clearMatrix();
        Serial.println("System Reset. Ready for next command.");
      } 
      else if (!isEstoppped) {
        parseAndExecute(input);
      } else {
        checkEmergencyState();
      }
    }
  }

  // 2. HARDWARE TRIGGER BUTTON CHECK (Runs 1cm sweep)
  int reading = digitalRead(BUTTON_PIN);

  if (reading != lastButtonReading) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY_MS) {
    if (reading != buttonState) {
      buttonState = reading;

      // Active LOW: Button is physically pressed down
      if (buttonState == LOW) {
        if (!isEstoppped) {
          Serial.println("Button Pressed: Triggering automated sequence.");
          enableDrivers(true);  // Turn ON for the whole sequence
          collectsample();
          enableDrivers(false); // Turn OFF when completely done
        } else {
          checkEmergencyState();
        }
      }
    }
  }
  lastButtonReading = reading;
}

// ---------------------------------------------------------
// POWER CONTROL HELPER
// ---------------------------------------------------------

void enableDrivers(bool enable) {
  // Active LOW: LOW enables the motors, HIGH disables them completely
  digitalWrite(ENABLE_PIN, enable ? LOW : HIGH);
}

// ---------------------------------------------------------
// COMMAND PARSER
// ---------------------------------------------------------

void parseAndExecute(String cmd) {
  char commandType = cmd.charAt(0);
  
  switch (commandType) {
    case 'S': case 's': {
      enableDrivers(true);
      collectsample();
      enableDrivers(false);
      break;
    }
    case 'Z': case 'z': {
      float zx = getFloatArg(cmd, 'X');
      float zy = getFloatArg(cmd, 'Y');
      float zw = getFloatArg(cmd, 'W');
      float zh = getFloatArg(cmd, 'H');
      int steps = (int)getFloatArg(cmd, 'S');
      if (steps <= 0) steps = 4; 

      Serial.print("Drawing Custom ZigZag with "); Serial.print(steps); Serial.println(" steps...");
      enableDrivers(true);
      drawZigZag(zx, zy, zw, zh, steps);
      enableDrivers(false);
      break;
    }
    case 'G': case 'g': {
      float targetX = getFloatArg(cmd, 'X');
      float targetY = getFloatArg(cmd, 'Y');
      Serial.print("Moving to X: "); Serial.print(targetX); Serial.print(" Y: "); Serial.println(targetY);
      enableDrivers(true);
      moveTo(targetX, targetY);
      enableDrivers(false);
      break;
    }
    case 'R': case 'r': {
      float rx = getFloatArg(cmd, 'X');
      float ry = getFloatArg(cmd, 'Y');
      float rw = getFloatArg(cmd, 'W');
      float rh = getFloatArg(cmd, 'H');
      enableDrivers(true);
      drawRectangle(rx, ry, rw, rh);
      enableDrivers(false);
      break;
    }
    case 'C': case 'c': {
      int r = (int)getFloatArg(cmd, 'R');
      int g = (int)getFloatArg(cmd, 'G');
      int b = (int)getFloatArg(cmd, 'B');
      fillMatrix(r, g, b);
      break;
    }
    case 'O': case 'o': {
      clearMatrix();
      break;
    }
    default:
      Serial.print("Unknown command: ");
      Serial.println(cmd);
      break;
  }
}

float getFloatArg(String cmd, char key) {
  int index = cmd.indexOf(key);
  if (index == -1) return 0.0;
  String valueStr = "";
  for (int i = index + 1; i < cmd.length(); i++) {
    char c = cmd.charAt(i);
    if (c == ' ' || c == '\r' || c == '\n') break;
    valueStr += c;
  }
  return valueStr.toFloat();
}

// ---------------------------------------------------------
// MOTION & EMERGENCY CONTROLS
// ---------------------------------------------------------

void emergencyStop() {
  // 2ms hardware debounce filter to ignore motor noise spikes
  delayMicroseconds(2000); 
  
  if (digitalRead(ESTOP_PIN) == LOW) {
    isEstoppped = true;
    enableDrivers(false); // Disables drivers immediately
  }
}

bool checkEmergencyState() {
  if (isEstoppped) {
    Serial.println("!!! EMERGENCY STOP ACTIVE !!! Motors disabled. Type 'RESET' to recover.");
    fillMatrix(255, 0, 0); // Turn matrix red as warning
    return true;
  }
  return false;
}

void moveTo(float xMM, float yMM) {
  if (checkEmergencyState()) return;

  long targetXSteps = round(xMM * X_STEPS_PER_MM);
  long targetYSteps = round(yMM * Y_STEPS_PER_MM);

  long positions[2] = {targetXSteps, targetYSteps};
  steppersXY.moveTo(positions);

  while (steppersXY.run() && !isEstoppped) {
    if (Serial.available() > 0) {
      char c = Serial.read();
      if (c == 'E' || c == 'e') {
        emergencyStop();
      }
    }
  }
  checkEmergencyState();
}

void drawRectangle(float x, float y, float width, float height) {
  moveTo(x, y);                  if(isEstoppped) return;
  moveTo(x + width, y);          if(isEstoppped) return;
  moveTo(x + width, y + height); if(isEstoppped) return;
  moveTo(x, y + height);         if(isEstoppped) return;
  moveTo(x, y);
}

void Pause(int ms) {
  unsigned long start = millis();
  while(millis() - start < ms) {
    if (isEstoppped) return;
  }
}

// ---------------------------------------------------------
// LED MATRIX FUNCTIONS
// ---------------------------------------------------------

void fillMatrix(uint8_t r, uint8_t g, uint8_t b) {
  for (int i = 0; i < NUM_PIXELS; i++) {
    matrix.setPixelColor(i, r, g, b);
  }
  matrix.show();
}

void clearMatrix() {
  matrix.clear();
  matrix.show();
}

// ---------------------------------------------------------
// CUSTOM SEQUENCES
// ---------------------------------------------------------

void collectsample() {
  Serial.println("Starting 1cm ZigZag sampling sweep...");
  
  // 10mm x 10mm (1cm x 1cm) coverage span, 4 sweep steps
  drawZigZag(0, 0, 10, 10, 4);
  if(isEstoppped) return;

  // Return back to the designated home position automatically
  Serial.println("Pattern complete. Returning to home (0,0)...");
  moveTo(0, 0);
  if(isEstoppped) return;

  // Flash matrix sequence on completion
  fillMatrix(225, 0, 0);   Pause(200);
  fillMatrix(0, 225, 0);   Pause(200);
  fillMatrix(0, 0, 225);   Pause(200);
  clearMatrix();
  
  Serial.println("Sample collection routine complete. Motors Powered Off.");
}

void drawZigZag(float startX, float startY, float totalWidth, float totalHeight, int steps) {
  if (checkEmergencyState() || steps <= 0) return;

  float stepHeight = totalHeight / steps;
  float currentY = startY;

  // Move to the initial starting position
  moveTo(startX, startY);

  for (int i = 0; i < steps; i++) {
    currentY += (stepHeight / 2.0);
    moveTo(startX + totalWidth, currentY);
    if (isEstoppped) return;

    currentY += (stepHeight / 2.0);
    moveTo(startX, currentY);
    if (isEstoppped) return;
  }
}