52 lines
1.7 KiB
C++
52 lines
1.7 KiB
C++
#include "DifferentialDrive.h"
|
|
|
|
#include "FloatSingle.h"
|
|
|
|
DifferentialDrive::DifferentialDrive(){};
|
|
|
|
DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
|
|
Placement rightMotorPlacement) {
|
|
this->motorCount = 2;
|
|
this->placement = new Placement[2];
|
|
this->placement[0] = leftMotorPlacement;
|
|
this->placement[1] = rightMotorPlacement;
|
|
Motor* leftMotor = (Motor*)leftMotorPlacement.thing;
|
|
leftMotor->direction = Motor::Direction::CounterClockwise;
|
|
Motor* rightMotor = (Motor*)rightMotorPlacement.thing;
|
|
rightMotor->direction = Motor::Direction::Clockwise;
|
|
}
|
|
|
|
void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
Thing* thing = placement[motorIx].thing;
|
|
if (thing->type == Thing::UncontrolledMotorType) {
|
|
Motor* motor = (Motor*)thing;
|
|
if (motor == nullptr)
|
|
continue;
|
|
|
|
float xPosition = placement[motorIx].position.x;
|
|
if (xPosition < 0)
|
|
motor->SetSpeed(leftSpeed);
|
|
else if (xPosition > 0)
|
|
motor->SetSpeed(rightSpeed);
|
|
}
|
|
};
|
|
}
|
|
|
|
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
|
|
float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
|
|
float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
|
|
SetTargetSpeeds(leftSpeed, rightSpeed);
|
|
}
|
|
|
|
void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
|
|
SetTwistSpeed(linear.y, yaw);
|
|
}
|
|
|
|
void DifferentialDrive::SetTwistSpeed(Vector3 linear,
|
|
float yaw,
|
|
float pitch,
|
|
float roll) {
|
|
SetTwistSpeed(linear.z, yaw);
|
|
}
|