Cleanup

Arduino/Examples/BB2A/BB2A.cpp

@@ -0,0 +1,95 @@
+#include "Arduino.h"
+
+#include "Things/ControlledMotor.h"
+#include "Things/DifferentialDrive.h"
+#include "Things/RelativeEncoder.h"
+#include "Things/TouchSensor.h"
+
+#include "Arduino/Things/DRV8833.h"
+#include "Arduino/Things/DigitalInput.h"
+#include "Arduino/Things/UltrasonicSensor.h"
+#include "Arduino/ArduinoUtils.h"
+
+#include "Participants/ParticipantUDP.h"
+
+#include "configuration.h"
+
+#include <iostream>
+
+using namespace RoboidControl;
+using namespace RoboidControl::Arduino;
+
+ParticipantUDP* localParticipant;
+
+DifferentialDrive* bb2b;
+TouchSensor* touchLeft;
+TouchSensor* touchRight;
+
+// RelativeEncoder* encoderLeft;
+// RelativeEncoder* encoderRight;
+
+void setup() {
+  Serial.begin(115200);
+  delay(3000);
+  Serial.println("started");
+
+  StartWifi("serrarens", "192.168.76.44");
+  localParticipant = new ParticipantUDP("192.168.77.76");
+
+  bb2b = new DifferentialDrive();
+  touchLeft = new TouchSensor(bb2b);
+  touchRight = new TouchSensor(bb2b);
+  
+  // bb2b = new DRV8833::DifferentialDrive(driveConfig);
+  // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
+  // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);
+
+  touchLeft->name = "Left Touch Sensor";
+  touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
+  touchRight->name = "Right Touch Sensor";
+  touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));
+
+  // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
+  // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);
+
+  // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
+  // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
+}
+
+void loop() {
+  
+  // std::cout << touchLeft->touchedSomething << " | "
+  //           << touchRight->touchedSomething << std::endl;
+  // std::cout << encoderLeft->rotationSpeed << " : "
+  //           << encoderRight->rotationSpeed << std::endl;
+  // std::cout << bb2b->leftWheel->encoder->rotationSpeed
+  //           << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;
+
+  
+  // 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 leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
+  // The right wheel does the same, but instead is controlled by
+  // touches on the left side
+  float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
+  // When both sides are touching something, both wheels will turn backward
+  // and the roboid will move backwards
+
+  bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
+  bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
+  // std::cout << "  " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;
+
+  // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
+  // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
+  // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
+  // std::cout << "  " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;
+
+  // std::cout << leftMotor->actualVelocity << std::endl;
+  
+  //Serial.println(".");
+  // Update the roboid state
+  localParticipant->Update();
+
+  // and sleep for 100ms
+  delay(10);
+}

Arduino/Examples/BB2A/configuration.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "Arduino/Things/UltrasonicSensor.h"
+#include "Arduino/Things/DRV8833.h"
+#include "Arduino/Things/DigitalInput.h"
+
+using namespace RoboidControl::Arduino;
+
+#if defined(ESP32)
+static constexpr DRV8833::Configuration driveConfig = {
+    17,  // AIn1
+    16,  // AIn2
+    14,  // BIn1
+    27   // BIn2
+};
+static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
+    25,  // Trigger
+    33   // Echo
+};
+static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
+    15,  // Trigger
+    5    // Echo
+};
+
+#elif defined(UNO) || defined(UNO_R4)
+static constexpr DRV8833::Configuration driveConfig = {
+    5,  // AIn1
+    6,  // AIn2
+    7,  // BIn1
+    10  // BIn2
+};
+
+static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
+    A0,  // Trigger
+    12   // Echo
+};
+static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
+    A1,  // Trigger
+    11   // Echo
+};
+
+static constexpr DigitalInput::RelativeEncoder::Configuration
+    leftEncoderConfig = {
+        2,  // Input pin
+        80  // Pulses per revolution
+};
+static constexpr DigitalInput::RelativeEncoder::Configuration
+    rightEncoderConfig = {
+        3,  // Input pin
+        80  // Pulses per revolution
+};
+#endif

Arduino/Things/DRV8833.cpp

@@ -49,8 +49,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
                            unsigned char pinIn1,
                            unsigned char pinIn2,
                            bool reverse)
-    : Motor() {
+    : Motor(driver) {
-  this->SetParent(driver);
 
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;

Participants/ParticipantUDP.cpp

@@ -86,8 +86,6 @@ void ParticipantUDP::SetupUDP(int localPort,
 void ParticipantUDP::Update() {
   unsigned long currentTimeMs = Thing::GetTimeMs();
 
-  PrepMyThings();
-
   if (this->isIsolated == false) {
     if (this->connected == false)
       begin();
@@ -111,68 +109,19 @@ void ParticipantUDP::Update() {
   UpdateOtherThings();
 }
 
-void ParticipantUDP::PrepMyThings() {
-  for (Thing* thing : this->things) {
-    if (thing == nullptr)
-      continue;
-
-    thing->PrepareForUpdate();
-  }
-}
-
 void ParticipantUDP::UpdateMyThings() {
-  // std::cout << this->things.size() << std::endl;
   for (Thing* thing : this->things) {
     if (thing == nullptr)  // || thing->GetParent() != nullptr)
       continue;
 
-    // std::cout << thing->name << "\n";
-    // if (thing->hierarchyChanged) {
-    //   if (!(this->isIsolated || this->networkId == 0)) {
-    //     ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-    //     this->remoteSite->Send(thingMsg);
-    //     delete thingMsg;
-
-    //     if (thing->nameChanged) {
-    //       NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-    //       this->remoteSite->Send(nameMsg);
-    //       delete nameMsg;
-    //     }
-    //   }
-    // }
-
-    // std::cout << "B\n";
     // Why don't we do recursive?
     // Because when a thing creates a thing in the update,
     // that new thing is not sent out (because of hierarchyChanged)
     // before it is updated itself: it is immediatedly updated!
     thing->Update(false);
 
-    // std::cout << "C\n";
-    // if (!(this->isIsolated || this->networkId == 0)) {
-    //   if (thing->terminate) {
-    //     DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
-    //     this->remoteSite->Send(destroyMsg);
-    //     delete destroyMsg;
-    //   } else {
-    //     //  Send to remote site
-    //     if (thing->nameChanged) {
-    //       NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-    //       this->remoteSite->Send(nameMsg);
-    //       delete nameMsg;
-    //     }
-    //     PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-    //     this->remoteSite->Send(poseMsg);
-    //     delete poseMsg;
-    //     BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-    //     this->remoteSite->Send(binaryMsg);
-    //     delete binaryMsg;
-    //   }
-    // }
-    // std::cout << "D\n";
     if (thing->terminate)
       this->Remove(thing);
-    // std::cout << "E\n";
   }
 }
 

Participants/ParticipantUDP.h

@@ -102,7 +102,7 @@ public:
   unsigned long nextPublishMe = 0;
 
   /// @brief Prepare the local things for the next update
-  virtual void PrepMyThings();
+  //virtual void PrepMyThings();
   virtual void UpdateMyThings();
   virtual void UpdateOtherThings();
 

Thing.cpp

@@ -253,12 +253,6 @@ unsigned long Thing::GetTimeMs() {
 #endif
 }
 
-// void Thing::Update(bool recursive) {
-//   Update(GetTimeMs(), recursive);
-// }
-
-void Thing::PrepareForUpdate() {}
-
 void Thing::Update(bool recursive) {
   this->positionUpdated = false;
   this->orientationUpdated = false;

Thing.h

@@ -223,7 +223,7 @@ class Thing {
 #pragma region Update
 
  public:
-  virtual void PrepareForUpdate();
+  //virtual void PrepareForUpdate();
 
   /// @brief Updates the state of the thing
   /// @param recurse When true, this will Update the descendants recursively

Things/TouchSensor.cpp

@@ -11,9 +11,9 @@ bool TouchSensor::IsTouching() {
   return this->internalTouch || this->externalTouch;
 }
 
-void TouchSensor::PrepareForUpdate() {
+// void TouchSensor::PrepareForUpdate() {
-  //this->internalTouch = this->externalTouch;
+//   //this->internalTouch = this->externalTouch;
-}
+// }
 
 void TouchSensor::Update(bool recursive) {
   Thing::Update(recursive);

Things/TouchSensor.h

@@ -18,7 +18,7 @@ class TouchSensor : public Thing {
   /// otherwise
   bool IsTouching();
 
-  virtual void PrepareForUpdate() override;
+  //virtual void PrepareForUpdate() override;
   virtual void Update(bool recursive) override;
 
   /// @brief Function used to generate binary data for this touch sensor