Robot ant works

Arduino/Things/DRV8833.cpp

@@ -7,7 +7,7 @@ namespace RoboidControl {
 DRV8833Motor::DRV8833Motor(Participant* participant,
                            unsigned char pinIn1,
                            unsigned char pinIn2,
-                           RotationDirection direction)
+                           bool reverse)
     : Thing(participant) {
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
@@ -24,7 +24,7 @@ DRV8833Motor::DRV8833Motor(Participant* participant,
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
 #endif
 
-  this->direction = direction;
+  this->reverse = reverse;
 }
 
 void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
@@ -46,9 +46,11 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
   // Determine the rotation direction
   if (velocity.direction.horizontal.InDegrees() < 0)
     motorSpeed = -motorSpeed;
+  if (this->reverse)
+    motorSpeed =-motorSpeed;
 
-  std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
+  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
-            << ", motor signal = " << (int)motorSignal << "\n";
+  //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)
   if (motorSpeed == 0) {  // stop
@@ -103,16 +105,16 @@ DRV8833::DRV8833(Participant* participant,
                  unsigned char pinBIn1,
                  unsigned char pinBIn2,
                  unsigned char pinStandby,
-                 DRV8833Motor::RotationDirection directionA,
+                 bool reverseA,
-                 DRV8833Motor::RotationDirection directionB)
+                 bool reverseB)
     : Thing(participant) {
   this->pinStandby = pinStandby;
   if (pinStandby != 255)
     pinMode(pinStandby, OUTPUT);
 
-  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, directionA);
+  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
   this->motorA->name = "Motor A";
-  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, directionB);
+  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
   this->motorB->name = "Motor B";
 }
 

Arduino/Things/DRV8833.h

@@ -18,15 +18,16 @@ class DRV8833Motor : public Thing {
   DRV8833Motor(Participant* participant,
                unsigned char pinIn1,
                unsigned char pinIn2,
-               RotationDirection direction = RotationDirection::Clockwise);
+               bool reverse = false);
   void SetMaxRPM(unsigned int rpm);
 
   virtual void SetAngularVelocity(Spherical velocity) override;
 
+  bool reverse = false;
+
  protected:
   unsigned char pinIn1 = 255;
   unsigned char pinIn2 = 255;
-  RotationDirection direction = RotationDirection::Clockwise;
   unsigned int maxRpm = 200;
 };
 
@@ -39,16 +40,16 @@ class DRV8833 : public Thing {
   /// @param pinBIn2 The pin number connceted to the BIn2 port
   /// @param pinStandby The pin number connected to the standby port, 255
   /// indicated that the port is not connected
-  /// @param directionA The forward turning direction of motor A
+  /// @param reverseA The forward turning direction of motor A
-  /// @param directionB The forward turning direction of motor B
+  /// @param reverseB The forward turning direction of motor B
   DRV8833(Participant* participant,
           unsigned char pinAIn1,
           unsigned char pinAIn2,
           unsigned char pinBIn1,
           unsigned char pinBIn2,
           unsigned char pinStandby = 255,
-          DRV8833Motor::RotationDirection directionA = DRV8833Motor::RotationDirection::Clockwise,
+          bool reverseA = false,
-          DRV8833Motor::RotationDirection directionB = DRV8833Motor::RotationDirection::Clockwise);
+          bool reverseB = false);
 
   DRV8833Motor* motorA = nullptr;
   DRV8833Motor* motorB = nullptr;

Arduino/Things/UltrasonicSensor.cpp

@@ -41,7 +41,7 @@ float UltrasonicSensor::GetDistance() {
   // if (distance > 30)
   //   distance = 0;
 
-  this->touchedSomething = (this->distance <= this->touchDistance);
+  this->touchedSomething |= (this->distance <= this->touchDistance);
 
 //   std::cout << "Ultrasonic " << this->distance << " " << this->touchedSomething << "\n";
 
@@ -49,8 +49,15 @@ float UltrasonicSensor::GetDistance() {
 }
 
 void UltrasonicSensor::Update(unsigned long currentTimeMs) {
+  this->touchedSomething = false;
   GetDistance();
 }
 
+// void UltrasonicSensor::ProcessBinary(char* bytes) {
+//   this->touchedSomething = (bytes[0] == 1);
+//   if (this->touchedSomething)
+//     std::cout << "Touching something!\n";
+// }
+
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.h

@@ -20,6 +20,8 @@ class UltrasonicSensor : public TouchSensor {
 
   virtual void Update(unsigned long currentTimeMs) override;
 
+  // virtual void ProcessBinary(char* bytes) override;
+
  protected:
   unsigned char pinTrigger = 0;
   unsigned char pinEcho = 0;

LocalParticipant.cpp

@@ -111,7 +111,7 @@ Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) {
 }
 
 Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) {
-  std::cout << "New LocalParticipant " << ipAddress << ":" << port << "\n";
+  std::cout << "New Participant " << ipAddress << ":" << port << "\n";
   Participant* participant = new Participant(ipAddress, port);
   participant->networkId = (unsigned char)this->senders.size();
   this->senders.push_back(participant);
@@ -289,8 +289,13 @@ void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr)
     thing->ProcessBinary(msg->bytes);
-  else
+  else {
-    std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n";
+    thing = this->Get(msg->networkId, msg->thingId);
+    if (thing != nullptr)
+      thing->ProcessBinary(msg->bytes);
+    else
+      std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n";
+  }
 }
 
 // Receive

Participant.cpp

@@ -28,8 +28,8 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
     if (thing->id == thingId)
       return thing;
   }
-  //   std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
+    // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
-  //             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
+    //           << "[" << (int)networkId << "/" << (int)thingId << "]\n";
   return nullptr;
 }
 

Things/DifferentialDrive.cpp

@@ -49,8 +49,8 @@ void DifferentialDrive::Update(unsigned long currentMs) {
   if (this->rightWheel != nullptr)
     this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right));
 
-    std::cout << "lin. speed " << linearVelocity << "   ang. speed " << angularVelocity.distance << "   left wheel  "
+    // std::cout << "lin. speed " << linearVelocity << "   ang. speed " << angularVelocity.distance << "   left wheel  "
-            << speedLeft << "   right wheel " << speedRight << "\n";
+    //         << speedLeft << "   right wheel " << speedRight << "\n";
 }
 
 }  // namespace RoboidControl

Things/TouchSensor.cpp

@@ -7,17 +7,12 @@ TouchSensor::TouchSensor(Participant* participant) : Thing(participant) {
   this->type = (unsigned char)Thing::Type::TouchSensor;
 }
 
-// TouchSensor::TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId) :
-// Thing(participant, networkId, thingId) {
-//     this->touchedSomething = false;
-// }
-
 void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  this->touchedSomething = (bytes[0] == 1);
+  if (bytes[0] == 1)
-  // if (this->touchedSomething)
+    std::cout << this->name << " is Touching something!\n";
-  //   std::cout << "Touching something!\n";
+  this->touchedSomething |= (bytes[0] == 1);
 }
 
 }  // namespace RoboidControl