321 lines
13 KiB
C++
321 lines
13 KiB
C++
#include "moveControl.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
MoveControl::MoveControl() {
|
|
this->debug = new DebugMqtt("MoveControl");
|
|
}
|
|
|
|
void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
|
|
Speedometer *left_speedometer, Speedometer *right_speedometer) {
|
|
debug->sendMsg(Loglevel::info, "Init...");
|
|
this->left_motor = left_motor;
|
|
this->right_motor = right_motor;
|
|
this->left_speedometer = left_speedometer;
|
|
this->right_speedometer = right_speedometer;
|
|
debug->sendMsg(Loglevel::info, "Init finished!");
|
|
}
|
|
|
|
void MoveControl::runMoveControl() {
|
|
static uint64_t last_millis = 0;
|
|
if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
|
|
return;
|
|
}
|
|
last_millis = millis();
|
|
|
|
this->calcWheelSpeed();
|
|
this->regulateMotors();
|
|
}
|
|
|
|
void MoveControl::setDrivingStatus(DrivingStatus status) {
|
|
this->driving_status = status;
|
|
}
|
|
|
|
void MoveControl::setSpeed(double speed) {
|
|
this->x_speed = speed;
|
|
Serial.printf("New speed: %f in moveControll.cpp \n", speed);
|
|
}
|
|
|
|
void MoveControl::setRotationspeed(double speed) {
|
|
this->rotation_speed = speed;
|
|
}
|
|
|
|
void MoveControl::calcWheelSpeed() {
|
|
// original formula:
|
|
// (1 / r) / 1 b \ / x \ = / Xl \
|
|
// \ 1 -b / \ T / \ Xr /
|
|
|
|
// (1 / r) * 1
|
|
const static double A = 15.82278481;
|
|
// (1 / r) * b
|
|
const static double B = 2.096518987;
|
|
|
|
this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
|
|
this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);
|
|
debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target);
|
|
debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target);
|
|
}
|
|
|
|
void MoveControl::regulateMotors() {
|
|
double ratio = 0;
|
|
switch (this->driving_status) {
|
|
case DrivingStatus::stop :
|
|
this->left_motor->setTargetPower(0);
|
|
this->right_motor->setTargetPower(0);
|
|
break;
|
|
|
|
case DrivingStatus::straightForward :
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::straightBackward :
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight
|
|
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
|
|
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed()) * ratio) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed()) * ratio) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft
|
|
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
|
|
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
|
|
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
|
|
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
|
|
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
|
|
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::rotateLeft :
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
case DrivingStatus::rotateRight :
|
|
// Left motor
|
|
// Too slow
|
|
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
|
|
&& this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
|
|
|
|
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
}
|
|
|
|
// Right motor
|
|
// Too slow
|
|
if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
|
|
|
|
// Too fast
|
|
} else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
|
|
&& this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
|
|
|
|
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
|
|
break;
|
|
}
|
|
|
|
|
|
} |