83 lines
3.0 KiB
C++
83 lines
3.0 KiB
C++
#include "DifferentialDrive.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
// DifferentialDrive::DifferentialDrive(Participant* participant,
|
|
// unsigned char thingId)
|
|
// : Thing(participant, Thing::Type::DifferentialDrive, thingId) {
|
|
// this->leftWheel = new Motor();
|
|
// this->rightWheel = new Motor();
|
|
// }
|
|
|
|
// DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId)
|
|
// : Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
|
|
|
|
DifferentialDrive::DifferentialDrive(Participant* owner) : Thing(owner, Type::DifferentialDrive) {
|
|
this->leftWheel = new Motor();
|
|
this->rightWheel = new Motor();
|
|
}
|
|
DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) {
|
|
this->leftWheel = new Motor();
|
|
this->rightWheel = new Motor();
|
|
}
|
|
|
|
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(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(recursive);
|
|
}
|
|
|
|
} // namespace RoboidControl
|