From 04474bfb948e1603badcf951347c5edff291f816 Mon Sep 17 00:00:00 2001
From: Clement <c.boesmier@aptatio.com>
Date: Thu, 18 Jan 2024 21:02:11 +0100
Subject: [PATCH] add main algo V1 with mock

---
 include/Program.h |  7 +++++++
 src/Program.cpp   | 34 +++++++++++++++++++++++++---------
 2 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/include/Program.h b/include/Program.h
index 033fd3a..05580e5 100644
--- a/include/Program.h
+++ b/include/Program.h
@@ -3,6 +3,7 @@
 
 #include "DolibarrClient.h"
 #include "GRBL.h"
+#include "NfcReader.h"
 #include <Arduino.h>
 #include <M5Stack.h>
 // #include <vector>
@@ -27,6 +28,12 @@ private:
      * @brief stepper motor controller
      */
     GRBL* grbl;
+
+    /**
+     * @brief nfc reader
+     *
+     */
+    NfcReader* NFC;
 };
 
 #endif
diff --git a/src/Program.cpp b/src/Program.cpp
index 918b53a..9feaee6 100644
--- a/src/Program.cpp
+++ b/src/Program.cpp
@@ -26,6 +26,8 @@ Program::Program() {
     this->grbl = new GRBL(STEPMOTOR_I2C_ADDR);
     Wire.begin(21, 22);
     grbl->init(STEPER_SPEED, STEPER_PAS, STEPER_ACC);
+
+    this->NFC = new NfcReader(NFC_ADDR);
 }
 
 void Program::loop() {
@@ -40,18 +42,36 @@ void Program::loop() {
     // if(this->NfcIDs->size() > 0){
     // }
 
-
-    //alog V1
+    //algo V1
     //je lis la valeur NFC
+    String nfcId = this->NFC->ReadNfc();
     //si qqc
+    if(nfcId != "0"){
+        //j'arrète le stepper
+        //this->grbl->stop(); //FIXME: implemente
+        //j'affiche le tag lue
+        Serial.print("new colis in comming : ");
+        Serial.println(nfcId);
         //je demande a dolibard ou j'envoie
+        Serial.println("Demande a dolibard");
+        delay(500);// TODO: remove mock
         //j'envoie au servo la commande
+        Serial.print("Servo : ");
+        Serial.println("Droite");
+        delay(500);// TODO: remove mock
         //j'avance le moteur jusqu'a X (a definir)
-    //si rien j'avance le moteur (5mm)
-
+        this->grbl->mouveForward(CONVOYER_LEN);
+    }else{
+        //si rien
+        //je check si le stepper est en iddle
+        if(this->grbl->isIddle()){
+            //si oui j'avance de 5 mm
+            this->grbl->mouveForward(5);
+        }
+    }
+    //j'update la position du servo
 
     //idée algo v1.5 (ajou petite gestion d'erreur)
-    //alog V1
     //je lis la valeur NFC
     //si qqc
         //je demande a dolibard ou j'envoie
@@ -65,15 +85,11 @@ void Program::loop() {
     //si il y a 2 colis (ou plus)
         //on affiche une erreur de bourrage
 
-
-
     //idée algo v2
     //utiliser une pile
     //utiliser le 2nd lecteur pour vidé la pile
     //gestion d'erreur complete avec celle du dessus plus celle de la pile
 
-
-
 }