kleiax
38c8a58988
- activatre auto load calibration data - give possibility to force update in navigation getCourseCorrection - added drive mode for compass calibration - removed compass calibration manualControl
52 lines
1.5 KiB
C++
52 lines
1.5 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"
|
|
|
|
ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) {
|
|
this->moveControl = moveControl;
|
|
this->input = input;
|
|
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
|
|
}
|
|
|
|
ManualControl::~ManualControl() {
|
|
this->moveControl->setSpeed(0);
|
|
this->moveControl->setRotationSpeed(0);
|
|
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
|
|
}
|
|
|
|
void ManualControl::loop() {
|
|
if (this->caliCompass)
|
|
this->caliCompass->loop();
|
|
|
|
if (millis() - this->lastMillis < delay) {
|
|
return;
|
|
}
|
|
this->runManualControl();
|
|
this->lastMillis = millis();
|
|
}
|
|
|
|
void ManualControl::runManualControl() {
|
|
// Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
|
|
int16_t y = this->input->x - 127;
|
|
int16_t x = this->input->y - 127;
|
|
|
|
// if (x || y) {
|
|
// std::cout << "x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
|
|
// std::cout << "x: " << (int) x << " y: " << (int) y << std::endl;
|
|
// }
|
|
|
|
double value_per_step = this->max_speed * 2 / 256;
|
|
this->moveControl->setSpeed(-y * value_per_step);
|
|
|
|
value_per_step = this->max_rotation * 2 / 256;
|
|
this->moveControl->setRotationSpeed(x * value_per_step);
|
|
}
|