Skip to content

Commit

Permalink
Fix map errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Nov 5, 2017
1 parent 511abb1 commit 840c78a
Show file tree
Hide file tree
Showing 9 changed files with 29 additions and 35 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -261,7 +261,7 @@ public class RobotMap2017 {
* @param doMP Whether to run a motion profile during autonomous. Defaults to true.
*/
@JsonCreator
public RobotMap2017(@Nullable @JsonProperty(required = true) List<CommandButton> buttons,
public RobotMap2017(@Nullable List<CommandButton> buttons,
@NotNull @JsonProperty(required = true) OI oi,
@NotNull @JsonProperty(required = true) Logger logger,
@NotNull @JsonProperty(required = true) DriveTalonCluster drive,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.fasterxml.jackson.annotation.JsonIdentityInfo;
import com.fasterxml.jackson.annotation.JsonProperty;
import com.fasterxml.jackson.annotation.ObjectIdGenerators;
import com.kauailabs.navx.frc.AHRS;
import org.jetbrains.annotations.NotNull;
import org.usfirst.frc.team449.robot.generalInterfaces.rumbleable.Rumbleable;
import org.usfirst.frc.team449.robot.jacksonWrappers.MappedAHRS;
Expand All @@ -14,21 +13,16 @@
import java.util.List;

/**
* A component to rumble controllers based off the jerk measurements from a NavX.
* A component to rumble controllers based off the jerk measurements from an AHRS.
*/
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
public class NavXRumbleComponent implements MappedRunnable {

/**
* The factor to multiply feet/(sec^3) by to get Gs/millisecond, according to WolframAlpha.
*/
private static final double FPS_CUBED_TO_GS_PER_MILLIS = 3.108e-5;
public class AHRSRumbleComponent implements MappedRunnable {

/**
* The NavX to get jerk measurements from.
*/
@NotNull
private final MappedAHRS navX;
private final MappedAHRS ahrs;

/**
* The things to rumble.
Expand Down Expand Up @@ -81,7 +75,7 @@ public class NavXRumbleComponent implements MappedRunnable {
/**
* Default constructor.
*
* @param navX The NavX to get jerk measurements from.
* @param ahrs The NavX to get jerk measurements from.
* @param rumbleables The things to rumble.
* @param minJerk The minimum jerk that will trigger rumbling, in feet/(sec^3).
* @param maxJerk The jerk, in feet/(sec^3), that's scaled to maximum rumble. All jerks of greater magnitude
Expand All @@ -91,13 +85,13 @@ public class NavXRumbleComponent implements MappedRunnable {
* @param invertLeftRight Whether to invert the left-right jerk measurement. Defaults to false.
*/
@JsonCreator
public NavXRumbleComponent(@NotNull @JsonProperty(required = true) MappedAHRS navX,
public AHRSRumbleComponent(@NotNull @JsonProperty(required = true) MappedAHRS ahrs,
@NotNull @JsonProperty(required = true) List<Rumbleable> rumbleables,
@JsonProperty(required = true) double minJerk,
@JsonProperty(required = true) double maxJerk,
boolean yIsFrontBack,
boolean invertLeftRight) {
this.navX = navX;
this.ahrs = ahrs;
this.rumbleables = rumbleables;
this.minJerk = minJerk;
this.maxJerk = maxJerk;
Expand All @@ -115,11 +109,11 @@ public NavXRumbleComponent(@NotNull @JsonProperty(required = true) MappedAHRS na
public void run() {
if (yIsFrontBack) {
//Put an abs() here because we can't differentiate front vs back when rumbling, so we only care about magnitude.
frontBack = Math.abs(navX.getYAccel());
leftRight = navX.getXAccel() * (invertLeftRight ? -1 : 1);
frontBack = Math.abs(ahrs.getYAccel());
leftRight = ahrs.getXAccel() * (invertLeftRight ? -1 : 1);
} else {
frontBack = Math.abs(navX.getYAccel());
leftRight = navX.getXAccel() * (invertLeftRight ? -1 : 1);
frontBack = Math.abs(ahrs.getYAccel());
leftRight = ahrs.getXAccel() * (invertLeftRight ? -1 : 1);
}

//Left is negative jerk, so we subtract it from left so that when we're going left, left is bigger and vice versa
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -273,7 +273,7 @@ public double getAngularVelCached() {
*/
@Override
public double getAngularDisplacement() {
return ahrs.getAngularVelocity();
return ahrs.getAngularDisplacement();
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,19 +34,19 @@ public class DriveTalonClusterShiftable extends DriveTalonCluster implements Dri
*
* @param leftMaster The master talon on the left side of the drive.
* @param rightMaster The master talon on the right side of the drive.
* @param navX The NavX on this drive.
* @param ahrs The NavX on this drive.
* @param VelScale The amount to scale the output to the motor by. Defaults to 1.
* @param shiftComponent The component that controls shifting.
* @param startingOverrideAutoshift Whether to start with autoshift disabled. Defaults to false.
*/
@JsonCreator
public DriveTalonClusterShiftable(@NotNull @JsonProperty(required = true) FPSTalon leftMaster,
@NotNull @JsonProperty(required = true) FPSTalon rightMaster,
@NotNull @JsonProperty(required = true) MappedAHRS navX,
@NotNull @JsonProperty(required = true) MappedAHRS ahrs,
@Nullable Double VelScale,
@NotNull @JsonProperty(required = true) ShiftComponent shiftComponent,
boolean startingOverrideAutoshift) {
super(leftMaster, rightMaster, navX, VelScale);
super(leftMaster, rightMaster, ahrs, VelScale);
//Initialize stuff
this.shiftComponent = shiftComponent;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -697,7 +697,7 @@ public void loadProfile(MotionProfileData data) {
point.velocityOnly = velocityOnly; // true => no position servo just velocity feedforward

// Set all the fields of the profile point
point.position = startPosition + feetToEncoder(data.getData()[i][0]) * (data.isInverted() ? -1 : 1);
point.position = feetToEncoder(startPosition + (data.getData()[i][0] * (data.isInverted() ? -1 : 1)));

//Calculate vel based off inversion
if (data.isInverted()) {
Expand Down
6 changes: 3 additions & 3 deletions RoboRIO/src/main/resources/nate_map.yml
Original file line number Diff line number Diff line change
Expand Up @@ -212,14 +212,14 @@ drive:
defaultDriveCommand:
org.usfirst.frc.team449.robot.commands.multiInterface.drive.UnidirectionalNavXShiftingDefaultDrive:
'@id': defaultDriveCommand
kP: 0.003
kP: 0.001
kI: 0.0
kD: 0.0
absoluteTolerance: 0
toleranceBuffer: 25
deadband: 2
maxAngularVelToEnterLoop: 0.05
inverted: true
inverted: false
highGearAngularCoefficient: 2
driveStraightLoopEntryTimer:
'@id': driveStraightLoopEntryTimer
Expand Down Expand Up @@ -551,7 +551,7 @@ startupCommand:
org.usfirst.frc.team449.robot.commands.general.RunRunnables:
'@id': runRunnables
runnables:
- org.usfirst.frc.team449.robot.components.NavXRumbleComponent:
- org.usfirst.frc.team449.robot.components.AHRSRumbleComponent:
'@id': rumbleComponent
ahrs:
driveNavX
Expand Down
6 changes: 3 additions & 3 deletions RoboRIO/src/main/resources/naveen_map.yml
Original file line number Diff line number Diff line change
Expand Up @@ -202,14 +202,14 @@ drive:
defaultDriveCommand:
org.usfirst.frc.team449.robot.commands.multiInterface.drive.UnidirectionalNavXShiftingDefaultDrive:
'@id': defaultDriveCommand
kP: 0.003
kP: 0.001
kI: 0.0
kD: 0.0
absoluteTolerance: 0
toleranceBuffer: 25
deadband: 2
maxAngularVelToEnterLoop: 0.05
inverted: true
inverted: false
highGearAngularCoefficient: 2
driveStraightLoopEntryTimer:
'@id': driveStraightLoopEntryTimer
Expand Down Expand Up @@ -541,7 +541,7 @@ startupCommand:
org.usfirst.frc.team449.robot.commands.general.RunRunnables:
'@id': runRunnables
runnables:
- org.usfirst.frc.team449.robot.components.NavXRumbleComponent:
- org.usfirst.frc.team449.robot.components.AHRSRumbleComponent:
'@id': rumbleComponent
ahrs:
driveNavX
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ <h2 title="Class NavXRumbleComponent" class="title">Class NavXRumbleComponent</h
<li>java.lang.Object</li>
<li>
<ul class="inheritance">
<li>org.usfirst.frc.team449.robot.components.NavXRumbleComponent</li>
<li>org.usfirst.frc.team449.robot.components.AHRSRumbleComponent</li>
</ul>
</li>
</ul>
Expand Down Expand Up @@ -134,7 +134,7 @@ <h3>Constructor Summary</h3>
<th class="colOne" scope="col">Constructor and Description</th>
</tr>
<tr class="altColor">
<td class="colOne"><code><span class="memberNameLink"><a href="../../../../../../org/usfirst/frc/team449/robot/components/NavXRumbleComponent.html#NavXRumbleComponent-org.usfirst.frc.team449.robot.jacksonWrappers.MappedAHRS-java.util.List-double-double-boolean-boolean-">NavXRumbleComponent</a></span>(<a href="../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;navX,
<td class="colOne"><code><span class="memberNameLink"><a href="../../../../../../org/usfirst/frc/team449/robot/components/NavXRumbleComponent.html#NavXRumbleComponent-org.usfirst.frc.team449.robot.jacksonWrappers.MappedAHRS-java.util.List-double-double-boolean-boolean-">NavXRumbleComponent</a></span>(<a href="../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;ahrs,
java.util.List&lt;<a href="../../../../../../org/usfirst/frc/team449/robot/generalInterfaces/rumbleable/Rumbleable.html" title="interface in org.usfirst.frc.team449.robot.generalInterfaces.rumbleable">Rumbleable</a>&gt;&nbsp;rumbleables,
double&nbsp;minJerk,
double&nbsp;maxJerk,
Expand Down Expand Up @@ -193,7 +193,7 @@ <h3>Constructor Detail</h3>
<li class="blockList">
<h4>NavXRumbleComponent</h4>
<pre>public&nbsp;NavXRumbleComponent(@NotNull
<a href="../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;navX,
<a href="../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;ahrs,
@NotNull
java.util.List&lt;<a href="../../../../../../org/usfirst/frc/team449/robot/generalInterfaces/rumbleable/Rumbleable.html" title="interface in org.usfirst.frc.team449.robot.generalInterfaces.rumbleable">Rumbleable</a>&gt;&nbsp;rumbleables,
double&nbsp;minJerk,
Expand All @@ -203,7 +203,7 @@ <h4>NavXRumbleComponent</h4>
<div class="block">Default constructor.</div>
<dl>
<dt><span class="paramLabel">Parameters:</span></dt>
<dd><code>navX</code> - The NavX to get jerk measurements from.</dd>
<dd><code>ahrs</code> - The NavX to get jerk measurements from.</dd>
<dd><code>rumbleables</code> - The things to rumble.</dd>
<dd><code>minJerk</code> - The minimum jerk that will trigger rumbling, in feet/(sec^3).</dd>
<dd><code>maxJerk</code> - The jerk, in feet/(sec^3), that's scaled to maximum rumble. All jerks of greater magnitude
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ <h3>Constructor Summary</h3>
<tr class="altColor">
<td class="colOne"><code><span class="memberNameLink"><a href="../../../../../../../org/usfirst/frc/team449/robot/drive/unidirectional/DriveTalonClusterShiftable.html#DriveTalonClusterShiftable-org.usfirst.frc.team449.robot.jacksonWrappers.FPSTalon-org.usfirst.frc.team449.robot.jacksonWrappers.FPSTalon-org.usfirst.frc.team449.robot.jacksonWrappers.MappedAHRS-java.lang.Double-org.usfirst.frc.team449.robot.components.ShiftComponent-boolean-">DriveTalonClusterShiftable</a></span>(<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">FPSTalon</a>&nbsp;leftMaster,
<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">FPSTalon</a>&nbsp;rightMaster,
<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;navX,
<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;ahrs,
java.lang.Double&nbsp;VelScale,
<a href="../../../../../../../org/usfirst/frc/team449/robot/components/ShiftComponent.html" title="class in org.usfirst.frc.team449.robot.components">ShiftComponent</a>&nbsp;shiftComponent,
boolean&nbsp;startingOverrideAutoshift)</code>
Expand Down Expand Up @@ -275,7 +275,7 @@ <h4>DriveTalonClusterShiftable</h4>
@NotNull
<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">FPSTalon</a>&nbsp;rightMaster,
@NotNull
<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;navX,
<a href="../../../../../../../org/usfirst/frc/team449/robot/jacksonWrappers/MappedAHRS.html" title="class in org.usfirst.frc.team449.robot.jacksonWrappers">MappedAHRS</a>&nbsp;ahrs,
@Nullable
java.lang.Double&nbsp;VelScale,
@NotNull
Expand All @@ -286,7 +286,7 @@ <h4>DriveTalonClusterShiftable</h4>
<dt><span class="paramLabel">Parameters:</span></dt>
<dd><code>leftMaster</code> - The master talon on the left side of the drive.</dd>
<dd><code>rightMaster</code> - The master talon on the right side of the drive.</dd>
<dd><code>navX</code> - The NavX on this drive.</dd>
<dd><code>ahrs</code> - The NavX on this drive.</dd>
<dd><code>VelScale</code> - The amount to scale the output to the motor by. Defaults to 1.</dd>
<dd><code>shiftComponent</code> - The component that controls shifting.</dd>
<dd><code>startingOverrideAutoshift</code> - Whether to start with autoshift disabled. Defaults to false.</dd>
Expand Down

0 comments on commit 840c78a

Please sign in to comment.