diff --git a/src/driveModi/autopilot.cpp b/src/driveModi/autopilot.cpp index e69de29..ae867e0 100644 --- a/src/driveModi/autopilot.cpp +++ b/src/driveModi/autopilot.cpp @@ -0,0 +1,14 @@ +#include "driveModi/autopilot.h" + +Autopilot::Autopilot() { + this->debug = new DebugMqtt("Autopilot"); +} + +void Autopilot::init(Route *route, MoveControl *moveControl) { + this->route = route; + this->moveControl = moveControl; +} + +void Autopilot::runAutopilot() { + +} \ No newline at end of file diff --git a/src/driveModi/autopilot.h b/src/driveModi/autopilot.h index ae6becc..9d16a2c 100644 --- a/src/driveModi/autopilot.h +++ b/src/driveModi/autopilot.h @@ -5,15 +5,20 @@ #include "route.h" #include "moveControl.h" +#include "debugMqtt.h" class Autopilot { public: Autopilot(); + void init(Route *route, MoveControl *moveControl); + void runAutopilot(); + + private: Route *route; MoveControl *moveControl; - TinyGPSPlus *gps; + DebugMqtt *debug; }; diff --git a/src/driveModi/captureRoute.cpp b/src/driveModi/captureRoute.cpp index e69de29..19e4407 100644 --- a/src/driveModi/captureRoute.cpp +++ b/src/driveModi/captureRoute.cpp @@ -0,0 +1,26 @@ +#include "driveModi/captureRoute.h" + +CaptureRoute::CaptureRoute() { + this->debug = new DebugMqtt("CaptureRoute"); +} + +void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) { + this->route = route; + ManualControl::init(moveControl, motorControlLeft, motorControlRight); + +} + +void CaptureRoute::runCaptureRoute() { + this->runManualControl(); + + static uint64_t last_millis = 0; + if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) { + return; + } + last_millis = millis(); + + //Add Point to route + Point p; + p.lat = this. + this->route->addPoint() +} \ No newline at end of file diff --git a/src/driveModi/captureRoute.h b/src/driveModi/captureRoute.h index e69de29..c22aabb 100644 --- a/src/driveModi/captureRoute.h +++ b/src/driveModi/captureRoute.h @@ -0,0 +1,22 @@ +#ifndef CAPTURE_ROUTE_H +#define CAPTURE_ROUTE_H + +#include "driveModi/manualControl.h" +#include "debugMqtt.h" +#include "route.h" + +#include "config.h" + +class CaptureRoute : ManualControl { + public: + CaptureRoute(); + + void init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight); + void runCaptureRoute(); + + private: + Route *route; + DebugMqtt *debug; +}; + +#endif // CAPTURE_ROUTE_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index c61ec35..a728a9a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,6 +12,7 @@ #include "driveModi/captureRoute.h" #include "driveModi/manualControl.h" #include "moveControl.h" +#include "route.h" #include "debugMqtt.h" void callbackControllerAction(); @@ -25,9 +26,12 @@ MotorControl right_motor; Speedometer speedometer_left; Speedometer speedometer_right; MoveControl moveController; -DebugMqtt debugger("main"); +Route route; +DebugMqtt debug("main"); ManualControl manualControl; +CaptureRoute captureRoute; +Autopilot autopilot; IPAddress local_IP(WLAN_IP); IPAddress gateway(WLAN_GATEWAY); @@ -90,6 +94,8 @@ void setup() { // Init driveModi manualControl.init(&moveController, &left_motor, &right_motor); + captureRoute.init(&route, &moveController, &left_motor, &right_motor); + autopilot.init(&route, &moveController); } void loop() { @@ -101,6 +107,7 @@ void loop() { right_motor.runMotorControl(); speedometer_left.runSpeedometer(); speedometer_right.runSpeedometer(); + route.runRoute(); switch (driveMode) { case manualControl_e: @@ -108,11 +115,11 @@ void loop() { break; case captureRoute_e: - /* code */ + captureRoute.runCaptureRoute(); break; case autopilot_e: - /* code */ + autopilot.runAutopilot(); break; default: @@ -143,14 +150,32 @@ void reconnectMqtt() { } void callbackControllerAction() { - if (Ps3.event.button_down.start) { - + if (Ps3.event.button_down.ps) { + switch (driveMode) { + case manualControl_e: + driveMode = DriveMode::captureRoute_e; + debug.sendMsg(Loglevel::info, "New driveMode = CaptureRoute"); + break; + + case captureRoute_e: + driveMode = DriveMode::autopilot_e; + debug.sendMsg(Loglevel::info, "New driveMode = Autopilot"); + break; + + case autopilot_e: + driveMode = DriveMode::manualControl_e; + debug.sendMsg(Loglevel::info, "New driveMode = ManualControl"); + break; + + default: + break; + } } } void callbackControllerConnect() { Serial.println("Controller connected to ESP32"); - debugger.sendMsg(Loglevel::info, "Controller connected to ESP32"); + debug.sendMsg(Loglevel::info, "Controller connected to ESP32"); } void controllerPrintBattery() { diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 57bedba..41751d6 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -25,6 +25,10 @@ void MoveControl::runMoveControl() { this->calcWheelSpeed(); this->regulateMotors(); + + char str[64]; + sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, tar_left: %3.2f, tar_right: %3.2f", this->x_speed, this->rotation_speed, this->wheelspeed_left_target, this->wheelspeed_right_target); + debug->sendMsg(Loglevel::debug, str); } void MoveControl::setDrivingStatus(DrivingStatus status) { diff --git a/src/route.cpp b/src/route.cpp index 51a51d8..9cc3123 100644 --- a/src/route.cpp +++ b/src/route.cpp @@ -1,14 +1,39 @@ #include "route.h" -Route::Route(){ - +Route::Route() { + Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX); + this->debug = new DebugMqtt("Route"); } -void Route::addPoint(Point point) { +void Route::runRoute() { + static uint64_t last_millis = 0; + if (millis() - last_millis < RUN_ROUTE_DELAY) { + return; + } + last_millis = millis(); + + while (Serial1.available() > 0) + gps.encode(Serial1.read()); + + if (gps.location.isUpdated() && gps.location.isValid()) { + this->currentLocation.lat = gps.location.lat(); + this->currentLocation.lon = gps.location.lng(); + } +} + +void Route::addPointToRoute(Point point) { this->points.push_back(point); this->count_points++; } +void Route::addCurrentLocationToRoute() { + if (*this->points.end() == this->currentLocation && + !this->nearlySameLocation(*this->points.end(), this->currentLocation)) { + return; + } + this->addPointToRoute(currentLocation); +} + void Route::delRoute() { this->points.clear(); this->count_points = 0; @@ -45,4 +70,10 @@ double Route::getDis(Point point_1, Point point_2) { double dx = 111.3 * cos(lat) * (point_1.lon - point_2.lon); double dy = 111.3 * (point_1.lat - point_2.lat); return sqrt(dx * dx + dy * dy); +} + +bool Route::nearlySameLocation(Point p1, Point p2) { + if (this->getDis(p1, p2) < DISTANCE_BETWEEN_POINTS) + return true; + return false; } \ No newline at end of file diff --git a/src/route.h b/src/route.h index b784c39..f208657 100644 --- a/src/route.h +++ b/src/route.h @@ -3,21 +3,30 @@ #include #include +#include +#include #include "hardware/motorControl.h" #include "hardware/speedometer.h" +#include "debugMqtt.h" #include "config.h" struct Point{ - float lat = 0; - float lon = 0; + double lat = 0; + double lon = 0; + bool operator==(const Point& rhs) const { + return this->lat == rhs.lat && this->lon == rhs.lon; + } }; class Route { public: Route(); - void addPoint(Point point); + void runRoute(); + + void addPointToRoute(Point point); + void addCurrentLocationToRoute(); void delRoute(); void resetRoute(); @@ -29,14 +38,21 @@ class Route { private: + bool nearlySameLocation(Point p1, Point p2); + std::list points; std::list::iterator it; + Point currentLocation; uint16_t count_points = 0; bool route_started = false; bool route_finished = false; + TinyGPSPlus gps; + + DebugMqtt *debug; + }; #endif // ROUTE_H \ No newline at end of file