RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

BB2B with HW works

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-15 15:59:43 +02:00
parent 169d842143
commit 31c725b0ca
14 changed files with 229 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
Arduino/Things/DRV8833.cpp
View File

@ -5,6 +5,46 @@
namespace RoboidControl {
namespace Arduino {
DRV8833::DRV8833(Configuration config, Participant* participant)
: Thing(participant) {
this->pinStandby = pinStandby;
if (pinStandby != 255)
pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
this->motorA->SetName("Motor A");
this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
this->motorB->SetName("Motor B");
}
DRV8833::DRV8833(Configuration config, Thing* parent)
: DRV8833(config, parent->owner) {
this->SetParent(parent);
}
#pragma region Differential drive
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
Participant* owner)
: RoboidControl::DifferentialDrive(owner), drv8833(config, owner) {
this->leftWheel = this->drv8833.motorA;
this->rightWheel = this->drv8833.motorB;
}
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
Thing* parent)
: RoboidControl::DifferentialDrive(parent), drv8833(config) {}
void DRV8833::DifferentialDrive::Update(unsigned long currentTimeMs,
bool recurse) {
RoboidControl::DifferentialDrive::Update(currentTimeMs, recurse);
this->drv8833.Update(currentTimeMs, recurse);
}
#pragma endregion Differential drive
#pragma region Motor
#if (ESP32)
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
#endif
@ -13,7 +53,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse)
: Thing(driver->owner) {
: Motor(driver->owner) {
this->SetParent(driver);
this->pinIn1 = pinIn1;
@ -33,34 +73,39 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode
#endif
this->reverse = reverse;
// this->reverse = reverse;
}
void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
this->maxRpm = rpm;
}
// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
// this->maxRpm = rpm;
// }
void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
Thing::SetAngularVelocity(velocity);
// void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
void DRV8833Motor::SetTargetSpeed(float motorSpeed) {
Motor::SetTargetSpeed(motorSpeed);
// Thing::SetAngularVelocity(velocity);
// ignoring rotation axis for now.
// Spherical angularVelocity = this->GetAngularVelocity();
float angularSpeed = velocity.distance; // in degrees/sec
// float angularSpeed = velocity.distance; // in degrees/sec
float rpm = angularSpeed / 360 * 60;
float motorSpeed = rpm / this->maxRpm;
// float rpm = angularSpeed / 360 * 60;
// float motorSpeed = rpm / this->maxRpm;
uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
// if (direction == RotationDirection::CounterClockwise)
// speed = -speed;
// Determine the rotation direction
if (velocity.direction.horizontal.InDegrees() < 0)
motorSpeed = -motorSpeed;
if (this->reverse)
motorSpeed = -motorSpeed;
uint8_t motorSignal =
(uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
// // if (direction == RotationDirection::CounterClockwise)
// // speed = -speed;
// // 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";
// "
// << rpm << ", motor signal = " << (int)motorSignal << "\n";
std::cout << "moto speed " << this->name << " = " << motorSpeed
<< ", motor signal = " << (int)motorSignal << "\n";
#if (ESP32)
if (motorSpeed == 0) { // stop
@ -109,43 +154,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
#endif
}
DRV8833::DRV8833(Participant* participant,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby,
bool reverseA,
bool reverseB)
: Thing(participant) {
this->pinStandby = pinStandby;
if (pinStandby != 255)
pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA);
this->motorA->SetName("Motor A");
this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB);
this->motorB->SetName("Motor B");
}
DRV8833::DRV8833(Thing* parent,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby,
bool reverseA,
bool reverseB)
: DRV8833(parent->owner,
pinAIn1,
pinAIn2,
pinBIn1,
pinBIn2,
pinStandby,
reverseA,
reverseB) {
this->SetParent(parent);
}
#pragma endregion Motor
} // namespace Arduino
} // namespace RoboidControl

76
Arduino/Things/DRV8833.h
View File

@ -1,8 +1,10 @@
#pragma once
#include <Arduino.h>
#include "Participants/IsolatedParticipant.h"
#include "Thing.h"
#include "Things/DifferentialDrive.h"
#include "Things/Motor.h"
namespace RoboidControl {
namespace Arduino {
@ -11,43 +13,49 @@ class DRV8833Motor;
class DRV8833 : public Thing {
public:
struct Configuration {
int AIn1;
int AIn2;
int BIn1;
int BIn2;
};
/// @brief Setup a DRV8833 motor controller
/// @param pinAIn1 The pin number connected to the AIn1 port
/// @param pinAIn2 The pin number connected to the AIn2 port
/// @param pinBIn1 The pin number connected to the BIn1 port
/// @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 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,
bool reverseA = false,
bool reverseB = false);
DRV8833(Thing* parent,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby = 255,
bool reverseA = false,
bool reverseB = false);
DRV8833(Configuration config, Participant* owner = nullptr);
DRV8833(Configuration config, Thing* parent);
DRV8833Motor* motorA = nullptr;
DRV8833Motor* motorB = nullptr;
protected:
unsigned char pinStandby = 255;
public:
class DifferentialDrive;
};
/// @brief Support for a DRV8833 motor controller
class DRV8833Motor : public Thing {
#pragma region Differential drive
class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
public:
DifferentialDrive(DRV8833::Configuration config,
Participant* participant = nullptr);
DifferentialDrive(DRV8833::Configuration config, Thing* parent);
virtual void Update(unsigned long currentTimeMs,
bool recurse = false) override;
protected:
DRV8833 drv8833;
};
#pragma endregion Differential drive
#pragma region Motor
/// @brief Support for a DRV8833 motor controller
class DRV8833Motor : public Motor {
public:
/// @brief Motor turning direction
enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
/// @brief Setup the DC motor
/// @param pinIn1 the pin number for the in1 signal
@ -57,16 +65,16 @@ class DRV8833Motor : public Thing {
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse = false);
void SetMaxRPM(unsigned int rpm);
//void SetMaxRPM(unsigned int rpm);
virtual void SetAngularVelocity(Spherical velocity) override;
bool reverse = false;
//virtual void SetAngularVelocity(Spherical velocity) override;
virtual void SetTargetSpeed(float targetSpeed) override;
//bool reverse = false;
protected:
unsigned char pinIn1 = 255;
unsigned char pinIn2 = 255;
unsigned int maxRpm = 200;
//unsigned int maxRpm = 200;
#if (ESP32)
uint8_t in1Ch;
@ -75,5 +83,7 @@ class DRV8833Motor : public Thing {
#endif
};
#pragma endregion Moto
} // namespace Arduino
} // namespace RoboidControl

49
Arduino/Things/UltrasonicSensor.cpp
View File

@ -1,39 +1,37 @@
#include "UltrasonicSensor.h"
#include <Arduino.h>
#include <iostream>
namespace RoboidControl {
namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho)
UltrasonicSensor::UltrasonicSensor(Configuration config,
Participant* participant)
: Thing(participant) {
this->pinTrigger = pinTrigger;
this->pinEcho = pinEcho;
this->pinTrigger = config.triggerPin;
this->pinEcho = config.echoPin;
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
}
UltrasonicSensor::UltrasonicSensor(Thing* parent,
unsigned char pinTrigger,
unsigned char pinEcho)
: UltrasonicSensor(parent->owner, pinTrigger, pinEcho) {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: UltrasonicSensor(config, parent->owner) {
this->SetParent(parent);
}
float UltrasonicSensor::GetDistance() {
// Start the ultrasonic 'ping'
digitalWrite(pinTrigger, LOW);
delayMicroseconds(5);
delayMicroseconds(2);
digitalWrite(pinTrigger, HIGH);
delayMicroseconds(10);
digitalWrite(pinTrigger, LOW);
// Measure the duration of the pulse on the echo pin
float duration_us =
pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds
unsigned long duration_us =
pulseIn(pinEcho, HIGH); // the result is in microseconds
// Calculate the distance:
// * Duration should be divided by 2, because the ping goes to the object
@ -44,14 +42,18 @@ float UltrasonicSensor::GetDistance() {
// * Now we calculate the distance based on the speed of sound (340 m/s):
// distance = duration_sec * 340;
// * The result calculation is therefore:
this->distance = duration_us / 2 / 1000000 * 340;
this->distance = (float)duration_us / 2 / 1000000 * 340;
// Serial.println(this->distance);
// std::cout << "US distance " << this->distance << std::endl;
// Filter faulty measurements. The sensor can often give values > 30 m which
// are not correct
// if (distance > 30)
// distance = 0;
return distance;
return this->distance;
}
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
@ -59,11 +61,18 @@ void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
Thing::Update(currentTimeMs, recursive);
}
UltrasonicSensor::TouchSensor::TouchSensor(Thing& parent,
unsigned char pinTrigger,
unsigned char pinEcho)
: RoboidControl::TouchSensor(&parent),
ultrasonic(&parent, pinTrigger, pinEcho) {
#pragma region Touch sensor
UltrasonicSensor::TouchSensor::TouchSensor(
UltrasonicSensor::Configuration config,
Thing& parent)
: RoboidControl::TouchSensor(&parent), ultrasonic(config, &parent) {
this->touchedSomething = false;
}
UltrasonicSensor::TouchSensor::TouchSensor(
UltrasonicSensor::Configuration config,
Thing* parent)
: RoboidControl::TouchSensor(parent), ultrasonic(config, parent) {
this->touchedSomething = false;
}
@ -74,5 +83,7 @@ void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs,
this->ultrasonic.distance <= this->touchDistance);
}
#pragma region Touch sensor
} // namespace Arduino
} // namespace RoboidControl

20
Arduino/Things/UltrasonicSensor.h
View File

@ -8,16 +8,17 @@ namespace Arduino {
/// @brief An HC-SR04 ultrasonic distance sensor
class UltrasonicSensor : Thing {
public:
struct Configuration {
int triggerPin;
int echoPin;
};
/// @brief Setup an ultrasonic sensor
/// @param participant The participant to use
/// @param pinTrigger The pin number of the trigger signal
/// @param pinEcho The pin number of the echo signal
UltrasonicSensor(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho);
UltrasonicSensor(Thing* parent,
unsigned char pinTrigger,
unsigned char pinEcho);
UltrasonicSensor(Configuration config, Participant* participant);
UltrasonicSensor(Configuration config, Thing* parent);
// parameters
@ -47,9 +48,12 @@ class UltrasonicSensor : Thing {
class TouchSensor;
};
#pragma region Touch sensor
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
public:
TouchSensor(Thing& parent, unsigned char pinTrigger, unsigned char pinEcho);
TouchSensor(UltrasonicSensor::Configuration config, Thing& parent);
TouchSensor(UltrasonicSensor::Configuration config, Thing* parent);
float touchDistance = 0.2f;
@ -61,5 +65,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
UltrasonicSensor ultrasonic;
};
#pragma region Touch sensor
} // namespace Arduino
} // namespace RoboidControl

8
Thing.cpp
View File

@ -5,6 +5,7 @@
#include "Participants/IsolatedParticipant.h"
#include <string.h>
// #include <iostream>
#if defined(ARDUINO)
#include "Arduino.h"
@ -19,12 +20,14 @@ namespace RoboidControl {
#pragma region Init
Thing::Thing(unsigned char thingType)
: Thing(IsolatedParticipant::Isolated(), thingType) {}
// Thing::Thing(unsigned char thingType)
// : Thing(IsolatedParticipant::Isolated(), thingType) {}
Thing::Thing(Participant* owner,
unsigned char thingType,
unsigned char thingId) {
if (owner == nullptr)
owner = IsolatedParticipant::Isolated();
this->owner = owner;
this->id = thingId;
this->type = thingType;
@ -245,6 +248,7 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) {
this->nameChanged = false;
if (recursive) {
// std::cout << "# chilren: " << (int)this->childCount << std::endl;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr)

4
Thing.h
View File

@ -43,14 +43,14 @@ class Thing {
/// @brief Create a new thing without communication abilities
/// @param thingType The type of thing (can use Thing::Type)
Thing(unsigned char thingType = Type::Undetermined);
//Thing(unsigned char thingType = Type::Undetermined);
/// @brief Create a new thing for a participant
/// @param owner The owning participant
/// @param thingType The type of thing (can use Thing::Type)
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
Thing(Participant* owner,
Thing(Participant* owner = nullptr,
unsigned char thingType = Type::Undetermined,
unsigned char thingId = 0);

33
Things/DifferentialDrive.cpp
View File

@ -2,12 +2,15 @@
namespace RoboidControl {
DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {}
DifferentialDrive::DifferentialDrive(Participant* participant,
unsigned char thingId)
: Thing(participant, Thing::Type::DifferentialDrive, thingId) {
this->leftWheel = new Motor();
this->rightWheel = new Motor();
}
DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId)
: Thing(participant, Thing::Type::DifferentialDrive, thingId) {}
DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId)
: Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
float wheelSeparation) {
@ -24,17 +27,17 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
float distance = this->wheelSeparation / 2;
this->leftWheel = leftWheel;
;
if (leftWheel != nullptr)
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
// void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
// float distance = this->wheelSeparation / 2;
// this->leftWheel = leftWheel;
// ;
// if (leftWheel != nullptr)
// this->leftWheel->SetPosition(Spherical(distance, Direction::left));
this->rightWheel = rightWheel;
if (rightWheel != nullptr)
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
// this->rightWheel = rightWheel;
// if (rightWheel != nullptr)
// this->rightWheel->SetPosition(Spherical(distance, Direction::right));
// }
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
if (this->leftWheel != nullptr)

18
Things/DifferentialDrive.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "Thing.h"
#include "Motor.h"
namespace RoboidControl {
@ -9,13 +10,14 @@ namespace RoboidControl {
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
class DifferentialDrive : public Thing {
public:
/// @brief Create a differential drive without networking support
DifferentialDrive();
// /// @brief Create a differential drive without networking support
// DifferentialDrive();
/// @brief Create a differential drive with networking support
/// @param participant The local participant
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
DifferentialDrive(Participant* participant, unsigned char thingId = 0);
DifferentialDrive(Participant* participant = nullptr,
unsigned char thingId = 0);
/// @brief Create a new child differential drive
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate
@ -45,6 +47,11 @@ class DifferentialDrive : public Thing {
/// @copydoc RoboidControl::Thing::Update(unsigned long)
virtual void Update(unsigned long currentMs, bool recursive = true) override;
/// @brief The left wheel
Motor* leftWheel = nullptr;
/// @brief The right wheel
Motor* rightWheel = nullptr;
protected:
/// @brief The radius of a wheel in meters
float wheelRadius = 1.0f;
@ -53,11 +60,6 @@ class DifferentialDrive : public Thing {
/// @brief Convert revolutions per second to meters per second
float rpsToMs = 1.0f;
/// @brief The left wheel
Thing* leftWheel = nullptr;
/// @brief The right wheel
Thing* rightWheel = nullptr;
};
} // namespace RoboidControl

2
Things/DigitalSensor.cpp
View File

@ -2,7 +2,7 @@
namespace RoboidControl {
DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
: Thing(owner, Type::Switch, thingId) {}

4
Things/DigitalSensor.h
View File

@ -8,12 +8,12 @@ namespace RoboidControl {
class DigitalSensor : public Thing {
public:
/// @brief Create a digital sensor without communication abilities
DigitalSensor();
//DigitalSensor();
/// @brief Create a digital sensor for a participant
/// @param owner The owning participant
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
DigitalSensor(Participant* owner, unsigned char thingId = 0);
DigitalSensor(Participant* owner = nullptr, unsigned char thingId = 0);
/// @brief Create a new child digital sensor
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate

9
Things/Motor.cpp Normal file
View File

@ -0,0 +1,9 @@
#include "Motor.h"
RoboidControl::Motor::Motor(Participant* owner) {}
RoboidControl::Motor::Motor(Thing* parent) {}
void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) {
this->targetSpeed = targetSpeed;
}

23
Things/Motor.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#include "Thing.h"
namespace RoboidControl {
class Motor : public Thing {
public:
Motor(Participant* owner = nullptr);
Motor(Thing* parent);
/// @brief Motor turning direction
enum class Direction { Clockwise = 1, CounterClockwise = -1 };
/// @brief The forward turning direction of the motor
Direction direction;
virtual void SetTargetSpeed(float targetSpeed); // -1..0..1
protected:
float targetSpeed = 0;
};
} // namespace RoboidControl

2
Things/TouchSensor.cpp
View File

@ -2,7 +2,7 @@
namespace RoboidControl {
TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
//TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
TouchSensor::TouchSensor(Participant* owner, unsigned char thingId)
: Thing(owner, Thing::Type::TouchSensor, thingId) {}

4
Things/TouchSensor.h
View File

@ -9,12 +9,12 @@ class TouchSensor : public Thing {
// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
public:
/// @brief Create a touch sensor without communication abilities
TouchSensor();
//TouchSensor();
/// @brief Create a touch sensor for a participant
/// @param owner The owning participant
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
TouchSensor(Participant* owner, unsigned char thingId = 0);
TouchSensor(Participant* owner = nullptr, unsigned char thingId = 0);
/// @brief Create a new child touch sensor
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate