From 7cf9e889959ee87a11699aa0aed4d23341481078 Mon Sep 17 00:00:00 2001
From: Clement <c.boesmier@aptatio.com>
Date: Thu, 16 Nov 2023 15:42:54 +0100
Subject: [PATCH] add test stepper

---
 platformio.ini |  1 +
 src/main.cpp   | 83 +++++++++++++++++++++++++++++++++++++++++++++++---
 2 files changed, 80 insertions(+), 4 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 9724c56..590c0d3 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -54,6 +54,7 @@ lib_deps =
     bblanchon/ArduinoJson@^6.21.3 ; JSON serializer et deserializer
     m5stack/M5Stack@^0.4.5 ; M5 Lib
     m5stack/M5GFX@^0.1.9 ; M5 Lib pour le LCD
+    m5stack/Module_GRBL_13.2@^0.0.3 ; M5 Lib pour Stepper (GRBL)
 ;    example:
 ;    erropix/ESP32 AnalogWrite@0.2
 
diff --git a/src/main.cpp b/src/main.cpp
index bf9fbf6..bdd6c43 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,10 +1,85 @@
-#include "Program.h"
-Program* program;
+// #include "Program.h"
+// Program* program;
+
+// void setup() {
+//     program = new Program();
+// }
+
+// void loop() {
+//     program->loop();
+// }
+
+#include <Arduino.h>
+
+#include <M5Stack.h>
+#include "Module_GRBL_13.2.h"
+
+
+#define STEPMOTOR_I2C_ADDR 0x70
+// #define STEPMOTOR_I2C_ADDR 0x71
+
+Module_GRBL _GRBL = Module_GRBL(STEPMOTOR_I2C_ADDR);
+
+/**
+ * @brief initialise le GRBL pour le stepper
+ *
+ * @param speed vitesse par defaut du moteur
+ * @param pas nombre de pas par mm du moteur
+ * @param accel vitesse d'acceleration du mptuer (mm/sec²)
+ * @param mode mode de fonctionnement (defaut = distance) absolute
+ */
+void init(int speed, double pas, int accel ,String mode = "distance"){
+    _GRBL.Init(&Wire);
+    _GRBL.setMode(mode);
+    Serial.print("3");
+    char* s = "";
+    Serial.print("4");
+    printf(s,"$0=%d", pas);  // step/mm
+    Serial.print("5");
+    _GRBL.sendGcode(s);
+    printf(s,"$4=%f", speed);  // speed
+    _GRBL.sendGcode(s);
+    printf(s,"$8=%d", pas);  // acceleration, mm/sec^2
+    _GRBL.sendGcode(s);
+}
 
 void setup() {
-    program = new Program();
+    M5.begin();
+    M5.Power.begin();
+    Wire.begin(21, 22);
+    Serial.begin(115200);
+    M5.Lcd.setTextColor(WHITE, BLACK);
+    M5.Lcd.setTextSize(3);
+    M5.lcd.setBrightness(100);
+    M5.Lcd.setCursor(80, 40);
+    M5.Lcd.println("GRBL 13.2");
+    M5.Lcd.setCursor(50, 80);
+    M5.Lcd.println("Press Btn A/B");
+    M5.Lcd.setCursor(50, 120);
+    delay(1000);
+    M5.Lcd.println("Control Motor");
+    Serial.println("\n\n\n\n\n0");
+    init(100, 755.906, 50);
+    Serial.println("\n\n\n\n\n000000000000000000");
 }
 
+
+
 void loop() {
-    program->loop();
+    if (M5.BtnA.wasPressed())
+    {
+        Serial.print(_GRBL.readStatus());
+        _GRBL.setMotor(5, 5, 5, 200);
+        _GRBL.setMotor(0, 0, 0, 200);
+    }
+
+    if (M5.BtnB.wasPressed()) {
+        _GRBL.sendGcode("G1 X5Y5Z5");
+        _GRBL.sendGcode("G1 X0Y0Z0");
+    }
+
+    if (M5.BtnC.wasReleased()) {
+        _GRBL.unLock();
+    }
+    M5.update();
 }