Compartilhamento de Código (Braco robótico MARK1)

Fiz uma estratégia diferente para movimentar o braço robótico.

O modelo que comprei foi o MARK 1 do vendedor MAKERS, do mercado livre. ( https://produto.mercadolivre.com.br/MLB-1315393322-braco-robotico-mark1-parafusos-_JM)

Em síntese o código não trabalha com o mapeamento direto entre o valor do joystick para o angulo do motor, o joystick gera um incremento ou decremento no angulo atual do motor gerando o movimento.

Segue o código caso alguém tenha interesse.

#include <Servo.h>

// --- ANGULOS ---
#define ANGULO_INICIAL_SERVO_BASE 90
#define ANGULO_INICIAL_SERVO_ALTURA_GARRA 145
#define ANGULO_INICIAL_SERVO_FLEX_EX_GARRA 90
#define ANGULO_INICIAL_SERVO_GARRA 0

// --- PINOS SERVO ---
#define PINO_SERVO_BASE 5
#define PINO_SERVO_ALTURA_GARRA 6
#define PINO_SERVO_FLEX_EX_GARRA 9
#define PINO_SERVO_GARRA 10

// --- Pinos Joysticks ---
#define JOYSTICK_1_X A0
#define JOYSTICK_1_Y A1

#define JOYSTICK_2_X A2
#define JOYSTICK_2_Y A3

// --- Servos ---
Servo servoBase;
Servo servoAlturaGarra;
Servo servoFlexaoExtensaoGarra;
Servo servoGarra;

int anguloServoBase;
int anguloServoAlturaGarra;
int anguloServoFlexaoExtensaoGarra;
int anguloServoGarra;

void setup() {
  mapearPinosJoysticks();
  mapearPinosServos();
  configurarAnguloInicialServos();

  Serial.begin(9600);
}

void loop() {

  moverBase();
  moverAlturaGarra();
  moverFlexaoExtensaoGarra();
  moverGarra();

  delay(50);
}

void configurarAnguloInicialServos() {

  anguloServoBase = ANGULO_INICIAL_SERVO_BASE;
  anguloServoAlturaGarra = ANGULO_INICIAL_SERVO_ALTURA_GARRA;
  anguloServoFlexaoExtensaoGarra = ANGULO_INICIAL_SERVO_FLEX_EX_GARRA;
  anguloServoGarra = ANGULO_INICIAL_SERVO_GARRA;

  servoBase.write(anguloServoBase);
  servoAlturaGarra.write(anguloServoAlturaGarra);
  servoFlexaoExtensaoGarra.write(anguloServoFlexaoExtensaoGarra);
  servoGarra.write(anguloServoGarra);
}

void mapearPinosServos(){
  servoBase.attach(PINO_SERVO_BASE);
  servoAlturaGarra.attach(PINO_SERVO_ALTURA_GARRA);
  servoFlexaoExtensaoGarra.attach(PINO_SERVO_FLEX_EX_GARRA);
  servoGarra.attach(PINO_SERVO_GARRA);
}

void mapearPinosJoysticks() {
  pinMode(JOYSTICK_1_X, INPUT);
  pinMode(JOYSTICK_1_Y, INPUT);

  pinMode(JOYSTICK_2_X, INPUT);
  pinMode(JOYSTICK_2_Y, INPUT);
}

void moverBase(){
  const int anguloMinimo = 0;
  const int anguloMaximo = 180;

  int leituraAnalogica = analogRead(JOYSTICK_1_Y);
  int incremento = obterIncremento(leituraAnalogica);
  anguloServoBase = obterNovoAngulo(anguloServoBase, incremento, anguloMinimo, anguloMaximo);
  servoBase.write(anguloServoBase);
}

void moverAlturaGarra(){
  const int anguloMinimo = 110;
  const int anguloMaximo = 140;

  int leituraAnalogica = analogRead(JOYSTICK_1_X);
  int incremento = obterIncremento(leituraAnalogica);
  anguloServoAlturaGarra = obterNovoAngulo(anguloServoAlturaGarra, incremento, anguloMinimo, anguloMaximo);
  servoAlturaGarra.write(anguloServoAlturaGarra);
}

void moverFlexaoExtensaoGarra(){
  const int anguloMinimo = 70;
  const int anguloMaximo = 140;

  int leituraAnalogica = analogRead(JOYSTICK_2_X);
  int incremento = obterIncremento(leituraAnalogica);
  anguloServoFlexaoExtensaoGarra = obterNovoAngulo(anguloServoFlexaoExtensaoGarra, incremento, anguloMinimo, anguloMaximo);
  servoFlexaoExtensaoGarra.write(anguloServoFlexaoExtensaoGarra);  
}
void moverGarra(){
  const int anguloMinimo = 0;
  const int anguloMaximo = 90;

  int leituraAnalogica = analogRead(JOYSTICK_2_Y);
  int incremento = obterIncremento(leituraAnalogica);
  anguloServoGarra = obterNovoAngulo(anguloServoGarra, incremento, anguloMinimo, anguloMaximo);
  servoGarra.write(anguloServoGarra);  
}

int obterIncremento(int leituraAnalogica){
  const int limiteSuperiorNaoLeitura = 563;
  const int limiteInferiorNaoLeitura = 460;

  const int limiteSuperiorTransicao = 793;
  const int limiteInferiorTransicao = 230;

  const int taxaNormal = 1;
  const int taxaRapida = 3;

  int taxaRetornada = 0;
  if ( leituraAnalogica < limiteInferiorTransicao )
    taxaRetornada = -taxaRapida;
  else if (leituraAnalogica < limiteInferiorNaoLeitura)
    taxaRetornada = -taxaNormal;
  else if (leituraAnalogica > limiteSuperiorTransicao)
    taxaRetornada = taxaRapida;
  else if (leituraAnalogica > limiteSuperiorNaoLeitura)
    taxaRetornada = taxaNormal;

  return taxaRetornada;
}

int obterNovoAngulo(int anguloAtual, int incremento, int anguloMinimo, int anguloMaximo) {

  int anguloNovo = anguloAtual + incremento;
  if(anguloNovo > anguloMaximo)
    anguloNovo = anguloMaximo;
  else if(anguloNovo < anguloMinimo)
    anguloNovo = anguloMinimo;

  return anguloNovo;
}