77 lines
2.2 KiB
C++
77 lines
2.2 KiB
C++
/**
|
|
* @file manualControl.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Implementation of the class manualControl.h
|
|
* @version 0.1
|
|
* @date 2021-12-13
|
|
*
|
|
* @copyright Copyright (c) 2021
|
|
*
|
|
*/
|
|
#include "manualControl.h"
|
|
|
|
void ManualControl::run() {
|
|
switch (this->inputMode) {
|
|
case InputMode::Analog :
|
|
this->analogControl();
|
|
break;
|
|
|
|
case InputMode::Digital :
|
|
this->digitalControl();
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void ManualControl::switchInputMode() {
|
|
this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
|
|
}
|
|
|
|
void ManualControl::analogControl() {
|
|
// Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
|
|
int16_t x = this->input->x - 127;
|
|
int16_t y = this->input->y - 127;
|
|
|
|
// static int counter = 0;
|
|
// if (counter % 60 == 0) {
|
|
// std::cout << "Input: x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
|
|
// std::cout << "Output: x: " << (int) x << " y: " << (int) y << std::endl;
|
|
// }
|
|
// counter++;
|
|
|
|
double value_per_step = this->maxForwardSpeed * 2 / UINT8_MAX;
|
|
this->moveControl->setSpeed(-x * value_per_step);
|
|
|
|
value_per_step = this->maxRotationSpeed * 2 / UINT8_MAX;
|
|
this->moveControl->setRotationSpeed(y * value_per_step);
|
|
}
|
|
|
|
void ManualControl::digitalControl() {
|
|
int16_t y = this->input->x - 127;
|
|
int16_t x = this->input->y - 127;
|
|
|
|
if (y > 120)
|
|
this->moveControl->setSpeed(-this->maxForwardSpeed);
|
|
else if (y < -120)
|
|
this->moveControl->setSpeed(this->maxForwardSpeed);
|
|
else
|
|
this->moveControl->setSpeed(0);
|
|
|
|
if (x > 120)
|
|
this->moveControl->setRotationSpeed(this->maxRotationSpeed);
|
|
else if (x < -120)
|
|
this->moveControl->setRotationSpeed(-this->maxRotationSpeed);
|
|
else
|
|
this->moveControl->setRotationSpeed(0);
|
|
|
|
if (this->directionChangeWrapper && (x > 120 || x < -120))
|
|
this->lastLoopTurned = true;
|
|
else if (this->lastLoopTurned) {
|
|
if (this->directionChangeWrapper)
|
|
this->directionChangeWrapper->action();
|
|
this->lastLoopTurned = false;
|
|
}
|
|
}
|