93 lines
2.1 KiB
C++
93 lines
2.1 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)
|
|
const int16_t xAxis = this->input->x - 127;
|
|
const int16_t yAxis = this->input->y - 127;
|
|
|
|
double value_per_step = this->maxSpeeds.x * 2 / UINT8_MAX;
|
|
this->moveControl->setSpeed(-xAxis * value_per_step);
|
|
|
|
value_per_step = this->maxSpeeds.rot * 2 / UINT8_MAX;
|
|
this->moveControl->setRotationSpeed(yAxis * value_per_step);
|
|
}
|
|
|
|
void ManualControl::digitalControl()
|
|
{
|
|
static constexpr uint8_t deadzone = 120;
|
|
const int16_t yAxis = this->input->x - 127;
|
|
const int16_t xAxis = this->input->y - 127;
|
|
|
|
if (yAxis > deadzone)
|
|
{
|
|
this->moveControl->setSpeed(-this->maxSpeeds.x);
|
|
}
|
|
else if (yAxis < -deadzone)
|
|
{
|
|
this->moveControl->setSpeed(this->maxSpeeds.rot);
|
|
}
|
|
else
|
|
{
|
|
this->moveControl->setSpeed(0);
|
|
}
|
|
|
|
if (xAxis > deadzone)
|
|
{
|
|
this->moveControl->setRotationSpeed(this->maxSpeeds.rot);
|
|
}
|
|
else if (xAxis < -deadzone)
|
|
{
|
|
this->moveControl->setRotationSpeed(-this->maxSpeeds.rot);
|
|
}
|
|
else
|
|
{
|
|
this->moveControl->setRotationSpeed(0);
|
|
}
|
|
|
|
if (static_cast<bool>(this->directionChangeWrapper) && (xAxis > deadzone || xAxis < -deadzone))
|
|
{
|
|
this->lastLoopTurned = true;
|
|
}
|
|
else if (this->lastLoopTurned)
|
|
{
|
|
if (static_cast<bool>(this->directionChangeWrapper))
|
|
{
|
|
this->directionChangeWrapper->action();
|
|
}
|
|
this->lastLoopTurned = false;
|
|
}
|
|
}
|