Calibracao

.gitignore

@@ -2,3 +2,7 @@
 *BASE*
 *REMOTE*
 *.swp
+
+FirmwareRope/\.vscode/
+
+FirmwareRope/build/

FirmwareRope/FirmwareRope.ino

@@ -1,14 +1,30 @@
+#include <EEPROM.h>
 #include "libs/Button/Button.cpp"
 #include "libs/RoPE_Steppers_28BYJ48/RoPE_Steppers_28BYJ48.cpp"
 
+
 #define QUANTIDADE_MAXIMA_ACOES 45
+
 bool testing_loop = 0;
 bool sound_off = 0;
+
 int easter_egg;
 int TURN_STEP_OVERWRITE = 170;
 int WALK_STEP_OVERWRITE = 360;
-//Entradas
 
+//Calibracao
+#define FRENTE 0
+#define TRAS 1
+#define DIREITA 2
+#define ESQUERDA 3
+
+bool calibrando = 0;
+int steps[] = {0,0,0,0};
+int posicaoMem[] = {0,2,4,6};
+int valoresCalibracao[] = {100,1,50,10};
+int stepSelecionado = -1;
+
+//Entradas
 Button btnTras = Button (A1); 
 Button btnDireita = Button (A2);
 Button btnIr = Button (A3);
@@ -200,24 +216,67 @@ void verificarFeedback(int acoesContExec)
   }
 }
 
+void salvarStep(int endereco, int valor)
+{
+  byte hiByte = highByte(valor);
+  byte loByte = lowByte(valor);
+  EEPROM.write(endereco, hiByte);
+  EEPROM.write(endereco+1, loByte);
+}
+
+int lerStep(int endereco)
+{
+  byte hiByte = EEPROM.read(endereco);
+  byte loByte = EEPROM.read(endereco+1);
+  return word(hiByte, loByte);
+}
+
+void limparMemoria()
+{
+  steps[FRENTE] = 0;
+  steps[TRAS] = 0;
+  steps[DIREITA] = 0;
+  steps[ESQUERDA] = 0;
+  salvarStep(posicaoMem[ESQUERDA], 0);
+  salvarStep(posicaoMem[DIREITA], 0);
+  salvarStep(posicaoMem[FRENTE], 0);
+  salvarStep(posicaoMem[TRAS], 0);
+}
+
 void verificarInstrucao(int acoesContExec)
 {
   switch (acoes[acoesContExec])
   {
     case acaoEsquerda:
-      motores_esquerda(rope_foi_parado,TURN_STEP_OVERWRITE);
+      if(steps[ESQUERDA]>0){
+        motores_esquerda(rope_foi_parado,steps[ESQUERDA]);
+      }else{
+        motores_esquerda(rope_foi_parado,TURN_STEP_OVERWRITE);
+      }      
       break;
 
     case acaoDireita:
-      motores_direita(rope_foi_parado,TURN_STEP_OVERWRITE);
+      if(steps[DIREITA]>0){
+        motores_direita(rope_foi_parado,steps[DIREITA]);    
+      }else{
+        motores_direita(rope_foi_parado,TURN_STEP_OVERWRITE);
+      }      
       break;
 
     case acaoFrente:
-      motores_frente(rope_foi_parado);
+      if(steps[FRENTE]>0){
+        motores_frente(rope_foi_parado, steps[FRENTE]);  
+      }else{
+        motores_frente(rope_foi_parado);
+      }      
       break;
 
     case acaoTras:
-      motores_tras(rope_foi_parado);
+      if(steps[TRAS]>0){
+        motores_tras(rope_foi_parado, steps[TRAS]);
+      }else{
+        motores_tras(rope_foi_parado);
+      }      
       break;
 
     default:
@@ -247,7 +306,10 @@ void executar() {
   easter_egg = -1;
   if (acoesContExec < acoesContProg)
   {
-    verificarFeedback(acoesContExec);
+    if(!testing_loop)
+    {
+      verificarFeedback(acoesContExec);
+    }
     verificarInstrucao(acoesContExec);
   }
 
@@ -293,7 +355,63 @@ bool rope_foi_parado(){
   btnIr.process();
   return ESTADO_ATUAL == ESTADO_AGUARDANDO ? true : false;
 }
+
+void andarPassosCalibracao(int direcao)
+{
+  if(stepSelecionado == FRENTE)
+  {
+    ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    motores_frente(rope_foi_parado, valoresCalibracao[direcao]);
+    steps[FRENTE]+=valoresCalibracao[direcao];
+    ESTADO_ATUAL = ESTADO_AGUARDANDO;
+  }
+  else if(stepSelecionado == TRAS)
+  {
+    ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    motores_tras(rope_foi_parado, valoresCalibracao[direcao]);
+    steps[TRAS]+=valoresCalibracao[direcao];
+    ESTADO_ATUAL = ESTADO_AGUARDANDO;
+  }
+  else if(stepSelecionado == DIREITA)
+  {
+    ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    motores_direita(rope_foi_parado, valoresCalibracao[direcao]);
+    steps[DIREITA]+=valoresCalibracao[direcao];
+    ESTADO_ATUAL = ESTADO_AGUARDANDO;
+  }
+  else if(stepSelecionado == ESQUERDA)
+  {
+    ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    motores_esquerda(rope_foi_parado, valoresCalibracao[direcao]);
+    steps[ESQUERDA]+=valoresCalibracao[direcao];
+    ESTADO_ATUAL = ESTADO_AGUARDANDO;
+  }
+}
+
 void onEsquerdaPress(Button &b){
+  if(calibrando && stepSelecionado < 0)
+  {
+    stepSelecionado = ESQUERDA;
+    steps[stepSelecionado] = 0;
+    feedbackEsquerda(true);
+    delay(100);
+    // ESTADO_ATUAL = ESTADO_EXECUTANDO;    
+    // steps[ESQUERDA] = motores_esquerda_calibracao(rope_foi_parado);
+    // ESTADO_ATUAL = ESTADO_AGUARDANDO;
+    // feedbackEsquerda(true);
+    // delay(100);
+    // salvarStep(posicaoMem[ESQUERDA], steps[ESQUERDA]);
+    return;
+  }
+  if(calibrando && stepSelecionado >= 0)
+  {
+    feedbackEsquerda(true);
+    delay(100);
+    feedbackEsquerda(true);
+    delay(1000);
+    andarPassosCalibracao(ESQUERDA);
+    return;
+  }
   if (acoesContProg < QUANTIDADE_MAXIMA_ACOES)
   {
     acoes[acoesContProg] = acaoEsquerda;
@@ -303,6 +421,29 @@ void onEsquerdaPress(Button &b){
   }
 }
 void onDireitaPress(Button &b){
+  if(calibrando && stepSelecionado < 0)
+  {
+    stepSelecionado = DIREITA;
+    steps[stepSelecionado] = 0;
+    feedbackDireita(true);
+    delay(100);
+    // ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    // steps[DIREITA] = motores_direita_calibracao(rope_foi_parado);
+    // ESTADO_ATUAL = ESTADO_AGUARDANDO;
+    // feedbackDireita(true);
+    // delay(100);
+    // salvarStep(posicaoMem[DIREITA], steps[DIREITA]);
+    return;
+  }
+  if(calibrando && stepSelecionado >= 0)
+  {
+    feedbackDireita(true);
+    delay(100);
+    feedbackDireita(true);
+    delay(1000);
+    andarPassosCalibracao(DIREITA);
+    return;
+  }
   if (acoesContProg < QUANTIDADE_MAXIMA_ACOES)
   {
     acoes[acoesContProg] = acaoDireita;
@@ -312,6 +453,29 @@ void onDireitaPress(Button &b){
   }
 }
 void onFrentePress(Button &b){
+  if(calibrando && stepSelecionado < 0)
+  {
+    stepSelecionado = FRENTE;
+    steps[stepSelecionado] = 0;
+    feedbackFrente(true);
+    delay(100);
+    // ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    // steps[FRENTE] = motores_frente_calibracao(rope_foi_parado);
+    // ESTADO_ATUAL = ESTADO_AGUARDANDO;
+    // feedbackFrente(true);
+    // delay(100);
+    // salvarStep(posicaoMem[FRENTE], steps[FRENTE]);
+    return;
+  }
+  if(calibrando && stepSelecionado >= 0)
+  {
+    feedbackFrente(true);
+    delay(100);
+    feedbackFrente(true);
+    delay(1000);
+    andarPassosCalibracao(FRENTE);
+    return;
+  }
   if (acoesContProg < QUANTIDADE_MAXIMA_ACOES)
   {
     acoes[acoesContProg] = acaoFrente;
@@ -321,6 +485,29 @@ void onFrentePress(Button &b){
   }
 }
 void onTrasPress(Button &b){
+  if(calibrando && stepSelecionado < 0)
+  {
+    stepSelecionado = TRAS;
+    steps[stepSelecionado] = 0;
+    feedbackTras(true);
+    delay(100);
+    // ESTADO_ATUAL = ESTADO_EXECUTANDO;
+    // steps[TRAS] = motores_tras_calibracao(rope_foi_parado);
+    // ESTADO_ATUAL = ESTADO_AGUARDANDO;
+    // feedbackTras(true);
+    // delay(100);
+    // salvarStep(posicaoMem[TRAS], steps[TRAS]);
+    return;
+  }
+  if(calibrando && stepSelecionado >= 0)
+  {
+    feedbackTras(true);
+    delay(100);
+    feedbackTras(true);
+    delay(1000);
+    andarPassosCalibracao(TRAS);
+    return;
+  }
   if (acoesContProg < QUANTIDADE_MAXIMA_ACOES)
   {
     acoes[acoesContProg] = acaoTras;
@@ -330,6 +517,20 @@ void onTrasPress(Button &b){
   }
 }
 void onIrPress(Button &b){
+  if(calibrando && stepSelecionado >= 0)
+  {
+    feedbackFrente(true);
+    delay(50);
+    feedbackTras(true);
+    delay(50);
+    feedbackEsquerda(true);
+    delay(50);
+    feedbackDireita(true);
+    delay(50);
+    salvarStep(posicaoMem[stepSelecionado], steps[stepSelecionado]);
+    stepSelecionado=-1;
+    return;
+  }  
   if(acoesContProg == 0 && easter_egg >= 0){
     easter_egg++;
   }
@@ -344,13 +545,41 @@ void onIrPress(Button &b){
   delay(100);
 }
 
+void onIrHold(Button &b){
+  feedbackEasterEggActivated();
+
+  if(easter_egg > 5)
+  {
+    limparMemoria();
+    feedbackFrente(true);
+    delay(50);
+    feedbackTras(true);
+    delay(50);
+    feedbackEsquerda(true);
+    delay(50);
+    feedbackDireita(true);
+    delay(50);
+    return;
+  }
+  acoesContProg = 0;
+  if(calibrando)
+  {
+    calibrando = 0;
+  }
+  else{
+    calibrando = 1;
+  }  
+  delay(100);
+}
+
 void definirCallBack()
 {
   btnTras.pressHandler(onTrasPress);
   btnFrente.pressHandler(onFrentePress);
   btnEsquerda.pressHandler(onEsquerdaPress);
   btnDireita.pressHandler(onDireitaPress);
   btnIr.pressHandler(onIrPress);
+  btnIr.holdHandler(onIrHold, 5000);
 }
 
 void setup_processar_estados_invalidos_iniciacao(){
@@ -362,6 +591,13 @@ void setup_processar_estados_invalidos_iniciacao(){
   btnIr.process();
 }
 
+void load_steps(){
+  steps[DIREITA] = lerStep(posicaoMem[DIREITA]);
+  steps[ESQUERDA] = lerStep(posicaoMem[ESQUERDA]);
+  steps[FRENTE] = lerStep(posicaoMem[FRENTE]);
+  steps[TRAS] = lerStep(posicaoMem[TRAS]);
+}
+
 void setup() {
   Serial.begin(9600);
 
@@ -373,12 +609,13 @@ void setup() {
   pinMode(SAIDA_SOM, OUTPUT);
 
   steppers_setup();
-  definirCallBack();
+  load_steps(); 
 
+  definirCallBack();
   ESTADO_ATUAL = ESTADO_AGUARDANDO;
-
-  setup_processar_estados_invalidos_iniciacao();
   easter_egg = 0;
+
+  setup_processar_estados_invalidos_iniciacao();  
 }
 
 void loop()
@@ -397,4 +634,4 @@ void loop()
       break;
   }
   //instrucoes_de_teste();
-}
+}