104 lines
3.5 KiB
C++
104 lines
3.5 KiB
C++
#include "DifferentialDrive.h"
|
|
|
|
#include "Messages/LowLevelMessages.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
DifferentialDrive::DifferentialDrive(Thing* parent)
|
|
: Thing(Type::DifferentialDrive, parent) {
|
|
this->name = "Differential drive";
|
|
|
|
this->leftWheel = new Motor(this);
|
|
this->leftWheel->name = "Left motor";
|
|
|
|
this->rightWheel = new Motor(this);
|
|
this->rightWheel->name = "Right motor";
|
|
}
|
|
|
|
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
|
|
Motor* rightMotor,
|
|
Thing* parent)
|
|
: Thing(Type::DifferentialDrive, parent) {
|
|
this->name = "Differential drive";
|
|
this->leftWheel = leftMotor;
|
|
this->rightWheel = rightMotor;
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
// Motor& DifferentialDrive::GetMotorLeft() {
|
|
// return *this->leftWheel;
|
|
// }
|
|
|
|
// Motor& DifferentialDrive::GetMotorRight() {
|
|
// return *this->rightWheel;
|
|
// }
|
|
|
|
void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
|
|
float distance = this->wheelSeparation / 2;
|
|
this->leftWheel = &leftMotor;
|
|
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
|
|
|
|
this->rightWheel = &rightMotor;
|
|
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
|
|
}
|
|
|
|
void DifferentialDrive::SetWheelVelocity(float velocityLeft,
|
|
float velocityRight) {
|
|
// if (this->leftWheel != nullptr)
|
|
// this->leftWheel->SetAngularVelocity(Spherical(velocityLeft,
|
|
// Direction::left));
|
|
// if (this->rightWheel != nullptr)
|
|
// this->rightWheel->SetAngularVelocity(
|
|
// Spherical(velocityRight, Direction::right));
|
|
if (this->leftWheel != nullptr)
|
|
this->leftWheel->SetTargetVelocity(velocityLeft);
|
|
if (this->rightWheel != nullptr)
|
|
this->rightWheel->SetTargetVelocity(velocityRight);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
|
|
data[(*ix)++] = this->leftWheel->id;
|
|
data[(*ix)++] = this->rightWheel->id;
|
|
LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
|
|
return 4;
|
|
}
|
|
|
|
} // namespace RoboidControl
|