diff --git a/PathWeaver/Paths/Bounce1 b/PathWeaver/Paths/Bounce1 index 3f78c70..fe78803 100644 --- a/PathWeaver/Paths/Bounce1 +++ b/PathWeaver/Paths/Bounce1 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -1.2190625523416865,-2.295277716936159,2.158615473813051,-0.2968869419570952,true, +1.2190625523416865,-2.295277716936159,2.4431321265219355,-0.012370289248212707,true, 2.295277716936159,-0.6685846807962377,-0.061851446241061314,0.5442927269213425,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index 4fb66d3..ec4f6d5 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -4,6 +4,6 @@ "maxVelocity": 2.6, "maxAcceleration": 2.7, "wheelBase": 0.648, - "gameName": "Slalom Path", + "gameName": "Bounce Path", "outputDir": ".." } \ No newline at end of file diff --git a/src/main/deploy/paths/Bounce1.wpilib.json b/src/main/deploy/paths/Bounce1.wpilib.json index e6d8b91..4d9bc17 100644 --- a/src/main/deploy/paths/Bounce1.wpilib.json +++ b/src/main/deploy/paths/Bounce1.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":1.2190625523416865,"y":-2.295277716936159},"rotation":{"radians":-0.13667831434709032}},"curvature":0.0},{"time":0.22438746704252197,"velocity":0.6058461610148095,"acceleration":2.700000000000001,"pose":{"translation":{"x":1.2864606822508553,"y":-2.304092719711363},"rotation":{"radians":-0.1170421427859319}},"curvature":0.5643286724104043},{"time":0.31659154428955705,"velocity":0.8547971695818043,"acceleration":2.700000000000002,"pose":{"translation":{"x":1.3535125948797755,"y":-2.3103001998044204},"rotation":{"radians":-0.06200050429081461}},"curvature":1.0620304408056782},{"time":0.3865335497601475,"velocity":1.0436405843523986,"acceleration":2.699999999999999,"pose":{"translation":{"x":1.4198872043666615,"y":-2.3117422180116645},"rotation":{"radians":0.02340135914057638}},"curvature":1.499027877863022},{"time":0.416844551908855,"velocity":1.125480290153909,"acceleration":2.6999999999999957,"pose":{"translation":{"x":1.4527217018630596,"y":-2.3101287966124655},"rotation":{"radians":0.07581618388523016}},"curvature":1.685127643900189},{"time":0.44497336103755447,"velocity":1.2014280748013975,"acceleration":2.700000000000003,"pose":{"translation":{"x":1.4852704747922563,"y":-2.306721971042591},"rotation":{"radians":0.1336105355281614}},"curvature":1.8407145612445912},{"time":0.471379880600669,"velocity":1.2727256776218068,"acceleration":2.6999999999999953,"pose":{"translation":{"x":1.5174970307016695,"y":-2.30137640225166},"rotation":{"radians":0.19578792312700474}},"curvature":1.9585936486426558},{"time":0.4964217065576293,"velocity":1.3403386077055994,"acceleration":2.7,"pose":{"translation":{"x":1.549366017414454,"y":-2.2939715408501544},"rotation":{"radians":0.26122235838180147}},"curvature":2.03295328985009},{"time":0.5203871822425515,"velocity":1.4050453920548895,"acceleration":2.6999999999999975,"pose":{"translation":{"x":1.5808432416930818,"y":-2.284410619275986},"rotation":{"radians":0.3287008506010951}},"curvature":2.0608068154284185},{"time":0.5435135951843582,"velocity":1.4674867069977673,"acceleration":2.699999999999998,"pose":{"translation":{"x":1.6118956879029251,"y":-2.2726196439610704},"rotation":{"radians":0.3969894480167388}},"curvature":2.0429019581541517},{"time":0.5659975247394938,"velocity":1.5281933167966335,"acceleration":2.7000000000000064,"pose":{"translation":{"x":1.6424915366758384,"y":-2.2585463874978995},"rotation":{"radians":0.46491024547085325}},"curvature":1.9837890618025127},{"time":0.5880010283970155,"velocity":1.5876027766719423,"acceleration":2.70000000000001,"pose":{"translation":{"x":1.67260018357374,"y":-2.2421593808061115},"rotation":{"radians":0.5314136858990691}},"curvature":1.8910366087386976},{"time":0.6096557249934808,"velocity":1.6460704574823986,"acceleration":2.3824430758266333,"pose":{"translation":{"x":1.7021922577521942,"y":-2.223446905299065},"rotation":{"radians":0.5956324894326599}},"curvature":1.7738908881089923},{"time":0.6311088152873973,"velocity":1.6971812239082236,"acceleration":2.39609176688845,"pose":{"translation":{"x":1.7312396406239934,"y":-2.2024159850504086},"rotation":{"radians":0.6569096615732013}},"curvature":1.6418268507415719},{"time":0.6524749443925159,"velocity":1.748376429947274,"acceleration":2.5423972750022616,"pose":{"translation":{"x":1.7597154845227396,"y":-2.179091378960654},"rotation":{"radians":0.7148003790857229}},"curvature":1.5033763689458892},{"time":0.6737841905632574,"velocity":1.8025529993441198,"acceleration":2.5993191212527873,"pose":{"translation":{"x":1.7875942313664275,"y":-2.153514572923748},"rotation":{"radians":0.7690532266628125}},"curvature":1.3654279101692903},{"time":0.6950460822266684,"velocity":1.857819440898829,"acceleration":2.579218978047936,"pose":{"translation":{"x":1.8148516313210254,"y":-2.1257427719936435},"rotation":{"radians":0.8195788318011353}},"curvature":1.2329942768056759},{"time":0.7162770095253918,"velocity":1.9125786515092522,"acceleration":2.43338352449836,"pose":{"translation":{"x":1.8414647614640578,"y":-2.0958478925508723},"rotation":{"radians":0.8664136765870671}},"curvature":1.1093247469853833},{"time":0.7587269745945849,"velocity":2.015875697124158,"acceleration":2.1245035714402802,"pose":{"translation":{"x":1.8926732671647957,"y":-2.030044073281779},"rotation":{"radians":0.9495798158971775}},"curvature":0.89432735819266},{"time":0.8012650300854439,"velocity":2.1062479479366125,"acceleration":1.7669130771236268,"pose":{"translation":{"x":1.9410636125360727,"y":-1.9569343750220183},"rotation":{"radians":1.0201473202306437}},"curvature":0.7235264433649939},{"time":0.8439404610869192,"velocity":2.1816517250450063,"acceleration":1.4244526372270472,"pose":{"translation":{"x":1.9865020866994656,"y":-1.8775209375629944},"rotation":{"radians":1.080021306548925}},"curvature":0.5918444106673741},{"time":0.8867044214537462,"velocity":2.242566961167806,"acceleration":1.1252799160815283,"pose":{"translation":{"x":2.028878001065319,"y":-1.7929445299122628},"rotation":{"radians":1.1310447963445298}},"curvature":0.49193108190754337},{"time":0.9294101433394317,"velocity":2.290622852307531,"acceleration":-0.06023521002664441,"pose":{"translation":{"x":2.0681042865673716,"y":-1.7044522996238305},"rotation":{"radians":1.1748459816272578}},"curvature":0.41685943141170323},{"time":0.9721945986679412,"velocity":2.288045721654943,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":2.1041180908973773,"y":-1.6133655221284542},"rotation":{"radians":1.2128035189738537}},"curvature":0.36102176191660384},{"time":1.0161481138765462,"velocity":2.169371230591709,"acceleration":-2.700000000000004,"pose":{"translation":{"x":2.1368813757397325,"y":-1.5210473500639399},"rotation":{"radians":1.2460656619288912}},"curvature":0.32023752262865446},{"time":1.0620737216703373,"velocity":2.045372089548473,"acceleration":-2.700000000000001,"pose":{"translation":{"x":2.166381514006097,"y":-1.4288705626054419},"rotation":{"radians":1.2755894219550927}},"curvature":0.2916029622975603},{"time":1.1097296494911872,"velocity":1.9167010844321783,"acceleration":-2.700000000000003,"pose":{"translation":{"x":2.1926318870700214,"y":-1.3381853147957623},"rotation":{"radians":1.3021844585295703}},"curvature":0.2733200598956795},{"time":1.1588367235587358,"velocity":1.784111984449797,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.215672482001568,"y":-1.250286886875649},"rotation":{"radians":1.326556269034668}},"curvature":0.2646072442463823},{"time":1.2090793402466995,"velocity":1.6484569193922949,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":2.2355704888019376,"y":-1.1663834336140964},"rotation":{"radians":1.3493482566795647}},"curvature":0.26575243847025265},{"time":1.260106347293777,"velocity":1.5106840003651854,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":2.252420897638091,"y":-1.0875637336386417},"rotation":{"radians":1.371184938708623}},"curvature":0.2783830448470933},{"time":1.311532833633159,"velocity":1.3718324872488539,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.2663470960773764,"y":-1.014764938765669},"rotation":{"radians":1.3927203241814738}},"curvature":0.3060902519064523},{"time":1.362944728553117,"velocity":1.2330203709649672,"acceleration":-2.7,"pose":{"translation":{"x":2.27750146632215,"y":-0.9487403233307015},"rotation":{"radians":1.4146968384455714}},"curvature":0.35567097701480005},{"time":1.4639957714232565,"velocity":0.9601825552155909,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.2922528076203808,"y":-0.8389138366943976},"rotation":{"radians":1.4638501053751873}},"curvature":0.5793213657302914},{"time":1.560302229108694,"velocity":0.7001551194649095,"acceleration":-2.7,"pose":{"translation":{"x":2.298496566767544,"y":-0.75920739334815},"rotation":{"radians":1.5292488420790011}},"curvature":1.1903140188336137},{"time":1.606657073494272,"velocity":0.574997039623849,"acceleration":-2.700000000000002,"pose":{"translation":{"x":2.299134147724131,"y":-0.7296595314270296},"rotation":{"radians":1.5718987210540936}},"curvature":1.739266666769078},{"time":1.653402330862959,"velocity":0.448784844728394,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":2.2985565261744956,"y":-0.7057380303558152},"rotation":{"radians":1.6201260709062921}},"curvature":2.2622779404584725},{"time":1.8196189400216236,"velocity":0.0,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":2.2952777169361562,"y":-0.6685846807962372},"rotation":{"radians":1.683947308385398}},"curvature":9.41080494104515E-14}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":1.2190625523416865,"y":-2.295277716936159},"rotation":{"radians":-0.005063247870847721}},"curvature":0.0},{"time":0.23764169808406713,"velocity":0.6416325848269814,"acceleration":2.7000000000000006,"pose":{"translation":{"x":1.2953018763449697,"y":-2.295251525617249},"rotation":{"radians":0.010949549347500726}},"curvature":0.40972371495957366},{"time":0.33542236151992944,"velocity":0.9056403761038098,"acceleration":2.6999999999999997,"pose":{"translation":{"x":1.370912030261624,"y":-2.2929007644225723},"rotation":{"radians":0.055714394589566285}},"curvature":0.7669736685380631},{"time":0.4096876384931884,"velocity":1.106156623931609,"acceleration":2.7000000000000024,"pose":{"translation":{"x":1.4453236780664336,"y":-2.2863057443118926},"rotation":{"radians":0.12498321185020449}},"curvature":1.0800248800896324},{"time":0.44186280346512774,"velocity":1.1930295693558453,"acceleration":2.700000000000006,"pose":{"translation":{"x":1.4819194496977797,"y":-2.2809310487777457},"rotation":{"radians":0.16750843496324888}},"curvature":1.2167808919573002},{"time":0.47168344191293404,"velocity":1.2735452931649225,"acceleration":2.700000000000003,"pose":{"translation":{"x":1.5180305251667259,"y":-2.273961920668121},"rotation":{"radians":0.21452182721062504}},"curvature":1.3366350444133313},{"time":0.49961027761634846,"velocity":1.3489477495641415,"acceleration":2.7000000000000055,"pose":{"translation":{"x":1.5536030650507113,"y":-2.2652703679026183},"rotation":{"radians":0.26536263992327724}},"curvature":1.4362096180614292},{"time":0.5259948559351011,"velocity":1.4201861110247738,"acceleration":2.7000000000000033,"pose":{"translation":{"x":1.5885868631044722,"y":-2.2547506951601366},"rotation":{"radians":0.319301168365105}},"curvature":1.5123140768865972},{"time":0.5511162592243557,"velocity":1.4880138999057613,"acceleration":2.6999999999999997,"pose":{"translation":{"x":1.6229352695319808,"y":-2.242318591437087},"rotation":{"radians":0.3755508101995731}},"curvature":1.5625402389991099},{"time":0.575202083272446,"velocity":1.553045624835605,"acceleration":2.6999999999999984,"pose":{"translation":{"x":1.6566051142583884,"y":-2.2279102176056074},"rotation":{"radians":0.43329232863303013}},"curvature":1.5857699243094716},{"time":0.5984408959914688,"velocity":1.6157904191769663,"acceleration":2.6999999999999984,"pose":{"translation":{"x":1.6895566302019644,"y":-2.2114812939717736},"rotation":{"radians":0.49170701115109644}},"curvature":1.582475933416692},{"time":0.6209899009134207,"velocity":1.6766727324662365,"acceleration":2.6999999999999993,"pose":{"translation":{"x":1.7217533765460369,"y":-2.193006187833815},"rotation":{"radians":0.550013546001952}},"curvature":1.5547418114095763},{"time":0.642979868898357,"velocity":1.7360456460255647,"acceleration":1.6879997853718869,"pose":{"translation":{"x":1.7531621620109346,"y":-2.172477001040325},"rotation":{"radians":0.6075027337768031}},"curvature":1.5060027623166354},{"time":0.6646509709102172,"velocity":1.7726264615703569,"acceleration":1.4286093250786047,"pose":{"translation":{"x":1.7837529681259259,"y":-2.149902657548476},"rotation":{"radians":0.66356490346266}},"curvature":1.440586659147591},{"time":0.6862370242380261,"velocity":1.8034644986461088,"acceleration":1.6268592413124754,"pose":{"translation":{"x":1.8134988725011614,"y":-2.1253079909822326},"rotation":{"radians":0.7177068279802047}},"curvature":1.3631778763922657},{"time":0.7077881227895433,"velocity":1.8385251024850804,"acceleration":1.7643488783697436,"pose":{"translation":{"x":1.8423759720996122,"y":-2.0987328321905636},"rotation":{"radians":0.769557329886478}},"curvature":1.2783242186863626},{"time":0.729293124524764,"velocity":1.8764674281759566,"acceleration":1.8446740404154778,"pose":{"translation":{"x":1.870363306509013,"y":-2.0702310968056565},"rotation":{"radians":0.8188628711882234}},"curvature":1.1900687366845921},{"time":0.7507474810992104,"velocity":1.916043722802655,"acceleration":1.8670531709141556,"pose":{"translation":{"x":1.8974427812138006,"y":-2.03986987280113},"rotation":{"radians":0.8654757371813367}},"curvature":1.10173694841449},{"time":0.7934978282307765,"velocity":1.9958608939723264,"acceleration":1.7723531568910438,"pose":{"translation":{"x":1.9488196425624462,"y":-1.9738976978841287},"rotation":{"radians":0.9504629314574979}},"curvature":0.9342469087335289},{"time":0.8360697947251532,"velocity":2.0713134531836945,"acceleration":1.5868111672064373,"pose":{"translation":{"x":1.996416202288854,"y":-1.9015817852692374},"rotation":{"radians":1.0248137424279251}},"curvature":0.787784484659639},{"time":0.8784534346768569,"velocity":2.138568286365915,"acceleration":1.3642881010655703,"pose":{"translation":{"x":2.040183050287781,"y":-1.8238399739746793},"rotation":{"radians":1.0894645028484775}},"curvature":0.6659464487247512},{"time":0.9205874994340326,"velocity":2.196051289563656,"acceleration":1.139730722918328,"pose":{"translation":{"x":2.080109264907537,"y":-1.7417132660700458},"rotation":{"radians":1.1456154277171473}},"curvature":0.567725938392014},{"time":0.9623467724696898,"velocity":2.2436456160091294,"acceleration":-0.4261087441360956,"pose":{"translation":{"x":2.1162199576520844,"y":-1.6563366285391186},"rotation":{"radians":1.1945018786066128}},"curvature":0.49021075431900635},{"time":1.0040610697871553,"velocity":2.2258707891666645,"acceleration":-2.6999999999999953,"pose":{"translation":{"x":2.1485738178831406,"y":-1.5689097951426918},"rotation":{"radians":1.237278715383875}},"curvature":0.43011509193279407},{"time":1.0468578975965073,"velocity":2.110319354081414,"acceleration":-2.699999999999998,"pose":{"translation":{"x":2.1772606575222793,"y":-1.480668068281394},"rotation":{"radians":1.2749756605653983}},"curvature":0.3845048418255803},{"time":1.091411329462198,"velocity":1.9900250880440487,"acceleration":-2.699999999999999,"pose":{"translation":{"x":2.2023989557530284,"y":-1.3928531208585118},"rotation":{"radians":1.3084920051668882}},"curvature":0.35108267862816467},{"time":1.1375097066154454,"velocity":1.865559469730281,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.224133403722975,"y":-1.3066837981428092},"rotation":{"radians":1.3386110539304803}},"curvature":0.3282812713988414},{"time":1.184904252577451,"velocity":1.737594195632866,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.242632449245864,"y":-1.2233269196313539},"rotation":{"radians":1.366023762819537}},"curvature":0.315312666863858},{"time":1.2333107866468274,"velocity":1.6068965536455497,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.258085841503699,"y":-1.1438680809123354},"rotation":{"radians":1.3913568392337845}},"curvature":0.312267998860509},{"time":1.2824115422640514,"velocity":1.474324513479045,"acceleration":-2.700000000000001,"pose":{"translation":{"x":2.2707021757488444,"y":-1.06928245552789},"rotation":{"radians":1.4152041135990487}},"curvature":0.3203504243404985},{"time":1.3318581338176085,"velocity":1.3408187162844405,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.280706438006125,"y":-1.000405596836922},"rotation":{"radians":1.4381621328281249}},"curvature":0.3423512275507715},{"time":1.3812776266134272,"velocity":1.20738608573573,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":2.28833754977493,"y":-0.9379042398779245},"rotation":{"radians":1.4608720121583854}},"curvature":0.3835443611862364},{"time":1.478505525328493,"velocity":0.9448707592050519,"acceleration":-2.700000000000001,"pose":{"translation":{"x":2.297490953430074,"y":-0.8336756908847058},"rotation":{"radians":1.5086330840822888}},"curvature":0.5673302027865098},{"time":1.5716270816790128,"velocity":0.6934425570586489,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.3002591668804566,"y":-0.7574447932352428},"rotation":{"radians":1.5661182732327583}},"curvature":1.0329548682264864},{"time":1.662742064333248,"velocity":0.44743210389221405,"acceleration":-2.7,"pose":{"translation":{"x":2.2988047988197104,"y":-0.7054897577106034},"rotation":{"radians":1.6377930168152517}},"curvature":1.7198718777663586},{"time":1.8284576583674013,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":2.2952777169361562,"y":-0.6685846807962426},"rotation":{"radians":1.6839473083854013}},"curvature":1.037327362819861E-13}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 100b794..ac2f717 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -353,7 +353,7 @@ public class RobotContainer { "DriveOffLineForward" }; - m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths)); + m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths)); String[] driveOffLineBackwardPaths = new String[]{ "DriveOffLineBackward" @@ -399,13 +399,13 @@ public class RobotContainer { //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - + //return m_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); } diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java index a5fb618..05b2533 100644 --- a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java +++ b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java @@ -7,8 +7,11 @@ package frc4388.robot.commands.auto; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.RobotContainer; import frc4388.robot.subsystems.Drive; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup { /** * Creates a new DriveOffLineForward. */ - public DriveOffLineForward(Drive drive, RamseteCommand[] paths) { + public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); addCommands( + new InstantCommand(() -> drive.switchReversed(true)), + new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())), paths[0] ); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 7f2487e..1a96ced 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -338,7 +338,7 @@ public class Drive extends SubsystemBase { public void updateOdometry(boolean reversed){ if (reversed){ - m_odometry.update(Rotation2d.fromDegrees( getHeading() - 180), + m_odometry.update(Rotation2d.fromDegrees( getHeading()), -inchesToMeters(getDistanceInches(m_rightFrontMotor)), inchesToMeters(getDistanceInches(m_leftFrontMotor))); }