#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo servo1, servo2, servo3, servo4, servo5, servo6; // Define servos for each joint
int potPin1 = A0; // Analog pin for potentiometer 1
int potPin2 = A1; // Analog pin for potentiometer 2
int potPin3 = A2; // Analog pin for potentiometer 3
int potPin4 = A3; // Analog pin for potentiometer 4
int potPin5 = A4; // Analog pin for potentiometer 5
int potPin6 = A5; // Analog pin for potentiometer 6

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  delay(2000);
  display.clearDisplay();

  servo1.attach(2); // Attach servo to pin 2
  servo2.attach(3); // Attach servo to pin 3
  servo3.attach(4); // Attach servo to pin 4
  servo4.attach(5); // Attach servo to pin 5
  servo5.attach(6); // Attach servo to pin 6
  servo6.attach(7); // Attach servo to pin 7
}

void loop() {
  int angle1 = map(analogRead(potPin1), 0, 1023, 0, 180); // Read potentiometer 1 angle
  int angle2 = map(analogRead(potPin2), 0, 1023, 0, 180); // Read potentiometer 2 angle
  int angle3 = map(analogRead(potPin3), 0, 1023, 0, 180); // Read potentiometer 3 angle
  int angle4 = map(analogRead(potPin4), 0, 1023, 0, 180); // Read potentiometer 4 angle
  int angle5 = map(analogRead(potPin5), 0, 1023, 0, 180); // Read potentiometer 5 angle
  int angle6 = map(analogRead(potPin6), 0, 1023, 0, 180); // Read potentiometer 6 angle

  servo1.write(angle1); // Move servo 1 to specified angle
  servo2.write(angle2); // Move servo 2 to specified angle
  servo3.write(angle3); // Move servo 3 to specified angle
  servo4.write(angle4); // Move servo 4 to specified angle
  servo5.write(angle5); // Move servo 5 to specified angle
  servo6.write(angle6); // Move servo 6 to specified angle

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.print("Joint 1: ");
  display.println(angle1);
  display.print("Joint 2: ");
  display.println(angle2);
  display.print("Joint 3: ");
  display.println(angle3);
  display.print("Joint 4: ");
  display.println(angle4);
  display.print("Joint 5: ");
  display.println(angle5);
  display.print("Joint 6: ");
  display.println(angle6);
  display.display();
  delay(100);
}