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

improve pid and other stuff

Browse Source
This commit is contained in:
kleiax
2021-11-24 16:47:29 +01:00
parent f086bfa6b3
commit c741546ba1
7 changed files with 152 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+25 -13
View File
@@ -35,10 +35,7 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
}
void MoveControl::runMoveControl() {
this->right_pid->Compute();
this->left_pid->Compute();
static uint64_t last_millis = 0;
static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
return;
}
@@ -46,13 +43,21 @@ void MoveControl::runMoveControl() {
this->updateWheelSpeed();
this->calcWheelSpeed();
this->right_pid->Compute();
this->left_pid->Compute();
this->regulateMotors();
char str[128];
sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f",
this->x_speed, this->rotation_speed, this->wheelspeed_left,
this->wheelspeed_right);
debug->sendMsg(Loglevel::debug, str);
static uint32_t functioncalls = 0;
functioncalls++;
if (functioncalls % 20 == 0) {
char str[128];
sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f",
this->x_speed, this->rotation_speed, this->wheelspeed_left,
this->wheelspeed_right);
debug->sendMsg(Loglevel::debug, str);
}
}
void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -60,12 +65,19 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
}
void MoveControl::setSpeed(double speed) {
this->x_speed = speed;
if (speed < 0.2 && speed > -0.2)
this->x_speed = 0;
else
this->x_speed = speed;
// Serial.printf("New speed: %f in moveControll.cpp \n", speed);
}
void MoveControl::setRotationspeed(double speed) {
this->rotation_speed = speed;
if (speed < 0.2 && speed > -0.2)
this->rotation_speed = 0;
else
this->rotation_speed = speed;
}
void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
@@ -91,8 +103,8 @@ void MoveControl::calcWheelSpeed() {
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);
//debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target);
//debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target);
}
void MoveControl::regulateMotors() {