Arm structure

#include <Servo.h>

// Joystick
const int JOY_X  = 26;
const int JOY_Y  = 27;
const int JOY_SW = 6;

// Touch senzory
const int TOUCH_1 = 7;   // dopredu/dozadu
const int TOUCH_2 = 8;   // dopredu/dozadu (opačný smer)

// Servá
#define SERVO_ROTACIA   12  // 360° – otáčanie doľava/doprava
#define SERVO_HORE_DOLE 13  // 180° – pohyb nahor/nadol
#define SERVO_DOPREDU   14  // 180° – pohyb dopredu/dozadu
#define SERVO_KLIESTE   15  // 180° – otvoriť/zatvoriť kliešte

Servo servoRotacia;
Servo servoHoreDole;
Servo servoDopredu;
Servo servoKlieste;

// Aktuálne pozície 180° serv (0–180°)
int posHoreDole = 90;
int posDopredu  = 90;
int posKlieste  = 90;  // 90 = zatvorené, 0 = otvorené

// Citlivosť joysticku – pásmo okolo stredu kde sa nič nedeje
const int DEAD_ZONE = 1000;  // stred je ~2048, ignorujeme 2048±1000, chceme okraje
const int SPEED = 2;        // o koľko stupňov sa servo pohne pri každom kroku

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);

  pinMode(JOY_SW,  INPUT_PULLUP);
  pinMode(TOUCH_1, INPUT);
  pinMode(TOUCH_2, INPUT);

  servoRotacia.attach(SERVO_ROTACIA,   500, 2500);
  servoHoreDole.attach(SERVO_HORE_DOLE, 500, 2500);
  servoDopredu.attach(SERVO_DOPREDU,   500, 2500);
  servoKlieste.attach(SERVO_KLIESTE,   500, 2500);

  // Nastav všetky servá do stredu
  servoRotacia.write(90);
  servoHoreDole.write(posHoreDole);
  servoDopredu.write(posDopredu);
  servoKlieste.write(posKlieste);
}

// Obmedzí hodnotu na rozsah min–max
int clamp(int val, int minVal, int maxVal) {
  if (val < minVal) return minVal;
  if (val > maxVal) return maxVal;
  return val;
}

void loop() {
  int x  = analogRead(JOY_X);
  int y  = analogRead(JOY_Y);
  int sw = digitalRead(JOY_SW);
  int t1 = digitalRead(TOUCH_1);
  int t2 = digitalRead(TOUCH_2);

  // 1. Otáčanie doľava/doprava – 360° servo
  // Stred (2048) = stoj, pod stred = jedna strana, nad stred = druhá strana
  if (x < 2048 - DEAD_ZONE) {
    // --- doplňte: otáčanie doľava ---
    // Nápoveda: servoRotacia.write(hodnota pod 90)
  } else if (x > 2048 + DEAD_ZONE) {
    // --- doplňte: otáčanie doprava ---
  } else {
    servoRotacia.write(90);  // stoj
  }

  // 2. Pohyb nahor/nadol – os Y joysticku
  if (y < 2048 - DEAD_ZONE) {
    posHoreDole = clamp(posHoreDole - SPEED, 0, 180);
  } else if (y > 2048 + DEAD_ZONE) {
    posHoreDole = clamp(posHoreDole + SPEED, 0, 180);
  }
  servoHoreDole.write(posHoreDole);

  // 3. Pohyb dopredu/dozadu – touch senzory
  if (t1 == HIGH) {
    posDopredu = clamp(posDopredu + SPEED, 0, 180);
  } else if (t2 == HIGH) {
    posDopredu = clamp(posDopredu - SPEED, 0, 180);
  }
  servoDopredu.write(posDopredu);

  // 4. Kliešte – stlačenie joysticku
  if (sw == LOW) {
    // --- doplňte: prepínanie otvorené/zatvorené ---
    // Nápoveda: použite bool premennú na zapamätanie stavu
    // a debouncing – pridajte delay alebo kontrolu zmeny stavu
  }
  servoKlieste.write(posKlieste);

  Serial.print("X:"); Serial.print(x);
  Serial.print(" Y:"); Serial.print(y);
  Serial.print(" HoreDole:"); Serial.print(posHoreDole);
  Serial.print(" Dopredu:"); Serial.print(posDopredu);
  Serial.print(" Klieste:"); Serial.println(posKlieste);

  delay(20);
}