156 lines
4.3 KiB
C++
156 lines
4.3 KiB
C++
/**
|
|
* @file testMode.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief
|
|
* @version 0.1
|
|
* @date 2022-09-08
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
#include "testMode.h"
|
|
|
|
TestMode::TestMode(MoveControl *moveControl, Navigation* navigation) {
|
|
this->moveControl = moveControl;
|
|
this->navigation = navigation;
|
|
}
|
|
|
|
TestMode::~TestMode() {
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
}
|
|
|
|
void TestMode::loop() {
|
|
if (millis() - this->lastMillis < this->delay)
|
|
return;
|
|
|
|
if (this->maneuver == Maneuver::Turn) {
|
|
uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
|
|
if (delta > this->degree)
|
|
this->abort = true;
|
|
}
|
|
|
|
if (millis() - this->actionStart > this->maneuverTime || this->abort) {
|
|
this->busy = false;
|
|
this->abort = false;
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
this->maneuver = Maneuver::None;
|
|
}
|
|
|
|
this->lastMillis = millis();
|
|
}
|
|
|
|
void TestMode::setSpeed(int16_t speed) {
|
|
this->speed = (double) speed / 100.0;
|
|
}
|
|
|
|
void TestMode::setRotationSpeed(int16_t speed) {
|
|
this->rotationSpeed = (double) speed / 100.0;
|
|
}
|
|
|
|
bool TestMode::drive(int16_t cm, int16_t degree) {
|
|
if (this->busy)
|
|
return false;
|
|
|
|
this->actionStart = millis();
|
|
this->moveControl->setDrivingStatus(DrivingStatus::drive);
|
|
|
|
if (cm == 0) {
|
|
//Only left or right
|
|
this->moveControl->setSpeed(0);
|
|
|
|
if (degree < 0)
|
|
this->moveControl->setRotationSpeed(-this->rotationSpeed);
|
|
else if (degree > 0)
|
|
this->moveControl->setRotationSpeed(this->rotationSpeed);
|
|
|
|
this->azimuth = this->navigation->getAzimuth();
|
|
this->degree = degree;
|
|
|
|
this->maneuverTime = 5 * 1000;
|
|
this->busy = true;
|
|
return true;
|
|
|
|
} else if (degree == 0) {
|
|
//Only forward or backward
|
|
this->moveControl->setRotationSpeed(0);
|
|
|
|
if (cm < 0)
|
|
this->moveControl->setSpeed(-this->speed);
|
|
else if (cm > 0)
|
|
this->moveControl->setSpeed(this->speed);
|
|
|
|
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
|
|
this->busy = true;
|
|
this->maneuver = Maneuver::Drive;
|
|
return true;
|
|
} else {
|
|
//forward or backward and left or right
|
|
// TODO: Calculate roationspeed
|
|
if (cm < 0)
|
|
this->moveControl->setSpeed(-this->speed);
|
|
else if (cm > 0)
|
|
this->moveControl->setSpeed(this->speed);
|
|
|
|
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
|
|
this->busy = true;
|
|
this->maneuver = Maneuver::Drive;
|
|
return true;
|
|
}
|
|
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
return false;
|
|
}
|
|
|
|
bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->engineInit(powerPercentage, seconds)) {
|
|
this->moveControl->setRawPowerLeft(powerPercentage);
|
|
this->maneuver = Maneuver::LeftEngine;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->engineInit(powerPercentage, seconds)) {
|
|
this->moveControl->setRawPowerRight(powerPercentage);
|
|
this->maneuver = Maneuver::RightEngine;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->engineInit(powerPercentage, seconds)) {
|
|
this->moveControl->setRawPowerLeft(powerPercentage);
|
|
this->moveControl->setRawPowerRight(powerPercentage);
|
|
this->maneuver = Maneuver::BothEngine;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void TestMode::abortManeuver() {
|
|
this->abort = true;
|
|
}
|
|
|
|
uint8_t TestMode::getRemainingManeuverTime() {
|
|
if (busy)
|
|
return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
|
|
return 0;
|
|
}
|
|
|
|
bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->busy)
|
|
return false;
|
|
if (powerPercentage >= 100 || powerPercentage <= -100)
|
|
return false;
|
|
if (seconds < 0)
|
|
return false;
|
|
|
|
this->actionStart = millis();
|
|
this->moveControl->setDrivingStatus(DrivingStatus::raw);
|
|
this->maneuverTime = seconds * 1000;
|
|
this->busy = true;
|
|
return true;
|
|
}
|