Skip to content

Commit

Permalink
Finish merging master
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Nov 4, 2017
1 parent a70e30f commit 71bdf9a
Show file tree
Hide file tree
Showing 303 changed files with 1,101 additions and 2,871 deletions.
7 changes: 0 additions & 7 deletions Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,15 +151,8 @@ public static void main(String[] args) throws IOException {
// the circumference of a circle moved by the robot via C = 360 * n / θ
//You then find the diameter via C / π.
double balbasaurWheelbase = 30. / 12.;
//200 in: 29.96
//50 in: 34.2

//433.415
<<<<<<< HEAD
double calciferWheelbase = 26.6536/12.;
=======
double calciferWheelbase = 26. / 12.;
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970

Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH,
0.05, 5., 4.5, 9.); //Units are seconds, feet/second, feet/(second^2), and feet/(second^3)
Expand Down
47 changes: 16 additions & 31 deletions ...O/src/main/java/org/usfirst/frc/team449/robot/drive/unidirectional/DriveTalonCluster.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,10 +222,20 @@ public void setDefaultCommandManual(Command defaultCommand) {
* @return robot heading, in degrees, on [-180, 180]
*/
@Override
public double getGyroHeading() {
public double getHeading() {
return navX.getHeading();
}

/**
* Set the robot's heading.
*
* @param heading The heading to set to, in degrees on [-180, 180].
*/
@Override
public void setHeading(double heading) {
navX.setHeading(heading);
}

/**
* Get the robot's cached heading.
*
Expand All @@ -243,26 +253,10 @@ public double getHeadingCached() {
*/
@Override
public double getAngularVel() {
return navX.getRate();
}

/**
<<<<<<< HEAD
* @param headingDegrees The angle, in degrees from [-180, 180], to set the NavX's heading to.
*/
@Override
public void setHeading(double headingDegrees) {
navX.setHeading(headingDegrees);
return navX.getAngularVelocity();
}

/**
* @return An AHRS object representing this subsystem's NavX.
*/
@Override
@NotNull
public MappedAHRS getNavX() {
return navX;
=======
* Get the robot's cached angular velocity.
*
* @return Angular velocity in degrees/sec
Expand All @@ -279,7 +273,7 @@ public double getAngularVelCached() {
*/
@Override
public double getAngularDisplacement() {
return navX.getAngle();
return navX.getAngularVelocity();
}

/**
Expand All @@ -306,7 +300,6 @@ public boolean getOverrideGyro() {
@Override
public void setOverrideGyro(boolean override) {
overrideGyro = override;
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
}

/**
Expand Down Expand Up @@ -358,20 +351,12 @@ public Object[] getData() {
cachedRightPos,
leftMaster.getError(),
rightMaster.getError(),
<<<<<<< HEAD
navX.getHeading(),
navX.get9AxisHeading(),
navX.getAngularVelocity(),
navX.getAngularDisplacement(),
navX.getXAccel(),
navX.getYAccel()};
=======
cachedHeading,
navX.get9AxisHeading(),
cachedAngularVel,
cachedAngularDisplacement,
MappedAHRS.gsToFeetPerSecondSquared(navX.getWorldLinearAccelX()),
MappedAHRS.gsToFeetPerSecondSquared(navX.getWorldLinearAccelY())};
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
navX.getXAccel(),
navX.getYAccel()};
}

/**
Expand Down
130 changes: 6 additions & 124 deletions RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,6 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import com.fasterxml.jackson.annotation.ObjectIdGenerators;
import org.jetbrains.annotations.NotNull;
<<<<<<< HEAD
=======
import org.jetbrains.annotations.Nullable;
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
import org.usfirst.frc.team449.robot.generalInterfaces.loggable.Loggable;
import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable;
Expand Down Expand Up @@ -87,19 +83,9 @@ public class UnidirectionalPoseEstimator <T extends SubsystemAHRS & DriveUnidire
private long lastTime;

/**
* The most recently calculated effective wheelbase diameter (from the Eli method), in feet.
* The most recently calculated effective wheelbase diameter, in feet.
*/
private double fudgedWheelbaseDiameter;

/**
* Whether or not the left side was re-calculated last tic using the Noah method.
*/
private boolean recalcedLeft;

/**
* The percent the Noah method changed the wrong encoder reading by.
*/
private double percentChanged;

/**
* Angle and magnitude of vector being calculated. Field to avoid garbage collection.
Expand Down Expand Up @@ -153,7 +139,6 @@ public UnidirectionalPoseEstimator(@JsonProperty(required = true) @NotNull T sub
lastTime = 0;
}

<<<<<<< HEAD
/**
* Calculate the x and y movement vector for the robot.
*
Expand All @@ -163,78 +148,27 @@ public UnidirectionalPoseEstimator(@JsonProperty(required = true) @NotNull T sub
* @param lastAngle The previous heading, in radians
* @return An array of length 2 containing the [x, y] displacement of the robot.
*/
private static double[] calcVector(double left, double right, double deltaTheta, double lastAngle){
//The vector for how much the robot moves, element 0 is x and element 1 is y.
double[] vector = new double[2];
=======
@NotNull
private double[] calcEliVector(double left, double right, double deltaTheta, double lastAngle) {
private double[] calcVector(double left, double right, double deltaTheta, double lastAngle) {

//If we're going in a straight line
if (deltaTheta == 0) {
//we could use deltaRight here, doesn't matter. Going straight means no change in angle and left and right are the same.
return new double[]{left * Math.cos(lastAngle), left * Math.sin(lastAngle)};
return new double[]{(left+right)/2. * Math.cos(lastAngle), (left+right)/2. * Math.sin(lastAngle)};
} else {
//This next part is too complicated to explain in comments. Read this wiki page instead:
// http://team449.shoutwiki.com/wiki/Pose_Estimation
vectorAngle = lastAngle + deltaTheta / 2.;
vectorMagnitude = 2. * ((left + right) / 2.) / deltaTheta * Math.sin(deltaTheta / 2.);
vectorMagnitude = 2. * ((left+right)/2.)/deltaTheta * Math.sin(deltaTheta / 2.);
return new double[]{vectorMagnitude * Math.cos(vectorAngle), vectorMagnitude * Math.sin(vectorAngle)};
}
}

@NotNull
private double[] calcVector(double left, double right, double robotDiameter, double deltaTheta, double lastAngle) {
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970

//If we're going in a straight line
if (deltaTheta == 0) {
//we could use deltaRight here, doesn't matter. Going straight means no change in angle and left and right are the same.
<<<<<<< HEAD
vector[0] = (left+right)/2. * Math.cos(lastAngle);
vector[1] = (left+right)/2. * Math.sin(lastAngle);
} else {
//This next part is too complicated to explain in comments. Read this wiki page instead:
// http://team449.shoutwiki.com/wiki/Pose_Estimation
double vectorAngle = lastAngle + deltaTheta/2.;
double vectorMagnitude = 2. * ((left+right)/2.)/deltaTheta * Math.sin(deltaTheta / 2.);
vector[0] = vectorMagnitude * Math.cos(vectorAngle);
vector[1] = vectorMagnitude * Math.sin(vectorAngle);
=======
return new double[]{left * Math.cos(lastAngle), left * Math.sin(lastAngle)};
} else {
//This next part is too complicated to explain in comments. Read this wiki page instead:
// http://team449.shoutwiki.com/wiki/Pose_Estimation
if (left - right == 0) {
vectorMagnitude = 2* left / deltaTheta * Math.sin(deltaTheta / 2.);
} else {
vectorMagnitude = 2* robotDiameter / 2. * (left + right) / (left - right) * Math.sin(deltaTheta / 2.);
}
vectorAngle = lastAngle + deltaTheta / 2.;
return new double[]{vectorMagnitude * Math.cos(vectorAngle), vectorMagnitude * Math.sin(vectorAngle)};
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
}
}

/**
* Use the current gyro and encoder data to calculate how the robot has moved since the last time run was called.
*/
@Override
public synchronized void run() {
//Record everything at the start, as it may change between executing lines of code and that would be bad.
<<<<<<< HEAD
double left = subsystem.getLeftPos();
double right = subsystem.getRightPos();
double theta = Math.toRadians(subsystem.getNavX().getAngularDisplacement());
long time = Clock.currentTimeMillis();

//Calculate differences versus the last measurement
double deltaLeft = left - lastLeftPos;
double deltaRight = right - lastRightPos;
double deltaTheta = theta - lastTheta;

if (deltaTheta == 0){
=======
left = subsystem.getLeftPos();
right = subsystem.getRightPos();
theta = Math.toRadians(subsystem.getAngularDisplacement());
Expand All @@ -245,54 +179,12 @@ public synchronized void run() {
deltaRight = right - lastRightPos;
deltaTheta = theta - lastTheta;
if (deltaTheta == 0) {
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
fudgedWheelbaseDiameter = -1;
} else {
fudgedWheelbaseDiameter = (deltaLeft - deltaRight) / deltaTheta;
}

<<<<<<< HEAD
double[] vector = calcVector(deltaLeft, deltaRight, deltaTheta, lastTheta);
=======
if (robotDiameter != null) {
//Noah's Approach:

//For this next part, we assume that the gyro is 100% accurate at measuring the change in angle over the given

//time period and that the encoders will possibly overmeasure (due to wheel slip) but never undermeasure.
//Given those constraints, we have an overdetermined system because deltaTheta should be equal to
//(deltaLeft-deltaRight)/robotDiameter. We can use this to determine which wheel slipped more, and replace its
//reading with a value calculated from the other encoder and the gyro.
if (deltaTheta < (deltaLeft - deltaRight) / robotDiameter) {
if (deltaLeft > 0) {
percentChanged = ((deltaRight + robotDiameter * deltaTheta) - deltaLeft) / deltaLeft;
deltaLeft = deltaRight + robotDiameter * deltaTheta;
recalcedLeft = true;
} else {
percentChanged = ((deltaLeft - robotDiameter * deltaTheta) - deltaRight) / deltaRight;
deltaRight = deltaLeft - robotDiameter * deltaTheta;
recalcedLeft = false;
}
} else if (deltaTheta > (deltaLeft - deltaRight) / robotDiameter) {
if (deltaLeft < 0) {
percentChanged = ((deltaRight + robotDiameter * deltaTheta) - deltaLeft) / deltaLeft;
deltaLeft = deltaRight + robotDiameter * deltaTheta;
recalcedLeft = true;
} else {
percentChanged = ((deltaLeft - robotDiameter * deltaTheta) - deltaRight) / deltaRight;
deltaRight = deltaLeft - robotDiameter * deltaTheta;
recalcedLeft = false;
}
}
vector = calcVector(deltaLeft, deltaRight, robotDiameter, deltaTheta, lastTheta);
} else {

//Eli's Approach

//Here we assume all the measured values are correct and adjust the diameter to match.
vector = calcEliVector(deltaLeft, deltaRight, deltaTheta, lastTheta);
}
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
vector = calcVector(deltaLeft, deltaRight, deltaTheta, lastTheta);

//The vector for how much the robot moves, element 0 is x and element 1 is y.

Expand Down Expand Up @@ -384,6 +276,7 @@ public synchronized boolean addAbsolutePos(double x, double y, long time, double
*
* @return The current x,y position in feet.
*/
@NotNull
public double[] getPos() {
return currentPos;
}
Expand Down Expand Up @@ -455,8 +348,6 @@ private int getFirstKeepableIndex(long time) {
public String[] getHeader() {
return new String[]{
"effective_wheelbase",
"recalced_left",
"percent_changed",
"x_displacement",
"y_displacement"
};
Expand All @@ -472,8 +363,6 @@ public String[] getHeader() {
public Object[] getData() {
return new Object[]{
fudgedWheelbaseDiameter,
recalcedLeft,
percentChanged,
getPos()[0],
getPos()[1]
};
Expand All @@ -487,13 +376,6 @@ public Object[] getData() {
@NotNull
@Override
public String getName() {
<<<<<<< HEAD
return "PoseEstimator";
=======
if (robotDiameter != null) {
return "NoahPoseEstimator";
}
return "EliPoseEstimator";
>>>>>>> 91adccbf504af72bacfec371e289d43997aaa970
}
}
7 changes: 7 additions & 0 deletions .../src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/AHRS/SubsystemAHRS.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,13 @@ public interface SubsystemAHRS {
*/
double getHeading();

/**
* Set the robot's heading.
*
* @param heading The heading to set to, in degrees on [-180, 180].
*/
void setHeading(double heading);

/**
* Get the robot's cached heading.
*
Expand Down
12 changes: 6 additions & 6 deletions docs/allclasses-frame.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<!-- NewPage -->
<html lang="en">
<head>
<!-- Generated by javadoc (1.8.0_121) on Sat Nov 04 14:26:19 EDT 2017 -->
<!-- Generated by javadoc (1.8.0_111) on Sat Nov 04 17:41:53 EDT 2017 -->
<title>All Classes (RoboRIO 1.0 API)</title>
<meta name="date" content="2017-11-04">
<link rel="stylesheet" type="text/css" href="stylesheet.css" title="Style">
Expand Down Expand Up @@ -70,16 +70,16 @@ <h1 class="bar">All&nbsp;Classes</h1>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/arcade/OIArcade.html" title="class in org.usfirst.frc.team449.robot.oi.unidirectional.arcade" target="classFrame">OIArcade</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/arcade/OIArcadeSimple.html" title="class in org.usfirst.frc.team449.robot.oi.unidirectional.arcade" target="classFrame">OIArcadeSimple</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/arcade/OIArcadeWithDPad.html" title="class in org.usfirst.frc.team449.robot.oi.unidirectional.arcade" target="classFrame">OIArcadeWithDPad</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/fieldoriented/OIFieldOriented.html" title="interface in org.usfirst.frc.team449.robot.oi.fieldoriented" target="classFrame"><span class="interfaceName">OIFieldOriented</span></a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/fieldoriented/OIFieldOriented.html" title="class in org.usfirst.frc.team449.robot.oi.fieldoriented" target="classFrame">OIFieldOriented</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/fieldoriented/OIFieldOrientedPosCos.html" title="class in org.usfirst.frc.team449.robot.oi.fieldoriented" target="classFrame">OIFieldOrientedPosCos</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/OIOutreach.html" title="class in org.usfirst.frc.team449.robot.oi.unidirectional" target="classFrame">OIOutreach</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/tank/OITank.html" title="class in org.usfirst.frc.team449.robot.oi.unidirectional.tank" target="classFrame">OITank</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/tank/OITankSimple.html" title="class in org.usfirst.frc.team449.robot.oi.unidirectional.tank" target="classFrame">OITankSimple</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/unidirectional/OIUnidirectional.html" title="interface in org.usfirst.frc.team449.robot.oi.unidirectional" target="classFrame"><span class="interfaceName">OIUnidirectional</span></a></li>
<li><a href="org/usfirst/frc/team449/robot/drive/shifting/commands/OverrideAutoShift.html" title="class in org.usfirst.frc.team449.robot.drive.shifting.commands" target="classFrame">OverrideAutoShift</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/navX/commands/OverrideNavX.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.commands" target="classFrame">OverrideNavX</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/AHRS/commands/OverrideNavX.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.commands" target="classFrame">OverrideNavX</a></li>
<li><a href="org/usfirst/frc/team449/robot/commands/general/ParallelCommandGroup.html" title="class in org.usfirst.frc.team449.robot.commands.general" target="classFrame">ParallelCommandGroup</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/navX/commands/PIDAngleCommand.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.commands" target="classFrame">PIDAngleCommand</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/AHRS/commands/PIDAngleCommand.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.commands" target="classFrame">PIDAngleCommand</a></li>
<li><a href="org/usfirst/frc/team449/robot/drive/unidirectional/commands/PIDBackAndForth.html" title="class in org.usfirst.frc.team449.robot.drive.unidirectional.commands" target="classFrame">PIDBackAndForth</a></li>
<li><a href="org/usfirst/frc/team449/robot/drive/unidirectional/commands/PIDTest.html" title="class in org.usfirst.frc.team449.robot.drive.unidirectional.commands" target="classFrame">PIDTest</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/singleImplementation/pneumatics/Pneumatics.html" title="class in org.usfirst.frc.team449.robot.subsystem.singleImplementation.pneumatics" target="classFrame">Pneumatics</a></li>
Expand Down Expand Up @@ -114,7 +114,7 @@ <h1 class="bar">All&nbsp;Classes</h1>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/shooter/commands/SpinUpShooter.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.shooter.commands" target="classFrame">SpinUpShooter</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/shooter/commands/SpinUpThenShoot.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.shooter.commands" target="classFrame">SpinUpThenShoot</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/singleImplementation/pneumatics/commands/StartCompressor.html" title="class in org.usfirst.frc.team449.robot.subsystem.singleImplementation.pneumatics.commands" target="classFrame">StartCompressor</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/navX/SubsystemAHRS.html" title="interface in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS" target="classFrame"><span class="interfaceName">SubsystemAHRS</span></a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/AHRS/SubsystemAHRS.html" title="interface in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS" target="classFrame"><span class="interfaceName">SubsystemAHRS</span></a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/binaryMotor/SubsystemBinaryMotor.html" title="interface in org.usfirst.frc.team449.robot.subsystem.interfaces.binaryMotor" target="classFrame"><span class="interfaceName">SubsystemBinaryMotor</span></a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/conditional/SubsystemConditional.html" title="interface in org.usfirst.frc.team449.robot.subsystem.interfaces.conditional" target="classFrame"><span class="interfaceName">SubsystemConditional</span></a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/intake/SubsystemIntake.html" title="interface in org.usfirst.frc.team449.robot.subsystem.interfaces.intake" target="classFrame"><span class="interfaceName">SubsystemIntake</span></a></li>
Expand All @@ -136,7 +136,7 @@ <h1 class="bar">All&nbsp;Classes</h1>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/intake/commands/ToggleIntaking.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.intake.commands" target="classFrame">ToggleIntaking</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/binaryMotor/commands/ToggleMotor.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.binaryMotor.commands" target="classFrame">ToggleMotor</a></li>
<li><a href="org/usfirst/frc/team449/robot/drive/shifting/commands/ToggleOverrideAutoShift.html" title="class in org.usfirst.frc.team449.robot.drive.shifting.commands" target="classFrame">ToggleOverrideAutoShift</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/navX/commands/ToggleOverrideNavX.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.commands" target="classFrame">ToggleOverrideNavX</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/AHRS/commands/ToggleOverrideNavX.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.commands" target="classFrame">ToggleOverrideNavX</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/shooter/commands/ToggleShooting.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.shooter.commands" target="classFrame">ToggleShooting</a></li>
<li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/solenoid/commands/ToggleSolenoid.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.commands" target="classFrame">ToggleSolenoid</a></li>
<li><a href="org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.html" title="class in org.usfirst.frc.team449.robot.oi.buttons" target="classFrame">TriggerButton</a></li>
Expand Down
Loading

0 comments on commit 71bdf9a

Please sign in to comment.