Jarvis AI Console

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVO_MIN  150
#define SERVO_MAX  600
#define NUM_JOINTS 6

int currentPos[NUM_JOINTS] = {90, 90, 90, 90, 90, 90};

void setup() {
  Serial.begin(115200);
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(50);
  Serial.println("HyTechAI Robotic Arm v2.1 Ready");
  homePosition();
}

void setServoAngle(int joint, int angle) {
  angle = constrain(angle, 0, 180);
  int pulse = map(angle, 0, 180, SERVO_MIN, SERVO_MAX);
  pwm.setPWM(joint, 0, pulse);
  currentPos[joint] = angle;
}

void smoothMove(int joint, int target, int speed) {
  int step = (target > currentPos[joint]) ? 1 : -1;
  while (currentPos[joint] != target) {
    currentPos[joint] += step;
    setServoAngle(joint, currentPos[joint]);
    delay(speed);
  }
}