17-DOF Humanoid Robot: Bipedal Kinematics & Teleoperation

#include <WiFi.h>
#include <WebServer.h>
#include <PS2X_lib.h> // Library for PS2 Controller communication

// --- Hardware Pins ---
#define PS2_DAT 19
#define PS2_CMD 23
#define PS2_SEL 5
#define PS2_CLK 18

// --- Wi-Fi Configuration ---
const char* ssid = "HUMANOID_NETWORK";
const char* password = "ROBOTICS_SECURE";
WebServer server(80);

// --- PS2 Controller Object ---
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;

// --- UART Setup for Servo Board ---
// Assuming Servo Controller uses Serial2 (RX: 16, TX: 17)
#define SERVO_SERIAL Serial2

void setup() {
  Serial.begin(115200);
  SERVO_SERIAL.begin(9600); // Baud rate required by the 18-ch servo board

  // 1. Initialize PS2 Controller
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, true, true);
  if (error == 0) {
    Serial.println("[SYSTEM] PS2 Controller Configured Successfully.");
  }

  // 2. Initialize Wi-Fi Access Point
  WiFi.softAP(ssid, password);
  Serial.print("[SYSTEM] Wi-Fi AP Started. IP: ");
  Serial.println(WiFi.softAPIP());

  // 3. Setup Web Server Endpoints
  server.on("/walk", HTTP_GET, []() {
    executeActionGroup(1); // Call action group 1 stored on the servo board
    server.send(200, "text/plain", "Walking Forward");
  });
  server.on("/stop", HTTP_GET, []() {
    executeActionGroup(0); // Call default standing pose
    server.send(200, "text/plain", "Stopping");
  });
  server.begin();
}

void loop() {
  // --- Handle Wi-Fi Commands ---
  server.handleClient();

  // --- Handle PS2 Joystick Commands ---
  if (error == 0) { // Only read if controller is connected
    ps2x.read_gamepad(false, vibrate);

    // Read D-Pad for discrete movements
    if (ps2x.Button(PSB_PAD_UP)) {
      Serial.println("[PS2] Forward Command");
      executeActionGroup(1); // Walk forward
      delay(500); // Prevent command spamming
    }
    else if (ps2x.Button(PSB_PAD_DOWN)) {
      Serial.println("[PS2] Backward Command");
      executeActionGroup(2); // Walk backward
      delay(500);
    }
    else if (ps2x.Button(PSB_CROSS)) {
      Serial.println("[PS2] Stop Command");
      executeActionGroup(0); // Default stance
      delay(500);
    }

    // Example of direct servo control using analog sticks (e.g., Head Pan)
    int headPanAnalog = ps2x.Analog(PSS_RX); // Read Right Stick X-axis (0-255)
    if (headPanAnalog > 150 || headPanAnalog < 100) {
      // Map 0-255 to Servo PWM range (500-2500)
      int pwmValue = map(headPanAnalog, 0, 255, 500, 2500);
      moveSingleServo(16, pwmValue, 500); // Move Head Servo (ID 16) in 500ms
    }
  }
  delay(50);
}

// --- Servo Board Communication Protocol ---
// Function to call pre-programmed action groups on the 18-channel board
// Protocol format: "#<ActionGroupID>G"
void executeActionGroup(int groupNumber) {
  if (groupNumber == 0) {
    SERVO_SERIAL.println("#000G"); // Example command for reset/stop
  } else if (groupNumber == 1) {
    SERVO_SERIAL.println("#001G"); // Example command for walk forward
  } else if (groupNumber == 2) {
    SERVO_SERIAL.println("#002G"); // Example command for walk backward
  }
}

// Function to move a specific servo directly
// Protocol format: "#<ServoID>P<PWMValue>T<TimeMS>"
void moveSingleServo(int servoID, int pwm, int timeMS) {
  String command = "#" + String(servoID) + "P" + String(pwm) + "T" + String(timeMS);
  SERVO_SERIAL.println(command);
  Serial.println("[SERVO] Sent: " + command);
}