Calibration.cpp

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

Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver board2 = Adafruit_PWMServoDriver(0x60);

// --- ROBOT CONFIGURATION ---
struct ServoJoint {
  int boardNum;
  int channel;
  String name;
};

// List of all servos in order
ServoJoint servos[] = {
  {1, 0, "Elbow-L"},    // Index 0
  {1, 1, "Shoulder-L"}, // Index 1
  {1, 2, "Hip-L"},      // Index 2
  {1, 3, "Knee-L"},     // Index 3
  {1, 4, "Head"},       // Index 4
  {2, 0, "Elbow-R"},    // Index 5
  {2, 1, "Shoulder-R"}, // Index 6
  {2, 2, "Hip-R"},      // Index 7
  {2, 3, "Knee-R"}      // Index 8
};

int totalServos = 9;
int currentIdx = 0;
int currentPulse = 300; // Start at safe midpoint

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

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

  Serial.println("--- RAPID CALIBRATION TOOL ---");
  Serial.println("'w' = +10 Pulse (Move Forward)");
  Serial.println("'s' = -10 Pulse (Move Backward)");
  Serial.println("'n' = Next Servo");
  Serial.println("------------------------------");
  
  printStatus();
  moveServo();
}

void loop() {
  if (Serial.available()) {
    char cmd = Serial.read();

    // Move Forward
    if (cmd == 'w' || cmd == 'W') {
      currentPulse += 5;
      if (currentPulse > 600) currentPulse = 600; // Safety Max
      moveServo();
      Serial.print("Pulse: "); Serial.println(currentPulse);
    }
    
    // Move Backward
    else if (cmd == 's' || cmd == 'S') {
      currentPulse -= 5;
      if (currentPulse < 100) currentPulse = 100; // Safety Min
      moveServo();
      Serial.print("Pulse: "); Serial.println(currentPulse);
    }

    // Next Servo
    else if (cmd == 'n' || cmd == 'N') {
      currentIdx++;
      if (currentIdx >= totalServos) currentIdx = 0; // Loop back to start
      
      currentPulse = 300; // Reset to midpoint for the new servo
      printStatus();
      moveServo();
    }
  }
}

void moveServo() {
  if (servos[currentIdx].boardNum == 1) {
    board1.setPWM(servos[currentIdx].channel, 0, currentPulse);
  } else {
    board2.setPWM(servos[currentIdx].channel, 0, currentPulse);
  }
}

void printStatus() {
  Serial.println("------------------------------");
  Serial.print("CALIBRATING: "); Serial.println(servos[currentIdx].name);
  Serial.print("Board: "); Serial.print(servos[currentIdx].boardNum);
  Serial.print(" | Channel: "); Serial.println(servos[currentIdx].channel);
  Serial.print("Current Pulse: "); Serial.println(currentPulse);
}