// STM32 Nucleo-C031C6 I2C Example
// Simulation: https://wokwi.com/projects/365421666018061313

#include <ServoSmooth.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

#define AngleX A0
#define AngleY A1

ServoSmooth servos[3];

int AngleX_got = 0, AngleY_got = 0, servo2 = 0, servo3 = 0, Angle_X, Angle_Y, Joy_X, Joy_Y;
const int AngleX_c = 511, AngleY_c = 511, servo_avg = 1167;

void setup() {
  pinMode(AngleX,INPUT_ANALOG);
  pinMode(AngleY,INPUT_ANALOG);
  /*Серво мотор 1*/
  servos[0].attach(3,500,2500); // Задаём параметры серво
  servos[0].setMaxAngle(270);   // Максимальный угол
  servos[0].setAccel(0.8);      // Ставим ускорение (плавное но меньше)
  servos[0].setSpeed(81);      // Ставим скорость (в соответсвии документации на серво)
  servos[0].setTarget(servo_avg);    // Ставим сброс на ось X(1166 == 90)
  /*Серво мотор 2*/
  servos[1].attach(5,500,2500);
  servos[1].setMaxAngle(270);
  servos[1].setAccel(0.8);
  servos[1].setSpeed(81);
  servos[1].setTarget(servo_avg);     // Ставим сброс на ось Y
  /*Серво мотор 3*/
  servos[2].attach(6,500,2500);
  servos[2].setMaxAngle(270);
  servos[2].setAccel(0.8);
  servos[2].setSpeed(81);
  servos[2].setTarget(servo_avg);     // Ставим сброс на ось Y

}

void loop() {
    static uint32_t tim = 0;
  if (millis() - tim >= 100)
  {
    if ((analogRead(AngleX) != AngleX_c) || (analogRead(AngleY) != AngleY_c))
    {
      AngleX_got = map(analogRead(AngleX),0,1023,833,1500);
      AngleY_got = map(analogRead(AngleY),0,1023,500,1167);
      Joy_X = map(analogRead(AngleX),0,1023,0,180);
      Joy_Y = map(analogRead(AngleY),0,1023,0,180);
      /* code */
      servos[0].setTarget(AngleX_got);
      servo2 = (servo_avg-AngleY_got>=500)?servo_avg-AngleY_got:500;
      servo3 = (servo_avg+AngleY_got<=1833)?servo_avg+AngleY_got:1833;
      servos[1].setTarget(servo2);
      servos[2].setTarget(servo3);
    }
    tim = (tim < UINT32_MAX)?millis():0;
  }
}