RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Clean & pid tuning

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-19 14:14:28 +01:00
parent f3021c1f58
commit b86484d59d
2 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ControlledMotor.cpp
View File

@ -20,25 +20,29 @@ void ControlledMotor::SetTargetSpeed(float speed) {
}
void ControlledMotor::Update(float currentTimeMs) {
actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
if (this->currentTargetSpeed < 0)
actualSpeed = -actualSpeed;
this->actualSpeed = -this->actualSpeed;
float deltaTime = currentTimeMs - this->lastUpdateTime;
float error = currentTargetSpeed - actualSpeed;
float error = this->currentTargetSpeed - this->actualSpeed;
float errorChange = (error - lastError) * deltaTime;
float delta = error * pidP + errorChange * pidD;
// Serial.print(" actualSpeed ");
// Serial.print(actualSpeed);
// Serial.print(" motor target speed ");
// Serial.print(motor->currentTargetSpeed);
// Serial.print(" + ");
// Serial.print(error * pidP);
// Serial.print(" + ");
// Serial.println(errorChange * pidD);
Serial.print(" actual Speed ");
Serial.print(actualSpeed);
Serial.print(" target Speed ");
Serial.print(this->currentTargetSpeed);
Serial.print(" motor target speed ");
Serial.print(motor->currentTargetSpeed);
Serial.print(" + ");
Serial.print(error * pidP);
Serial.print(" + ");
Serial.print(errorChange * pidD);
Serial.print(" = ");
Serial.println(motor->currentTargetSpeed + delta);
motor->SetTargetSpeed(motor->currentTargetSpeed +
delta); // or something like that

6
ControlledMotor.h
View File

@ -19,9 +19,9 @@ public:
}
float velocity;
float pidP = 0.02F;
float pidD = 0.001F;
float pidI = 0;
float pidP = 0.1F;
float pidD = 0.0F;
float pidI = 0.0F;
void Update(float currentTimeMs) override;