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

Added controlledMotor, ptr to ref for hierarchy

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-19 08:55:00 +02:00
parent 5b5513971c
commit da8831a968
14 changed files with 160 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Arduino/Things/DRV8833.cpp
View File

@ -5,6 +5,8 @@
namespace RoboidControl {
namespace Arduino {
#pragma region DRV8833
DRV8833::DRV8833(Configuration config, Participant* participant)
: Thing(participant) {
this->pinStandby = pinStandby;
@ -22,6 +24,8 @@ DRV8833::DRV8833(Configuration config, Thing* parent)
this->SetParent(parent);
}
#pragma endregion DRV8833
#pragma region Differential drive
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
@ -32,7 +36,7 @@ DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
}
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
Thing* parent)
Thing& parent)
: RoboidControl::DifferentialDrive(parent), drv8833(config) {}
void DRV8833::DifferentialDrive::Update(bool recurse) {
@ -79,8 +83,8 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
// this->maxRpm = rpm;
// }
void DRV8833Motor::SetTargetSpeed(float motorSpeed) {
Motor::SetTargetSpeed(motorSpeed);
void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
Motor::SetTargetVelocity(motorSpeed);
uint8_t motorSignal =
(uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);

4
Arduino/Things/DRV8833.h
View File

@ -40,7 +40,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
public:
DifferentialDrive(DRV8833::Configuration config,
Participant* participant = nullptr);
DifferentialDrive(DRV8833::Configuration config, Thing* parent);
DifferentialDrive(DRV8833::Configuration config, Thing& parent);
virtual void Update(bool recurse = false) override;
@ -66,7 +66,7 @@ class DRV8833Motor : public Motor {
// void SetMaxRPM(unsigned int rpm);
// virtual void SetAngularVelocity(Spherical velocity) override;
virtual void SetTargetSpeed(float targetSpeed) override;
virtual void SetTargetVelocity(float targetSpeed) override;
// bool reverse = false;
protected:

21
Arduino/Things/DigitalInput.cpp
View File

@ -14,12 +14,17 @@ DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
pinMode(this->pin, INPUT);
}
DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) {
DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) {
this->pin = pin;
pinMode(this->pin, INPUT);
}
// DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) {
// this->pin = pin;
// pinMode(this->pin, INPUT);
// }
void DigitalInput::Update(bool recursive) {
this->isHigh = digitalRead(this->pin);
this->isLow = !this->isHigh;
@ -31,8 +36,8 @@ void DigitalInput::Update(bool recursive) {
#pragma region Touch sensor
DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
: RoboidControl::TouchSensor(parent), digitalInput(parent, pin) {}
DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent)
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
void DigitalInput::TouchSensor::Update(bool recursive) {
this->touchedSomething = digitalInput.isLow;
@ -47,9 +52,9 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
Thing* parent)
Thing& parent)
: RoboidControl::RelativeEncoder(parent),
digitalInput(parent, config.pin),
digitalInput(config.pin, parent),
pulsesPerRevolution(config.pulsesPerRevolution) {
// We support up to 2 pulse counters
@ -94,8 +99,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
this->pulseFrequency = pulseCount / timeStep;
this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
// std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
// << " timestep " << timeStep << std::endl;
std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
<< " timestep " << timeStep << std::endl;
this->lastUpdateTime = currentTimeMs;
}

7
Arduino/Things/DigitalInput.h
View File

@ -13,7 +13,8 @@ class DigitalInput : public Thing {
/// @param participant The participant to use
/// @param pin The digital pin
DigitalInput(Participant* participant, unsigned char pin);
DigitalInput(Thing* parent, unsigned char pin);
//DigitalInput(Thing* parent, unsigned char pin);
DigitalInput(unsigned char pin, Thing& parent);
bool isHigh = false;
bool isLow = false;
@ -34,7 +35,7 @@ class DigitalInput : public Thing {
class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
public:
TouchSensor(unsigned char pin, Thing* parent);
TouchSensor(unsigned char pin, Thing& parent);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recurse = false) override;
@ -54,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
unsigned char pulsesPerRevolution;
};
RelativeEncoder(Configuration config, Thing* parent);
RelativeEncoder(Configuration config, Thing& parent);
unsigned char pulsesPerRevolution;

22
Arduino/Things/UltrasonicSensor.cpp
View File

@ -16,9 +16,13 @@ UltrasonicSensor::UltrasonicSensor(Configuration config,
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
}
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: UltrasonicSensor(config, parent->owner) {
this->SetParent(parent);
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent)
: Thing(parent) {
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
}
float UltrasonicSensor::GetDistance() {
@ -63,15 +67,15 @@ void UltrasonicSensor::Update(bool recursive) {
#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;
}
UltrasonicSensor::TouchSensor::TouchSensor(
UltrasonicSensor::Configuration config,
Thing* parent)
: RoboidControl::TouchSensor(parent), ultrasonic(config, parent) {
this->touchedSomething = false;
}

4
Arduino/Things/UltrasonicSensor.h
View File

@ -18,7 +18,7 @@ class UltrasonicSensor : Thing {
/// @param pinTrigger The pin number of the trigger signal
/// @param pinEcho The pin number of the echo signal
UltrasonicSensor(Configuration config, Participant* participant);
UltrasonicSensor(Configuration config, Thing* parent);
UltrasonicSensor(Configuration config, Thing& parent);
// parameters
@ -52,7 +52,7 @@ class UltrasonicSensor : Thing {
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
public:
TouchSensor(UltrasonicSensor::Configuration config, Thing& parent);
TouchSensor(UltrasonicSensor::Configuration config, Thing* parent);
//TouchSensor(UltrasonicSensor::Configuration config, Thing* parent);
float touchDistance = 0.2f;

2
Posix/PosixParticipant.h
View File

@ -13,9 +13,11 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
bool Publish(IMessage* msg);
protected:
#if defined(__unix__) || defined(__APPLE__)
sockaddr_in remote_addr;
sockaddr_in server_addr;
sockaddr_in broadcast_addr;
#endif
};
} // namespace Posix

50
Things/ControlledMotor.cpp Normal file
View File

@ -0,0 +1,50 @@
#include "ControlledMotor.h"
namespace RoboidControl {
ControlledMotor::ControlledMotor(Motor* motor, RelativeEncoder* encoder)
: Motor(), motor(motor), encoder(encoder) {
this->type = Type::ControlledMotor;
Thing* parent = motor->GetParent();
this->SetParent(parent);
}
void ControlledMotor::SetTargetVelocity(float velocity) {
this->targetVelocity = velocity;
this->rotationDirection =
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
}
void ControlledMotor::Update(bool recurse) {
encoder->Update(recurse);
this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
unsigned long currentTimeMs = GetTimeMs();
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
this->lastUpdateTime = currentTimeMs;
float error = this->targetVelocity - this->actualVelocity;
float acceleration =
error * timeStep * pidP; // Just P is used at this moment
motor->SetTargetVelocity(targetVelocity +
acceleration); // or something like that
motor->Update(recurse);
}
// float ControlledMotor::GetActualVelocity() {
// return (int)rotationDirection * encoder->rotationSpeed;
// }
// bool ControlledMotor::Drive(float distance) {
// if (!driving) {
// targetDistance = distance;
// startDistance = encoder->GetDistance();
// driving = true;
// }
// float totalDistance = encoder->GetDistance() - startDistance;
// bool completed = totalDistance > targetDistance;
// return completed;
// }
} // namespace RoboidControl

52
Things/ControlledMotor.h Normal file
View File

@ -0,0 +1,52 @@
#pragma once
#include "Motor.h"
#include "RelativeEncoder.h"
namespace RoboidControl {
/// @brief A motor with speed control
/// It uses a feedback loop from an encoder to regulate the speed
/// The speed is measured in revolutions per second.
class ControlledMotor : public Motor {
public:
ControlledMotor(Motor* motor, RelativeEncoder* encoder);
float pidP = 1;
float pidD = 0;
float pidI = 0;
float actualVelocity;
enum Direction { Forward = 1, Reverse = -1 };
Direction rotationDirection;
virtual void Update(bool recurse = false) override;
/// @brief Set the target verlocity for the motor controller
/// @param speed the target velocity in revolutions per second.
virtual void SetTargetVelocity(float velocity) override;
/// @brief Get the actual velocity from the encoder
/// @return The velocity in revolutions per second
// float GetActualVelocity();
// bool Drive(float distance);
Motor* motor;
RelativeEncoder* encoder;
protected:
float lastUpdateTime;
//float targetVelocity;
// float netDistance = 0;
// float startDistance = 0;
// bool driving = false;
// float targetDistance = 0;
// float lastEncoderPosition = 0;
};
} // namespace RoboidControl

12
Things/DifferentialDrive.cpp
View File

@ -52,12 +52,16 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
// this->rightWheel->SetPosition(Spherical(distance, Direction::right));
// }
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
void DifferentialDrive::SetWheelVelocity(float velocityLeft, float velocityRight) {
// if (this->leftWheel != nullptr)
// this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, Direction::left));
// if (this->rightWheel != nullptr)
// this->rightWheel->SetAngularVelocity(
// Spherical(velocityRight, Direction::right));
if (this->leftWheel != nullptr)
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
this->leftWheel->SetTargetVelocity(velocityLeft);
if (this->rightWheel != nullptr)
this->rightWheel->SetAngularVelocity(
Spherical(speedRight, Direction::right));
this->rightWheel->SetTargetVelocity(velocityRight);
}
void DifferentialDrive::Update(bool recursive) {

4
Things/Motor.cpp
View File

@ -10,8 +10,8 @@ RoboidControl::Motor::Motor(Participant* owner)
Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {}
void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) {
this->targetSpeed = targetSpeed;
void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) {
this->targetVelocity = targetSpeed;
}
} // namespace RoboidControl

4
Things/Motor.h
View File

@ -15,10 +15,10 @@ class Motor : public Thing {
/// @brief The forward turning direction of the motor
Direction direction;
virtual void SetTargetSpeed(float targetSpeed); // -1..0..1
virtual void SetTargetVelocity(float velocity); // -1..0..1
protected:
float targetSpeed = 0;
float targetVelocity = 0;
};
} // namespace RoboidControl

4
Things/RelativeEncoder.cpp
View File

@ -4,8 +4,8 @@ namespace RoboidControl {
RelativeEncoder::RelativeEncoder(Participant* owner)
: Thing(owner, Type::IncrementalEncoder) {}
// RelativeEncoder::RelativeEncoder(Thing* parent)
// : Thing(parent, Type::IncrementalEncoder) {}
RelativeEncoder::RelativeEncoder(Thing& parent)
: Thing(Type::IncrementalEncoder, parent) {}
float RelativeEncoder::GetRotationSpeed() {
return rotationSpeed;

2
Things/RelativeEncoder.h
View File

@ -16,7 +16,7 @@ class RelativeEncoder : public Thing {
/// rotation
RelativeEncoder(Participant* owner);
// RelativeEncoder(Thing* parent);
RelativeEncoder(Thing& parent);
RelativeEncoder(Thing& parent = Thing::Root);
/// @brief Get the rotation speed
/// @return The speed in revolutions per second