56 lines
2.3 KiB
C++
56 lines
2.3 KiB
C++
#include "DifferentialDrive.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {
|
|
// this->leftWheel = new Thing(participant);
|
|
// this->rightWheel = new Thing(participant);
|
|
}
|
|
|
|
void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
|
|
this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
|
|
this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
|
|
this->rpsToMs = wheelDiameter * Passer::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::Update(unsigned long currentMs) {
|
|
// 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;
|
|
if (this->leftWheel != nullptr)
|
|
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
|
|
|
|
float speedRight = (linearVelocity - angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg;
|
|
if (this->rightWheel != nullptr)
|
|
this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right));
|
|
|
|
// std::cout << "lin. speed " << linearVelocity << " ang. speed " << angularVelocity.distance << " left wheel "
|
|
// << speedLeft << " right wheel " << speedRight << "\n";
|
|
}
|
|
|
|
} // namespace RoboidControl
|