From 438d8ba94100f1c00c7f4b3915d12e8402f89505 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 23 Nov 2023 16:52:21 +0100 Subject: [PATCH 1/7] Added a bit of documentation --- Sensor.h | 1 + Thing.h | 9 +++++---- library.json | 11 +++++++++++ 3 files changed, 17 insertions(+), 4 deletions(-) create mode 100644 library.json diff --git a/Sensor.h b/Sensor.h index f49bfe6..1f47259 100644 --- a/Sensor.h +++ b/Sensor.h @@ -2,6 +2,7 @@ #include "Thing.h" +/// @brief A sensor is a thing which can perform measurements in the environment class Sensor : public Thing { public: Sensor(); diff --git a/Thing.h b/Thing.h index 610542c..dc29ba0 100644 --- a/Thing.h +++ b/Thing.h @@ -1,8 +1,9 @@ #pragma once +/// @brief A thing is a functional component on a robot class Thing { -public: - Thing() { isSensor = false, isMotor = false; } - bool isSensor; - bool isMotor; + public: + Thing() { isSensor = false, isMotor = false; } + bool isSensor; + bool isMotor; }; \ No newline at end of file diff --git a/library.json b/library.json new file mode 100644 index 0000000..8b014b3 --- /dev/null +++ b/library.json @@ -0,0 +1,11 @@ +{ + "name": "RoboidControl", + "version": "0.0", + "build": { + "srcDir": ".", + "flags": [ + "-I .", + "-I ./VectorAlgebra/include" + ] + } +} \ No newline at end of file From afa459b120a02aa6fb72b64ad2e79f1e535ca147 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 23 Nov 2023 17:51:20 +0100 Subject: [PATCH 2/7] Added accelerometer & magnetometer framework --- Accelerometer.h | 19 +++++++++++++++++++ Magnetometer.h | 23 +++++++++++++++++++++++ Roboid.h | 6 ++++++ 3 files changed, 48 insertions(+) create mode 100644 Accelerometer.h create mode 100644 Magnetometer.h diff --git a/Accelerometer.h b/Accelerometer.h new file mode 100644 index 0000000..b7bf662 --- /dev/null +++ b/Accelerometer.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Sensor.h" + +/// @brief A Sensor which can measure the magnetic field +class Accelerometer : public Sensor { + public: + Accelerometer(){}; + + /// @brief This gives the gravity direciton relative to the down direction. + /// @param forward the forward direction, negative is backward, positive is forward + /// @param sideward the sideward direction, negative is left, positive is to the right + /// @note the units (degrees, radians, -1..1, ...) depend on the sensor + void GetAccelerationDirection(float* forward, float* sideward); + /// @brief The magnitude of the gravity field. + /// @return The magnitude. This value is never negative. + /// @note the unity (m/s^2, 0..1) depends on the sensor. + float GetAccelerationMagnitude(); +}; \ No newline at end of file diff --git a/Magnetometer.h b/Magnetometer.h new file mode 100644 index 0000000..ee9cf88 --- /dev/null +++ b/Magnetometer.h @@ -0,0 +1,23 @@ +#pragma once + +#include "Sensor.h" + +/// @brief A Sensor which can measure the magnetic field +class Magnetometer : public Sensor { + public: + Magnetometer(); + + /// @brief Returns the direction of the magnetic field relative to the forward direction + /// @return The direction, negative is to the left, positive is to the right + /// @note The actual unit (degrees, radians, -1..1, ...) depends on the sensor. + virtual float GetDirection(); + /// @brief Returns the inclination of the magnetic field relative to the horizontal plane + /// @return The direction, negative is downward, positive is upward + /// @note The actual unit (degrees, radias, -1..1, ...) depends on the sensor. + virtual float GetInclination(); + + /// @brief Returns the strength of the magnetic field + /// @return The strength. This values should always be positive + /// @note The actual unit (tesla, 0..1, ...) depends on the sensor. + virtual unsigned float GetMagnitude(); +} \ No newline at end of file diff --git a/Roboid.h b/Roboid.h index 7aa1b23..6b16a6d 100644 --- a/Roboid.h +++ b/Roboid.h @@ -5,6 +5,11 @@ #include "Placement.h" #include "Propulsion.h" +class Acceleration { + public: + float GetMagnitude() { return 0;}; +}; + class Roboid { public: Roboid(); @@ -12,6 +17,7 @@ class Roboid { Perception perception; Propulsion propulsion; + Acceleration acceleration; Placement* configuration; unsigned int thingCount; From 28ee532700aacbb30a4cf9faf947be39680d1b39 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 24 Nov 2023 12:28:56 +0100 Subject: [PATCH 3/7] Initial attempt for scripting --- Accelerometer.h | 23 +++++++++++++++++++---- ControlledMotor.cpp | 11 +++++++---- ControlledMotor.h | 34 ++++++++++++++++++++++++++++------ Encoder.cpp | 8 +++++++- Encoder.h | 7 +++++-- Motor.h | 3 +-- Propulsion.cpp | 33 ++++++++++++++++++++++++++++----- Propulsion.h | 32 +++++++++++++++++--------------- Roboid.cpp | 19 ++++++++++++++++++- Roboid.h | 4 +++- Thing.h | 1 + 11 files changed, 134 insertions(+), 41 deletions(-) diff --git a/Accelerometer.h b/Accelerometer.h index b7bf662..44addc8 100644 --- a/Accelerometer.h +++ b/Accelerometer.h @@ -7,11 +7,26 @@ class Accelerometer : public Sensor { public: Accelerometer(){}; - /// @brief This gives the gravity direciton relative to the down direction. - /// @param forward the forward direction, negative is backward, positive is forward - /// @param sideward the sideward direction, negative is left, positive is to the right + /// @brief Returns the direction of the acceleration in the horizontal plane + /// @return The direction of the acceleration, negative is left, positive is + /// right + /// @note The horizontal plane is defined as being orthogonal to the gravity + /// vector /// @note the units (degrees, radians, -1..1, ...) depend on the sensor - void GetAccelerationDirection(float* forward, float* sideward); + float GetHorizontalAccelerationDirection(); + /// @brief Returns the magnitude of the acceleration in the horizontal plane + /// @return The magnitude. This value is never negative. + /// @note the unity (m/s^2, 0..1) depends on the sensor. + float GetHorizontalAccelerationMagnitude(); + + /// @brief This gives the gravity direciton relative to the down direction. + /// @param horizontal the horizontal direction, negative is left, positive is + /// right + /// @param vertical the vertical direction, negative is down, positive is up + /// @note The horizontal plane is defined as being orthogonal to the gravity + /// vector + /// @note the units (degrees, radians, -1..1, ...) depend on the sensor + void GetAccelerationDirection(float* horizontal, float* vertical); /// @brief The magnitude of the gravity field. /// @return The magnitude. This value is never negative. /// @note the unity (m/s^2, 0..1) depends on the sensor. diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index f8145d4..bdef4ef 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -1,20 +1,23 @@ #include "ControlledMotor.h" -ControlledMotor::ControlledMotor() {} +ControlledMotor::ControlledMotor() { + this->isControlledMotor = true; +} ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) { this->motor = motor; this->encoder = encoder; } -void ControlledMotor::SetTargetVelocity(float velocity) { +void ControlledMotor::SetTargetSpeed(float velocity) { this->targetVelocity = velocity; } void ControlledMotor::Update(float timeStep) { - float velocity = GetActualVelocity(); + float velocity = GetActualSpeed(); float error = targetVelocity - velocity; - float acceleration = error * timeStep * pidP; // Just P is used at this moment + float acceleration = + error * timeStep * pidP; // Just P is used at this moment motor->SetSpeed(targetVelocity + acceleration); // or something like that } \ No newline at end of file diff --git a/ControlledMotor.h b/ControlledMotor.h index a283fce..fc896a2 100644 --- a/ControlledMotor.h +++ b/ControlledMotor.h @@ -3,6 +3,9 @@ #include "Encoder.h" #include "Motor.h" +/// @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 Thing { public: ControlledMotor(); @@ -16,17 +19,36 @@ class ControlledMotor : public Thing { void Update(float timeStep); - void SetTargetVelocity(float rotationsPerSecond); - float GetActualVelocity() { - return (int)rotationDirection * encoder->GetRotationsPerSecond(); - } // in rotations per second + /// @brief Set the target speed for the motor controller + /// @param speed the target in revolutions per second. + void SetTargetSpeed(float speed); + + /// @brief Get the actual speed from the encoder + /// @return The speed in revolutions per second + float GetActualSpeed() { + return (int)rotationDirection * encoder->GetRevolutionsPerSecond(); + } + + bool Drive(float distance) { + if (!driving) { + targetDistance = distance; + encoder->StartCountingRevolutions(); + driving = true; + } else + targetDistance -= encoder->RestartCountingRevolutions(); + + return (targetDistance <= 0); + } protected: float targetVelocity; Motor* motor; Encoder* encoder; - enum Direction { Forward = 1, - Reverse = -1 }; + enum Direction { Forward = 1, Reverse = -1 }; Direction rotationDirection; + + bool driving = false; + float targetDistance = 0; + float lastEncoderPosition = 0; }; \ No newline at end of file diff --git a/Encoder.cpp b/Encoder.cpp index 9b3aba8..05a68bc 100644 --- a/Encoder.cpp +++ b/Encoder.cpp @@ -20,7 +20,7 @@ void Encoder::InterruptHandler() { transitionCount++; } -float Encoder::GetRotationsPerSecond() { +float Encoder::GetRevolutionsPerSecond() { return rps; } @@ -41,4 +41,10 @@ void Encoder::Update(float timeStep) { // float rps = distanceThisUpdate * timeStep; distance += distanceThisUpdate; +} + +void Encoder::StartCountingRevolutions() {} + +float Encoder::RestartCountingRevolutions() { + return 0; } \ No newline at end of file diff --git a/Encoder.h b/Encoder.h index e734139..fc16410 100644 --- a/Encoder.h +++ b/Encoder.h @@ -5,17 +5,20 @@ class Encoder { Encoder(); Encoder(unsigned char pin, unsigned char transitionsPerRotation); - float GetRotationsPerSecond(); + float GetPulsesPerSecond(); + float GetRevolutionsPerSecond(); void ResetDistance(); float GetRotationDistance(); void Update(float timeStep); + virtual void StartCountingRevolutions(); + virtual float RestartCountingRevolutions(); + protected: static void InterruptHandler(); static volatile unsigned char transitionCount; - unsigned char transitionsPerRotation; float rps; diff --git a/Motor.h b/Motor.h index 921171d..a6a8576 100644 --- a/Motor.h +++ b/Motor.h @@ -6,8 +6,7 @@ class Motor : public Thing { public: Motor(); /// @brief Turning direction of the motor - enum Direction { Forward = 1, - Reverse = -1 }; + enum Direction { Forward = 1, Reverse = -1 }; /// @brief Set the turning direction of the motor // void SetDirection(Direction direction); diff --git a/Propulsion.cpp b/Propulsion.cpp index 775ee88..e8c30dd 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -77,7 +77,8 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { }; } -void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) { +void Propulsion::SetDiffDriveVelocities(float leftVelocity, + float rightVelocity) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { // Placement placement = placement[motorIx]; // if (placement.position.x < 0) @@ -88,7 +89,8 @@ void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) } void Propulsion::SetTwistSpeed(float forward, float yaw) { - // This is configuration dependent, a drone will do something completely different... + // This is configuration dependent, a drone will do something completely + // different... float leftSpeed = Float::Clamp(forward - yaw, -1, 1); float rightSpeed = Float::Clamp(forward + yaw, -1, 1); SetDiffDriveSpeed(leftSpeed, rightSpeed); @@ -109,17 +111,38 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) { quadcopter->SetTwistSpeed(linear, yaw); } -void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) { +void Propulsion::SetTwistVelocity(float forwardVelocity, + float turningVelocity) { float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1); float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1); SetDiffDriveVelocities(leftVelocity, rightVelocity); } -void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) { +void Propulsion::SetLinearSpeed(Vector3 velocity, + float yawSpeed, + float rollSpeed) { if (quadcopter != nullptr) quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); } Quadcopter* Propulsion::GetQuadcopter() { return quadcopter; -} \ No newline at end of file +} + +// bool Propulsion::Drive(Roboid* roboid, float forwardDistance) { +// bool finished = true; +// for (unsigned int motorIx = 0; motorIx < roboid->propulsion->motorCount; +// motorIx++) { +// Motor* motor = (Motor*)roboid->placement[motorIx].thing; +// if (motor == nullptr) +// continue; +// if (motor->isControlledMotor == false) +// continue; + +// ControlledMotor* controlledMotor = (ControlledMotor*)motor; +// bool motorFinished = controlledMotor->Drive(forwardDistance); +// if (motorFinished == false) +// finished = false; +// } +// return finished; +// } diff --git a/Propulsion.h b/Propulsion.h index 3d6c70f..8da9003 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -1,17 +1,10 @@ #pragma once - -#include "Quadcopter.h" #include "ControlledMotor.h" #include "Placement.h" +#include "Quadcopter.h" #include "Vector2.h" -#include - -//struct MotorPlacement { -// Motor* motor; -// ControlledMotor* controlledMotor; -// Vector2 position; -//}; +// #include class Propulsion { public: @@ -20,13 +13,14 @@ class Propulsion { void Update(); - //void AddMotors(MotorPlacement* motors, unsigned int motorCount); void AddMotors(Placement* motors, unsigned int motorCount); void AddQuadcopter(Quadcopter* quadcopter); + Quadcopter* GetQuadcopter(); unsigned int GetMotorCount(); Motor* GetMotor(unsigned int motorIx); + // Velocity control void SetDiffDriveSpeed(float leftSpeed, float rightSpeed); void SetDiffDriveVelocities(float leftVelocity, float rightVelocity); @@ -36,14 +30,22 @@ class Propulsion { void SetTwistVelocity(float forward, float yaw); // Think: drones - Quadcopter* GetQuadcopter(); - void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F, float rollSpeed = 0.0F); + void SetLinearSpeed(Vector3 direction, + float yawSpeed = 0.0F, + float rollSpeed = 0.0F); + + // Position control (or actually, distance control) + // void Drive(float forwardDistance); + // void Turn(float turnDistance); + + // void Update(); - protected: - //unsigned long lastMillis; - //MotorPlacement* motors = nullptr; Placement* placement = nullptr; unsigned int motorCount = 0; + protected: Quadcopter* quadcopter = nullptr; + + bool driving = false; + float targetDistance = 0; }; diff --git a/Roboid.cpp b/Roboid.cpp index b0cca1e..6884af3 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -11,4 +11,21 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) { perception.AddSensors(configuration, thingCount); propulsion.AddMotors(configuration, thingCount); -} \ No newline at end of file +} + +bool Roboid::Drive(float forwardDistance) { + bool finished = true; + for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) { + Motor* motor = (Motor*)propulsion.placement[motorIx].thing; + if (motor == nullptr) + continue; + if (motor->isControlledMotor == false) + continue; + + ControlledMotor* controlledMotor = (ControlledMotor*)motor; + bool motorFinished = controlledMotor->Drive(forwardDistance); + if (motorFinished == false) + finished = false; + } + return finished; +} diff --git a/Roboid.h b/Roboid.h index 6b16a6d..a83626d 100644 --- a/Roboid.h +++ b/Roboid.h @@ -7,7 +7,7 @@ class Acceleration { public: - float GetMagnitude() { return 0;}; + float GetMagnitude() { return 0; }; }; class Roboid { @@ -21,4 +21,6 @@ class Roboid { Placement* configuration; unsigned int thingCount; + + bool Drive(float forwardDistance); }; \ No newline at end of file diff --git a/Thing.h b/Thing.h index dc29ba0..0351995 100644 --- a/Thing.h +++ b/Thing.h @@ -6,4 +6,5 @@ class Thing { Thing() { isSensor = false, isMotor = false; } bool isSensor; bool isMotor; + bool isControlledMotor; }; \ No newline at end of file From 6058d270795fedb31e54a2a2dac50384d04d05ef Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 24 Nov 2023 17:11:18 +0100 Subject: [PATCH 4/7] First trajectory --- ControlledMotor.cpp | 3 ++- Motor.cpp | 26 +++++++++++++++++++--- Motor.h | 6 +++++ Propulsion.cpp | 9 +++++--- Roboid.cpp | 53 +++++++++++++++++++++++++++++++++++++-------- Roboid.h | 17 +++++++++++++++ Sensing.cpp | 13 +++++++---- Sensor.cpp | 3 ++- Thing.h | 14 ++++++++---- 9 files changed, 119 insertions(+), 25 deletions(-) diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index bdef4ef..5f331cb 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -1,7 +1,8 @@ #include "ControlledMotor.h" ControlledMotor::ControlledMotor() { - this->isControlledMotor = true; + // this->isControlledMotor = true; + this->type = Type::ControlledMotor; } ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) { diff --git a/Motor.cpp b/Motor.cpp index 7842cfa..a8f8a60 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -1,7 +1,11 @@ #include "Motor.h" +#include + +#include Motor::Motor() { - this->isMotor = true; + // this->isMotor = true; + type = Type::Motor; } // Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) { @@ -25,9 +29,25 @@ Motor::Motor() { // } float Motor::GetSpeed() { - return this->currentSpeed; + return this->currentSpeed; } void Motor::SetSpeed(float speed) { - this->currentSpeed = speed; + this->currentSpeed = speed; +} + +bool Motor::Drive(float distance) { + if (!this->driving) { + this->startTime = time(NULL); + this->targetDistance = abs(distance); + this->driving = true; + } + double duration = difftime(time(NULL), this->startTime); + if (duration >= this->targetDistance) { + this->driving = false; + return true; + } + + SetSpeed(distance < 0 ? -1 : 1); + return false; } \ No newline at end of file diff --git a/Motor.h b/Motor.h index a6a8576..e42fb05 100644 --- a/Motor.h +++ b/Motor.h @@ -1,5 +1,6 @@ #pragma once +#include #include "Thing.h" class Motor : public Thing { @@ -14,6 +15,11 @@ class Motor : public Thing { virtual void SetSpeed(float speed); float GetSpeed(); + bool Drive(float distance); + protected: float currentSpeed = 0; + bool driving = false; + float targetDistance = 0; + time_t startTime = 0; }; diff --git a/Propulsion.cpp b/Propulsion.cpp index e8c30dd..612817e 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -19,7 +19,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { this->motorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if (thing->isMotor) + // if (thing->isMotor) + if (thing->type == Thing::Type::Motor) motorCount++; } this->placement = new Placement[motorCount]; @@ -27,7 +28,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { unsigned int motorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if (thing->isMotor) + // if (thing->isMotor) + if (thing->type == Thing::Type::Motor) this->placement[motorIx++] = things[thingIx]; } } @@ -45,7 +47,8 @@ Motor* Propulsion::GetMotor(unsigned int motorId) { return nullptr; Thing* thing = this->placement[motorId].thing; - if (thing->isMotor) + // if (thing->isMotor) + if (thing->type == Thing::Type::Motor) return (Motor*)thing; return nullptr; diff --git a/Roboid.cpp b/Roboid.cpp index 6884af3..a3bd8b2 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -1,5 +1,5 @@ #include "Roboid.h" - +#include Roboid::Roboid() { this->configuration = nullptr; this->thingCount = 0; @@ -16,16 +16,51 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) { bool Roboid::Drive(float forwardDistance) { bool finished = true; for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) { - Motor* motor = (Motor*)propulsion.placement[motorIx].thing; - if (motor == nullptr) - continue; - if (motor->isControlledMotor == false) - continue; + bool motorFinished = false; + Thing* thing = propulsion.placement[motorIx].thing; - ControlledMotor* controlledMotor = (ControlledMotor*)motor; - bool motorFinished = controlledMotor->Drive(forwardDistance); - if (motorFinished == false) + switch (thing->type) { + case Thing::Type::ControlledMotor: { + ControlledMotor* controlledMotor = (ControlledMotor*)thing; + motorFinished = controlledMotor->Drive(forwardDistance); + break; + } + case Thing::Type::Motor: { + Motor* motor = (Motor*)thing; + motorFinished = motor->Drive(forwardDistance); + break; + } + default: + break; + } + if (motorFinished == false) { finished = false; + } else + Serial.printf("Motor finished\n"); } return finished; } + +float waypointArray[] = {1, -1}; +void Roboid::FollowTrajectory(Trajectory* Trajectory) { + this->waypoints = waypointArray; + this->waypointCount = 2; + this->waypointIx = 0; +} + +void Roboid::Update() { + if (this->waypoints == nullptr) + return; + + // Serial.printf("Driving waypoints %d %f\n", this->waypointIx, + // waypoints[waypointIx]); + if (Drive(this->waypoints[this->waypointIx])) { + this->waypointIx++; + if (this->waypointIx == this->waypointCount) { + this->waypoints = nullptr; + this->waypointIx = 0; + this->waypointCount = 0; + propulsion.SetTwistSpeed(0, 0); + } + } +} \ No newline at end of file diff --git a/Roboid.h b/Roboid.h index a83626d..17af5f3 100644 --- a/Roboid.h +++ b/Roboid.h @@ -5,6 +5,15 @@ #include "Placement.h" #include "Propulsion.h" +class WayPoint { + public: + float distance = 0; +}; +class Trajectory { + public: + float* waypoints; +}; + class Acceleration { public: float GetMagnitude() { return 0; }; @@ -22,5 +31,13 @@ class Roboid { Placement* configuration; unsigned int thingCount; + void Update(); + bool Drive(float forwardDistance); + void FollowTrajectory(Trajectory* Trajectory); + + public: + float* waypoints = nullptr; + unsigned int waypointIx = 0; + unsigned int waypointCount = 0; }; \ No newline at end of file diff --git a/Sensing.cpp b/Sensing.cpp index 84ec2c2..0a214ac 100644 --- a/Sensing.cpp +++ b/Sensing.cpp @@ -29,7 +29,8 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) { sensorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if (thing->isSensor) + // if (thing->isSensor) + if (thing->type == Thing::Type::Sensor) sensorCount++; } @@ -38,7 +39,8 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) { unsigned int sensorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if (thing->isSensor) + // if (thing->isSensor) + if (thing->type == Thing::Type::Sensor) sensorPlacements[sensorIx++] = things[thingIx]; } } @@ -52,7 +54,8 @@ Sensor* Sensing::GetSensor(unsigned int sensorId) { return nullptr; Thing* thing = this->sensorPlacements[sensorId].thing; - if (thing->isSensor) + // if (thing->isSensor) + if (thing->type == Thing::Type::Sensor) return (Sensor*)thing; return nullptr; @@ -167,7 +170,9 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) { } unsigned int Sensing::ToDepthMapIndex(float angle) { - unsigned int depthMapIx = (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * (float)resolution); + unsigned int depthMapIx = + (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * + (float)resolution); return depthMapIx; } diff --git a/Sensor.cpp b/Sensor.cpp index c6511d3..e4ee22d 100644 --- a/Sensor.cpp +++ b/Sensor.cpp @@ -1,5 +1,6 @@ #include "Sensor.h" Sensor::Sensor() { - this->isSensor = true; + // this->isSensor = true; + type = Type::Sensor; } \ No newline at end of file diff --git a/Thing.h b/Thing.h index 0351995..8e0561d 100644 --- a/Thing.h +++ b/Thing.h @@ -3,8 +3,14 @@ /// @brief A thing is a functional component on a robot class Thing { public: - Thing() { isSensor = false, isMotor = false; } - bool isSensor; - bool isMotor; - bool isControlledMotor; + Thing() { + type = Type::Undetermined; + // isSensor = false, isMotor = false; + } + + enum class Type { Undetermined, Sensor, Motor, ControlledMotor }; + Type type = Type::Undetermined; + // bool isSensor; + // bool isMotor; + // bool isControlledMotor; }; \ No newline at end of file From d2b240a514b34e433f2f5a5fefcb5430761b0790 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 24 Nov 2023 17:32:21 +0100 Subject: [PATCH 5/7] Real trajectory --- Roboid.cpp | 57 ++++++++++++++++++++++++++++++++++++++---------------- Roboid.h | 24 ++++++++++++++++++----- 2 files changed, 59 insertions(+), 22 deletions(-) diff --git a/Roboid.cpp b/Roboid.cpp index a3bd8b2..00223c2 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -13,7 +13,34 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) { propulsion.AddMotors(configuration, thingCount); } -bool Roboid::Drive(float forwardDistance) { +// bool Roboid::Drive(float forwardDistance) { +// bool finished = true; +// for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) +// { +// bool motorFinished = false; +// Thing* thing = propulsion.placement[motorIx].thing; + +// switch (thing->type) { +// case Thing::Type::ControlledMotor: { +// ControlledMotor* controlledMotor = (ControlledMotor*)thing; +// motorFinished = controlledMotor->Drive(forwardDistance); +// break; +// } +// case Thing::Type::Motor: { +// Motor* motor = (Motor*)thing; +// motorFinished = motor->Drive(forwardDistance); +// break; +// } +// default: +// break; +// } +// if (motorFinished == false) +// finished = false; +// } +// return finished; +// } + +bool Roboid::Drive(Waypoint* waypoint) { bool finished = true; for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) { bool motorFinished = false; @@ -22,44 +49,40 @@ bool Roboid::Drive(float forwardDistance) { switch (thing->type) { case Thing::Type::ControlledMotor: { ControlledMotor* controlledMotor = (ControlledMotor*)thing; - motorFinished = controlledMotor->Drive(forwardDistance); + motorFinished = controlledMotor->Drive(waypoint->point.z); break; } case Thing::Type::Motor: { Motor* motor = (Motor*)thing; - motorFinished = motor->Drive(forwardDistance); + motorFinished = motor->Drive(waypoint->point.z); break; } default: break; } - if (motorFinished == false) { + if (motorFinished == false) finished = false; - } else - Serial.printf("Motor finished\n"); } return finished; } -float waypointArray[] = {1, -1}; -void Roboid::FollowTrajectory(Trajectory* Trajectory) { - this->waypoints = waypointArray; - this->waypointCount = 2; +void Roboid::FollowTrajectory(Trajectory* trajectory) { + this->trajectory = trajectory; this->waypointIx = 0; } void Roboid::Update() { - if (this->waypoints == nullptr) + if (this->trajectory == nullptr) return; - // Serial.printf("Driving waypoints %d %f\n", this->waypointIx, - // waypoints[waypointIx]); - if (Drive(this->waypoints[this->waypointIx])) { + Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx]; + // Serial.printf("Driving waypoints %d: %f %f\n", this->waypointIx, + // waypoint->point.z, waypoint->rotation); + if (Drive(waypoint)) { this->waypointIx++; - if (this->waypointIx == this->waypointCount) { - this->waypoints = nullptr; + if (this->waypointIx == this->trajectory->waypointCount) { + this->trajectory = nullptr; this->waypointIx = 0; - this->waypointCount = 0; propulsion.SetTwistSpeed(0, 0); } } diff --git a/Roboid.h b/Roboid.h index 17af5f3..c8f9c37 100644 --- a/Roboid.h +++ b/Roboid.h @@ -5,13 +5,27 @@ #include "Placement.h" #include "Propulsion.h" -class WayPoint { +class Waypoint { public: + Waypoint(float forwardDistance, float rotation) { + this->point = Vector3(0, 0, forwardDistance); + this->distance = forwardDistance; + this->rotation = rotation; + } float distance = 0; + Vector3 point = Vector3(0, 0, 0); + float rotation = 0; }; class Trajectory { public: - float* waypoints; + Trajectory(){}; + Trajectory(Waypoint* waypoints, unsigned int waypointCount) { + this->waypoints = waypoints; + this->waypointCount = waypointCount; + } + + Waypoint* waypoints; + unsigned int waypointCount; }; class Acceleration { @@ -33,11 +47,11 @@ class Roboid { void Update(); - bool Drive(float forwardDistance); + // bool Drive(float forwardDistance); + bool Drive(Waypoint* waypoint); void FollowTrajectory(Trajectory* Trajectory); public: - float* waypoints = nullptr; + Trajectory* trajectory; unsigned int waypointIx = 0; - unsigned int waypointCount = 0; }; \ No newline at end of file From ae5ed7970016208bcc2023764f097a7765446071 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 24 Nov 2023 17:48:09 +0100 Subject: [PATCH 6/7] trajectory using propulsion --- Motor.cpp | 23 +---------------------- Propulsion.cpp | 39 +++++++++++++++++++++++---------------- Propulsion.h | 8 +++----- Roboid.cpp | 50 +------------------------------------------------- Roboid.h | 1 - 5 files changed, 28 insertions(+), 93 deletions(-) diff --git a/Motor.cpp b/Motor.cpp index a8f8a60..76f8646 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -4,30 +4,9 @@ #include Motor::Motor() { - // this->isMotor = true; type = Type::Motor; } -// Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) { -// this->pinIn1 = pinIn1; -// this->pinIn2 = pinIn2; - -// pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode -// pinMode(pinIn2, OUTPUT); // configure the in2 pin to output mode -// } - -// void Motor::SetDirection(Direction direction) { -// digitalWrite(pinIn1, direction); -// digitalWrite(pinIn2, !direction); // This is the opposite of pinIn1 -// } - -// void Motor::SetSpeed(float speed) { // 0..1 -// currentSpeed = speed; -// uint8_t motorSignal = (uint8_t)(speed * 255); -// analogWrite(pinIn1, speed); -// analogWrite(pinIn2, 255 - speed); -// } - float Motor::GetSpeed() { return this->currentSpeed; } @@ -48,6 +27,6 @@ bool Motor::Drive(float distance) { return true; } - SetSpeed(distance < 0 ? -1 : 1); + SetSpeed(distance < 0 ? -1 : 1); // max speed return false; } \ No newline at end of file diff --git a/Propulsion.cpp b/Propulsion.cpp index 612817e..ea7e9c5 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -3,6 +3,8 @@ #include "FloatSingle.h" +#include + Propulsion::Propulsion() { this->placement = nullptr; this->motorCount = 0; @@ -112,6 +114,8 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) { void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) { if (quadcopter != nullptr) quadcopter->SetTwistSpeed(linear, yaw); + else + SetTwistSpeed(linear.z, yaw); } void Propulsion::SetTwistVelocity(float forwardVelocity, @@ -132,20 +136,23 @@ Quadcopter* Propulsion::GetQuadcopter() { return quadcopter; } -// bool Propulsion::Drive(Roboid* roboid, float forwardDistance) { -// bool finished = true; -// for (unsigned int motorIx = 0; motorIx < roboid->propulsion->motorCount; -// motorIx++) { -// Motor* motor = (Motor*)roboid->placement[motorIx].thing; -// if (motor == nullptr) -// continue; -// if (motor->isControlledMotor == false) -// continue; +bool Propulsion::Drive(Vector3 point, float rotation) { + if (!this->driving) { + this->startTime = time(NULL); + this->targetDistance = point.magnitude(); + this->driving = true; + } + double duration = difftime(time(NULL), this->startTime); + if (duration >= this->targetDistance) { + this->driving = false; + return true; + } -// ControlledMotor* controlledMotor = (ControlledMotor*)motor; -// bool motorFinished = controlledMotor->Drive(forwardDistance); -// if (motorFinished == false) -// finished = false; -// } -// return finished; -// } + if (rotation > 0) + rotation = 1; + if (rotation < 0) + rotation = -1; + SetTwistSpeed(point.normalized(), rotation); + + return false; +} \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index 8da9003..c9f3d6c 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -4,7 +4,7 @@ #include "Quadcopter.h" #include "Vector2.h" -// #include +#include class Propulsion { public: @@ -35,10 +35,7 @@ class Propulsion { float rollSpeed = 0.0F); // Position control (or actually, distance control) - // void Drive(float forwardDistance); - // void Turn(float turnDistance); - - // void Update(); + bool Drive(Vector3 point, float rotation); Placement* placement = nullptr; unsigned int motorCount = 0; @@ -48,4 +45,5 @@ class Propulsion { bool driving = false; float targetDistance = 0; + time_t startTime; }; diff --git a/Roboid.cpp b/Roboid.cpp index 00223c2..f01a808 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -13,56 +13,8 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) { propulsion.AddMotors(configuration, thingCount); } -// bool Roboid::Drive(float forwardDistance) { -// bool finished = true; -// for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) -// { -// bool motorFinished = false; -// Thing* thing = propulsion.placement[motorIx].thing; - -// switch (thing->type) { -// case Thing::Type::ControlledMotor: { -// ControlledMotor* controlledMotor = (ControlledMotor*)thing; -// motorFinished = controlledMotor->Drive(forwardDistance); -// break; -// } -// case Thing::Type::Motor: { -// Motor* motor = (Motor*)thing; -// motorFinished = motor->Drive(forwardDistance); -// break; -// } -// default: -// break; -// } -// if (motorFinished == false) -// finished = false; -// } -// return finished; -// } - bool Roboid::Drive(Waypoint* waypoint) { - bool finished = true; - for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) { - bool motorFinished = false; - Thing* thing = propulsion.placement[motorIx].thing; - - switch (thing->type) { - case Thing::Type::ControlledMotor: { - ControlledMotor* controlledMotor = (ControlledMotor*)thing; - motorFinished = controlledMotor->Drive(waypoint->point.z); - break; - } - case Thing::Type::Motor: { - Motor* motor = (Motor*)thing; - motorFinished = motor->Drive(waypoint->point.z); - break; - } - default: - break; - } - if (motorFinished == false) - finished = false; - } + bool finished = propulsion.Drive(waypoint->point, waypoint->rotation); return finished; } diff --git a/Roboid.h b/Roboid.h index c8f9c37..3d0952d 100644 --- a/Roboid.h +++ b/Roboid.h @@ -47,7 +47,6 @@ class Roboid { void Update(); - // bool Drive(float forwardDistance); bool Drive(Waypoint* waypoint); void FollowTrajectory(Trajectory* Trajectory); From 5a6c4fcaef1e78fba27dc3e36cc26c618f0737ba Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Sat, 25 Nov 2023 16:10:08 +0100 Subject: [PATCH 7/7] controlled motor support improved --- ControlledMotor.cpp | 33 ++++++++++++--- ControlledMotor.h | 30 ++++++-------- Encoder.cpp | 54 +++++++------------------ Encoder.h | 20 +++------ Propulsion.cpp | 99 +++++++++++++++++++++++++++++++-------------- Propulsion.h | 9 ++++- Roboid.cpp | 13 +++--- Roboid.h | 4 +- 8 files changed, 144 insertions(+), 118 deletions(-) diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index 5f331cb..5396c6e 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -1,24 +1,45 @@ #include "ControlledMotor.h" - +#include ControlledMotor::ControlledMotor() { - // this->isControlledMotor = true; this->type = Type::ControlledMotor; } -ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) { +ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) + : ControlledMotor() { this->motor = motor; this->encoder = encoder; } void ControlledMotor::SetTargetSpeed(float velocity) { this->targetVelocity = velocity; + this->rotationDirection = (targetVelocity < 0) ? Direction::Reverse : Direction::Forward; } -void ControlledMotor::Update(float timeStep) { - float velocity = GetActualSpeed(); +void ControlledMotor::Update(float currentTimeMs) { + actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); float error = targetVelocity - velocity; + float timeStep = currentTimeMs - lastUpdateTime; float acceleration = - error * timeStep * pidP; // Just P is used at this moment + error * timeStep * pidP; // Just P is used at this moment motor->SetSpeed(targetVelocity + acceleration); // or something like that + this->lastUpdateTime = currentTimeMs; +} + +float ControlledMotor::GetActualSpeed() { + return actualVelocity; //(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); +} + +bool ControlledMotor::Drive(float distance) { + if (!driving) { + targetDistance = distance; + startDistance = encoder->GetDistance(); + driving = true; + } + // else + // targetDistance = encoder->GetDistance(); // encoder->RestartCountingRevolutions(); + float totalDistance = encoder->GetDistance() - startDistance; + Serial.printf("total distance = %f\n", totalDistance); + bool completed = totalDistance > targetDistance; + return completed; } \ No newline at end of file diff --git a/ControlledMotor.h b/ControlledMotor.h index fc896a2..d60c948 100644 --- a/ControlledMotor.h +++ b/ControlledMotor.h @@ -17,7 +17,7 @@ class ControlledMotor : public Thing { float pidD = 0; float pidI = 0; - void Update(float timeStep); + void Update(float currentTimeMs); /// @brief Set the target speed for the motor controller /// @param speed the target in revolutions per second. @@ -25,26 +25,22 @@ class ControlledMotor : public Thing { /// @brief Get the actual speed from the encoder /// @return The speed in revolutions per second - float GetActualSpeed() { - return (int)rotationDirection * encoder->GetRevolutionsPerSecond(); - } + float GetActualSpeed(); - bool Drive(float distance) { - if (!driving) { - targetDistance = distance; - encoder->StartCountingRevolutions(); - driving = true; - } else - targetDistance -= encoder->RestartCountingRevolutions(); + bool Drive(float distance); - return (targetDistance <= 0); - } - - protected: - float targetVelocity; Motor* motor; Encoder* encoder; - enum Direction { Forward = 1, Reverse = -1 }; + + protected: + float lastUpdateTime; + float targetVelocity; + float actualVelocity; + float netDistance = 0; + float startDistance = 0; + + enum Direction { Forward = 1, + Reverse = -1 }; Direction rotationDirection; diff --git a/Encoder.cpp b/Encoder.cpp index 05a68bc..652b0f6 100644 --- a/Encoder.cpp +++ b/Encoder.cpp @@ -1,50 +1,26 @@ #include "Encoder.h" -volatile unsigned char Encoder::transitionCount = 0; - Encoder::Encoder() { - rps = 0; transitionsPerRotation = 1; // to prevent devide by zero - distance = 0; } -Encoder::Encoder(unsigned char pin, unsigned char transitionsPerRotation) +Encoder::Encoder(unsigned char transitionsPerRotation) : Encoder::Encoder() { - /// Hmm. Arduino dependent code - // pinMode(pin, INPUT_PULLUP); - // attachInterrupt(digitalPinToInterrupt(pin), InterruptHandler, CHANGE); this->transitionsPerRotation = transitionsPerRotation; } -void Encoder::InterruptHandler() { - transitionCount++; -} - -float Encoder::GetRevolutionsPerSecond() { - return rps; -} - -void Encoder::ResetDistance() { - distance = 0; -} - -float Encoder::GetRotationDistance() { - return distance; -} - -void Encoder::Update(float timeStep) { - // Hmmm. Arduino-dependent code... - // noInterrupts(); - float distanceThisUpdate = transitionCount / transitionsPerRotation; - transitionCount = 0; - // interrupts(); - - // float rps = distanceThisUpdate * timeStep; - distance += distanceThisUpdate; -} - -void Encoder::StartCountingRevolutions() {} - -float Encoder::RestartCountingRevolutions() { +int Encoder::GetPulseCount() { return 0; -} \ No newline at end of file +} + +float Encoder::GetPulsesPerSecond(float currentTimeMs) { + return 0; +} + +float Encoder::GetDistance() { + return 0; +} + +float Encoder::GetRevolutionsPerSecond(float currentTimeMs) { + return 0; +} diff --git a/Encoder.h b/Encoder.h index fc16410..4573901 100644 --- a/Encoder.h +++ b/Encoder.h @@ -3,24 +3,14 @@ class Encoder { public: Encoder(); - Encoder(unsigned char pin, unsigned char transitionsPerRotation); + Encoder(unsigned char transitionsPerRotation); - float GetPulsesPerSecond(); - float GetRevolutionsPerSecond(); + virtual int GetPulseCount(); + virtual float GetPulsesPerSecond(float currentTimeMs); - void ResetDistance(); - float GetRotationDistance(); - - void Update(float timeStep); - - virtual void StartCountingRevolutions(); - virtual float RestartCountingRevolutions(); + virtual float GetDistance(); + virtual float GetRevolutionsPerSecond(float currentTimeMs); protected: - static void InterruptHandler(); - static volatile unsigned char transitionCount; unsigned char transitionsPerRotation; - float rps; - - float distance; // this is direction agnostic }; diff --git a/Propulsion.cpp b/Propulsion.cpp index ea7e9c5..463e03a 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -10,28 +10,21 @@ Propulsion::Propulsion() { this->motorCount = 0; } -// void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) { -// this->palce = motors; -// this->motorCount = motorCount; -// } - void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { - // this->placement = motors; - // this->motorCount = motorCount; this->motorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - // if (thing->isMotor) - if (thing->type == Thing::Type::Motor) + if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor) motorCount++; + if (thing->type == Thing::Type::ControlledMotor) + hasOdometer = true; } this->placement = new Placement[motorCount]; unsigned int motorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - // if (thing->isMotor) - if (thing->type == Thing::Type::Motor) + if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor) this->placement[motorIx++] = things[thingIx]; } } @@ -56,29 +49,48 @@ Motor* Propulsion::GetMotor(unsigned int motorId) { return nullptr; } -void Propulsion::Update() { - // Hmmm. Arduino dependent code - // unsigned long curMillis = millis(); - // float timeStep = (float)(curMillis - lastMillis) / 1000; - // lastMillis = curMillis; +void Propulsion::Update(float currentTimeMs) { + // time_t currentTime = time(NULL); + float timeStep = currentTimeMs - this->lastUpdateTime; // difftime(currentTime, this->lastUpdateTime); for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - // Placement placement = placement[motorIx]; - // placement.controlledMotor->Update(timeStep); + Thing* thing = placement[motorIx].thing; + if (thing->type == Thing::Type::ControlledMotor) { + ControlledMotor* motor = (ControlledMotor*)thing; + motor->Update(currentTimeMs); + } } + this->lastUpdateTime = currentTimeMs; } void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Motor* motor = (Motor*)placement[motorIx].thing; - if (motor == nullptr) - continue; + Thing* thing = placement[motorIx].thing; + if (thing->type == Thing::Type::Motor) { + Motor* motor = (Motor*)placement[motorIx].thing; + if (motor == nullptr) + continue; - float xPosition = placement[motorIx].position.x; - if (xPosition < 0) - motor->SetSpeed(leftSpeed); - else if (xPosition > 0) - motor->SetSpeed(rightSpeed); + float xPosition = placement[motorIx].position.x; + if (xPosition < 0) + motor->SetSpeed(leftSpeed); + else if (xPosition > 0) + motor->SetSpeed(rightSpeed); + + Serial.printf("motor %d, speed = %f\n", motorIx, motor->GetSpeed()); + } else if (thing->type == Thing::Type::ControlledMotor) { + ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing; + if (motor == nullptr) + continue; + + float xPosition = placement[motorIx].position.x; + if (xPosition < 0) + motor->SetTargetSpeed(leftSpeed); + else if (xPosition > 0) + motor->SetTargetSpeed(rightSpeed); + + Serial.printf("controlled motor %d, speed = %f\n", motorIx, motor->GetActualSpeed()); + } }; } @@ -136,16 +148,39 @@ Quadcopter* Propulsion::GetQuadcopter() { return quadcopter; } -bool Propulsion::Drive(Vector3 point, float rotation) { +float Propulsion::GetOdometer() { + float odometer = 0; + for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { + Thing* thing = placement[motorIx].thing; + if (thing->type == Thing::Type::ControlledMotor) { + ControlledMotor* motor = (ControlledMotor*)thing; + odometer += motor->encoder->GetDistance() / this->motorCount; + } + } + return odometer; +} + +bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) { if (!this->driving) { this->startTime = time(NULL); this->targetDistance = point.magnitude(); + if (hasOdometer) + this->startOdometer = GetOdometer(); this->driving = true; } - double duration = difftime(time(NULL), this->startTime); - if (duration >= this->targetDistance) { - this->driving = false; - return true; + if (hasOdometer) { + float distance = GetOdometer() - this->startOdometer; + Serial.printf("Odometer = %f\n", distance); + if (distance >= this->targetDistance) { + this->driving = false; + return true; + } + } else { + double duration = difftime(time(NULL), this->startTime); + if (duration >= this->targetDistance) { + this->driving = false; + return true; + } } if (rotation > 0) @@ -154,5 +189,7 @@ bool Propulsion::Drive(Vector3 point, float rotation) { rotation = -1; SetTwistSpeed(point.normalized(), rotation); + Update(currentTimeMs); + return false; } \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index c9f3d6c..dad7296 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -11,7 +11,7 @@ class Propulsion { /// @brief Setup sensing Propulsion(); - void Update(); + void Update(float currentTimeMs); void AddMotors(Placement* motors, unsigned int motorCount); void AddQuadcopter(Quadcopter* quadcopter); @@ -35,7 +35,9 @@ class Propulsion { float rollSpeed = 0.0F); // Position control (or actually, distance control) - bool Drive(Vector3 point, float rotation); + bool Drive(Vector3 point, float rotation, float currentTimeMs); + + float GetOdometer(); Placement* placement = nullptr; unsigned int motorCount = 0; @@ -45,5 +47,8 @@ class Propulsion { bool driving = false; float targetDistance = 0; + bool hasOdometer = false; + float startOdometer; time_t startTime; + float lastUpdateTime; }; diff --git a/Roboid.cpp b/Roboid.cpp index f01a808..7bf37ef 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -13,8 +13,8 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) { propulsion.AddMotors(configuration, thingCount); } -bool Roboid::Drive(Waypoint* waypoint) { - bool finished = propulsion.Drive(waypoint->point, waypoint->rotation); +bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) { + bool finished = propulsion.Drive(waypoint->point, waypoint->rotation, currentTimeMs); return finished; } @@ -23,18 +23,19 @@ void Roboid::FollowTrajectory(Trajectory* trajectory) { this->waypointIx = 0; } -void Roboid::Update() { +void Roboid::Update(float currentTimeMs) { if (this->trajectory == nullptr) return; Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx]; - // Serial.printf("Driving waypoints %d: %f %f\n", this->waypointIx, - // waypoint->point.z, waypoint->rotation); - if (Drive(waypoint)) { + Serial.printf("Driving waypoints %d: %f %f\n", this->waypointIx, + waypoint->point.z, waypoint->rotation); + if (Drive(waypoint, currentTimeMs)) { this->waypointIx++; if (this->waypointIx == this->trajectory->waypointCount) { this->trajectory = nullptr; this->waypointIx = 0; + // stop propulsion.SetTwistSpeed(0, 0); } } diff --git a/Roboid.h b/Roboid.h index 3d0952d..3b10ec4 100644 --- a/Roboid.h +++ b/Roboid.h @@ -45,9 +45,9 @@ class Roboid { Placement* configuration; unsigned int thingCount; - void Update(); + void Update(float currentTimeMs); - bool Drive(Waypoint* waypoint); + bool Drive(Waypoint* waypoint, float currentTimeMs); void FollowTrajectory(Trajectory* Trajectory); public: