97 lines
2.5 KiB
C++
97 lines
2.5 KiB
C++
/**
|
|
* @file driveManager.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Implementation of the class DriveManager
|
|
* @version 0.1
|
|
* @date 2021-12-14
|
|
*
|
|
* @copyright Copyright (c) 2021
|
|
*
|
|
*/
|
|
|
|
#include "driveModi/driveManager.h"
|
|
|
|
Modi& operator++(Modi& m, int) {
|
|
return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
|
|
}
|
|
|
|
DriveManager::DriveManager(MoveControl *moveControl, bool wifi) {
|
|
this->moveControl = moveControl;
|
|
|
|
this->spiPort = new SPIClass(HSPI);
|
|
spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS);
|
|
this->navigation = new Navigation(spiPort, UBLOX_GNSS_SPI_CS);
|
|
if (wifi)
|
|
this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
|
|
// if (wifi)
|
|
// std::cout << "Wifi is true" << std::endl;
|
|
}
|
|
|
|
DriveManager::~DriveManager() {
|
|
delete this->navigation;
|
|
delete this->spiPort;
|
|
}
|
|
|
|
void DriveManager::loop() {
|
|
DebugTimes moveControlTime;
|
|
this->moveControl->loop();
|
|
moveControlTime.stopConsol("MoveControlTime", 5);
|
|
|
|
DebugTimes navigationTime;
|
|
this->navigation->loop();
|
|
navigationTime.stopConsol("NavigationTime", 20);
|
|
|
|
if (currentModusPtr)
|
|
this->currentModusPtr->loop();
|
|
}
|
|
|
|
void DriveManager::nextModus() {
|
|
changeModus(this->currentModus++);
|
|
}
|
|
|
|
void DriveManager::changeModus(Modi modus) {
|
|
this->currentModus = modus;
|
|
|
|
//Set moveControl to a safe state
|
|
delete this->currentModusPtr;
|
|
|
|
this->moveControl->setSpeed(0);
|
|
this->moveControl->setRotationSpeed(0);
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
|
|
switch (modus) {
|
|
case Modi::Off:
|
|
this->currentModusPtr = nullptr;
|
|
break;
|
|
|
|
case Modi::ManualControl: {
|
|
this->currentModusPtr = new ManualControl(this->moveControl);
|
|
}
|
|
break;
|
|
|
|
case Modi::CaptureRoute: {
|
|
this->currentModusPtr = new CaptureRoute(this->moveControl, this->navigation);
|
|
}
|
|
break;
|
|
|
|
case Modi::Autopilot: {
|
|
this->currentModusPtr = new Autopilot(this->moveControl, this->navigation);
|
|
}
|
|
break;
|
|
|
|
case Modi::ConsolControl:
|
|
this->currentModusPtr = nullptr;
|
|
break;
|
|
|
|
case Modi::TestMode: {
|
|
this->currentModusPtr = new TestMode(this->moveControl);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
this->currentModusPtr = nullptr;
|
|
break;
|
|
}
|
|
}
|
|
|