wip

lib/ble/ble.cpp

@@ -16,7 +16,7 @@
 #define SERVICE_UUID            "da6bd24f-3f5e-44c8-8f68-dd615ab2d89d"
 #define CUT_UUID                "1971f36f-2c78-4f3d-8550-7f47d2d61bd3"
 #define MOVE_UUID               "9fe842b6-6b68-43e3-959e-d3896620a18f"
-#define TRIP_STOP_UUID          "17f9daa1-63a0-4808-9f77-b0f4fa4d054f"
+#define TRIP_STATE_UUID         "17f9daa1-63a0-4808-9f77-b0f4fa4d054f"
 #define ROTATE_TO_BEARING_UUID  "1263cfb8-1212-4029-a2eb-7a223c3362e8"
 #define POSITION_UUID           "c1235944-2ca3-4a4f-99eb-31ea440e7515"
 #define COMPASS_CAL_UUID        "31f91b0b-f7c9-40e4-8836-e517b1eb9d23"
@@ -66,19 +66,31 @@ class MoveCallbacks : public BLECharacteristicCallbacks {
     }
 };
 
-class TripStopCallbacks : public BLECharacteristicCallbacks {
+class TripStateCallbacks : public BLECharacteristicCallbacks {
   public:
     void onRead(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
-      uint8_t val = 0;
+      uint8_t val = TripController->GetState();
       pCharacteristic->setValue(&val, 1);
     }
 
     void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
-      uint8_t newValue = *pCharacteristic->getData();
-      if (newValue == 1) {
+      u_int8_t newValue = *pCharacteristic->getData();
+      switch (newValue)
+      {
+        case TripState::STOP:
           log_d("Stop trip");
-        TripController->Stop();
+          break;
+        case TripState::RUN:
+          log_d("Run trip");
+          break;
+        case TripState::PAUSE:
+          log_d("Pause trip");
+          break;
+        default:
+          log_e("Invalid state %i", newValue);
+          return;
       }
+      TripController->SetState((TripState)newValue);
     }
 };
 
@@ -176,9 +188,9 @@ void setup_ble() {
     pService->createCharacteristic(MOVE_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
   pMoveSpeed->setCallbacks(new MoveCallbacks());
   // Trip stop
-  BLECharacteristic *pTripStop =
-    pService->createCharacteristic(TRIP_STOP_UUID, BLECharacteristic::PROPERTY_WRITE);
-  pTripStop->setCallbacks(new TripStopCallbacks());
+  BLECharacteristic *pTripSate =
+    pService->createCharacteristic(TRIP_STATE_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+  pTripSate->setCallbacks(new TripStateCallbacks());
   // Rotate to bearing
   BLECharacteristic *pRotateToBearing =
     pService->createCharacteristic(ROTATE_TO_BEARING_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);

lib/trip/trip.h

@@ -5,15 +5,25 @@
 #include <Arduino.h>
 #include "geo-util.h"
 
+enum TripState : byte {
+  STOP = 0,
+  RUN = 1,
+  PAUSE = 2
+};
+
 class TripControllerClass
 {
 private:
   TaskHandle_t _currentTask;
+  TripState _state;
   void _startTask(TaskFunction_t task, void *);
   static void _rotateTask(void *);
   static void _gotoTask(void *);
+  void _handlePause(uint32_t& startTime);
 public:
   TripControllerClass(/* args */);
+  void SetState(TripState state);
+  TripState GetState();
   /// @brief Starts a task that rotates to desired heading
   /// @param heading target heading (degrees)
   /// @param timeout (ms)
@@ -24,8 +34,6 @@ public:
   /// @param speed desired speed (0 - 255)
   /// @param timeout (ms)
   void StartGoTo(geo_pos &pos, u_int8_t speed, uint timeout = 0);
-  /// @brief Stops current task
-  void Stop();
   /// @brief Synchronously rotates to desired heading
   /// @note Motors may be stopped when compled
   /// @param heading target heading (degrees)

lib/trip/tripController.cpp

@@ -21,11 +21,31 @@ TripControllerClass::TripControllerClass() {
   _currentTask = nullptr;
 }
 
+void TripControllerClass::SetState(TripState state)
+{
+  _state = state;
+  if (_state == TripState::STOP) {
+    if (_currentTask != nullptr) {
+      vTaskDelete(_currentTask);
+      _currentTask = nullptr;
+      log_i("Trip stopped !");
+    }
+    MotorsController->SetMoveSpeed({0 , 0});
+  }
+}
+
+TripState TripControllerClass::GetState()
+{
+  return _state;
+}
+
 void TripControllerClass::StartRotateToHeading(double heading, uint timeout) {
   static _rotate_args params;
   params.heading = heading;
   params.timeout = timeout;
   
+  SetState(TripState::RUN);
+
   _startTask(
     _rotateTask,
     &params
@@ -38,21 +58,14 @@ void TripControllerClass::StartGoTo(geo_pos &pos, u_int8_t speed, uint timeout)
   params.speed = speed;
   params.timeout = timeout;
   
+  SetState(TripState::RUN);
+
   _startTask(
     _gotoTask,
     &params
   );
 }
 
-void TripControllerClass::Stop() {
-  if (_currentTask != nullptr) {
-    vTaskDelete(_currentTask);
-    _currentTask = nullptr;
-    log_i("Trip stopped !");
-  }
-  MotorsController->SetMoveSpeed({0 , 0});
-}
-
 void TripControllerClass::RotateToHeading(double heading, uint timeout) {
   int16_t delta = angle_difference(Compass->GetHeading(), heading);
   int16_t lastDelta = delta;
@@ -62,7 +75,7 @@ void TripControllerClass::RotateToHeading(double heading, uint timeout) {
   }
   
   u_int32_t startTime = millis();
-  while ((delta < -10 || delta > 10 || abs(delta) < abs(lastDelta)) && millis() <= startTime + timeout)
+  while (_state != TripState::STOP && (delta < -10 || delta > 10 || abs(delta) < abs(lastDelta)) && millis() <= startTime + timeout)
   {
     MoveSpeed move = {
       .left = (int16_t)(delta > 0 ? ROTATE_MIN_SPEED + delta / 2 : -ROTATE_MIN_SPEED + delta / 2),
@@ -70,6 +83,9 @@ void TripControllerClass::RotateToHeading(double heading, uint timeout) {
     };
     MotorsController->SetMoveSpeed(move);
     delay(100);
+
+    _handlePause(startTime);
+
     lastDelta = delta;
     delta = angle_difference(Compass->GetHeading(), heading);
     if (abs(delta) > 90 && delta * lastDelta < 0) {
@@ -103,7 +119,7 @@ void TripControllerClass::GoTo(geo_pos &pos, u_int8_t speed, uint timeout) {
     .right = speed
   };
 
-  while ((distance > 0.4 || distance <= lastDistance) && millis() <= startTime + timeout)
+  while (_state != TripState::STOP && (distance > 0.4 || distance <= lastDistance) && millis() <= startTime + timeout)
   {
     lastDistance = distance;
     correction = constrain(
@@ -114,6 +130,9 @@ void TripControllerClass::GoTo(geo_pos &pos, u_int8_t speed, uint timeout) {
 
     MotorsController->SetMoveSpeed(move);
     delay(100);
+
+    _handlePause(startTime);
+
     origin = GnssService->GetPosition();
     bearing = bearing_to(origin, pos);
     if (millis() >= startTime + 2000) { // wait before try to correct
@@ -150,4 +169,33 @@ void TripControllerClass::_gotoTask(void * param) {
   vTaskDelete(NULL);
 }
 
+void TripControllerClass::_handlePause(uint32_t& startTime)
+{
+    if (_state == TripState::PAUSE)
+    {
+      log_d("Entering pause mode");
+      uint32_t pauseTime = millis();
+
+      // store motors state
+      MoveSpeed move = MotorsController->GetMoveSpeed();
+      int16_t cut = MotorsController->GetCutSpeed();
+
+      // stop motors
+      MotorsController->SetCutSpeed(0);
+      MotorsController->SetMoveSpeed({0, 0});
+
+      while (_state == TripState::PAUSE)
+      {
+        delay(1000);
+      }
+
+      // restart motors
+      MotorsController->SetCutSpeed(cut);
+      MotorsController->SetMoveSpeed(move);
+
+      startTime = millis() - (pauseTime - startTime);
+      log_d("Leaving pause mode");
+    }
+}
+
 TripControllerClass* TripController = new TripControllerClass();

work.gcode

@@ -1,8 +1,8 @@
-G0 X-146 Y-882
-G1 X-5003 Y2791
-G1 X-11560 Y-6810
-G1 X-4495 Y-11850
-G1 X1089 Y-6304
-G1 X-2298 Y-3843
-G1 X-146 Y-882
+G0 X-90 Y-890
+G1 X-4946 Y2191
+G1 X-11110 Y-7523
+G1 X-3742 Y-12110
+G1 X1488 Y-6227
+G1 X-2052 Y-3980
+G1 X-90 Y-890
 M30