71 lines
2.6 KiB
C++
71 lines
2.6 KiB
C++
#include "DifferentialDrive.h"
|
|
|
|
namespace RoboidControl {
|
|
DifferentialDrive::DifferentialDrive() : Thing() {}
|
|
|
|
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
|
|
: Thing(participant) {}
|
|
|
|
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 == false)
|
|
return;
|
|
// 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);
|
|
// std::cout << "lin. speed " << linearVelocity << " ang. speed " <<
|
|
// angularVelocity.distance << " left wheel "
|
|
// << speedLeft << " right wheel " << speedRight << "\n";
|
|
}
|
|
|
|
} // namespace RoboidControl
|