Arduino con AccelStepper, CNCShield y bluetooth HC-06

Estos últimos meses estuve trabajando con un Arduino uno para mover unos motores enviando información desde Android a través de bluetooth.
Me encontré con muchas dificultades, por ejemplo, que los tutoriales que conseguís por internet suponen que uno tiene cierta idea de lo que hace, que no es mi caso, así que aquí vengo a compartir mi experiencia y el producto terminado.

(el modulo bluetooth esta enchufado en cualquier lado porque estaba probando si podía usarlo con otros pines, al final se enchufa en los Rx y Tx del shield)

Terminología:
* MD - modulo bluetooth
* Pasos – los motores estos funcionan con pasos, vos le mandas una señal y estos se mueven un paso, si le mandas otra, se mueve otro paso, si le mandas las señales rápido se mueve rápido.

1.Rx y Tx no son nombres de canales.
los pines 0 y 1 (Rx y Tx), se utilizan para enviar y recibir información, pero no es simplemente un nombre, si vos unís el pin Rx del MD al Rx del Arduino no funciona, porque Rx significa que ese pin recibe información por lo tanto hay que unirlo con el Tx que envía información.

2.La información de la computadora al Arduino van por los pines Rx y Tx.
Si enchufan el MD a estos pines mientras el Arduino esta enchufado por usb a una computadora no van a poder subir el código al Arduino y tampoco va a andar la conexión entre el MD y el Arduino, esto se puede solucionar enchufando el MD a otros pines, pero yo tenía un shield enchufado al Arduino y no era tan fácil saber que pin va a qué lado así que desenchufo los Tx y Rx cuando necesito subir código. Para que el Arduino funcione con el MD la corriente se la doy enchufando el usb a la pared, con un cargador de celular.

3.El cable marcado de los transformadores es el positivo.
Eso, el shield toma la alimentación desde un transformador de, tene4 que pelar los cables y enchufarlo, si lo ponen al revés queman el fusible (creo, se me rompió y me lo arreglaron cambiando algo).

4.GRBL es demasiado extensa.
la librería GRBL es muy buena, vos la subís al Arduino y funciona, le pasas comandos y los ejecuta y si querés pasarle los comandos por MD solo tenés que enchufarlo y mandárselos con un '\n' al final y si la transmisión serial de datos del MD es diferente a la que tiene el GRBL por defecto hay que cambiarla en config.h y listo. Pero es muy difícil de entender y no pude extenderla, de fábrica podés usar 3 motores, 4 con un poco de trabajo creo pero yo necesitaba 5, así que tuve que cambiar a AccelStepper que es más simple y no tiene restricciones de cantidad.

5.No es tan fácil.
Para hacer funcionar algo necesitas mínimo saber a qué pines se conecta para mandar la señal, pero si tenés un shield que te acomoda los pines no sabes cuál es cual. ¿la solución? que alguien que sepa te ayude; o estudiar, no sé. pero yo tuve que ir a un proveedor de Arduinos para que me ayude a saber que pines iban a los motores para usarlos en el código y el con un tester fue siguiendo los pines y me dio los números.

Abajo les paso el código terminado, pero antes les explico algunas cosas.
AccelStepper es una librería que te enmascara la cosa rara de Arduino y te lo presenta como objetos, pero el Arduino no tiene hilos, no pod´es decirle a un motor que corra y listo, lo que Arduino te provee es un método que se llama constantemente, ahí ponemos nuestra lógica y ahí llamamos al método run de cada motor, este método moverá el motor si fuese necesario pero hay que llamarlo lo más posible porque si no los motores se podrían perder pasos funcionando mal, cosa que me paso al querer llamar a GetLine(), que esperaba comandos y los motores no recibían las señales con la rapidez necesaria, así que cree una variable working para que no se llamara a GetLine() si algún  motor estaba corriendo. Pero al hacer esto descubrí que el Arduino tiene un buffer, así que, si vos le mandabas dos veces el movimiento de un motor, al terminar de moverse comenzaría de vuelta y yo lo que quería es que si mandas un comando mientras otro esta en ejecución devuelva un error, así que cuando esta working leemos desde MD, pero no hacemos nada, salvo que llegue un '\n' entonces mandamos un error ya que significa que alguien mando un comando.


#include <AccelStepper.h>
#include <SoftwareSerial.h>


AccelStepper Xaxis(1, 2, 5); // pin 2 = step, pin 5 = direction
AccelStepper Yaxis(1, 3, 6); // pin 3 = step, pin 6 = direction
AccelStepper Zaxis(1, 4, 7); // pin 4 = step, pin 7 = direction
AccelStepper Aaxis(1, 12, 13); // pin 12 = step, pin 13 = direction
bool working = false;
char separator = ':';
char valueSeparator = ',';
SoftwareSerial BT1(0,1);

void setup() {
  BT1.begin(9600);
  Xaxis.setMaxSpeed(1200);
  Yaxis.setMaxSpeed(1200);
  Zaxis.setMaxSpeed(1200);
  Aaxis.setMaxSpeed(1200);
  Xaxis.setAcceleration(400);
  Yaxis.setAcceleration(400);
  Zaxis.setAcceleration(400);
  Aaxis.setAcceleration(400);
  Xaxis.setEnablePin(8);
  Yaxis.setEnablePin(8);
  Zaxis.setEnablePin(8);
  Aaxis.setEnablePin(8);
  Xaxis.setPinsInverted(false, false, true);
  Zaxis.setPinsInverted(false, false, true);
  Yaxis.setPinsInverted(false, false, true);
  Aaxis.setPinsInverted(false, false, true);
  Xaxis.enableOutputs();
  Zaxis.enableOutputs();
  Yaxis.enableOutputs();
  Aaxis.enableOutputs();
}
//multiple(m)||simple(s):motors(x|x,y,...):steps:velocity:acceleration
void loop() {

  if (BT1.available())
  {
    if(!working){
//si no hay motores en movimiento, esperamos a que el comando entre
      String S = GetLine();
      working=true;
      motorsParse(S);
    }
    else{
      char c = BT1.read();//tiramos a la mierda el buffer
      if(c == '\n'){
        //alguien mando un comando, le decimos que estamos ocupados
        BT1.println("busy");
      }
    }
  }

    Xaxis.run();
    Zaxis.run();
    Yaxis.run();
    Aaxis.run();
    if(Xaxis.distanceToGo()==0&&Yaxis.distanceToGo()==0&&Zaxis.distanceToGo()==0&&Aaxis.distanceToGo()==0){
//ya ningun motor se mueve, ponemos working en false para que espere al proximo comando
      working = false;
      Xaxis.disableOutputs();
      Zaxis.disableOutputs();
      Yaxis.disableOutputs();
      Aaxis.disableOutputs();
    }
     
}
void motorsParse(String input){
  int sepIndex = input.indexOf(separator);
  int secondSepIndex = input.indexOf(separator, sepIndex+1);
  int thirdSepIndex = input.indexOf(separator, secondSepIndex+1);
  int fourthSepIndex = input.indexOf(separator, thirdSepIndex+1);
 
  char moveType = input.substring(0, sepIndex).charAt(0);
  String motors = input.substring(sepIndex+1, secondSepIndex);
  long steps = input.substring(secondSepIndex+1, thirdSepIndex).toInt();
  long velocityx100 = input.substring(thirdSepIndex+1, fourthSepIndex).toInt();
  long accelerationx100 = input.substring(fourthSepIndex+1).toInt();

  if(moveMotors(moveType, motors, steps, velocityx100, accelerationx100)){
    BT1.println("ok");
  }else{
    BT1.println("error");
  }
}

bool moveMotors(char moveType, String motors, long steps, long velocityx100, long accelerationx100){
  if(steps == 0 || velocityx100 == 0 || accelerationx100 == 0){
    return false;
  }
  char motorArray[5]  = {motors.charAt(0), separator, separator, separator, separator};// como mucho son 4 motores y el : para marcar el fin
  if(moveType == 's'){
    //ya esta inicializado arrba el array de motores si es para un solo motor
  }else{
    if(moveType == 'm'){
      int i = 0;
      String value = getValue(motors, valueSeparator, i);
      while(value != ""){
        motorArray[i] = value.charAt(0);
        i++;
        value = getValue(motors, valueSeparator, i);
      }
    }
  }
  int j = 0;
  bool terminar = false;
  while (!terminar){
    char currentMotor = motorArray[j];  
    j++;
    terminar =  (motorArray[j] == separator);
    if(currentMotor == 'x'){
      Xaxis.enableOutputs();
      Xaxis.setMaxSpeed(velocityx100/100);
//mando los numeros multiplicados x 100 para no andar con decimales
      Xaxis.setAcceleration(accelerationx100/100);
      Xaxis.move(steps);
    }else{
      if(currentMotor == 'z'){
        Zaxis.enableOutputs();
        Zaxis.setMaxSpeed(velocityx100/100);
        Zaxis.setAcceleration(accelerationx100/100);
        Zaxis.move(steps);
      }else{
        if(currentMotor == 'y'){
          Yaxis.enableOutputs();
          Yaxis.setMaxSpeed(velocityx100/100);
          Yaxis.setAcceleration(accelerationx100/100);
          Yaxis.move(steps);
        }else{
          if(currentMotor == 'a'){
            Aaxis.enableOutputs();
            Aaxis.setMaxSpeed(velocityx100/100);
            Aaxis.setAcceleration(accelerationx100/100);
            Aaxis.move(steps);
          }else{
            // ningun motor conocido
            return false;
          }
        }
      }
    }
  }
  return true;
}

String GetLine()
{  
  String S = "" ;
  if (BT1.available())
  {
    char c = BT1.read(); ;
    while ( c != '\n')            //Hasta que el caracter sea intro
    {
      S = S + c ;
      delay(25) ;
      c = BT1.read();
    }
    return(S) ;
  }
}

String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = {
    0, -1  };
  int maxIndex = data.length()-1;
  for(int i=0; i<=maxIndex && found<=index; i++){
    if(data.charAt(i)==separator || i==maxIndex){
      found++;
      strIndex[0] = strIndex[1]+1;
      strIndex[1] = (i == maxIndex) ? i+1 : i;
    }
  }
  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}