kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

clangtidy corrections part 1

Browse Source
This commit is contained in:
kleiax
2023-10-11 17:35:40 +02:00
parent 77a52b82c3
commit 23d854a826
36 changed files with 2383 additions and 1165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+111 -77
View File
@@ -4,41 +4,40 @@
* @brief Contains an implementation of the class MoveControl
* @version 0.1
* @date 2022-02-15
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "moveControl.h"
MoveControl::MoveControl() {
this->loopDelay = 20;
this->left_motor = new MotorControl();
this->right_motor = new MotorControl();
this->left_speedometer = new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
this->right_speedometer = new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
this->left_pid = new PID( &this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
Settings::Pid::Left::P,
Settings::Pid::Left::I,
Settings::Pid::Left::D,
DIRECT);
this->right_pid = new PID( &this->wheelspeed_right,
&this->right_pid_out,
&this->wheelspeed_right_target,
Settings::Pid::Right::P,
Settings::Pid::Right::I,
Settings::Pid::Right::D,
DIRECT);
MoveControl::MoveControl()
: Component(MoveControl::loopDelay),
left_motor{new MotorControl()},
right_motor{new MotorControl()},
left_speedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
right_speedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
left_pid{new PID(&this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
Settings::Pid::Left::P,
Settings::Pid::Left::I,
Settings::Pid::Left::D,
DIRECT)},
right_pid{new PID(&this->wheelspeed_right,
&this->right_pid_out,
&this->wheelspeed_right_target,
Settings::Pid::Right::P,
Settings::Pid::Right::I,
Settings::Pid::Right::D,
DIRECT)}
{
this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->left_pid->SetSampleTime(Settings::Pid::sampleTime);
this->left_pid->SetMode(AUTOMATIC);
this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
this->right_pid->SetMode(AUTOMATIC);
this->right_pid->SetMode(AUTOMATIC);
this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
@@ -46,10 +45,11 @@ MoveControl::MoveControl() {
this->addChildComponent(this->left_motor);
this->addChildComponent(this->right_motor);
this->addChildComponent(this->left_speedometer);
this->addChildComponent(this->right_speedometer);
this->addChildComponent(this->right_speedometer);
}
MoveControl::~MoveControl() {
MoveControl::~MoveControl()
{
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
@@ -59,10 +59,10 @@ MoveControl::~MoveControl() {
delete this->right_speedometer;
delete this->left_pid;
delete this->right_pid;
}
void MoveControl::run() {
void MoveControl::run()
{
this->updateCurrentWheelSpeed();
this->calcTargetWheelSpeed();
@@ -72,7 +72,8 @@ void MoveControl::run() {
this->regulateMotors();
}
void MoveControl::setDrivingStatus(Status status) {
void MoveControl::setDrivingStatus(Status status)
{
this->setSpeed(0);
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
@@ -97,7 +98,8 @@ void MoveControl::setDrivingStatus(Status status) {
// }
}
void MoveControl::emergencyStop() {
void MoveControl::emergencyStop()
{
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
this->setSpeed(0);
@@ -107,8 +109,10 @@ void MoveControl::emergencyStop() {
this->driving_status = Status::Stop;
}
void MoveControl::setSpeed(double speed) {
if (speed < 0.2 && speed > -0.2) {
void MoveControl::setSpeed(double speed)
{
if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
{
this->drivingSpeeds.x = 0;
return;
}
@@ -116,8 +120,10 @@ void MoveControl::setSpeed(double speed) {
this->drivingSpeeds.x = speed;
}
void MoveControl::setRotationSpeed(double speed) {
if (speed < 0.1 && speed > -0.1){
void MoveControl::setRotationSpeed(double speed)
{
if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
{
this->drivingSpeeds.rot = 0;
return;
}
@@ -125,92 +131,120 @@ void MoveControl::setRotationSpeed(double speed) {
this->drivingSpeeds.rot = speed;
}
void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds) {
void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds)
{
this->setSpeed(drivingSpeeds.x);
this->setRotationSpeed(drivingSpeeds.rot);
}
void MoveControl::setRawPowerLeft(int16_t power) {
if (power <= 100 && power >= -100)
this->rawPowerLeft = power;
void MoveControl::setRawPowerLeft(int16_t power)
{
if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage)
{
this->rawPowerLeft = static_cast<int8_t>(power);
}
}
void MoveControl::setRawPowerRight(int16_t power) {
if (power <= 100 && power >= -100)
this->rawPowerRight = power;
void MoveControl::setRawPowerRight(int16_t power)
{
if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage)
{
this->rawPowerRight = static_cast<int8_t>(power);
}
}
void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
PID* selectedPID = nullptr;
void MoveControl::setPidTunings(uint8_t side, double pPart, double iPart, double dPart)
{
PID *selectedPID = nullptr;
if (side == 0)
{
selectedPID = this->left_pid;
}
else if (side == 1)
{
selectedPID = this->right_pid;
}
selectedPID->SetTunings(p, i, d);
selectedPID->SetTunings(pPart, iPart, dPart);
}
PID* MoveControl::getPID(uint8_t side) const {
PID *MoveControl::getPID(uint8_t side) const
{
if (side == 0)
{
return this->left_pid;
else if (side == 1)
}
if (side == 1)
{
return this->right_pid;
}
return nullptr;
}
void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value)
{
if (value > 0)
{
speedometer->setDirection(Speedometer::Direction::Forward);
}
else if (value < 0)
{
speedometer->setDirection(Speedometer::Direction::Backward);
}
else
{
speedometer->setDirection(Speedometer::Direction::None);
}
}
void MoveControl::calcTargetWheelSpeed() {
void MoveControl::calcTargetWheelSpeed()
{
/* original formula:
(1 / r) / 1 b \ / x \ = / Xl \
\ 1 -b / \ T / \ Xr / */
// (1 / r) * 1
constexpr double A = 1.0 / (Settings::wheelDiameter / 2);
constexpr double A1r1 = 1.0 / (Settings::wheelDiameter / 2);
// (1 / r) * b
constexpr double B = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);
constexpr double B1rb = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);
this->wheelspeed_right_target = (A * this->drivingSpeeds.x + B * this->drivingSpeeds.rot);
this->wheelspeed_left_target = (A * this->drivingSpeeds.x + (-B) * this->drivingSpeeds.rot);
this->wheelspeed_right_target = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
this->wheelspeed_left_target = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
}
void MoveControl::regulateMotors() {
switch (this->driving_status) {
case Status::Stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
this->setSpeedometerDirection(this->left_speedometer, 0);
this->setSpeedometerDirection(this->right_speedometer, 0);
break;
void MoveControl::regulateMotors()
{
switch (this->driving_status)
{
case Status::Stop:
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
this->setSpeedometerDirection(this->left_speedometer, 0);
this->setSpeedometerDirection(this->right_speedometer, 0);
break;
case Status::Drive :
this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
break;
case Status::Drive:
this->left_motor->setTargetPower(static_cast<int8_t>(this->left_pid_out));
this->right_motor->setTargetPower(static_cast<int8_t>(this->right_pid_out));
this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
break;
case Status::Raw :
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
break;
case Status::Raw:
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
}
}
void MoveControl::updateCurrentWheelSpeed() {
void MoveControl::updateCurrentWheelSpeed()
{
this->wheelspeed_left = this->left_speedometer->getSpeedRad();
this->wheelspeed_right = this->right_speedometer->getSpeedRad();
}