141 lines
3.7 KiB
C++
141 lines
3.7 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) {
|
|
this->moveControl = moveControl;
|
|
}
|
|
|
|
TestMode::~TestMode() {
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
}
|
|
|
|
void TestMode::loop() {
|
|
if (millis() - this->lastMillis < this->delay)
|
|
return;
|
|
|
|
if (millis() - this->actionStart > this->maneuverTime) {
|
|
this->busy = false;
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
if (true) {
|
|
std::cout << "It works... cm: " << cm << " degree: " << degree << std::endl;
|
|
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);
|
|
|
|
//TODO: calc this shit
|
|
this->maneuverTime = 10;
|
|
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;
|
|
return true;
|
|
} else {
|
|
//forward or backward and left or right
|
|
}
|
|
|
|
this->moveControl->setDrivingStatus(DrivingStatus::stop);
|
|
return false;
|
|
}
|
|
|
|
bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->busy)
|
|
return false;
|
|
|
|
if (powerPercentage >= 100 || powerPercentage <= -100)
|
|
return false;
|
|
|
|
if (true) {
|
|
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
|
|
return false;
|
|
}
|
|
|
|
this->actionStart = millis();
|
|
this->moveControl->setDrivingStatus(DrivingStatus::raw);
|
|
this->moveControl->setRawPowerLeft(powerPercentage);
|
|
this->maneuverTime = seconds * 1000;
|
|
this->busy = true;
|
|
}
|
|
|
|
bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->busy)
|
|
return false;
|
|
|
|
if (powerPercentage >= 100 || powerPercentage <= -100)
|
|
return false;
|
|
|
|
if (true) {
|
|
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
|
|
return false;
|
|
}
|
|
|
|
this->actionStart = millis();
|
|
this->moveControl->setDrivingStatus(DrivingStatus::raw);
|
|
this->moveControl->setRawPowerRight(powerPercentage);
|
|
this->maneuverTime = seconds * 1000;
|
|
this->busy = true;
|
|
}
|
|
|
|
bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
|
|
if (this->busy)
|
|
return false;
|
|
|
|
if (powerPercentage >= 100 || powerPercentage <= -100)
|
|
return false;
|
|
|
|
if (true) {
|
|
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
|
|
return false;
|
|
}
|
|
|
|
this->actionStart = millis();
|
|
this->moveControl->setDrivingStatus(DrivingStatus::raw);
|
|
this->moveControl->setRawPowerLeft(powerPercentage);
|
|
this->moveControl->setRawPowerRight(powerPercentage);
|
|
this->maneuverTime = seconds * 1000;
|
|
this->busy = true;
|
|
}
|