Test gcode

lib/gcode/gcode.h

@@ -0,0 +1,25 @@
+#pragma once
+#ifndef _GCODE_H_
+#define _GCODE_H_
+
+#include <Arduino.h>
+
+struct MachineState {
+  double x;
+  double y;
+  double z;
+  int moveSpeed;
+  int cutSpeed;
+};
+
+class GCodeHandler
+{
+private:
+  Stream* _gcodeStream;
+  MachineState _currentState;
+public:
+  GCodeHandler(Stream* gcodeStream);
+  bool readStep(MachineState* step);
+};
+
+#endif

lib/gcode/gcodeHandler.cpp

@@ -0,0 +1,79 @@
+#include <gcode.h>
+
+GCodeHandler::GCodeHandler(Stream* gcodeStream)
+{
+  _gcodeStream = gcodeStream;
+
+  // Init state
+  _currentState.x = 0;
+  _currentState.y = 0;
+  _currentState.z = 0;
+  _currentState.moveSpeed = 0;
+  _currentState.cutSpeed = 0;
+}
+
+bool GCodeHandler::readStep(MachineState* step)
+{
+  MachineState newState = _currentState;
+  bool hasCommand = false;
+  for(;;) {
+    switch (_gcodeStream->read())
+    {
+      case 'G':
+        switch (_gcodeStream->parseInt())
+        {
+          case 0:
+            hasCommand = true;
+            newState.moveSpeed = 120;
+            break;
+          case 1:
+            hasCommand = true;
+            newState.moveSpeed = 80;
+            break;
+          default:
+            break;
+        }
+        break;
+      case 'M':
+        switch (_gcodeStream->parseInt())
+        {
+          case 2:
+          case 30:
+            // Program end
+            return false;
+          default:
+            break;
+        }
+        break;
+      case 'X':
+        hasCommand = true;
+        newState.x = _gcodeStream->parseFloat();
+        break;
+      case 'Y':
+        hasCommand = true;
+        newState.y = _gcodeStream->parseFloat();
+        break;
+      case 'Z':
+        hasCommand = true;
+        newState.z = _gcodeStream->parseFloat();
+        break;
+      case '(':
+        _gcodeStream->readStringUntil(')');
+        break;
+      case ';':
+        _gcodeStream->readStringUntil('\n');
+        break;
+      case '\n':
+        if (hasCommand) {
+          *step = newState;
+          _currentState = newState;
+          return true;
+        }
+        break;
+      case ' ':
+        break;
+      default:
+        break;
+    }
+  }
+}

lib/gnss/gnss.h

@@ -15,7 +15,6 @@ public:
   geo_pos GetPosition();
   float GetMotionHeading();
   float GetMotionSpeed();
-  ~GnssServiceClass();
 };
 
 

lib/gnss/gnssServiceClass.cpp

@@ -59,7 +59,3 @@ float GnssServiceClass::GetMotionSpeed() {
   //gnss->getPVT();
   return gnss->getGroundSpeed() / 1000.;
 }
-
-GnssServiceClass::~GnssServiceClass()
-{
-}

lib/motors/motors.h

@@ -40,7 +40,6 @@ public:
   void SetMoveSpeed(MoveSpeed speed);
   MoveSpeed GetMoveSpeed();
   void UpdateMotors();
-  ~MotorsControllerClass();
 };
 
 void setup_motors(

lib/motors/motorsController.cpp

@@ -97,7 +97,3 @@ void MotorsControllerClass::UpdateMotors() {
     log_w("Unable to obtain semaphore");
   }
 }
-
-MotorsControllerClass::~MotorsControllerClass()
-{
-}

lib/shared-msc/shared-msc.cpp

@@ -0,0 +1,93 @@
+#include <Arduino.h>
+#include <USB.h>
+#include <USBMSC.h>
+#include "wear_levelling.h"
+
+USBMSC msc;
+const esp_partition_t* partition;
+wl_handle_t wl_handle = WL_INVALID_HANDLE;
+bool mscEnabled = false;
+
+static int32_t onWrite(uint32_t lba, uint32_t offset, uint8_t* buffer, uint32_t bufsize){
+  wl_erase_range(wl_handle, (size_t)((lba * CONFIG_WL_SECTOR_SIZE) + offset), (size_t)bufsize);
+  wl_write(wl_handle, (size_t)((lba * CONFIG_WL_SECTOR_SIZE) + offset), (const void*) buffer, (size_t)bufsize);
+  
+  return bufsize;
+}
+
+static int32_t onRead(uint32_t lba, uint32_t offset, void* buffer, uint32_t bufsize){
+  wl_read(wl_handle, (size_t)((lba * CONFIG_WL_SECTOR_SIZE) + offset), (void*)buffer, (size_t)bufsize);
+
+  return bufsize;
+}
+
+static bool onStartStop(uint8_t power_condition, bool start, bool load_eject) {
+  log_i("MSC START/STOP: power: %u, start: %u, eject: %u\n", power_condition, start, load_eject);
+
+  return true;
+}
+
+static void enableMsc()
+{
+  if (mscEnabled)
+    return;
+
+  log_i("Turn on MSC");
+  wl_mount(partition, &wl_handle);
+  msc.vendorID("ESP32");
+  msc.productID("USB_MSC");
+  msc.productRevision("1.0");
+  msc.onRead(onRead);
+  msc.onWrite(onWrite);
+  msc.onStartStop(onStartStop);
+  msc.mediaPresent(true);
+  msc.begin(partition->size/CONFIG_WL_SECTOR_SIZE, CONFIG_WL_SECTOR_SIZE);
+  mscEnabled = true;
+}
+
+static void disbaleMsc()
+{
+  if (!mscEnabled)
+    return;
+
+  log_i("Turn off MSC");
+  msc.end();
+  log_i("Unmount WL partition");
+  wl_unmount(wl_handle);
+  wl_handle = WL_INVALID_HANDLE;
+  mscEnabled = false;
+}
+
+static void usbEventCallback(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data) {
+  if (event_base == ARDUINO_USB_EVENTS) {
+    arduino_usb_event_data_t *data = (arduino_usb_event_data_t *)event_data;
+    switch (event_id) {
+      case ARDUINO_USB_STARTED_EVENT:
+        log_i("USB PLUGGED");
+        enableMsc();
+        break;
+      case ARDUINO_USB_STOPPED_EVENT:
+        log_i("USB UNPLUGGED");
+        disbaleMsc();
+        break;
+      case ARDUINO_USB_SUSPEND_EVENT:
+        log_i("USB SUSPENDED: remote_wakeup_en: %u\n", data->suspend.remote_wakeup_en);
+        disbaleMsc();
+        break;
+      case ARDUINO_USB_RESUME_EVENT:
+        log_i("USB RESUMED");
+        enableMsc();
+        break;
+
+      default: break;
+    }
+  }
+}
+
+void setup_shared_msc()
+{
+  partition = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_DATA_FAT, NULL);
+
+  USB.onEvent(usbEventCallback);
+  USB.begin();
+}

lib/shared-msc/shared-msc.h

@@ -0,0 +1,7 @@
+#pragma once
+#ifndef _SHARED_MSC_H_
+#define _SHARED_MSC_H_
+
+void setup_shared_msc();
+
+#endif

lib/trip/trip.h

@@ -37,7 +37,6 @@ public:
   /// @param speed desired speed (0 - 255)
   /// @param timeout (ms)
   void GoTo(geo_pos &pos, u_int8_t speed, uint timeout);
-  ~TripControllerClass();
 };
 
 extern TripControllerClass* TripController;

lib/trip/tripController.cpp

@@ -4,6 +4,8 @@
 #include <motors.h>
 #include <trip.h>
 
+#define ROTATE_MIN_SPEED  12
+
 struct _rotate_args {
   double heading;
   uint timeout;
@@ -59,8 +61,8 @@ void TripControllerClass::RotateToHeading(double heading, uint timeout) {
   while ((delta < -10 || delta > 10 || abs(delta) < abs(lastDelta)) && millis() <= startTime + timeout)
   {
     MoveSpeed move = {
-      .left = (int16_t)(delta > 0 ? 10 + delta / 2 : -10 + delta / 2),
+      .left = (int16_t)(delta > 0 ? ROTATE_MIN_SPEED + delta / 2 : -ROTATE_MIN_SPEED + delta / 2),
-      .right = (int16_t)(delta > 0 ? -10 - delta / 2 : 10 - delta / 2)
+      .right = (int16_t)(delta > 0 ? -ROTATE_MIN_SPEED - delta / 2 : ROTATE_MIN_SPEED - delta / 2)
     };
     MotorsController->SetMoveSpeed(move);
     delay(100);
@@ -136,8 +138,4 @@ void TripControllerClass::_gotoTask(void * param) {
   vTaskDelete(NULL);
 }
 
-TripControllerClass::~TripControllerClass()
-{
-}
-
 TripControllerClass* TripController = new TripControllerClass();

lib/work/work.h

@@ -3,6 +3,8 @@
 #define _WORK_H_
 
 #include <Arduino.h>
+#include <FS.h>
+#include <gcode.h>
 #include <geo-util.h>
 
 typedef struct {
@@ -19,7 +21,6 @@ private:
   double mmPerDegreeLng;
 public:
   WorkZone(geo_pos origin);
-  geo_pos ToGeoPos(double x, double y);
   geo_pos ToGeoPos(work_pos workPos);
   work_pos ToWorkPos(geo_pos geoPos);
   ~WorkZone();
@@ -29,13 +30,16 @@ class WorkControllerClass
 {
 private:
   TaskHandle_t _currentTask;
+  File* _workFile;
+  GCodeHandler* _gcode;
   static void _workTask(void *);
+  void _openWorkFile();
+  void _closeWorkFile();
 public:
   WorkControllerClass();
   bool IsWorking();
   void StartWork();
   void StopWork();
-  ~WorkControllerClass();
 };
 
 extern WorkControllerClass* WorkController;

lib/work/workController.cpp

@@ -3,14 +3,19 @@
 #include <gnss.h>
 #include <motors.h>
 #include <trip.h>
+#include <gcode.h>
+
+#include <FFat.h>
+
+#define WORK_FILE_NAME  "/work.gcode"
 
 struct _workArgs {
-
 };
 
 WorkControllerClass::WorkControllerClass(/* args */)
 {
   _currentTask = nullptr;
+  _workFile = nullptr;
 }
 
 bool WorkControllerClass::IsWorking()
@@ -33,41 +38,78 @@ void WorkControllerClass::StopWork()
 if (_currentTask != nullptr) {
     vTaskDelete(_currentTask);
     _currentTask = nullptr;
+    _closeWorkFile();
     log_i("Work stopped !");
   }
   MotorsController->SetCutSpeed(0);
   MotorsController->SetMoveSpeed({0 , 0});
 }
 
-void WorkControllerClass::_workTask(void *)
+void WorkControllerClass::_workTask(void *param)
 {
+  _workArgs* args = (_workArgs*)param;
   log_i("Work starting");
+  WorkController->_openWorkFile();
   geo_pos origin = GnssService->GetPosition();
   WorkZone workZone(origin);
-  double track[][2] = {
-    { 0, 1000 },
-    { 1000, 1000 },
-    { 1000, -1000 },
-    { -1000, -1000 },
-    { -1000, 1000 },
-    { 1000, 0 },
-    { 0, 0 },
-  };
 
   geo_pos target;
-  for(int i = 0; i < sizeof(track) / sizeof(track[0]); i++)
+  MachineState state;
+  while (WorkController->_gcode->readStep(&state))
   {
-    target = workZone.ToGeoPos(0, 2000);
+    target = workZone.ToGeoPos({ .x = state.x, .y = state.y});
-    TripController->GoTo(target, 100, 120000);
+    TripController->GoTo(target, state.moveSpeed, 120000);
   }
+  
+  MotorsController->SetCutSpeed(0);
+  MotorsController->SetMoveSpeed({0, 0});
+
   log_i("Work completed !");
 
+  WorkController->_closeWorkFile();
   WorkController->_currentTask = nullptr;
   vTaskDelete(NULL);
 }
 
-WorkControllerClass::~WorkControllerClass()
+void WorkControllerClass::_openWorkFile()
 {
+  if (_workFile == nullptr) {
+    if (!FFat.begin()) {
+      log_w("FFAT is not formated");
+      if (FFat.format()) {
+        log_i("FFAT formated");
+      }
+      if (!FFat.begin()) {
+        log_e("Failed to format FFAT");
+        return;
+      }
+    }
+    log_d("FFAT mounted");
+
+    File workFile;
+    if (!FFat.exists(WORK_FILE_NAME)) {
+      workFile = FFat.open(WORK_FILE_NAME, FILE_WRITE, true);
+      log_i("%s file created", WORK_FILE_NAME);
+    }
+    else {
+      workFile = FFat.open(WORK_FILE_NAME, FILE_READ, false);
+      log_d("%s file opened", WORK_FILE_NAME);
+    }
+    _workFile = &workFile;
+    _gcode = new GCodeHandler(_workFile);
+  }
+}
+
+void WorkControllerClass::_closeWorkFile()
+{
+  if (_workFile != nullptr) {
+    delete _gcode;
+    _gcode = NULL;
+    _workFile->close();
+    _workFile = nullptr;
+    FFat.end();
+    log_i("%s file closed", WORK_FILE_NAME);
+  }
 }
 
 WorkControllerClass* WorkController = new WorkControllerClass();

lib/work/workZone.cpp

@@ -8,18 +8,13 @@ WorkZone::WorkZone(geo_pos origin)
   this->mmPerDegreeLng = 40074155890 * cos(DEG_TO_RAD * originLat) / 360;
 }
 
-geo_pos WorkZone::ToGeoPos(double x, double y)
-{
-  geo_pos result = {
-    .lat = originLat + y / mmPerDegreeLat,
-    .lng = originLng + x / mmPerDegreeLng
-  };
-  return result;
-}
-
 geo_pos WorkZone::ToGeoPos(work_pos workPos)
 {
-  return ToGeoPos(workPos.x, workPos.y);
+  geo_pos result = {
+    .lat = originLat + workPos.y / mmPerDegreeLat,
+    .lng = originLng + workPos.x / mmPerDegreeLng
+  };
+  return result;
 }
 
 work_pos WorkZone::ToWorkPos(geo_pos geoPos)

sampleWork.gcode

@@ -0,0 +1,8 @@
+G0 Y1000
+G1 X1000
+G1 Y-1000
+G1 X-1000
+G1 Y1000
+G1 X0
+G0 Y0
+M30

src/main.cpp

@@ -4,10 +4,12 @@
 #include "compass.h"
 #include "gnss.h"
 #include "motors.h"
+#include "shared-msc.h"
 #include "trip.h"
 #include "work.h"
 
 void setup() {
+  setup_shared_msc();
   setup_ble();
   setup_motors(
     MB_LEFT_DIR_PIN,