From a1a6941a283527463d485be33a215fe559986dd1 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 4 Mar 2025 16:01:55 +0100 Subject: [PATCH] Robot ant works --- Arduino/Things/DRV8833.cpp | 18 ++++++++++-------- Arduino/Things/DRV8833.h | 13 +++++++------ Arduino/Things/UltrasonicSensor.cpp | 9 ++++++++- Arduino/Things/UltrasonicSensor.h | 2 ++ LocalParticipant.cpp | 11 ++++++++--- Participant.cpp | 4 ++-- Things/DifferentialDrive.cpp | 4 ++-- Things/TouchSensor.cpp | 11 +++-------- 8 files changed, 42 insertions(+), 30 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 1e36be5..26cc8c1 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/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 - << ", motor signal = " << (int)motorSignal << "\n"; + // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm " << rpm + // << ", 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, - DRV8833Motor::RotationDirection directionB) + bool reverseA, + 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"; } diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 7ce556c..836b15a 100644 --- a/Arduino/Things/DRV8833.h +++ 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 directionB The forward turning direction of motor B + /// @param reverseA The forward turning direction of motor A + /// @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, - DRV8833Motor::RotationDirection directionB = DRV8833Motor::RotationDirection::Clockwise); + bool reverseA = false, + bool reverseB = false); DRV8833Motor* motorA = nullptr; DRV8833Motor* motorB = nullptr; diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 343288e..de9ad5b 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/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 \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 2f5784e..c5dc4f3 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/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; diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 88ab631..94044f0 100644 --- a/LocalParticipant.cpp +++ b/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 - std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; + else { + 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 diff --git a/Participant.cpp b/Participant.cpp index 8a3d929..fbcc64e 100644 --- a/Participant.cpp +++ b/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 - // << "[" << (int)networkId << "/" << (int)thingId << "]\n"; + // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port + // << "[" << (int)networkId << "/" << (int)thingId << "]\n"; return nullptr; } diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index c60e8c1..854c34a 100644 --- a/Things/DifferentialDrive.cpp +++ b/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 " - << speedLeft << " right wheel " << speedRight << "\n"; + // std::cout << "lin. speed " << linearVelocity << " ang. speed " << angularVelocity.distance << " left wheel " + // << speedLeft << " right wheel " << speedRight << "\n"; } } // namespace RoboidControl \ No newline at end of file diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 13879cb..e9e7b41 100644 --- a/Things/TouchSensor.cpp +++ b/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 (this->touchedSomething) - // std::cout << "Touching something!\n"; + if (bytes[0] == 1) + std::cout << this->name << " is Touching something!\n"; + this->touchedSomething |= (bytes[0] == 1); } } // namespace RoboidControl \ No newline at end of file