diff --git a/Arduino/Examples/BB2A/BB2A.cpp b/Arduino/Examples/BB2A/BB2A.cpp new file mode 100644 index 0000000..440fc87 --- /dev/null +++ b/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 + +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); +} diff --git a/Arduino/Examples/BB2A/configuration.h b/Arduino/Examples/BB2A/configuration.h new file mode 100644 index 0000000..dc4e4e1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 7421e73..9ab9839 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -49,8 +49,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Motor() { - this->SetParent(driver); + : Motor(driver) { this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 06d3fed..dc26caf 100644 --- a/Participants/ParticipantUDP.cpp +++ b/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"; } } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 8b4b769..a23bb6b 100644 --- a/Participants/ParticipantUDP.h +++ b/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(); diff --git a/Thing.cpp b/Thing.cpp index 5f9694c..efb6bfb 100644 --- a/Thing.cpp +++ b/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; diff --git a/Thing.h b/Thing.h index e425f9b..f538c33 100644 --- a/Thing.h +++ b/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 diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 9d347db..e5a593b 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -11,9 +11,9 @@ bool TouchSensor::IsTouching() { return this->internalTouch || this->externalTouch; } -void TouchSensor::PrepareForUpdate() { - //this->internalTouch = this->externalTouch; -} +// void TouchSensor::PrepareForUpdate() { +// //this->internalTouch = this->externalTouch; +// } void TouchSensor::Update(bool recursive) { Thing::Update(recursive); diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index e8eb2bc..9890ef8 100644 --- a/Things/TouchSensor.h +++ b/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