Cleanup

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

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;

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

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?
 }