71 lines
2.7 KiB
C++
71 lines
2.7 KiB
C++
#include "DifferentialDrive.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {}
|
|
|
|
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) {}
|
|
|
|
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
|
|
float wheelSeparation) {
|
|
this->wheelRadius =
|
|
wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
|
|
this->wheelSeparation =
|
|
wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
|
|
this->rpsToMs = wheelDiameter * LinearAlgebra::pi;
|
|
|
|
float distance = this->wheelSeparation / 2;
|
|
if (leftWheel != nullptr)
|
|
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
|
|
if (rightWheel != nullptr)
|
|
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));
|
|
|
|
this->rightWheel = rightWheel;
|
|
if (rightWheel != nullptr)
|
|
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
|
|
}
|
|
|
|
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
|
|
if (this->leftWheel != nullptr)
|
|
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
|
|
if (this->rightWheel != nullptr)
|
|
this->rightWheel->SetAngularVelocity(
|
|
Spherical(speedRight, Direction::right));
|
|
}
|
|
|
|
void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
|
|
if (this->linearVelocityUpdated) {
|
|
// this assumes forward velocity only....
|
|
float linearVelocity = this->GetLinearVelocity().distance;
|
|
|
|
Spherical angularVelocity = this->GetAngularVelocity();
|
|
float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec
|
|
// Determine the rotation direction
|
|
if (angularVelocity.direction.horizontal.InDegrees() < 0)
|
|
angularSpeed = -angularSpeed;
|
|
|
|
// wheel separation can be replaced by this->leftwheel->position->distance
|
|
float speedLeft =
|
|
(linearVelocity + angularSpeed * this->wheelSeparation / 2) /
|
|
this->wheelRadius * Rad2Deg;
|
|
float speedRight =
|
|
(linearVelocity - angularSpeed * this->wheelSeparation / 2) /
|
|
this->wheelRadius * Rad2Deg;
|
|
|
|
this->SetWheelVelocity(speedLeft, speedRight);
|
|
}
|
|
Thing::Update(currentMs, recursive);
|
|
}
|
|
|
|
} // namespace RoboidControl
|