From 86200232774ddcbd8d6f2af7843ca3962bfca922 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 23 May 2025 17:57:14 +0200 Subject: [PATCH] Working controlled motor --- Arduino/Things/DRV8833.h | 2 +- Arduino/Things/DigitalInput.cpp | 18 ++++++++++----- Arduino/Things/UltrasonicSensor.cpp | 4 ++-- Arduino/Things/UltrasonicSensor.h | 4 ++-- Things/ControlledMotor.cpp | 36 ++++++++++++++++++++--------- Things/ControlledMotor.h | 6 +++-- Things/RelativeEncoder.h | 4 ++-- 7 files changed, 48 insertions(+), 26 deletions(-) diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index f2c2d3f..9762980 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -18,7 +18,7 @@ class DRV8833 : public Thing { int AIn2; int BIn1; int BIn2; - int standby; + int standby = 255; }; /// @brief Setup a DRV8833 motor controller diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index 12f3b12..ef41ad9 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,7 +7,8 @@ namespace Arduino { #pragma region Digital input -DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) { +DigitalInput::DigitalInput(unsigned char pin, Thing* parent) + : Thing(Type::Undetermined, parent) { this->pin = pin; pinMode(this->pin, INPUT); std::cout << "digital input start\n"; @@ -46,13 +47,15 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, void DigitalInput::RelativeEncoder::Start() { // We support up to 2 pulse counters - if (interruptCount == 0) + if (interruptCount == 0) { + std::cout << "pin interrupt 1 activated" << std::endl; attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0, RISING); - else if (interruptCount == 1) + } else if (interruptCount == 1) { + std::cout << "pin interrupt 2 activated" << std::endl; attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1, RISING); - else { + } else { // maximum interrupt count reached std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" << std::endl; @@ -85,14 +88,17 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) { } float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f; + if (timeStep <= 0) + return; + int pulseCount = GetPulseCount(); netPulseCount += pulseCount; 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; } diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index d92da44..2568070 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -9,8 +9,8 @@ namespace Arduino { UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) { this->name = "Ultrasonic sensor"; - this->pinTrigger = config.triggerPin; - this->pinEcho = config.echoPin; + this->pinTrigger = config.trigger; + this->pinEcho = config.echo; pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode pinMode(pinEcho, INPUT); // configure the echo pin to input mode diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 7c02778..8e72887 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -9,8 +9,8 @@ namespace Arduino { class UltrasonicSensor : Thing { public: struct Configuration { - int triggerPin; - int echoPin; + int trigger; + int echo; }; UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot()); diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index 1eb1417..26b16bc 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -1,4 +1,5 @@ #include "ControlledMotor.h" +#include "LinearAlgebra/FloatSingle.h" namespace RoboidControl { @@ -7,9 +8,10 @@ ControlledMotor::ControlledMotor(Motor* motor, Thing* parent) : Motor(parent), motor(motor), encoder(encoder) { this->type = Type::ControlledMotor; - // encoder.SetParent(*this); + //encoder->SetParent(null); // Thing parent = motor.GetParent(); // this->SetParent(parent); + this->integral = 0; } void ControlledMotor::SetTargetVelocity(float velocity) { @@ -19,22 +21,34 @@ void ControlledMotor::SetTargetVelocity(float velocity) { } void ControlledMotor::Update(bool recurse) { - encoder->Update(false); - - this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; - unsigned long currentTimeMs = GetTimeMs(); float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; this->lastUpdateTime = currentTimeMs; + if (timeStep <= 0) + return; + + // encoder->Update(false); + + this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; float error = this->targetVelocity - this->actualVelocity; - float acceleration = - error * timeStep * pidP; // Just P is used at this moment - std::cout << "motor acc. " << acceleration << std::endl; - motor->SetTargetVelocity(targetVelocity + - acceleration); // or something like that + float p_term = error * pidP; + this->integral += error * timeStep; + float i_term = pidI * this->integral; + float d_term = pidD * (error - this->lastError) / timeStep; + this->lastError = error; - motor->Update(false); + float output = p_term + i_term + d_term; + std::cout << "target " << this->targetVelocity << " actual " + << this->actualVelocity << " output = " << output << std::endl; + // float acceleration = + // error * timeStep * pidP; // Just P is used at this moment + // std::cout << "motor acc. " << acceleration << std::endl; + + // float newTargetVelocity = motor->targetVelocity + acceleration; + output = LinearAlgebra::Float::Clamp(output, -1, 1); + motor->SetTargetVelocity(output); // or something like that + //motor->Update(false); } // float ControlledMotor::GetActualVelocity() { diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h index 462f206..cfb0229 100644 --- a/Things/ControlledMotor.h +++ b/Things/ControlledMotor.h @@ -12,9 +12,9 @@ class ControlledMotor : public Motor { public: ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot()); - float pidP = 1; + float pidP = 0.5; float pidD = 0; - float pidI = 0; + float pidI = 0.2; /// @brief The actual velocity in revolutions per second float actualVelocity; @@ -32,6 +32,8 @@ class ControlledMotor : public Motor { RelativeEncoder* encoder; protected: + float integral = 0; + float lastError = 0; float lastUpdateTime; }; diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index b2aeda4..350d8a5 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -14,14 +14,14 @@ class RelativeEncoder : public Thing { /// full rotation /// @param distancePerRevolution The distance a wheel travels per full /// rotation - RelativeEncoder(Participant* owner); + //RelativeEncoder(Participant* owner); // RelativeEncoder(Thing* parent); RelativeEncoder(Thing* parent = Thing::LocalRoot()); /// @brief Get the rotation speed /// @return The speed in revolutions per second virtual float GetRotationSpeed(); - float rotationSpeed; + float rotationSpeed = 0; /// @brief Function used to generate binary data for this touch sensor /// @param buffer The byte array for thw binary data