Feat/rebuild movement

.gitignore

@@ -5,7 +5,15 @@ cmake-build-debug
 ### Arduino ###
 .theia/
 .arduino/
+libraries/
+tools
 
 ### Project-specific ###
 .doxygen/theme
 docs/
+
+### vcsode ###
+.vscode/
+
+### python ###
+tests/__pycache__/

pathfinder.ino

@@ -45,6 +45,7 @@ JsonDocument apiResponse;
 #include "src/extras.h"
 #include "src/wifi.h"
 #include "src/api.h"
+#include "src/api_v2.h"
 #include "src/uart.h"
 #include "src/http_server.h"
 

src/api.h

@@ -1,4 +1,4 @@
-
+#if API_V1
 String jsonAPIHandler(String stringInput)
 {
   JsonDocument jsonInput;
@@ -546,17 +546,7 @@ void planStepToCommand(String movementPlanStep)
 // }
 // }
 
-String checkPlanSchema(JsonArray steps)
-{
-  for (String step : steps)
-  {
-    if (step.indexOf("angle") == -1 || step.indexOf("distance") == -1  || step.indexOf("direction") == -1 )
-    {
-      return "Invalid movement plan schema, it needs to contain angle, distance and direction";
-    }
-  }
-  return "valid";
-}
+
 
 String apiMovementPlanHandler(String jsonInput)
 {
@@ -579,4 +569,18 @@ String apiMovementPlanHandler(String jsonInput)
     planStepToCommand(i);
   }
   return "movement plan executed.";
+}
+
+#endif
+
+String checkPlanSchema(JsonArray steps)
+{
+  for (String step : steps)
+  {
+    if (step.indexOf("angle") == -1 || step.indexOf("distance") == -1  || step.indexOf("direction") == -1 )
+    {
+      return "Invalid movement plan schema, it needs to contain angle, distance and direction";
+    }
+  }
+  return "valid";
 }

src/api_v2.h

@@ -0,0 +1,184 @@
+#if API_V2
+#include "modles.h"
+#include <vector>
+
+const unsigned long AUTO_STOP_DELAY = 3000;
+unsigned long lastCommandTime = 0;
+
+std::vector<step> movementSteps;
+
+
+void move(int angle, int distance, int direction)
+{
+  // Constants
+  const float speed = 0.5;
+  const int spinDelay = 0;       // 2 seconds delay before spinning
+  const int msPerDegree = 800 / 90; // ms needed for 1 degree of rotation
+  //move amount
+  // float L;
+  // float R;
+
+  struct vector movement;
+
+
+  // Step 1: Spin according to angle
+  if (angle != 0)
+  {
+    String rotationCommand;
+    int rotationDuration;
+
+
+    // Wait before spinning
+    delay(spinDelay);
+
+    if (angle > 0 && angle <= 180)
+    {
+      // Spin right
+      movement.l = speed;
+      movement.r = speed * -1;
+      rotationDuration = angle * msPerDegree;
+    }
+    else if (angle > 180 && angle < 360)
+    {
+      // Spin left
+      movement.l = speed * -1;
+      movement.r = speed;
+      rotationDuration = (360 - angle) * msPerDegree;
+    }
+    else
+    {
+      Serial.println("Invalid angle value");
+      return;
+    }
+
+    // Perform rotation
+    Serial.println("Rotation command: " + toString(&movement));
+    Serial.println("Rotation duration: " + String(rotationDuration) + " ms");
+    setAPIGoalSpeed(&movement);
+    delay(rotationDuration);
+
+    // Stop rotation
+    Serial.println("Stopping rotation");
+    setAPIGoalSpeed(0, 0);
+    delay(100); // Short pause after rotation
+  }
+
+  // Step 2: Move in the specified direction for the given distance
+  movement.l = speed * direction;
+  movement.r = speed * direction;
+
+  // Perform movement
+  Serial.println("Movement command: " + toString(&movement));
+  setAPIGoalSpeed(&movement);
+
+  // Calculate total movement time
+  unsigned long movementDuration = 100UL * distance; // 100ms per unit of distance
+  unsigned long startTime = millis();
+
+  while (millis() - startTime < movementDuration)
+  {
+    Serial.println(startTime);
+    Serial.println(movementDuration);
+    Serial.println(AUTO_STOP_DELAY);
+    // Check if we need to send a new command to prevent auto-stop
+    if (millis() - lastCommandTime >= AUTO_STOP_DELAY - 100)
+    { // Send command 100ms before auto-stop
+      setAPIGoalSpeed(&movement);
+      lastCommandTime = millis();
+    }
+    delay(10); // Small delay to prevent excessive looping
+  }
+
+  // Stop movement
+  Serial.println("Stopping movement");
+  movement.l = 0;
+  movement.r = 0;
+  setAPIGoalSpeed(&movement);
+  lastCommandTime = millis();
+}
+
+void planStepToCommandV2(step movementPlanStep)
+{
+
+  Serial.println("angle: " + String(movementPlanStep.angle));
+  Serial.println("Distance: " + String(movementPlanStep.distance));
+  Serial.println("Direction: " + movementPlanStep.direction);
+  if (movementPlanStep.direction == "forward")
+  {
+    move(movementPlanStep.angle, movementPlanStep.distance, 1);
+  }
+  else if (movementPlanStep.direction == "backward")
+  {
+    move(movementPlanStep.angle, movementPlanStep.distance, -1);
+  }
+  else
+  {
+    Serial.println("Invalid direction");
+    return;
+  }
+}
+
+String getSteps(){
+  JsonDocument doc;
+  String output;
+  // for (step s : movementSteps)
+  for (int i = 0; i < movementSteps.size(); i++)
+  {
+    doc[i]["angle"] = movementSteps[i].angle;
+    doc[i]["distance"] = movementSteps[i].distance;
+    doc[i]["direction"] = movementSteps[i].direction;
+  }
+  serializeJson(doc, output);
+  Serial.println(output);
+
+  return output;
+
+}
+
+String runSteps(){
+  String output = "run steps\n";
+  for (step s : movementSteps)
+  // for (int i = 0; i < arraySize; i++)
+  {
+    planStepToCommandV2(s);
+    output = output + "run command: angle" + s.angle + " distance: " + s.distance + "direction: " + s.direction + "\n";
+
+  }
+
+  return output;
+}
+
+String apiv2MovementPlanHandler(String jsonInput)
+{
+  JsonDocument input;
+
+  // parse the JSON file using ArduinoJson.
+  deserializeJson(input, jsonInput);
+  Serial.println("Received a movement plan"); //, String(input["name"]));
+                                              //    Serial.println(input);
+  // for (int i = 0; i < input["steps"].size(); i++) {
+  JsonArray steps = input["steps"].as<JsonArray>();
+  String checkResult = checkPlanSchema(steps);
+  movementSteps.clear();
+  step moveStep;
+  if (checkResult != "valid")
+  {
+    return checkResult;
+  }
+  for (String i : steps)
+  {
+    Serial.println("Step");
+    JsonDocument step;
+    deserializeJson(step, i);
+
+    moveStep.angle = step["angle"].as<int>();
+    moveStep.distance = step["distance"].as<int>();
+    moveStep.direction = step["direction"].as<String>();
+    // planStepToCommandV2(moveSteps[count]);
+    movementSteps.push_back(moveStep);
+
+  }
+  // runSteps();
+  return "movement plan executed.";
+}
+#endif

src/config.h

@@ -1,8 +1,10 @@
 //compile time flags
 #define PROD 0
-#define INFO_PRINT 0
+#define INFO_PRINT 1
 #define WEB_PAGE 0
-#define BASE_FEEDBACK_FLOW 0
+#define BASE_FEEDBACK_FLOW 1
+#define API_V1 0
+#define API_V2 1
 #if PROD
   #define INFO_PRINT 0
   #define WEB_PAGE 0
@@ -33,7 +35,7 @@ byte moduleType = 0;
 #if BASE_FEEDBACK_FLOW
 // 0: turn off base info feedback flow.
 // 1: [default] turn on base info feedback flow.
-bool baseFeedbackFlow = 1;
+bool baseFeedbackFlow = 0;
 #endif
 
 String thisMacStr;

src/http_server.h

@@ -26,13 +26,16 @@ void webCtrlServer() {
     jsonCmdReceive.clear();
   });
 
+  server.on("/v1/device/status", []() {
+    server.send(200, "application/json", deviceStatusFeedback());
+  });
+
+  #if API_V1
   server.on("/v1", []() {
     server.send(200, "text/plain", "/v1");
   });
 
-  server.on("/v1/device/status", []() {
-    server.send(200, "application/json", deviceStatusFeedback());
-  });
+
 
   server.on("/v1/device/battery", []() {
     server.send(200, "application/json", batteryStatusFeedback());
@@ -70,7 +73,18 @@ void webCtrlServer() {
   server.on("/v1/movement-plan", HTTP_POST, []() {
     server.send(200, "text/plain", apiMovementPlanHandler(server.arg("plain")));
  });
-
+ #endif
+ #if API_V2
+ server.on("/v2/movement", HTTP_GET, []() {
+    server.send(200, "text/plain", getSteps());
+  });
+  server.on("/v2/movement", HTTP_POST, []() {
+    server.send(200, "text/plain", apiv2MovementPlanHandler(server.arg("plain")));
+  });
+  server.on("/v2/movement/run", HTTP_POST, []() {
+    server.send(200, "text/plain", runSteps());
+  });
+ #endif
 
   // Start server
   server.begin();

src/modles.h

@@ -0,0 +1,19 @@
+#pragma once
+
+struct vector
+{
+    float r;
+    float l;
+};
+
+
+String toString(vector* self){
+    return "L: " + String(self->l) +" R: " + String(self->r);
+}
+
+struct step
+{
+    int angle;
+    String direction;
+    int distance;
+};

src/motion.h

@@ -1,3 +1,5 @@
+#include "modles.h"
+
 // switch parts
 int switch_pwm_A = 0;
 int switch_pwm_B = 0;
@@ -248,6 +250,12 @@ void rightCtrl(float pwmInputB){
   }
 }
 
+void setAPIGoalSpeed(vector *movement) {
+  usePIDCompute = false;
+  leftCtrl(movement->l * 512 * 0.5);
+  rightCtrl(movement->r * 512 * 0.5);
+}
+
 void setAPIGoalSpeed(float inputLeft, float inputRight) {
   usePIDCompute = false;
   leftCtrl(inputLeft * 512 * 0.5);

tests/.python-version

@@ -0,0 +1 @@
+3.10

tests/README.md

@@ -0,0 +1,11 @@
+# Testing
+
+## pre recs
+
+uv: https://github.com/astral-sh/uv
+
+## run tests
+
+As tests run with user input they require a -s. 
+
+`uv run python3 -m pytest -s` or in a bash ternimal you can run `./runtest.sh`

tests/common.py

@@ -0,0 +1 @@
+ip = "192.168.0.105"

tests/main.py

@@ -0,0 +1 @@
+print("hello")

tests/pyproject.toml

@@ -0,0 +1,10 @@
+[project]
+name = "tests"
+version = "0.1.0"
+description = "Add your description here"
+readme = "README.md"
+requires-python = ">=3.10"
+dependencies = [
+    "pytest>=8.4.2",
+    "requests>=2.32.5",
+]

tests/runtest.sh

@@ -0,0 +1 @@
+uv run python3 -m pytest $* -s