Código Arduino com gravação de movimentos

#include <Servo.h>
 
#define VELMAX1 100
#define VELMAX2 105
#define VELMAX3 115
#define VELMAX4 125
#define VELMAX5 135
#define VELMAX6 145
#define VELMAX7 155
#define VELMAX8 165
#define VELMAX9 175
#define VELMAX0 90
#define VELMAX100 180
 
 
#define PARADO 90
 
#define TEMPOMOVIMENTO 100
#define TEMPOWATCHDOG 300
#define TEMPODEBUG 1000
 
 
int pos = PARADO;
int velocidadeMax = VELMAX1;
char inChar;
int ajustePos = 0;
 
int estadoAjustePos = 0; // 0 desabilitado 1 ajustando
int estadoMovimento = 0; //parado 0 frente 1 tras 2 esquerda 3 direita 4
int estadoGravacao = 0; // parado 0, gravando 1 , executando 2
long int gravaMov[2][40]; // estado contador
int indiceMov = 0;
int contMov = 0;
int numeroMov = 0; //numero de movimentos para executar
int estadoAnterior = 0;
long int tsMovimento, tsDebug, tsWatchDog;
 
 
Servo motor1;
Servo motor2;
 
 
void movimento(void){
  if (millis() - tsDebug >= TEMPODEBUG){
    tsDebug = millis();
    Serial.print("Pos =>");
    Serial.println(pos); 
  }
  if (millis() - tsMovimento >= TEMPOMOVIMENTO){
    tsMovimento = millis();
 
    if (estadoGravacao == 1){
       if (estadoMovimento == estadoAnterior){
         gravaMov[1][indiceMov]++;
       }else{
         if(indiceMov == 40){
           estadoGravacao = 0;
           Serial.println("Memoria Cheia");
         }
         indiceMov++;
         gravaMov[0][indiceMov]=estadoMovimento;
         gravaMov[1][indiceMov]=1;
         estadoAnterior = estadoMovimento;      
       }
    }
    if (estadoGravacao == 2){
      if (indiceMov <= numeroMov){
        if (gravaMov[1][indiceMov] > contMov){
          estadoMovimento = gravaMov[0][indiceMov];
          contMov ++;  
        }else{
          contMov = 0;
          indiceMov ++;
        }
      }
    }
    
    Serial.print("Estado Movimento =>");
    Serial.println(estadoMovimento);
    Serial.print("Indice Mov =>");
    Serial.println(indiceMov);
    Serial.print("gravaMov 0 indice =>");
    Serial.println(gravaMov[0][indiceMov]);
 
    if (estadoMovimento == 0){  //parado
      pos = PARADO;
      motor1.write (PARADO);
      motor2.write (PARADO);
    }
    if (estadoMovimento == 1){   //frente
      if (pos < velocidadeMax) pos ++;
      motor1.write (pos);
      motor2.write (pos + ajustePos);
    }  
    if (estadoMovimento == 2){   //tras
      if (pos > (180-velocidadeMax)) pos --;
      motor1.write (pos);
      motor2.write (pos);
    }
    if (estadoMovimento == 3){  //direita
        if (pos < velocidadeMax) pos ++;
      motor1.write (pos);
      motor2.write (PARADO);
    }
    if (estadoMovimento == 4){  //esquerda
        if (pos < velocidadeMax) pos ++;
      motor1.write (PARADO);
      motor2.write (pos);
    }
  }  
  
}
 
 
void setup()
{   
  Serial.begin(38400);
  pinMode(13,OUTPUT);
  digitalWrite(13,LOW);
  motor1.attach(9);    //motor1
  motor2.attach(10);    //motor2
  motor1.write(pos);
  motor2.write(pos);
  tsMovimento = millis();
  tsDebug = millis();
  tsWatchDog = millis();
  gravaMov[0][0] = 0;
}
 
void loop()
{
  movimento();
  if (millis() - tsWatchDog >= TEMPOWATCHDOG){
    tsWatchDog = millis();
    estadoMovimento = 0;
  }
  if (Serial.available() > 0){
    tsWatchDog = millis();
    inChar = Serial.read();
    if (inChar == 'F') estadoMovimento = 1; 
    if (inChar == 'B') estadoMovimento = 2;
    if (inChar == 'R') {
      if (estadoAjustePos) ajustePos ++; else estadoMovimento = 3;
    }
    if (inChar == 'L'){
      if (estadoAjustePos) ajustePos --; else estadoMovimento = 4;
    }
    if (inChar == 'S' &&  estadoGravacao != 2) estadoMovimento = 0;  
    if (inChar == 'U') estadoAjustePos = 1; 
    if (inChar == 'u') estadoAjustePos = 0;
    if (inChar == 'W') digitalWrite(13,HIGH);  
    if (inChar == 'w') digitalWrite(13,LOW);  
    if (inChar == '1') velocidadeMax = VELMAX1; 
    if (inChar == '2') velocidadeMax = VELMAX2;
    if (inChar == '3') velocidadeMax = VELMAX3;
    if (inChar == '4') velocidadeMax = VELMAX4;
    if (inChar == '5') velocidadeMax = VELMAX5;
    if (inChar == '6') velocidadeMax = VELMAX6;
    if (inChar == '7') velocidadeMax = VELMAX7;
    if (inChar == '8') velocidadeMax = VELMAX8;
    if (inChar == '9') velocidadeMax = VELMAX9;
    if (inChar == '0') velocidadeMax = VELMAX0;
    if (inChar == 'q') velocidadeMax = VELMAX100;
    if (inChar == 'V') {
      estadoGravacao = 1; 
      gravaMov[1][0] = 1;
    }
    if (inChar == 'v') {
      numeroMov = indiceMov;
      indiceMov = 0;
      estadoGravacao = 0;
    }
    if (inChar == 'X') estadoGravacao = 2; 
    if (inChar == 'x') {
      indiceMov = 0;
      estadoGravacao = 0;
    }
 
    //Serial.print("inChar => ");
    //Serial.println(inChar);
  }
 
  
}

Atividades recentes