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

Improved velocity functions

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-23 10:58:33 +01:00
parent b998903bd8
commit 31bd49dde9
6 changed files with 135 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
DifferentialDrive.cpp
View File

@ -47,28 +47,35 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
};
}
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
float leftSpeed =
Float::Clamp(forward - yaw, -1, 1); // revolutions per second
float rightSpeed =
Float::Clamp(forward + yaw, -1, 1); // revolutions per second
void DifferentialDrive::SetVelocity(float forwardVelocity) {
targetVelocity = Vector3::forward * forwardVelocity;
Update();
// float leftSpeed = Float::Clamp(forwardVelocity - targetYawVelocity, -1,
// 1); // revolutions per second
// float rightSpeed = Float::Clamp(forwardVelocity + targetYawVelocity, -1,
// 1); // revolutions per second
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
// float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
// float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
// SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
}
void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
SetTwistSpeed(linear.y, yaw);
void DifferentialDrive::SetAngularVelocity(float yawVelocity) {
targetYawVelocity = yawVelocity;
Update();
// float leftSpeed = Float::Clamp(currentVelocity - yawVelocity, -1,
// 1); // revolutions per second
// float rightSpeed = Float::Clamp(currentVelocity + yawVelocity, -1,
// 1); // revolutions per second
// float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
// float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
// SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
}
void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
float roll) {
SetTwistSpeed(linear.z, yaw);
}
Polar DifferentialDrive::GetVelocity() {
Polar DifferentialDrive::GetActualVelocity() {
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
@ -84,7 +91,7 @@ Polar DifferentialDrive::GetVelocity() {
return velocity;
}
float DifferentialDrive::GetAngularVelocity() {
float DifferentialDrive::GetActualYawVelocity() {
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
@ -100,4 +107,17 @@ float DifferentialDrive::GetAngularVelocity() {
float angularVelocity = degreesPerSecond;
return angularVelocity;
}
void DifferentialDrive::Update() {
float forwardVelocity = targetVelocity.Forward();
float leftSpeed = Float::Clamp(forwardVelocity - targetYawVelocity, -1,
1); // revolutions per second
float rightSpeed = Float::Clamp(forwardVelocity + targetYawVelocity, -1,
1); // revolutions per second
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
}

57
DifferentialDrive.h
View File

@ -36,28 +36,37 @@ public:
/// @param leftSpeed The target speed of the left Motor
/// @param rightSpeed The target speed of the right Motor
void SetMotorTargetSpeeds(float leftSpeed, float rightSpeed);
/*
/// @brief Controls the motors through forward and rotation speeds
/// @param forward The target forward speed of the Roboid
/// @param yaw The target rotation speed of the Roboid
virtual void SetTwistSpeed(float forward, float yaw) override;
/// @brief Controls the motors through forward and rotation speeds
/// @param linear The target linear speed of the Roboid
/// @param yaw The target rotation speed of the Roboid
/// @note As a DifferentialDrive cannot move sideward, this function has the
/// same effect as using the void SetTwistSpeed(float forward, float yaw)
/// function.
virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
/// @brief Controls the motors through forward and rotation speeds
/// @param linear The target linear speed
/// @param yaw The target rotation speed around the vertical axis
/// @param pitch Pitch is not supported and is ignored
/// @param roll Roll is not supported and is ignores
/// @note As a DifferentialDrive cannot move sideward or vertical, this
/// function has the same effect as using the void SetTwistSpeed(float
/// forward, float yaw) function.
virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
float pitch = 0.0F, float roll = 0.0F);
*/
/// @brief Controls the motors through forward and rotation speeds
/// @param forward The target forward speed of the Roboid
/// @param yaw The target rotation speed of the Roboid
virtual void SetTwistSpeed(float forward, float yaw) override;
/// @brief Controls the motors through forward and rotation speeds
/// @param linear The target linear speed of the Roboid
/// @param yaw The target rotation speed of the Roboid
/// @note As a DifferentialDrive cannot move sideward, this function has the
/// same effect as using the void SetTwistSpeed(float forward, float yaw)
/// function.
virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
/// @brief Controls the motors through forward and rotation speeds
/// @param linear The target linear speed
/// @param yaw The target rotation speed around the vertical axis
/// @param pitch Pitch is not supported and is ignored
/// @param roll Roll is not supported and is ignores
/// @note As a DifferentialDrive cannot move sideward or vertical, this
/// function has the same effect as using the void SetTwistSpeed(float
/// forward, float yaw) function.
virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
float pitch = 0.0F, float roll = 0.0F);
/// @brief Controls the motors through forward velocity of the roboid
/// @param forwardVelocity The target forward speed of the Roboid
virtual void SetVelocity(float forwardVelocity);
/// @brief Controls the motors through rotation velocity of the roboid
/// @param yawVelocity The target rotation speed of the Roboid
virtual void SetAngularVelocity(float yawVelocity);
/// @brief Calculate the linear velocity of the roboid based on the wheel
/// velocities
@ -66,7 +75,7 @@ public:
/// information
/// @remark This will be more expanded/detailed in a future version of Roboid
/// Control
virtual Polar GetVelocity() override;
virtual Polar GetActualVelocity() override;
/// @brief Calculate the angular velocity of the roboid based on the wheel
/// velocities
/// @return The angular speed of the roboid in local space
@ -74,7 +83,9 @@ public:
/// information
/// @remark This will be more expanded/detailed in a future version of Roboid
/// Control
virtual float GetAngularVelocity() override;
virtual float GetActualYawVelocity() override;
virtual void Update();
protected:
float wheelDiameter = 1.0F; // in meters

31
NetworkSync.cpp
View File

@ -4,25 +4,24 @@
#include <Arduino.h>
#endif
UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) {
UInt8 NetworkSync::CreatePoseMsg(Roboid *roboid, UInt8 *buffer) {
Vector3 position = roboid->GetPosition();
UInt8 bufferSize = 3 + 12;
buffer[0] = PoseMsg;
buffer[1] = 0; // ObjectId;
buffer[1] = 0; // ObjectId;
buffer[2] = Pose_Position;
SendVector3(buffer, 3, position);
return bufferSize;
}
void NetworkSync::SendVector3(unsigned char* data, int startIndex, Vector3 v) {
void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
SendSingle100(data, startIndex, v.x);
SendSingle100(data, startIndex + 4, v.y);
SendSingle100(data, startIndex + 8, v.z);
}
void NetworkSync::SendSingle100(unsigned char* data,
int startIndex,
void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
float value) {
// Sends a float with truncated 2 decimal precision
Int32 intValue = value * 100;
@ -32,16 +31,16 @@ void NetworkSync::SendSingle100(unsigned char* data,
// }
}
void NetworkSync::SendInt32(unsigned char* data, int startIndex, Int32 value) {
void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
for (unsigned char ix = 0; ix < 4; ix++) {
data[startIndex + ix] = ((unsigned char*)&value)[ix];
data[startIndex + ix] = ((unsigned char *)&value)[ix];
}
}
void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
TrackedObject** objects) {
TrackedObject **objects) {
for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
TrackedObject* obj = objects[objIx];
TrackedObject *obj = objects[objIx];
if (obj == nullptr)
continue;
// if (obj->sensor->type == Thing::ExternalType)
@ -54,7 +53,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
}
void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
TrackedObject* object) {
TrackedObject *object) {
Vector2 worldPosition2 = Vector2::Rotate(
Vector2::forward * object->position.distance, -object->position.angle);
Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
@ -72,7 +71,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
UInt16 bufferSize = 3 + 12;
UInt8 buffer[bufferSize] = {
PoseMsg,
object->id,
(UInt8)object->id,
Pose_Position,
};
SendVector3(buffer, 3, worldPosition3);
@ -80,13 +79,13 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
#endif
}
void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
Polar velocity = roboid->propulsion->GetVelocity();
void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
Polar velocity = roboid->propulsion->GetActualVelocity();
Vector2 worldVelocity2 =
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
float angularVelocity = roboid->propulsion->GetAngularVelocity();
float angularVelocity = roboid->propulsion->GetActualYawVelocity();
Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
#ifdef RC_DEBUG
@ -106,7 +105,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
const unsigned int bufferSize = 3 + 12 + 12;
unsigned char buffer[bufferSize] = {
PoseMsg,
0, // objectId;
0, // objectId;
Pose_LinearVelocity | Pose_AngularVelocity,
};
SendVector3(buffer, 3, worldVelocity3);
@ -116,7 +115,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
#endif
}
void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject* obj) {
void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
#ifdef RC_DEBUG
Serial.print("Send Destroy ");
Serial.println((int)obj->id);

53
Propulsion.cpp
View File

@ -8,21 +8,19 @@ Propulsion::Propulsion() {
this->motorCount = 0;
}
unsigned int Propulsion::GetMotorCount() {
return this->motorCount;
}
unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
Motor* Propulsion::GetMotor(unsigned int motorId) {
Motor *Propulsion::GetMotor(unsigned int motorId) {
if (motorId >= this->motorCount)
return nullptr;
Motor* motor = this->motors[motorId];
Motor *motor = this->motors[motorId];
return motor;
}
void Propulsion::Update(float currentTimeMs) {
for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) {
Motor* motor = this->motors[motorIx];
Motor *motor = this->motors[motorIx];
if (motor == nullptr)
continue;
@ -30,28 +28,29 @@ void Propulsion::Update(float currentTimeMs) {
}
}
/*
void Propulsion::SetTwistSpeed(float forward, float yaw) {}
void Propulsion::SetTargetVelocity(float forwardVelocity) {
this->targetVelocity = Vector3::forward * forwardVelocity;
}
void Propulsion::SetTargetVelocity(Vector2 horizontalVelocity) {
this->targetVelocity = Vector3::FromHorizontal(horizontalVelocity);
}
void Propulsion::SetTargetVelocity(Vector3 velocity) {
this->targetVelocity = velocity;
}
// void Propulsion::SetTargetVelocity(Polar velocity) {}
// void Propulsion::SetTargetVelocity(Spherical velocity) {}
void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
void Propulsion::SetTargetYawVelocity(float yawVelocity) {
this->targetYawVelocity = yawVelocity;
}
// void Propulsion::SetAngularVelocity(float yaw, float pitch, float roll) {}
void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
float roll) {}
*/
void Propulsion::SetVelocity(float velocity) {}
void Propulsion::SetVelocity(Vector2 velocity) {}
void Propulsion::SetVelocity(Vector3 verlocity) {}
void Propulsion::SetVelocity(Polar velocity) {}
void Propulsion::SetVelocity(Spherical velocity) {}
void Propulsion::SetAngularVelocity(float yaw) {}
void Propulsion::SetAngularVelocity(float yaw, float pitch, float roll) {}
Polar Propulsion::GetVelocity() {
return Polar(0, 0);
float Propulsion::GetTargetForwardVelocity() {
return this->targetVelocity.Forward();
}
float Propulsion::GetAngularVelocity() {
return 0;
}
float Propulsion::GetActualForwardVelocity() { return 0; }
Polar Propulsion::GetActualVelocity() { return Polar(0, 0); }
float Propulsion::GetActualYawVelocity() { return 0; }

38
Propulsion.h
View File

@ -18,7 +18,7 @@ class Roboid;
/// for a robot. This base class does not implement the functions to move the
/// Roboid around.
class Propulsion {
public:
public:
/// @brief Default Constructor for Propulsion
Propulsion();
@ -33,7 +33,7 @@ class Propulsion {
/// @param motorIx The index of the motor
/// @return Returns the motor or a nullptr when no motor with the given index
/// could be found
Motor* GetMotor(unsigned int motorIx);
Motor *GetMotor(unsigned int motorIx);
/// @brief Get the Placement of a specific Motor
/// @param motorIx The index of the Motor
/// @return Returns the Placement or a nullptr when no Placement with the give
@ -66,35 +66,41 @@ class Propulsion {
float pitch = 0.0F,
float roll = 0.0F);
*/
virtual void SetVelocity(float velocity);
virtual void SetVelocity(Polar velocity);
virtual void SetVelocity(Spherical velocity);
virtual void SetVelocity(Vector2 velocity);
virtual void SetVelocity(Vector3 velocity);
virtual void SetTargetVelocity(float velocity);
// virtual void SetTargetVelocity(Polar velocity);
// virtual void SetTargetVelocity(Spherical velocity);
virtual void SetTargetVelocity(Vector2 velocity);
virtual void SetTargetVelocity(Vector3 velocity);
virtual void SetAngularVelocity(float yaw);
virtual void SetAngularVelocity(float yaw, float pitch, float roll);
virtual void SetTargetYawVelocity(float yaw);
// virtual void SetTargetAngularVelocity(float yaw, float pitch, float roll);
virtual float GetTargetForwardVelocity();
virtual float GetActualForwardVelocity();
/// @brief Retrieve the current velocity of the roboid
/// @return The velocity in polar coordinates
/// The actual units of the velocity depend on the implementation
virtual Polar GetVelocity();
virtual Polar GetActualVelocity();
/// @brief Retrieve the current angular velocity of the roboid
/// @return The angular velocity
/// The actual unit of the angular velocity depend on the implementation
virtual float GetAngularVelocity();
virtual float GetActualYawVelocity();
/// @brief The roboid of this propulsion system
Roboid* roboid = nullptr;
Roboid *roboid = nullptr;
protected:
protected:
/// @brief The number of motors used for Propulsion
unsigned int motorCount = 0;
/// @brief The Placement of the motors used for Propulsion
// Placement *placement = nullptr;
Motor** motors = nullptr;
Motor **motors = nullptr;
Vector3 targetVelocity;
float targetYawVelocity;
};
} // namespace RoboidControl
} // namespace Passer
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

2
VectorAlgebra

@ -1 +1 @@
Subproject commit dc9d4ee42e42bff2fc051ffb48197232d01e53d6
Subproject commit fd252d4b454548d6dd8eed17c0cae543f9655c18