SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Rename dead wheel params

Browse Source
This commit is contained in:
Ryan Brott
2023-09-15 18:45:36 -07:00
parent dc1b43e13f
commit 2f2c6e6295
2 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
View File

@ -15,9 +15,9 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
@Config
public final class ThreeDeadWheelLocalizer implements Localizer {
public static class Params {
public double PAR0_Y_TICKS = 0.0;
public double PAR1_Y_TICKS = 0.0;
public double PERP_X_TICKS = 0.0;
public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units)
public double par1YTicks = 0.0; // y position of the second parallel encoder (in tick units)
public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
}
public static Params PARAMS = new Params();
@ -54,17 +54,17 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
(PARAMS.PAR0_Y_TICKS * par1PosDelta - PARAMS.PAR1_Y_TICKS * par0PosDelta) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
(PARAMS.PAR0_Y_TICKS * par1PosVel.velocity - PARAMS.PAR1_Y_TICKS * par0PosVel.velocity) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
(PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
(PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
}).times(inPerTick),
new DualNum<Time>(new double[] {
(PARAMS.PERP_X_TICKS / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS) * (par1PosDelta - par0PosDelta) + perpPosDelta),
(PARAMS.PERP_X_TICKS / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
(PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
(PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
}).times(inPerTick)
),
new DualNum<>(new double[] {
(par0PosDelta - par1PosDelta) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
(par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
(par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
(par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
})
);

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
View File

@ -19,8 +19,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@Config
public final class TwoDeadWheelLocalizer implements Localizer {
public static class Params {
public double PAR_Y_TICKS = 0.0;
public double PERP_X_TICKS = 0.0;
public double parYTicks = 0.0; // y position of the parallel encoder (in tick units)
public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
}
public static Params PARAMS = new Params();
@ -61,12 +61,12 @@ public final class TwoDeadWheelLocalizer implements Localizer {
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
parPosDelta - PARAMS.PAR_Y_TICKS * headingDelta,
parPosVel.velocity - PARAMS.PAR_Y_TICKS * headingVel,
parPosDelta - PARAMS.parYTicks * headingDelta,
parPosVel.velocity - PARAMS.parYTicks * headingVel,
}).times(inPerTick),
new DualNum<Time>(new double[] {
perpPosDelta - PARAMS.PERP_X_TICKS * headingDelta,
perpPosVel.velocity - PARAMS.PERP_X_TICKS * headingVel,
perpPosDelta - PARAMS.perpXTicks * headingDelta,
perpPosVel.velocity - PARAMS.perpXTicks * headingVel,
}).times(inPerTick)
),
new DualNum<>(new double[] {