Motor_test.cpp

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_NeoPixel.h>

// --- PIN DEFINITIONS ---
#define LED_PIN 4
#define LED_COUNT 40
#define START_BTN_PIN 15  
#define STOP_BTN_PIN 2    
#define HOME_SENSOR_PIN 27 // Sensor at the back

#define RPWM 12         
#define LPWM 14         
#define L_R_EN 13       // Your EN is on G13

// --- TIMING ---
#define PHASE_TIME 15000     
#define HEAD_SNAP_TIME 11000 
#define PAUSE_BEFORE_HOME 3000 // 3 Second Pause

Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver board2 = Adafruit_PWMServoDriver(0x60);

// --- SERVO DATA ---
int sStart1[] = {240, 220, 310, 370, 100}; 
int sEnd1[]   = {150, 300, 250, 300, 550}; 
int sStart2[] = {350, 230, 230, 470}; 
int sEnd2[]   = {490, 310, 300, 350}; 

enum RobotState { IDLE, FORWARD_PHASE, REVERSE_PHASE, HOMING };
RobotState currentState = IDLE;
unsigned long phaseStartTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  
  pinMode(START_BTN_PIN, INPUT_PULLUP);
  pinMode(STOP_BTN_PIN, INPUT_PULLUP);
  pinMode(HOME_SENSOR_PIN, INPUT_PULLUP);
  pinMode(RPWM, OUTPUT);
  pinMode(LPWM, OUTPUT);
  pinMode(L_R_EN, OUTPUT);
  digitalWrite(L_R_EN, HIGH); 

  board1.begin(); board1.setPWMFreq(60);
  board2.begin(); board2.setPWMFreq(60);
  strip.begin(); strip.setBrightness(255); strip.show(); 

  // Initial Home on Power Up
  performHoming();
}

void loop() {
  if (digitalRead(STOP_BTN_PIN) == LOW && currentState != IDLE) {
    Serial.println("STOP PRESSED! Pausing then going home...");
    performHoming();
  }

  if (digitalRead(START_BTN_PIN) == LOW && currentState == IDLE) {
    currentState = FORWARD_PHASE;
    phaseStartTime = millis();
    delay(500); 
  }

  if (currentState == FORWARD_PHASE) runForward();
  else if (currentState == REVERSE_PHASE) runReverse();
}

void runForward() {
  unsigned long elapsed = millis() - phaseStartTime;
  if (elapsed >= PHASE_TIME) {
    currentState = REVERSE_PHASE;
    phaseStartTime = millis();
    return;
  }
  analogWrite(RPWM, 180); analogWrite(LPWM, 0);
  updateCrawl();
  setLEDs(255, 0, 0); // RED
  if (elapsed < HEAD_SNAP_TIME) board1.setPWM(4, 0, sStart1[4]);
  else board1.setPWM(4, 0, sEnd1[4]);
}

void runReverse() {
  unsigned long elapsed = millis() - phaseStartTime;
  if (elapsed >= PHASE_TIME) {
    currentState = FORWARD_PHASE;
    phaseStartTime = millis();
    return;
  }
  analogWrite(RPWM, 0); analogWrite(LPWM, 180);
  updateCrawl();
  setLEDs(255, 0, 0); 
  board1.setPWM(4, 0, sStart1[4]); 
}

// ==========================================
//        UPDATED HOMING WITH 3S PAUSE
// ==========================================
void performHoming() {
  currentState = HOMING;
  
  // 1. Stop Motor and Reset Servos immediately
  analogWrite(RPWM, 0);
  analogWrite(LPWM, 0);
  resetServos();
  Serial.println("Stopping for 3 seconds...");

  // 2. The 3 Second Wait
  // We pulse the LEDs Yellow to show the robot is "Resetting"
  unsigned long pauseStart = millis();
  while(millis() - pauseStart < PAUSE_BEFORE_HOME) {
    setLEDs(255, 150, 0); // Yellow/Orange
    delay(200);
    setLEDs(0, 0, 0);
    delay(200);
  }

  Serial.println("3s Over. Reversing to Home Sensor...");

  // 3. Reverse until the back limit switch (G27) is hit
  while(digitalRead(HOME_SENSOR_PIN) == HIGH) {
    analogWrite(LPWM, 210); // Slightly faster reverse to ensure it reaches
    setLEDs(255, 100, 0);   // Solid Orange
    delay(10);
  }

  // 4. Finished
  analogWrite(LPWM, 0);
  setLEDs(0, 0, 0);
  Serial.println("Robot is HOME.");
  currentState = IDLE;
}

void updateCrawl() {
  float legPhase = (millis() % 1500) / 1500.0;
  float legWave = (sin(legPhase * 2 * PI) + 1) / 2;
  for (int i = 0; i < 4; i++) {
    board1.setPWM(i, 0, sStart1[i] + (legWave * (sEnd1[i] - sStart1[i])));
    board2.setPWM(i, 0, sStart2[i] + ((1.0 - legWave) * (sEnd2[i] - sStart2[i]))); 
  }
}

void resetServos() {
  for(int i=0; i<4; i++) {
    board1.setPWM(i, 0, sStart1[i]);
    board2.setPWM(i, 0, sStart2[i]);
  }
  board1.setPWM(4, 0, sStart1[4]);
}

void setLEDs(int r, int g, int b) {
  for(int i=0; i<LED_COUNT; i++) strip.setPixelColor(i, strip.Color(r, g, b));
  strip.show();
}