diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index adc8c5f..750dd11 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/drivers/CMakeLists.txt b/src/drivers/CMakeLists.txt index a10c00a..e3e8489 100644 --- a/src/drivers/CMakeLists.txt +++ b/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) diff --git a/src/drivers/motors/noah_test.cpp b/src/drivers/motors/noah_test.cpp deleted file mode 100644 index ef223e6..0000000 --- a/src/drivers/motors/noah_test.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "motor.hpp" -#include "sim_motor.hpp" -#include -#include -#include -int main() { - srand(time(0)); - Motor *m = new SimulatedMotor(SimulatedMotor::BackRight); - Motor *m1 = new SimulatedMotor(SimulatedMotor::FrontRight); - Motor *m2 = new SimulatedMotor(SimulatedMotor::FrontLeft); - Motor *m3 = new SimulatedMotor(SimulatedMotor::BackLeft); - m->armMotor(); - m1->armMotor(); - m2->armMotor(); - m3->armMotor(); - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_real_distribution dis(-1.0, 1.0); - while (true) { - if (rand() % 2) - m->setSpeed(dis(gen)); - if (rand() % 2) - m1->setSpeed(dis(gen)); - sleep(3); - } -} diff --git a/src/drivers/motors/sim_motor.cpp b/src/drivers/motors/sim_motor.cpp deleted file mode 100644 index d79a32d..0000000 --- a/src/drivers/motors/sim_motor.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "sim_motor.hpp" -#include -#include "../common/sim.hpp" -std::string SimulatedMotor::GetMotorPath() { - switch (this->ml) { - case SimulatedMotor::FrontLeft: - return "/motor/top_left/set"; - break; - case SimulatedMotor::FrontRight: - return "/motor/top_right/set"; - break; - case SimulatedMotor::BackLeft: - return "/motor/bottom_left/set"; - break; - case SimulatedMotor::BackRight: - return "/motor/bottom_right/set"; - break; - default: - // Error? - break; - } - return ""; -} - -SimulatedMotor::SimulatedMotor(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 SimulatedMotor::setSpeed(float speed) { - 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 SimulatedMotor::setFrequency(float cycle) { - // idk if this will be called at all - return 0; -} - -void SimulatedMotor::armMotor() { this->armed = true; } diff --git a/src/drivers/motors/sim_motor.hpp b/src/drivers/motors/sim_motor.hpp index dfd05e4..5a4f829 100644 --- a/src/drivers/motors/sim_motor.hpp +++ b/src/drivers/motors/sim_motor.hpp @@ -1,22 +1,78 @@ #pragma once #include "motor.hpp" #include +#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 ""; + } }; diff --git a/src/neptune.hpp b/src/neptune.hpp new file mode 100644 index 0000000..ab0e225 --- /dev/null +++ b/src/neptune.hpp @@ -0,0 +1,59 @@ +#pragma once +#include "drivers/motors/motor.hpp" +#include "drivers/gps/gps.hpp" +#include +#include +#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, 2> GetMotorVectorMatrice() { + std::array, 2> out = { + }; + for(int m = 0; m <= 3; m ++) { + out[0][m] = thrust_vector(static_cast(m)).latitude; + out[1][m] = thrust_vector(static_cast(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()); + } + + } + +}; diff --git a/src/sim_main.cpp b/src/sim_main.cpp index 6b574e5..0fd5f18 100644 --- a/src/sim_main.cpp +++ b/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(); + } diff --git a/src/states/CMakeLists.txt b/src/states/CMakeLists.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/states/idle.hpp b/src/states/idle.hpp new file mode 100644 index 0000000..6ecca7f --- /dev/null +++ b/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); + } +}; diff --git a/src/states/navigate.hpp b/src/states/navigate.hpp new file mode 100644 index 0000000..066ec3c --- /dev/null +++ b/src/states/navigate.hpp @@ -0,0 +1,96 @@ +#pragma once +#include "state.hpp" +#include "../../neptune.hpp" +#include +#include +#include +#include +/**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> solveNonNegative(const std::array, 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 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, 2> mat = Neptune::instance->GetMotorVectorMatrice(); + mat[0][4] = goal_dif.latitude; + mat[1][4] = goal_dif.longitude; + std::optional> 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); + } +}; diff --git a/src/states/state.hpp b/src/states/state.hpp new file mode 100644 index 0000000..795b956 --- /dev/null +++ b/src/states/state.hpp @@ -0,0 +1,18 @@ +#pragma once +#include "../neptune.hpp" +#include +//Represents a state the usv can be in +class State{ + public: + State(){ + + } + + /** + * @brief The driving logic for this tick + * @returns The time, in ms, to wait before ticking again + */ + virtual std::chrono::milliseconds tick(){ + return std::chrono::milliseconds(5000); + } +}; diff --git a/src/types/GPSCoordinate.hpp b/src/types/GPSCoordinate.hpp index 724593c..5ca6c4d 100644 --- a/src/types/GPSCoordinate.hpp +++ b/src/types/GPSCoordinate.hpp @@ -7,6 +7,84 @@ struct GPSCoordinate { double longitude; }; +// Equality operators +inline bool operator==(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return lhs.latitude == rhs.latitude && lhs.longitude == rhs.longitude; +} + +inline bool operator!=(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return !(lhs == rhs); +} + +// Comparison operators (lexicographical) +inline bool operator<(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + if (lhs.latitude != rhs.latitude) return lhs.latitude < rhs.latitude; + return lhs.longitude < rhs.longitude; +} + +inline bool operator>(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return rhs < lhs; +} + +inline bool operator<=(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return !(rhs < lhs); +} + +inline bool operator>=(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return !(lhs < rhs); +} + +// Arithmetic operators +inline GPSCoordinate operator+(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return {lhs.latitude + rhs.latitude, lhs.longitude + rhs.longitude}; +} + +inline GPSCoordinate operator-(GPSCoordinate const& lhs, GPSCoordinate const& rhs) { + return {lhs.latitude - rhs.latitude, lhs.longitude - rhs.longitude}; +} + +inline GPSCoordinate operator*(GPSCoordinate const& coord, double scalar) { + return {coord.latitude * scalar, coord.longitude * scalar}; +} + +inline GPSCoordinate operator*(double scalar, GPSCoordinate const& coord) { + return coord * scalar; +} + +inline GPSCoordinate operator/(GPSCoordinate const& coord, double scalar) { + return {coord.latitude / scalar, coord.longitude / scalar}; +} + +// Unary minus +inline GPSCoordinate operator-(GPSCoordinate const& coord) { + return {-coord.latitude, -coord.longitude}; +} + +// Compound assignment +inline GPSCoordinate& operator+=(GPSCoordinate& lhs, GPSCoordinate const& rhs) { + lhs.latitude += rhs.latitude; + lhs.longitude += rhs.longitude; + return lhs; +} + +inline GPSCoordinate& operator-=(GPSCoordinate& lhs, GPSCoordinate const& rhs) { + lhs.latitude -= rhs.latitude; + lhs.longitude -= rhs.longitude; + return lhs; +} + +inline GPSCoordinate& operator*=(GPSCoordinate& coord, double scalar) { + coord.latitude *= scalar; + coord.longitude *= scalar; + return coord; +} + +inline GPSCoordinate& operator/=(GPSCoordinate& coord, double scalar) { + coord.latitude /= scalar; + coord.longitude /= scalar; + return coord; +} + inline std::ostream &operator<<(std::ostream &os, GPSCoordinate const &coord) { return os << "GPSCoordinate{" << coord.latitude << ", " << coord.longitude << "}"; diff --git a/src/types/MotorLocation.hpp b/src/types/MotorLocation.hpp new file mode 100644 index 0000000..84d971d --- /dev/null +++ b/src/types/MotorLocation.hpp @@ -0,0 +1,22 @@ +#pragma once +#include "GPSCoordinate.hpp" +#include +enum MotorLocation { FrontRight, FrontLeft, BackRight, BackLeft }; + +constexpr double SQRT2 = 1.41421356237309504880; +//TODO make sure these stay up to date, and maybe consider a seperate point class that isn't a GPScoord +GPSCoordinate thrust_vector(MotorLocation m){ + switch(m){ + case FrontLeft: + return {0.5 * SQRT2, 0.5 * SQRT2}; + case FrontRight: + return {-0.5* SQRT2, 0.5* SQRT2}; + case BackRight: + return {-0.5* SQRT2, -0.5* SQRT2}; + case BackLeft: + return {0.5* SQRT2, -0.5* SQRT2}; + } + +} +// 0 - lat +// 1 - long