From 7a0d9e4cdae9fcfed7df4a25d9fa641b66551f7d Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 9 Mar 2020 19:17:44 -0600 Subject: [PATCH] Added functionality to get time for trajectory --- PathWeaver/Groups/FirstPathGroup | 1 + PathWeaver/Paths/FirstPath2 | 3 +++ src/main/deploy/paths/FirstPath2.wpilib.json | 1 + src/main/java/frc4388/robot/RobotContainer.java | 10 ++++++++++ 4 files changed, 15 insertions(+) create mode 100644 PathWeaver/Paths/FirstPath2 create mode 100644 src/main/deploy/paths/FirstPath2.wpilib.json diff --git a/PathWeaver/Groups/FirstPathGroup b/PathWeaver/Groups/FirstPathGroup index 58e567a..1d5efec 100644 --- a/PathWeaver/Groups/FirstPathGroup +++ b/PathWeaver/Groups/FirstPathGroup @@ -1,2 +1,3 @@ +FirstPath2 FirstPath0 FirstPath1 diff --git a/PathWeaver/Paths/FirstPath2 b/PathWeaver/Paths/FirstPath2 new file mode 100644 index 0000000..db2f102 --- /dev/null +++ b/PathWeaver/Paths/FirstPath2 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +7.2,-0.715,1.5,0.0,true, +6.8264744033479925,-2.6310096463529717,-0.5948912429093554,-0.29744562145467723,true, diff --git a/src/main/deploy/paths/FirstPath2.wpilib.json b/src/main/deploy/paths/FirstPath2.wpilib.json new file mode 100644 index 0000000..25ef969 --- /dev/null +++ b/src/main/deploy/paths/FirstPath2.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.18574091419977973,"velocity":0.5015004683394053,"acceleration":2.6999999999999966,"pose":{"translation":{"x":7.246571637767237,"y":-0.71552330340216},"rotation":{"radians":-0.03359339040978542}},"curvature":-1.4405435131169915},{"time":0.2266410226892701,"velocity":0.6119307612610292,"acceleration":2.699999999999999,"pose":{"translation":{"x":7.269309656446672,"y":-0.716724580735192},"rotation":{"radians":-0.07482060995567351}},"curvature":-2.1920137727383837},{"time":0.26044483766611803,"velocity":0.7032010616985185,"acceleration":1.4167228940696506,"pose":{"translation":{"x":7.2914220857138154,"y":-0.7189906269967898},"rotation":{"radians":-0.1324433145295062}},"curvature":-3.007297576134003},{"time":0.29029013543616583,"velocity":0.745483578329671,"acceleration":-2.699999999999993,"pose":{"translation":{"x":7.312735725027567,"y":-0.7226066855070661},"rotation":{"radians":-0.206853162026066}},"curvature":-3.8886464823836606},{"time":0.30487021129889263,"velocity":0.7061173735003088,"acceleration":-2.669760925580698,"pose":{"translation":{"x":7.323043539168388,"y":-0.725000944213698},"rotation":{"radians":-0.2504088093160685}},"curvature":-4.344233814092265},{"time":0.32009331314664224,"velocity":0.6654753310210515,"acceleration":-2.20371219622657,"pose":{"translation":{"x":7.333094382263903,"y":-0.7278246052076687},"rotation":{"radians":-0.2981374104428669}},"curvature":-4.798040174521225},{"time":0.33600884596997993,"velocity":0.6304020772288179,"acceleration":-1.7722927303336353,"pose":{"translation":{"x":7.342871296984669,"y":-0.7311041261586553},"rotation":{"radians":-0.34989917578712737}},"curvature":-5.236702536281345},{"time":0.3525829471208297,"velocity":0.6010279182473525,"acceleration":-1.3699347820320806,"pose":{"translation":{"x":7.352358319786087,"y":-0.7348640257199522},"rotation":{"radians":-0.4054537862993204}},"curvature":-5.643480174935811},{"time":0.36976286076987,"velocity":0.5774925569872245,"acceleration":-0.9919241283235749,"pose":{"translation":{"x":7.361540463598101,"y":-0.7391269205615391},"rotation":{"radians":-0.4644440730100708}},"curvature":-5.999261987332941},{"time":0.3874752540937721,"velocity":0.5599232066788886,"acceleration":-0.633760498617797,"pose":{"translation":{"x":7.370403700514882,"y":-0.7439135624031483},"rotation":{"radians":-0.5263880359703766}},"curvature":-6.284353805966965},{"time":0.40562669827129005,"velocity":0.5484195383663117,"acceleration":-0.2909483020003185,"pose":{"translation":{"x":7.378934944484527,"y":-0.7492428750473332},"rotation":{"radians":-0.5906828726845003}},"curvature":-6.480915495174348},{"time":0.42410675665832054,"velocity":0.5430427967577385,"acceleration":0.040867278011913304,"pose":{"translation":{"x":7.3871220339987556,"y":-0.755131991412536},"rotation":{"radians":-0.6566233239679846}},"curvature":-6.57564300066678},{"time":0.4427936520640061,"velocity":0.5438064793074622,"acceleration":0.36551173046498503,"pose":{"translation":{"x":7.394953714782599,"y":-0.7615962905661549},"rotation":{"radians":-0.7234341130651554}},"curvature":-6.562074297460497},{"time":0.46156189622249166,"velocity":0.5506664927076196,"acceleration":0.6857461024631776,"pose":{"translation":{"x":7.402419622484099,"y":-0.7686494347576127},"rotation":{"radians":-0.7903131138986692}},"curvature":-6.44187668356058},{"time":0.48029075301778956,"velocity":0.5635097332585861,"acceleration":1.002705190054949,"pose":{"translation":{"x":7.409510265363997,"y":-0.7763034064514244},"rotation":{"radians":-0.8564791640564974}},"curvature":-6.224712372184579},{"time":0.4988721753143321,"velocity":0.582141421833932,"acceleration":1.3154389982154877,"pose":{"translation":{"x":7.4162170069854305,"y":-0.7845685453602647},"rotation":{"radians":-0.921217194032659}},"curvature":-5.926705551959653},{"time":0.5172170027973768,"velocity":0.6062729233206643,"acceleration":1.620684901595592,"pose":{"translation":{"x":7.422532048903632,"y":-0.7934535854780363},"rotation":{"radians":-0.9839140979634297}},"curvature":-5.567955823359643},{"time":0.5352586722305678,"velocity":0.6355127845706157,"acceleration":1.9129717995948965,"pose":{"translation":{"x":7.428448413355612,"y":-0.8029656921129377},"rotation":{"radians":-1.044081205566804}},"curvature":-5.169769119324686},{"time":0.5529542844520852,"velocity":0.6693639917269453,"acceleration":2.1851010903529495,"pose":{"translation":{"x":7.433959925949866,"y":-0.8131104989205302},"rotation":{"radians":-1.1013623608918062}},"curvature":-4.752235515342323},{"time":0.5702833868798093,"velocity":0.7072298323366033,"acceleration":2.4289676387655197,"pose":{"translation":{"x":7.4390611983560575,"y":-0.8238921449368072},"rotation":{"radians":-1.1555293832873916}},"curvature":-4.332545539867548},{"time":0.587245135472715,"velocity":0.7484293707656479,"acceleration":2.636597030345389,"pose":{"translation":{"x":7.443747610994719,"y":-0.8353133116112605},"rotation":{"radians":-1.2064683750474912}},"curvature":-3.9241463540123545},{"time":0.6038545747404133,"velocity":0.7922217690145634,"acceleration":2.699999999999996,"pose":{"translation":{"x":7.448015295726941,"y":-0.8473752598399495},"rotation":{"radians":-1.2541608106925795}},"curvature":-3.536616605499757},{"time":0.6362024768433319,"velocity":0.8795611046924436,"acceleration":2.7000000000000006,"pose":{"translation":{"x":7.4552826622574155,"y":-0.8734196639755125},"rotation":{"radians":-1.3400856851183165}},"curvature":-2.845585051317401},{"time":0.6677848651954601,"velocity":0.9648335532431899,"acceleration":2.7,"pose":{"translation":{"x":7.460846723855813,"y":-0.9020084406998853},"rotation":{"radians":-1.4143103108236423}},"curvature":-2.277572509988386},{"time":0.6989057195952202,"velocity":1.048859860122542,"acceleration":2.7000000000000037,"pose":{"translation":{"x":7.464701821619865,"y":-0.9331043146308698},"rotation":{"radians":-1.478233001222403}},"curvature":-1.8251885024621224},{"time":0.7297320912150541,"velocity":1.1320910634960937,"acceleration":2.6999999999999966,"pose":{"translation":{"x":7.466852657906909,"y":-0.9666508367058511},"rotation":{"radians":-1.5333229489020965}},"curvature":-1.4707911285203954},{"time":0.7603482420772625,"velocity":1.2147546708240562,"acceleration":2.4689382496452787,"pose":{"translation":{"x":7.467313742404098,"y":-1.0025735692399675},"rotation":{"radians":-1.5809522845751747}},"curvature":-1.1950333509651316},{"time":0.7908701889907861,"velocity":1.2901114730124972,"acceleration":1.6478061425997925,"pose":{"translation":{"x":7.466108838198616,"y":-1.040781270984282},"rotation":{"radians":-1.6223272462013492}},"curvature":-0.9806036708623775},{"time":0.8528650627556515,"velocity":1.3922670068119412,"acceleration":1.1265054842673399,"pose":{"translation":{"x":7.458839059449773,"y":-1.1236097096364015},"rotation":{"radians":-1.6902497191987305}},"curvature":-0.6821918383506778},{"time":0.9169237989671473,"velocity":1.4644295244694259,"acceleration":0.7553834508087204,"pose":{"translation":{"x":7.445397445026421,"y":-1.2141151835996804},"rotation":{"radians":-1.743407706342333}},"curvature":-0.4964864179346202},{"time":0.9832824629764142,"velocity":1.5145557610798024,"acceleration":0.5047361844321614,"pose":{"translation":{"x":7.426250721188545,"y":-1.3110837105092963},"rotation":{"radians":-1.7861325178852274}},"curvature":-0.37790537450487255},{"time":1.051770787169082,"velocity":1.5491242965109624,"acceleration":0.3362979412462314,"pose":{"translation":{"x":7.40196049133683,"y":-1.4131462165595934},"rotation":{"radians":-1.821408517045324}},"curvature":-0.3005993467136097},{"time":1.1219365275677993,"velocity":1.5727208905530685,"acceleration":0.2198034851609914,"pose":{"translation":{"x":7.373165510259402,"y":-1.5188164583655404},"rotation":{"radians":-1.8513381322664064}},"curvature":-0.24978161090879322},{"time":1.1931386100223174,"velocity":1.5883713564272919,"acceleration":0.13404480115479672,"pose":{"translation":{"x":7.340563958378583,"y":-1.6265289448241926},"rotation":{"radians":-1.8774525545327976}},"curvature":-0.2169095086856791},{"time":1.2646178303499827,"velocity":1.5979527743028137,"acceleration":0.06404129568110227,"pose":{"translation":{"x":7.304895715997633,"y":-1.7346768589761474},"rotation":{"radians":-1.9009171561391245}},"curvature":-0.19710255409574393},{"time":1.3355531046387532,"velocity":1.602495561177761,"acceleration":-0.002096322275086082,"pose":{"translation":{"x":7.2669246375475,"y":-1.8416499798670074},"rotation":{"radians":-1.9226712973054454}},"curvature":-0.1877943585524745},{"time":1.405109094208867,"velocity":1.6023497494074594,"acceleration":-0.07612871060578774,"pose":{"translation":{"x":7.227420825833567,"y":-1.9458726044088372},"rotation":{"radians":-1.943529946970129}},"curvature":-0.18809230783361033},{"time":1.4724793462205135,"velocity":1.5972209389886258,"acceleration":-0.17301087241769844,"pose":{"translation":{"x":7.187142906282408,"y":-2.045841469241623},"rotation":{"radians":-1.9642661669856805}},"curvature":-0.19860704074543745},{"time":1.5369281583145993,"velocity":1.5860705937819437,"acceleration":-0.31554579820498185,"pose":{"translation":{"x":7.146820301188521,"y":-2.1401636725947344},"rotation":{"radians":-1.98568919023132}},"curvature":-0.22170133611162526},{"time":1.5978341143112027,"velocity":1.566851975281558,"acceleration":-1.3309955874047237,"pose":{"translation":{"x":7.107135503961091,"y":-2.2275945961483803},"rotation":{"radians":-2.0087318859865846}},"curvature":-0.26227793056302745},{"time":1.6555960479201848,"velocity":1.4899710965280382,"acceleration":-2.7,"pose":{"translation":{"x":7.068706353370726,"y":-2.3070758268950673},"rotation":{"radians":-2.034563405456838}},"curvature":-0.3294600185346016},{"time":1.7119114730269265,"velocity":1.3379194487398354,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.032068307796214,"y":-2.377773079001069},"rotation":{"radians":-2.064747104509842}},"curvature":-0.4399216872784071},{"time":1.7676117337250405,"velocity":1.1875287448549274,"acceleration":-2.7,"pose":{"translation":{"x":6.997656719471268,"y":-2.439114115667869},"rotation":{"radians":-2.101467131011381}},"curvature":-0.6243017215544429},{"time":1.8221431594281243,"velocity":1.040293895456601,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.965789108731271,"y":-2.490826670993634},"rotation":{"radians":-2.1478338626356113}},"curvature":-0.9388068028845457},{"time":1.8750311267951567,"velocity":0.8974963835656139,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.936647438260028,"y":-2.5329763718346676},"rotation":{"radians":-2.2081880495457282}},"curvature":-1.4818665318907704},{"time":1.9260491103410267,"velocity":0.7597478279917649,"acceleration":-2.700000000000004,"pose":{"translation":{"x":6.910260387336512,"y":-2.5660046596668717},"rotation":{"radians":-2.2879536229403787}},"curvature":-2.39507742000404},{"time":1.9509462729406097,"velocity":0.6925254889728903,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":6.898062530851734,"y":-2.5793483183090196},"rotation":{"radians":-2.336664233442283}},"curvature":-3.0225092551252013},{"time":1.9756123368228327,"velocity":0.6259271164908882,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.8864856260816145,"y":-2.5907667124471985},"rotation":{"radians":-2.391450278038916}},"curvature":-3.7370486015184476},{"time":2.000334632533119,"velocity":0.5591769180731152,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":6.875482253478922,"y":-2.6004376072065902},"rotation":{"radians":-2.4513985995970358}},"curvature":-4.444436732694084},{"time":2.0256138831254242,"velocity":0.49092294147389115,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.864992089704889,"y":-2.6085693664751215},"rotation":{"radians":-2.5140796747043557}},"curvature":-4.951976919791685},{"time":2.052333259353671,"velocity":0.4187806256576248,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.854941353699419,"y":-2.61540213796164},"rotation":{"radians":-2.5750408517708028}},"curvature":-4.970238408366828},{"time":2.082203426917947,"velocity":0.3381311732340795,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.845242252751303,"y":-2.6212090382540865},"rotation":{"radians":-2.627762206688829}},"curvature":-4.195902581911151},{"time":2.207437194782421,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.8264744033479845,"y":-2.6310096463529753},"rotation":{"radians":-2.677945044589006}},"curvature":-8.61988124756193E-14}] \ 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 184e156..a21de35 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,6 +92,7 @@ public class RobotContainer { /* Autos */ SixBallAutoMiddle m_sixBallAutoMiddle; + double m_totalTimeAuto; /** @@ -260,6 +261,7 @@ public class RobotContainer { // Run path following command, then stop at the end. try { + SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); @@ -304,7 +306,9 @@ public class RobotContainer { public RamseteCommand[] buildPaths(String[] paths) { RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; + double[] times = new double[paths.length]; Trajectory initialTrajectory; + m_totalTimeAuto = 0; try { if (true) { @@ -315,6 +319,7 @@ public class RobotContainer { initialTrajectory = trajectory; RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); ramseteCommands[0] = ramseteCommand; + times[0] = initialTrajectory.getTotalTimeSeconds(); } for(int i = 1; i < paths.length; i++) { @@ -324,11 +329,16 @@ public class RobotContainer { Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); ramseteCommands[i] = ramseteCommand; + times[i] = trajectory.getTotalTimeSeconds(); } } catch (Exception e) { DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); } + for (int i = 0; i < times.length; i++) { + m_totalTimeAuto += times[i]; + } + return ramseteCommands; }