119 lines
4.3 KiB
C++
119 lines
4.3 KiB
C++
#include "DifferentialDrive.h"
|
|
#include "LinearAlgebra/Angle.h"
|
|
#include "LinearAlgebra/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->SetPosition(Spherical16(distance, Direction16::left));
|
|
// leftMotor->position.direction.horizontal = Angle16::Degrees(-90);
|
|
// leftMotor->position.distance = distance;
|
|
rightMotor->direction = Motor::Direction::Clockwise;
|
|
rightMotor->SetPosition(Spherical16(distance, Direction16::right));
|
|
// rightMotor->position.direction.horizontal = Angle16::Degrees(90);
|
|
// rightMotor->position.distance = distance;
|
|
}
|
|
|
|
void DifferentialDrive::SetDimensions(float wheelDiameter,
|
|
float wheelSeparation) {
|
|
this->wheelDiameter = wheelDiameter;
|
|
this->wheelSeparation = wheelSeparation;
|
|
this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
|
|
|
|
float distance = this->wheelSeparation / 2;
|
|
Spherical16 motor0Position = this->motors[0]->GetPosition();
|
|
motor0Position.distance = distance;
|
|
this->motors[0]->SetPosition(motor0Position);
|
|
Spherical16 motor1Position = this->motors[0]->GetPosition();
|
|
motor1Position.distance = distance;
|
|
this->motors[1]->SetPosition(motor1Position);
|
|
}
|
|
|
|
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]->GetPosition().direction.horizontal.InDegrees();
|
|
if (xPosition < 0)
|
|
motor->SetTargetSpeed(leftSpeed);
|
|
else if (xPosition > 0)
|
|
motor->SetTargetSpeed(rightSpeed);
|
|
};
|
|
}
|
|
|
|
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
|
|
float leftSpeed =
|
|
Float::Clamp(forward + yaw, -1, 1); // revolutions per second
|
|
float rightSpeed =
|
|
Float::Clamp(forward - yaw, -1, 1); // revolutions per second
|
|
|
|
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
|
|
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
|
|
|
|
SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
}
|
|
|
|
void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
|
|
SetTwistSpeed(linear.y, yaw);
|
|
}
|
|
|
|
void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
|
|
float roll) {
|
|
SetTwistSpeed(linear.Forward(), yaw);
|
|
}
|
|
|
|
// void DifferentialDrive::SetVelocity(Polar velocity) {
|
|
// SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
|
|
// }
|
|
|
|
// Spherical16 DifferentialDrive::GetVelocity() {
|
|
// 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);
|
|
// Spherical16 velocity =
|
|
// Spherical16(magnitude, Angle16::Degrees(direction),
|
|
// Angle16::Degrees(0)); // Polar(direction, magnitude);
|
|
// return velocity;
|
|
// }
|
|
|
|
// float DifferentialDrive::GetAngularVelocity() {
|
|
// 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 * Passer::LinearAlgebra::pi;
|
|
// float rotationsPerSecond = angularSpeed / angularDistance;
|
|
// float degreesPerSecond =
|
|
// Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees();
|
|
// float angularVelocity = degreesPerSecond;
|
|
|
|
// return angularVelocity;
|
|
// }
|