state machine basics

src/CMakeLists.txt

@@ -100,7 +100,6 @@ if(NOT BUILD_SIMULATION)
 endif()
 
 add_library(sim_motor STATIC
-  drivers/motors/sim_motor.cpp
   drivers/motors/motor.cpp
 )
 target_include_directories(sim_motor PUBLIC

src/drivers/CMakeLists.txt

@@ -1,5 +1,5 @@
 
-add_library(motors  motors/motor.cpp motors/sim_motor.cpp motors/thruster.cpp)
+add_library(motors  motors/motor.cpp  motors/thruster.cpp)
 
 
 find_package(PkgConfig REQUIRED)

src/drivers/motors/sim_motor.hpp

@@ -1,22 +1,78 @@
 #pragma once
 #include "motor.hpp"
 #include <httplib.h>
+#include "../../types/MotorLocation.hpp"
 
-class SimulatedMotor : public Motor {
+class SimulatedMotor : public Motor
+{
 
 public:
-  // At some point we should prob use this datatype everywhere, for now its here
-  enum MotorLocation { FrontRight, FrontLeft, BackRight, BackLeft };
   MotorLocation ml;
   bool armed;
-  SimulatedMotor(SimulatedMotor::MotorLocation ml, int speed = 0.0,
-                 float cycle = 50);
-  int setSpeed(float speed) override;
 
-  int setFrequency(float cycle) override;
+  SimulatedMotor(MotorLocation ml, int speed,
+                                 float cycle) : Motor(speed, cycle)
+  {
+    if (sim_connection == 0)
+    {
+      sim_connection = new httplib::Client(SIM_ADDR);
+      httplib::Result out = sim_connection->Get("/heartbeat");
+      if (out == nullptr)
+      {
+        throw std::runtime_error("No simulation connection found");
+      }
+      std::cout << "Status code from sim:" << out->status << std::endl;
+    }
+    this->ml = ml;
+  }
+  int setSpeed(float speed) override {
+  if (!this->armed) {
+    throw std::logic_error("Tried to set the speed of an unarmed motor");
+  }
+  this->speed = speed;
+  // In the long run we should add a json lib
+  std::ostringstream oss;
+  oss << "{\"speed\":" << speed << "}";
+  std::string s = oss.str();
+  auto out = sim_connection->Post(
+      this->GetMotorPath().c_str(), s.c_str(), "application/json");
+  if (out == nullptr) {
+    std::cout << "Error sending sim info" << std::endl;
+  } else {
+    std::cout << out->status << std::endl;
+  }
+
+  return 0;
+}
+int setFrequency(float cycle) override {
+  // idk if this will be called at all
+  return 0;
+}
+
+void armMotor()override { this->armed = true; }
 
-  void armMotor() override;
 
 private:
-  std::string GetMotorPath();
+  std::string GetMotorPath()
+  {
+    switch (this->ml)
+    {
+    case FrontLeft:
+      return "/motor/top_left/set";
+      break;
+    case FrontRight:
+      return "/motor/top_right/set";
+      break;
+    case BackLeft:
+      return "/motor/bottom_left/set";
+      break;
+    case BackRight:
+      return "/motor/bottom_right/set";
+      break;
+    default:
+      // Error?
+      break;
+    }
+    return "";
+  }
 };

src/neptune.hpp

@@ -0,0 +1,59 @@
+#pragma once
+#include "drivers/motors/motor.hpp"
+#include "drivers/gps/gps.hpp"
+#include <cstdlib>
+#include <unistd.h>
+#include "states/state.hpp"
+
+class Neptune {
+    public:
+    State* state;
+    inline static Neptune* instance = nullptr;
+    GPS* gps;
+    Motor* front_left;
+    Motor* front_right;
+    Motor* back_left;
+    Motor* back_right;
+    Motor* all_motors[4];
+    /**
+     * Gets the heading, in radians of the robot. 0 indicates north, pi indicates south
+     * @return The heading
+     */
+    double get_heading(){
+        //TODO - Use IMU to calculate this
+        return 0;
+    }
+    std::array<std::array<double, 5>, 2> GetMotorVectorMatrice() {
+         std::array<std::array<double, 5>, 2>  out = {
+         };
+        for(int m =  0; m <= 3;  m ++) {
+            out[0][m] = thrust_vector(static_cast<MotorLocation>(m)).latitude;
+            out[1][m] = thrust_vector(static_cast<MotorLocation>(m)).longitude;
+        }
+        return out;
+     }
+    /**
+     * Starts neptune. This method will functionally take over the thread it is called from
+     */
+    void start(){
+        Neptune::instance = this;
+        gps->await_lock(5, 500);
+        std::cout << "Arming motors" << std::endl;
+        front_left->armMotor();
+        front_right->armMotor();
+        back_left->armMotor();
+        back_right->armMotor();
+
+        all_motors[0] = front_left;
+        all_motors[1] = front_right;
+        all_motors[2] = back_left;
+        all_motors[3] = back_right;
+
+        while (true)
+        {
+            std::this_thread::sleep_for(this->state->tick());
+        }
+
+    }
+
+};

src/sim_main.cpp

@@ -1,10 +1,28 @@
 #include "drivers/gps/gps.hpp"
 #include "drivers/gps/simulated_gps.hpp"
+#include "drivers/motors/sim_motor.hpp"
+#include "drivers/motors/motor.hpp"
+#include "types/MotorLocation.hpp"
+#include "neptune.hpp"
+#include "states/navigate.hpp"
 int main() {
   GPS *g = new Simulated_GPS();
 
-  while (true) {
-    std::cout << *g->location() << std::endl;
-    sleep(1);
-  }
+  Motor* front_left = new SimulatedMotor(MotorLocation::FrontLeft, 0 ,50);
+  Motor* front_right = new SimulatedMotor(MotorLocation::FrontRight, 0 ,50);
+  Motor* back_left = new SimulatedMotor(MotorLocation::BackLeft, 0, 50);
+  Motor* back_right = new SimulatedMotor(MotorLocation::BackRight, 0, 50);
+  Neptune* neptune = new Neptune();
+  neptune->gps = g;
+  neptune->back_left = back_left;
+  neptune->back_right = back_right;
+  neptune->front_left = front_left;
+  neptune->front_right = front_right;
+  GPSCoordinate goal = GPSCoordinate{
+            0,
+            10
+    };
+neptune->state = new Navigate(goal);
+  neptune->start();
+
 }

src/states/idle.hpp

@@ -0,0 +1,11 @@
+
+#pragma once
+#include "state.hpp"
+class Idle : public State {
+    public:
+
+    std::chrono::milliseconds tick() override {
+        std::cout << "Idling" << std::endl;
+        return std::chrono::milliseconds(5000);
+    }
+};

src/states/navigate.hpp

@@ -0,0 +1,96 @@
+#pragma once
+#include "state.hpp"
+#include "../../neptune.hpp"
+#include <optional>
+#include <array>
+#include <cmath>
+#include <iostream>
+/**Tries to solve with no negative inputs, so motors don't fire backwards
+ * This is vibe coded slop. In the future, we should add a linear algebra lib instead of using this
+ */
+std::optional<std::array<double, 4>> solveNonNegative(const std::array<std::array<double, 5>, 2>& mat)
+{
+    constexpr int    VARS = 4;
+    constexpr double EPS  = 1e-9;
+
+    // Try every pair of "basic" variables; the remaining two are fixed at 0.
+    for (int p = 0; p < VARS; ++p) {
+        for (int q = p + 1; q < VARS; ++q) {
+
+            // Extract the 2x2 sub-system for columns p and q.
+            const double a00 = mat[0][p],  a01 = mat[0][q],  b0 = mat[0][4];
+            const double a10 = mat[1][p],  a11 = mat[1][q],  b1 = mat[1][4];
+
+            const double det = a00 * a11 - a01 * a10;
+            if (std::abs(det) < EPS)
+                continue;   // columns are linearly dependent — skip
+
+            // Cramer's Rule
+            const double xp = (b0 * a11 - b1 * a01) / det;
+            const double xq = (a00 * b1 - a10 * b0) / det;
+
+            if (xp < -EPS || xq < -EPS)
+                continue;   // a basic variable went negative — skip
+
+            // Valid non-negative basic feasible solution found.
+            std::array<double, 4> solution{};   // nosn-basics default to 0
+            solution[p] = std::max(0.0, xp);    // clamp -0 artefacts
+            solution[q] = std::max(0.0, xq);
+            return solution;
+        }
+    }
+
+    return std::nullopt;   // all 6 bases checked, none feasible
+}
+
+
+
+class Navigate : public State {
+    public:
+    bool finished;
+    GPSCoordinate goal;
+    Navigate(GPSCoordinate goal) : goal(goal){
+
+    }
+
+
+    std::chrono::milliseconds tick() override {
+        double current_heading = Neptune::instance->get_heading();
+        GPSCoordinate* loc = Neptune::instance->gps->location();
+        GPSCoordinate goal_dif =  goal - *loc;
+        double mag = pow( goal_dif.latitude * goal_dif.latitude + goal_dif.longitude * goal_dif.longitude, 0.5);
+        std::cout << "Dist:" << mag << std::endl;
+        if(mag < 0.1){
+            //super close, get a random new cord
+            goal.latitude = ((rand() % 30) - 15);
+            goal.longitude = ((rand() % 30) - 15);
+        }
+        goal_dif.latitude /= mag;
+        goal_dif.longitude /= mag;
+        /**
+         * We know we want to translate along goal_dif
+         * We can form goal_dif by combining the vectors of each motor's thrust vectors
+         * Thus, our problem is to form a linear combination of the goal vector through the 4 thrusters, and take one solution
+         * ex an array
+         * m1x m2x m3x m4x gx
+         * and for y
+         */
+        std::array<std::array<double, 5>, 2> mat = Neptune::instance->GetMotorVectorMatrice();
+        mat[0][4] = goal_dif.latitude;
+        mat[1][4] = goal_dif.longitude;
+        std::optional<std::array<double, 4>> out = solveNonNegative(mat);
+        std::cout << goal <<   *loc << goal_dif  << std::endl;
+        if(out.has_value()){
+            for(int i = 0; i < out.value().size(); i ++) {
+                std::cout << i << ":" << out.value()[i] << std::endl;
+                //TODO normalize to some constant range based on desired activation
+                Neptune::instance->all_motors[i]->setSpeed(out.value()[i]);
+            }
+        }
+        else {
+            std::cout << "Failed to solve" << std::endl;
+        }
+        delete loc;
+        return std::chrono::milliseconds(1000);
+    }
+};