Crawling.cpp

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

// --- SETTINGS ---
#define LED_PIN 4
#define LED_COUNT 40
#define TRIGGER_PIN 15
#define RUN_TIME 15000       // 15 Seconds Total
#define HEAD_SNAP_TIME 11000 // Head moves at 11 Seconds

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

// --- NEW CALIBRATION DATA (From Logbook 1) ---
// Structure: [0]Elbow, [1]Shoulder, [2]Hip, [3]Knee, [4]Head
// Note: We map 'From' to 'sStart' and 'To' to 'sEnd' for clarity

// Board 1 (Left Side + Head)
int sStart1[] = {240, 220, 310, 370, 100}; // From Column
int sEnd1[]   = {150, 300, 250, 300, 550}; // To Column

// Board 2 (Right Side)
int sStart2[] = {350, 230, 230, 470}; // From Column
int sEnd2[]   = {490, 310, 300, 350}; // To Column

// --- STATE VARIABLES ---
bool isRunning = false;
unsigned long startTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  pinMode(TRIGGER_PIN, INPUT_PULLUP);

  board1.begin(); board1.setPWMFreq(60);
  board2.begin(); board2.setPWMFreq(60);

  strip.begin();
  strip.setBrightness(200); // High brightness
  strip.show(); 
  
  Serial.println("System Updated. Waiting for Trigger (G15)...");
}

void loop() {
  // 1. Check Trigger (Active LOW)
  if (digitalRead(TRIGGER_PIN) == LOW && !isRunning) {
    isRunning = true;
    startTime = millis();
    delay(200); // Debounce
  }

  // 2. Main Logic Loop
  if (isRunning) {
    unsigned long elapsed = millis() - startTime;
    
    if (elapsed < RUN_TIME) {
      updateCrawl(elapsed);   
      updateLEDs(true);       
    } else {
      // Time is up
      isRunning = false;
      resetRobot();           
      updateLEDs(false); 
      Serial.println("Sequence Complete. Resetting.");
    }
  } else {
    updateLEDs(false); // Standby mode
  }
}

// --- MOVEMENT ENGINE ---
void updateCrawl(unsigned long elapsed) {
  // A. Legs Simultaneous Movement (Sine Wave)
  float legPhase = (millis() % 1500) / 1500.0; // 1.5 second cycle
  float legWave = (sin(legPhase * 2 * PI) + 1) / 2;

  for (int i = 0; i < 4; i++) {
    // Interpolate between Start and End positions based on the wave
    board1.setPWM(i, 0, sStart1[i] + (legWave * (sEnd1[i] - sStart1[i])));
    board2.setPWM(i, 0, sStart2[i] + ((1.0 - legWave) * (sEnd2[i] - sStart2[i]))); 
  }

  // B. Head Logic (Snap at 11s)
  if (elapsed < HEAD_SNAP_TIME) {
    board1.setPWM(4, 0, sStart1[4]); // Hold at 100
  } else {
    board1.setPWM(4, 0, sEnd1[4]);   // Snap to 550 and hold
  }
}

void resetRobot() {
  // Return all legs to midpoints
  for(int i=0; i<4; i++) {
    board1.setPWM(i, 0, (sStart1[i] + sEnd1[i])/2);
    board2.setPWM(i, 0, (sStart2[i] + sEnd2[i])/2);
  }
  // Return Head to Start Position (100)
  board1.setPWM(4, 0, sStart1[4]);
}

// --- LED ENGINE ---
void updateLEDs(bool isActive) {
  if (isActive) {
    // 1. First 34 LEDs: Rapid "Glitch" Flickering Red
    for (int i = 0; i < 34; i++) {
      // Randomly turn off pixels to create a spark effect
      int brightness = (random(0, 10) > 6) ? random(100, 255) : 0; 
      strip.setPixelColor(i, strip.Color(brightness, 0, 0));
    }
    // 2. Last 6 LEDs: Static Solid Red (Tail lights)
    for (int i = 34; i < LED_COUNT; i++) {
      strip.setPixelColor(i, strip.Color(255, 0, 0));
    }
  } else {
    // Standby: Very Dim Red "Breathing"
    int breathe = (sin(millis()/500.0) * 10) + 15;
    for (int i = 0; i < LED_COUNT; i++) strip.setPixelColor(i, strip.Color(breathe, 0, 0));
  }
  strip.show(); 
}