123 lines
4.5 KiB
C++
123 lines
4.5 KiB
C++
#include "DifferentialDrive.h"
|
|
#include "VectorAlgebra/Angle.h"
|
|
#include "VectorAlgebra/FloatSingle.h"
|
|
|
|
#include <math.h>
|
|
|
|
DifferentialDrive::DifferentialDrive(){};
|
|
|
|
DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
|
|
this->motorCount = 2;
|
|
this->motors = new Motor *[2];
|
|
this->motors[0] = leftMotor;
|
|
this->motors[1] = rightMotor;
|
|
|
|
float distance = this->wheelSeparation / 2;
|
|
leftMotor->direction = Motor::Direction::CounterClockwise;
|
|
leftMotor->position.angle = -90;
|
|
leftMotor->position.distance = distance;
|
|
rightMotor->direction = Motor::Direction::Clockwise;
|
|
rightMotor->position.angle = 90;
|
|
rightMotor->position.distance = distance;
|
|
}
|
|
|
|
void DifferentialDrive::SetDimensions(float wheelDiameter,
|
|
float wheelSeparation) {
|
|
this->wheelDiameter = wheelDiameter;
|
|
this->wheelSeparation = wheelSeparation;
|
|
this->rpsToMs = wheelDiameter * Angle::pi;
|
|
|
|
float distance = this->wheelSeparation / 2;
|
|
this->motors[0]->position.distance = distance;
|
|
this->motors[1]->position.distance = distance;
|
|
}
|
|
|
|
void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
|
|
float rightSpeed) {
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
Motor *motor = motors[motorIx];
|
|
if (motor == nullptr)
|
|
continue;
|
|
|
|
float xPosition = motors[motorIx]->position.angle;
|
|
if (xPosition < 0)
|
|
motor->SetTargetSpeed(leftSpeed);
|
|
else if (xPosition > 0)
|
|
motor->SetTargetSpeed(rightSpeed);
|
|
};
|
|
}
|
|
|
|
void DifferentialDrive::SetVelocity(float forwardVelocity) {
|
|
targetVelocity = Vector3::forward * forwardVelocity;
|
|
Update();
|
|
// float leftSpeed = Float::Clamp(forwardVelocity - targetYawVelocity, -1,
|
|
// 1); // revolutions per second
|
|
// float rightSpeed = Float::Clamp(forwardVelocity + targetYawVelocity, -1,
|
|
// 1); // revolutions per second
|
|
|
|
// float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
|
|
// float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
|
|
|
|
// SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
}
|
|
|
|
void DifferentialDrive::SetAngularVelocity(float yawVelocity) {
|
|
targetYawVelocity = yawVelocity;
|
|
Update();
|
|
// float leftSpeed = Float::Clamp(currentVelocity - yawVelocity, -1,
|
|
// 1); // revolutions per second
|
|
// float rightSpeed = Float::Clamp(currentVelocity + yawVelocity, -1,
|
|
// 1); // revolutions per second
|
|
|
|
// float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
|
|
// float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
|
|
|
|
// SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
}
|
|
|
|
Polar DifferentialDrive::GetActualVelocity() {
|
|
Motor *leftMotor = motors[0];
|
|
Motor *rightMotor = motors[1];
|
|
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
|
|
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
|
|
|
|
leftSpeed = leftSpeed * rpsToMs; // in meters per second
|
|
rightSpeed = rightSpeed * rpsToMs; // in meters per second
|
|
float speed = (leftSpeed + rightSpeed) / 2;
|
|
|
|
float direction = speed >= 0 ? 0.0F : 180.0F;
|
|
float magnitude = fabsf(speed);
|
|
Polar velocity = Polar(direction, magnitude);
|
|
return velocity;
|
|
}
|
|
|
|
float DifferentialDrive::GetActualYawVelocity() {
|
|
Motor *leftMotor = motors[0];
|
|
Motor *rightMotor = motors[1];
|
|
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
|
|
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
|
|
|
|
leftSpeed = leftSpeed * rpsToMs; // in meters per second
|
|
rightSpeed = rightSpeed * rpsToMs; // in meters per second
|
|
|
|
float angularSpeed = (leftSpeed - rightSpeed) / 2;
|
|
float angularDistance = wheelSeparation / 2 * Angle::pi;
|
|
float rotationsPerSecond = angularSpeed / angularDistance;
|
|
float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond);
|
|
float angularVelocity = degreesPerSecond;
|
|
|
|
return angularVelocity;
|
|
}
|
|
|
|
void DifferentialDrive::Update() {
|
|
float forwardVelocity = targetVelocity.Forward();
|
|
float leftSpeed = Float::Clamp(forwardVelocity - targetYawVelocity, -1,
|
|
1); // revolutions per second
|
|
float rightSpeed = Float::Clamp(forwardVelocity + targetYawVelocity, -1,
|
|
1); // revolutions per second
|
|
|
|
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
|
|
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
|
|
|
|
SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
} |