/*
  Arduino | coding-help
  Drawing robot
  Edward. — 6/12/24 at 7:17 AM
  it should make a triangle but it doesn't work, why?
*/

#include <Servo.h>

Servo servo1;  // Servo pentru baza
Servo servo2;  // Servo pentru braț

// Coordonate pentru desen
int coordinates[][2] = {
  {90, 348}, {96, 338}, {103, 329}, {109, 319}, {116, 309}, {122, 298},
  {129, 288}, {136, 278}, {142, 267}, {149, 257}, {155, 247}, {159, 242},
  {162, 251}, {166, 261}, {170, 270}, {174, 281}, {178, 290}, {182, 301},
  {186, 312}, {190, 323}, {195, 334}, {198, 342}, {200, 348}, {193, 346},
  {185, 345}, {176, 346}, {167, 345}, {156, 345}, {145, 346}, {135, 345},
  {124, 346}, {115, 345}, {107, 345}, {98, 345}
};

int numCoordinates = sizeof(coordinates) / sizeof(coordinates[0]);

void setup() {
  servo1.attach(9);  // Atașați servo1 la pinul 9
  servo2.attach(10); // Atașați servo2 la pinul 10

  // Opțional: Inițializați servomotoarele la poziția de start
  servo1.write(90);  // Valoare inițială pentru testare
  servo2.write(90);  // Valoare inițială pentru testare
  delay(1000);  // Așteptați ca servomotoarele să ajungă la poziția inițială
}

void loop() {
  for (int i = 0; i < numCoordinates; i++) {
    int x = coordinates[i][0];
    int y = coordinates[i][1];

    // Convertiți (x, y) în unghiuri pentru servomotoare
    int angle1 = mapXToServo1(x, y);
    int angle2 = mapYToServo2(x, y);

    // Mutați servomotoarele la unghiurile calculate
    servo1.write(angle1);
    servo2.write(angle2);

    // Așteptați ca servomotoarele să ajungă la poziția dorită
    delay(500);
  }

  // Opțional: Lăsați codul să se oprească după desenare
  while (true);
}

int mapXToServo1(int x, int y) {
  // Maparea coordonatelor x în unghiuri pentru servo1
  return map(x, 90, 200, 0, 90);  // Mapare între 50 și 110 grade
}

int mapYToServo2(int x, int y) {
  // Maparea coordonatelor y în unghiuri pentru servo2
  return map(y, 242, 348, 0, 90);  // Mapare între 50 și 110 grade
}