From a74671b8f4d6ecb09e21ad8e1caed138d6a9576b Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Thu, 6 Mar 2025 11:19:44 +0100
Subject: [PATCH] Cleaned up BB2B example

---
 Examples/BB2B.cpp            | 52 +++++++++++++++++++-----------------
 Things/DifferentialDrive.cpp | 25 +++++++++--------
 Things/DifferentialDrive.h   |  3 +--
 3 files changed, 40 insertions(+), 40 deletions(-)

diff --git a/Examples/BB2B.cpp b/Examples/BB2B.cpp
index c53acea..98d74d7 100644
--- a/Examples/BB2B.cpp
+++ b/Examples/BB2B.cpp
@@ -1,38 +1,40 @@
-#include <iostream>
-
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
 #include "Things/TouchSensor.h"
 
+#include <chrono>
+#include <thread>
+
 using namespace RoboidControl;
 
-void CollisionSteering();
-
-DifferentialDrive* bb2b = nullptr;
-TouchSensor* touchLeft = nullptr;
-TouchSensor* touchRight = nullptr;
-
 int main() {
-  bb2b = new DifferentialDrive();
-
-  touchLeft = new TouchSensor(bb2b);
-  touchRight = new TouchSensor(bb2b);
+  // The robot's propulsion is a differential drive
+  DifferentialDrive* bb2b = new DifferentialDrive();
+  // Is has a touch sensor at the front left of the roboid
+  TouchSensor* touchLeft = new TouchSensor(bb2b);
+  // and other one on the right
+  TouchSensor* touchRight = new TouchSensor(bb2b);
 
+  // Do forever:
   while (true) {
-    CollisionSteering();
+    // The left wheel turns forward when nothing is touched on the right side
+    // and turn backward when the roboid hits something on the right
+    float leftWheelSpeed = (touchRight->touchedSomething) ? -0.5f : 0.5f;
+    // The right wheel does the same, but instead is controlled by
+    // touches on the left side
+    float rightWheelSpeed = (touchLeft->touchedSomething) ? -0.5f : 0.5f;
+    // When both sides are touching something, both wheels will turn backward
+    // and the roboid will move backwards
+    bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
+
+    // Update the roboid state
+    Thing::UpdateThings(Thing::GetTimeMs());
+    // would like to do this:
+    // bb2b->Update();
+
+    // and sleep for 100ms
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
 
   return 0;
-}
-
-void CollisionSteering() {
-  if (touchLeft->touchedSomething)
-    bb2b->SetRightWheelVelocity(-0.5f);
-  else
-    bb2b->SetRightWheelVelocity(0.5f);
-
-  if (touchRight->touchedSomething)
-    bb2b->SetLeftWheelVelocity(-0.5f);
-  else
-    bb2b->SetLeftWheelVelocity(0.5f);
 }
\ No newline at end of file
diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp
index 7daadbc..029e95f 100644
--- a/Things/DifferentialDrive.cpp
+++ b/Things/DifferentialDrive.cpp
@@ -4,10 +4,7 @@ namespace RoboidControl {
 DifferentialDrive::DifferentialDrive() : Thing() {}
 
 RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
-    : Thing(participant) {
-  // this->leftWheel = new Thing(participant);
-  // this->rightWheel = new Thing(participant);
-}
+    : Thing(participant) {}
 
 void DifferentialDrive::SetDimensions(float wheelDiameter,
                                       float wheelSeparation) {
@@ -35,11 +32,17 @@ void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::SetLeftWheelVelocity(float angularSpeed) {}
-
-void DifferentialDrive::SetRightWheelVelocity(float angularSpeed) {}
+void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
+  if (this->leftWheel != nullptr)
+    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
+  if (this->rightWheel != nullptr)
+    this->rightWheel->SetAngularVelocity(
+        Spherical(speedRight, Direction::right));
+}
 
 void DifferentialDrive::Update(unsigned long currentMs) {
+  if (this->linearVelocityUpdated == false)
+    return;
   // this assumes forward velocity only....
   float linearVelocity = this->GetLinearVelocity().distance;
 
@@ -53,15 +56,11 @@ void DifferentialDrive::Update(unsigned long currentMs) {
   float speedLeft =
       (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
       this->wheelRadius * Rad2Deg;
-  if (this->leftWheel != nullptr)
-    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
-
   float speedRight =
       (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
       this->wheelRadius * Rad2Deg;
-  if (this->rightWheel != nullptr)
-    this->rightWheel->SetAngularVelocity(
-        Spherical(speedRight, Direction::right));
+
+  this->SetWheelVelocity(speedLeft, speedRight);
 
   // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
   // angularVelocity.distance << "   left wheel  "
diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h
index 8f9ae7d..4e34bfa 100644
--- a/Things/DifferentialDrive.h
+++ b/Things/DifferentialDrive.h
@@ -13,8 +13,7 @@ class DifferentialDrive : public Thing {
   void SetDimensions(float wheelDiameter, float wheelSeparation);
   void SetMotors(Thing* leftWheel, Thing* rightWheel);
 
-  void SetLeftWheelVelocity(float angularSpeed);
-  void SetRightWheelVelocity(float angularSpeed);
+  void SetWheelVelocity(float speedLeft, float speedRight);
 
   virtual void Update(unsigned long currentMs) override;