From df8bb6a72232d1bf2d84b9b7c264d1f29098dfc7 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 9 Jan 2024 17:30:12 +0100 Subject: [PATCH] Cleanup --- ControlledMotor.cpp | 20 ++++++++------------ DifferentialDrive.cpp | 41 +++++++++++++++++++++++++++++------------ DifferentialDrive.h | 7 ++++++- test/BB2B_Test.cc | 2 +- 4 files changed, 44 insertions(+), 26 deletions(-) diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index 9ec5ef4..2e4769b 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -31,18 +31,14 @@ void ControlledMotor::Update(float currentTimeMs) { float delta = error * pidP + errorChange * pidD; - // Serial.print("direction "); - // Serial.print(this->direction); - Serial.print(" actualSpeed "); - Serial.print(actualSpeed); - // Serial.print(" target "); - // Serial.println(this->currentTargetSpeed); - Serial.print(" motor target speed "); - Serial.print(motor->currentTargetSpeed); - Serial.print(" + "); - Serial.print(error * pidP); - Serial.print(" + "); - Serial.println(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); motor->SetTargetSpeed(motor->currentTargetSpeed + delta); // or something like that diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index ce46f08..dd617c5 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -6,15 +6,13 @@ DifferentialDrive::DifferentialDrive(){}; -DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor, - float separation) { +DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) { this->motorCount = 2; this->motors = new Motor *[2]; this->motors[0] = leftMotor; this->motors[1] = rightMotor; - this->wheelSeparation = separation; - float distance = wheelSeparation / 2; + float distance = this->wheelSeparation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; leftMotor->position.angle = -90; leftMotor->position.distance = distance; @@ -23,6 +21,17 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor, rightMotor->position.distance = distance; } +void DifferentialDrive::SetDimensions(float wheelDiameter, + float wheelSeparation) { + this->wheelDiameter = wheelDiameter; + this->wheelSeparation = wheelSeparation; + this->rpsToMs = wheelDiameter * Angle::PI; + + float distance = this->wheelSeparation / 2; + this->motors[0]->position.distance = distance; + this->motors[1]->position.distance = distance; +} + void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, float rightSpeed) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { @@ -39,10 +48,15 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, } void DifferentialDrive::SetTwistSpeed(float forward, float yaw) { - float leftSpeed = Float::Clamp(forward - yaw, -1, 1); - float rightSpeed = Float::Clamp(forward + yaw, -1, 1); - // The speeds should be translated to m/s to revolutions per second here - SetMotorTargetSpeeds(leftSpeed, rightSpeed); + float leftSpeed = + Float::Clamp(forward - yaw, -1, 1); // revolutions per second + float rightSpeed = + Float::Clamp(forward + yaw, -1, 1); // revolutions per second + + float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second + float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second + + SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); } void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) { @@ -59,8 +73,10 @@ Polar DifferentialDrive::GetVelocity() { Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second - leftSpeed = leftSpeed * wheelDiameter * Angle::PI; // in meters per second - rightSpeed = rightSpeed * wheelDiameter * Angle::PI; // in meters per second + // float rpsToMs = wheelDiameter * Angle::PI; + + leftSpeed = leftSpeed * rpsToMs; // in meters per second + rightSpeed = rightSpeed * rpsToMs; // in meters per second float speed = (leftSpeed + rightSpeed) / 2; float direction = speed >= 0 ? 0.0F : 180.0F; @@ -74,8 +90,9 @@ float DifferentialDrive::GetAngularVelocity() { Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second - leftSpeed = leftSpeed * wheelDiameter * Angle::PI; // in meters per second - rightSpeed = rightSpeed * wheelDiameter * Angle::PI; // in meters per second + // float rpsToMs = wheelDiameter * Angle::PI; + leftSpeed = leftSpeed * rpsToMs; // in meters per second + rightSpeed = rightSpeed * rpsToMs; // in meters per second float angularSpeed = (rightSpeed - leftSpeed) / 2; float angularDistance = wheelSeparation / 2 * Angle::PI; diff --git a/DifferentialDrive.h b/DifferentialDrive.h index 155f73c..a3526b8 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -28,7 +28,9 @@ public: // DifferentialDrive(Placement leftMotorPlacement, // Placement rightMotorPlacement); - DifferentialDrive(Motor *leftMotor, Motor *rightMotor, float separation = 1); + DifferentialDrive(Motor *leftMotor, Motor *rightMotor); + + void SetDimensions(float wheelDiameter, float wheelSeparation); /// @brief Set the target speeds of the motors directly /// @param leftSpeed The target speed of the left Motor @@ -74,8 +76,11 @@ public: /// Control virtual float GetAngularVelocity() override; +protected: float wheelDiameter = 1.0F; // in meters float wheelSeparation = 1.0F; // in meters; + + float rpsToMs; // convert revolutions per second to meters per second }; } // namespace RoboidControl diff --git a/test/BB2B_Test.cc b/test/BB2B_Test.cc index 9ebbe7f..4b89d9c 100644 --- a/test/BB2B_Test.cc +++ b/test/BB2B_Test.cc @@ -32,7 +32,7 @@ TEST(BB2B, Basics) { float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F; DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion; - diffDrive->SetTargetSpeeds(leftMotorSpeed, rightMotorSpeed); + diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); // cannot chek verlocity in this branch? }