Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor RobotDrive squaring (#390)

Browse Source
This commit is contained in:
Austin Shalit
2017-05-07 09:40:48 -07:00
committed by Peter Johnson
parent d348a5b947
commit 4dae747343
3 changed files with 27 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/athena/include/RobotDrive.h
View File

@@ -98,7 +98,7 @@ class RobotDrive : public MotorSafety, public ErrorBase {
protected:
void InitRobotDrive();
double Limit(double num);
double Limit(double number);
void Normalize(double* wheelSpeeds);
void RotateVector(double& x, double& y, double angle);

41
wpilibc/athena/src/RobotDrive.cpp
View File

@@ -312,21 +312,14 @@ void RobotDrive::TankDrive(double leftValue, double rightValue,
reported = true;
}
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
leftValue = Limit(leftValue);
rightValue = Limit(rightValue);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (squaredInputs) {
if (leftValue >= 0.0) {
leftValue = (leftValue * leftValue);
} else {
leftValue = -(leftValue * leftValue);
}
if (rightValue >= 0.0) {
rightValue = (rightValue * rightValue);
} else {
rightValue = -(rightValue * rightValue);
}
leftValue = std::copysign(leftValue * leftValue, leftValue);
rightValue = std::copysign(rightValue * rightValue, rightValue);
}
SetLeftRightMotorOutputs(leftValue, rightValue);
@@ -443,19 +436,11 @@ void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
moveValue = Limit(moveValue);
rotateValue = Limit(rotateValue);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (squaredInputs) {
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (moveValue >= 0.0) {
moveValue = (moveValue * moveValue);
} else {
moveValue = -(moveValue * moveValue);
}
if (rotateValue >= 0.0) {
rotateValue = (rotateValue * rotateValue);
} else {
rotateValue = -(rotateValue * rotateValue);
}
moveValue = std::copysign(moveValue * moveValue, moveValue);
rotateValue = std::copysign(rotateValue * rotateValue, rotateValue);
}
if (moveValue > 0.0) {
@@ -625,14 +610,14 @@ void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
double RobotDrive::Limit(double num) {
if (num > 1.0) {
double RobotDrive::Limit(double number) {
if (number > 1.0) {
return 1.0;
}
if (num < -1.0) {
if (number < -1.0) {
return -1.0;
}
return num;
return number;
}
/**

39
wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java
View File

@@ -276,21 +276,14 @@ public class RobotDrive implements MotorSafety {
kTank_Reported = true;
}
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
leftValue = limit(leftValue);
rightValue = limit(rightValue);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (squaredInputs) {
if (leftValue >= 0.0) {
leftValue = leftValue * leftValue;
} else {
leftValue = -(leftValue * leftValue);
}
if (rightValue >= 0.0) {
rightValue = rightValue * rightValue;
} else {
rightValue = -(rightValue * rightValue);
}
leftValue = Math.copySign(leftValue * leftValue, leftValue);
rightValue = Math.copySign(rightValue * rightValue, rightValue);
}
setLeftRightMotorOutputs(leftValue, rightValue);
}
@@ -391,19 +384,13 @@ public class RobotDrive implements MotorSafety {
moveValue = limit(moveValue);
rotateValue = limit(rotateValue);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (squaredInputs) {
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (moveValue >= 0.0) {
moveValue = moveValue * moveValue;
} else {
moveValue = -(moveValue * moveValue);
}
if (rotateValue >= 0.0) {
rotateValue = rotateValue * rotateValue;
} else {
rotateValue = -(rotateValue * rotateValue);
}
moveValue = Math.copySign(moveValue * moveValue, moveValue);
rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
}
if (moveValue > 0.0) {
@@ -580,14 +567,14 @@ public class RobotDrive implements MotorSafety {
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
protected static double limit(double num) {
if (num > 1.0) {
protected static double limit(double number) {
if (number > 1.0) {
return 1.0;
}
if (num < -1.0) {
if (number < -1.0) {
return -1.0;
}
return num;
return number;
}
/**