From ca86cca520cbc529091cb36c8dd3fe03168435cb Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Mon, 14 Mar 2022 16:51:54 -0700 Subject: [PATCH 001/108] refactor: Hopper to use picoColorSensor --- src/main/java/frc/subsystem/Hopper.java | 56 ++-- .../utility/colorsensor/PicoColorSensor.java | 286 ++++++++++++++++++ 2 files changed, 311 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc/utility/colorsensor/PicoColorSensor.java diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 9782d27a..ae57932b 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -3,12 +3,12 @@ import com.revrobotics.CANSparkMaxLowLevel; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.ColorMatch; -import com.revrobotics.ColorSensorV3; -import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import frc.robot.Constants; import frc.utility.OrangeUtility; +import frc.utility.colorsensor.PicoColorSensor; +import frc.utility.colorsensor.PicoColorSensor.RawColor; import frc.utility.controllers.LazyCANSparkMax; import org.jetbrains.annotations.NotNull; @@ -17,12 +17,7 @@ public final class Hopper extends AbstractSubsystem { public static @NotNull Hopper INSTANCE = new Hopper(); private final @NotNull LazyCANSparkMax hopperMotor; - private final @NotNull ColorSensorV3 topBall; - //private final ColorSensorV3 bottomBall = new ColorSensorV3(Port.kMXP); - - private boolean topBallDetected = false; - private boolean bottomBallDetected = false; - + private final @NotNull PicoColorSensor colorSensor; /** * A Rev Color Match object is used to register and detect known colors. This can be calibrated ahead of time or during @@ -38,6 +33,8 @@ public final class Hopper extends AbstractSubsystem { private final Color blueTarget = new Color(0.184, 0.427, 0.39); private final Color redTarget = new Color(0.38, 0.41, 0.2); + private boolean ballDetected; + public static Hopper getInstance() { return INSTANCE; } @@ -60,17 +57,21 @@ private Hopper() { colorMatcher.addColorMatch(blueTarget); colorMatcher.addColorMatch(redTarget); - topBall = new ColorSensorV3(I2C.Port.kMXP); + colorSensor = new PicoColorSensor(); + } + + private Color convertRawColor(RawColor rawColor) { + return new Color(rawColor.red, rawColor.green, rawColor.blue); } public void checkBallColor() { - Color detectedColorTop = topBall.getColor(); + Color detectedColorTop = convertRawColor(colorSensor.getRawColor0()); //Color detectedColorBottom = bottomBall.getColor(); - logData("Top Ball Red", detectedColorTop.red); - logData("Top Ball Green", detectedColorTop.green); - logData("Top Ball Blue", detectedColorTop.blue); - logData("Top Confidence", colorMatcher.matchClosestColor(detectedColorTop).confidence); + logData("Ball Red", detectedColorTop.red); + logData("Ball Green", detectedColorTop.green); + logData("Ball Blue", detectedColorTop.blue); + logData("Color Confidence", colorMatcher.matchClosestColor(detectedColorTop).confidence); // logData("Bottom Red", detectedColorBottom.red); // logData("Bottom Green", detectedColorBottom.green); @@ -78,14 +79,14 @@ public void checkBallColor() { // logData("Bottom Confidence", colorMatcher.matchClosestColor(detectedColorBottom).confidence); if (colorMatcher.matchClosestColor(detectedColorTop).color == redTarget) { - logData("Top Ball Color", "Red"); - topBallDetected = true; + logData("Ball Color", "Red"); + ballDetected = true; } else if (colorMatcher.matchClosestColor(detectedColorTop).color == blueTarget) { - logData("Top Ball Color", "Blue"); - topBallDetected = true; + logData("Ball Color", "Blue"); + ballDetected = true; } else { - logData("Top Ball Color", "No Ball Detected"); - topBallDetected = false; + logData("Ball Color", "No Ball Detected"); + ballDetected = false; } // if (colorMatcher.matchClosestColor(detectedColorBottom).color == redTarget) { @@ -100,16 +101,6 @@ public void checkBallColor() { // } } - public void numberOfBalls() { - if (topBallDetected && bottomBallDetected) { - logData("Number of Balls", 2); - } else if (bottomBallDetected || topBallDetected) { - logData("Number of Balls", 1); - } else { - logData("Number of Balls", 0); - } - } - public void setHopperState(HopperState hopperState) { switch (hopperState) { case ON: @@ -131,11 +122,14 @@ public void setHopperState(HopperState hopperState) { } } + public boolean isBallDetected() { + return ballDetected; + } + @Override public void update() { setHopperState(wantedHopperState); checkBallColor(); - numberOfBalls(); } public void setWantedHopperState(HopperState hopperState) { diff --git a/src/main/java/frc/utility/colorsensor/PicoColorSensor.java b/src/main/java/frc/utility/colorsensor/PicoColorSensor.java new file mode 100644 index 00000000..c41129db --- /dev/null +++ b/src/main/java/frc/utility/colorsensor/PicoColorSensor.java @@ -0,0 +1,286 @@ +package frc.utility.colorsensor; + +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.SerialPortJNI; +import edu.wpi.first.wpilibj.Timer; + +import java.nio.charset.StandardCharsets; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.locks.ReentrantLock; + +public class PicoColorSensor implements AutoCloseable { + public static class RawColor { + public RawColor(int r, int g, int b, int _ir) { + red = r; + green = g; + blue = b; + ir = _ir; + } + + public RawColor() { + } + + public int red; + public int green; + public int blue; + public int ir; + } + + private static class SingleCharSequence implements CharSequence { + public byte[] data; + + @Override + public int length() { + return data.length; + } + + @Override + public char charAt(int index) { + return (char) data[index]; + } + + @Override + public CharSequence subSequence(int start, int end) { + return new String(data, start, end, StandardCharsets.UTF_8); + } + } + + private static class IntRef { + int value; + } + + int parseIntFromIndex(SingleCharSequence charSeq, int readLen, IntRef lastComma) { + int nextComma = 0; + try { + nextComma = findNextComma(charSeq.data, readLen, lastComma.value); + int value = Integer.parseInt(charSeq, lastComma.value + 1, nextComma, 10); + lastComma.value = nextComma; + return value; + } catch (Exception ex) { + return 0; + } + } + + private int findNextComma(byte[] data, int readLen, int lastComma) { + while (true) { + if (readLen <= lastComma + 1) { + return readLen; + } + lastComma++; + if (data[lastComma] == ',') { + break; + } + } + return lastComma; + } + + private final AtomicBoolean debugPrints = new AtomicBoolean(); + private boolean hasColor0; + private boolean hasColor1; + private int prox0; + private int prox1; + private final RawColor color0 = new RawColor(); + private final RawColor color1 = new RawColor(); + private double lastReadTime; + private final ReentrantLock threadLock = new ReentrantLock(); + private final Thread readThread; + private final AtomicBoolean threadRunning = new AtomicBoolean(true); + + private void threadMain() { + // Using JNI for a non allocating read + int port = SerialPortJNI.serialInitializePort((byte) 1); + SerialPortJNI.serialSetBaudRate(port, 115200); + SerialPortJNI.serialSetDataBits(port, (byte) 8); + SerialPortJNI.serialSetParity(port, (byte) 0); + SerialPortJNI.serialSetStopBits(port, (byte) 10); + + SerialPortJNI.serialSetTimeout(port, 1); + SerialPortJNI.serialEnableTermination(port, '\n'); + + HAL.report(tResourceType.kResourceType_SerialPort, 2); + + byte[] buffer = new byte[257]; + SingleCharSequence charSeq = new SingleCharSequence(); + charSeq.data = buffer; + IntRef lastComma = new IntRef(); + + RawColor color0 = new RawColor(); + RawColor color1 = new RawColor(); + + while (threadRunning.get()) { + int read = SerialPortJNI.serialRead(port, buffer, buffer.length - 1); + if (read <= 0) { + try { + threadLock.lock(); + this.hasColor0 = false; + this.hasColor1 = false; + } finally { + threadLock.unlock(); + } + continue; + } + if (!threadRunning.get()) { + break; + } + + // Trim trailing newline if exists + if (buffer[read - 1] == '\n') { + read--; + } + + if (read == 0) { + continue; + } + + if (debugPrints.get()) { + System.out.println(new String(buffer, 0, read, StandardCharsets.UTF_8)); + } + + lastComma.value = -1; + + boolean hasColor0 = parseIntFromIndex(charSeq, read, lastComma) != 0; + boolean hasColor1 = parseIntFromIndex(charSeq, read, lastComma) != 0; + color0.red = parseIntFromIndex(charSeq, read, lastComma); + color0.green = parseIntFromIndex(charSeq, read, lastComma); + color0.blue = parseIntFromIndex(charSeq, read, lastComma); + color0.ir = parseIntFromIndex(charSeq, read, lastComma); + int prox0 = parseIntFromIndex(charSeq, read, lastComma); + color1.red = parseIntFromIndex(charSeq, read, lastComma); + color1.green = parseIntFromIndex(charSeq, read, lastComma); + color1.blue = parseIntFromIndex(charSeq, read, lastComma); + color1.ir = parseIntFromIndex(charSeq, read, lastComma); + int prox1 = parseIntFromIndex(charSeq, read, lastComma); + + double ts = Timer.getFPGATimestamp(); + + try { + threadLock.lock(); + this.lastReadTime = ts; + this.hasColor0 = hasColor0; + this.hasColor1 = hasColor1; + if (hasColor0) { + this.color0.red = color0.red; + this.color0.green = color0.green; + this.color0.blue = color0.blue; + this.color0.ir = color0.ir; + this.prox0 = prox0; + } + if (hasColor1) { + this.color1.red = color1.red; + this.color1.green = color1.green; + this.color1.blue = color1.blue; + this.color1.ir = color1.ir; + this.prox1 = prox1; + } + } finally { + threadLock.unlock(); + } + } + + SerialPortJNI.serialClose(port); + } + + public PicoColorSensor() { + readThread = new Thread(this::threadMain); + readThread.setName("PicoColorSensorThread"); + readThread.start(); + } + + public boolean isSensor0Connected() { + try { + threadLock.lock(); + return hasColor0; + } finally { + threadLock.unlock(); + } + } + + public boolean isSensor1Connected() { + try { + threadLock.lock(); + return hasColor1; + } finally { + threadLock.unlock(); + } + } + + public RawColor getRawColor0() { + try { + threadLock.lock(); + return new RawColor(color0.red, color0.green, color0.blue, color0.ir); + } finally { + threadLock.unlock(); + } + } + + public void getRawColor0(RawColor rawColor) { + try { + threadLock.lock(); + rawColor.red = color0.red; + rawColor.green = color0.green; + rawColor.blue = color0.blue; + rawColor.ir = color0.ir; + } finally { + threadLock.unlock(); + } + } + + public int getProximity0() { + try { + threadLock.lock(); + return prox0; + } finally { + threadLock.unlock(); + } + } + + public RawColor getRawColor1() { + try { + threadLock.lock(); + return new RawColor(color1.red, color1.green, color1.blue, color1.ir); + } finally { + threadLock.unlock(); + } + } + + public void getRawColor1(RawColor rawColor) { + try { + threadLock.lock(); + rawColor.red = color1.red; + rawColor.green = color1.green; + rawColor.blue = color1.blue; + rawColor.ir = color1.ir; + } finally { + threadLock.unlock(); + } + } + + public int getProximity1() { + try { + threadLock.lock(); + return prox1; + } finally { + threadLock.unlock(); + } + } + + public double getLastReadTimestampSeconds() { + try { + threadLock.lock(); + return lastReadTime; + } finally { + threadLock.unlock(); + } + } + + void setDebugPrints(boolean debug) { + debugPrints.set(debug); + } + + @Override + public void close() throws Exception { + threadRunning.set(false); + readThread.join(); + } +} From 2e841268d0a35ab52b3a52944481deb12e98169b Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Mon, 14 Mar 2022 16:54:18 -0700 Subject: [PATCH 002/108] small rename --- src/main/java/frc/subsystem/Hopper.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index ae57932b..fa27ed57 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -65,23 +65,23 @@ private Color convertRawColor(RawColor rawColor) { } public void checkBallColor() { - Color detectedColorTop = convertRawColor(colorSensor.getRawColor0()); + Color detectedColor = convertRawColor(colorSensor.getRawColor0()); //Color detectedColorBottom = bottomBall.getColor(); - logData("Ball Red", detectedColorTop.red); - logData("Ball Green", detectedColorTop.green); - logData("Ball Blue", detectedColorTop.blue); - logData("Color Confidence", colorMatcher.matchClosestColor(detectedColorTop).confidence); + logData("Ball Red", detectedColor.red); + logData("Ball Green", detectedColor.green); + logData("Ball Blue", detectedColor.blue); + logData("Color Confidence", colorMatcher.matchClosestColor(detectedColor).confidence); // logData("Bottom Red", detectedColorBottom.red); // logData("Bottom Green", detectedColorBottom.green); // logData("Bottom Blue", detectedColorBottom.blue); // logData("Bottom Confidence", colorMatcher.matchClosestColor(detectedColorBottom).confidence); - if (colorMatcher.matchClosestColor(detectedColorTop).color == redTarget) { + if (colorMatcher.matchClosestColor(detectedColor).color == redTarget) { logData("Ball Color", "Red"); ballDetected = true; - } else if (colorMatcher.matchClosestColor(detectedColorTop).color == blueTarget) { + } else if (colorMatcher.matchClosestColor(detectedColor).color == blueTarget) { logData("Ball Color", "Blue"); ballDetected = true; } else { From 4c99dc2f6fad66b0e23a1d435f23dbdc9d7ba7f4 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Mon, 14 Mar 2022 17:09:04 -0700 Subject: [PATCH 003/108] new: setup preliminary outtake class --- src/main/java/frc/robot/Constants.java | 3 ++ src/main/java/frc/subsystem/Hopper.java | 4 +- src/main/java/frc/subsystem/Outtake.java | 64 ++++++++++++++++++++++++ 3 files changed, 69 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/subsystem/Outtake.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cd76aa8a..4488fbce 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -222,6 +222,9 @@ public final class Constants { public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; + // Outtake Constants + public static final int OUTTAKE_PERIOD = 200; + // Shooter Constants diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index fa27ed57..44cd6806 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -15,7 +15,7 @@ import static frc.robot.Constants.HOPPER_CURRENT_LIMIT; public final class Hopper extends AbstractSubsystem { - public static @NotNull Hopper INSTANCE = new Hopper(); + private static @NotNull Hopper INSTANCE = new Hopper(); private final @NotNull LazyCANSparkMax hopperMotor; private final @NotNull PicoColorSensor colorSensor; @@ -122,7 +122,7 @@ public void setHopperState(HopperState hopperState) { } } - public boolean isBallDetected() { + public synchronized boolean isBallDetected() { return ballDetected; } diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java new file mode 100644 index 00000000..f6e96f91 --- /dev/null +++ b/src/main/java/frc/subsystem/Outtake.java @@ -0,0 +1,64 @@ +package frc.subsystem; + +import frc.robot.Constants; +import org.jetbrains.annotations.NotNull; + +public class Outtake extends AbstractSubsystem { + + private Hopper hopper = Hopper.getInstance(); + + private static @NotNull Outtake instance = new Outtake(); + + public static Outtake getInstance() { + return instance; + } + + /** + * Outtake can either be OFF or ON + */ + public enum OuttakeState { + OFF, + ON + } + + private OuttakeState outtakeState = OuttakeState.OFF; + + private Outtake() { + super(Constants.OUTTAKE_PERIOD); + } + + private void updateOuttakeState() { + if (hopper.isBallDetected()) { + outtakeState = OuttakeState.ON; + } else { + outtakeState = OuttakeState.OFF; + } + } + + @Override + public void update() { + updateOuttakeState(); + + switch (outtakeState) { + case OFF: + break; + case ON: + break; + } + } + + @Override + public void selfTest() { + + } + + @Override + public void logData() { + + } + + @Override + public void close() throws Exception { + + } +} From 638d810ae18ce1c0084b6ecc7c6f37f8252969ca Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sat, 19 Mar 2022 12:10:19 -0700 Subject: [PATCH 004/108] Setup Outtake --- src/main/java/frc/robot/Constants.java | 8 +++ src/main/java/frc/robot/Robot.java | 7 ++- src/main/java/frc/subsystem/Hopper.java | 18 ++++--- src/main/java/frc/subsystem/Outtake.java | 66 ++++++++++++++++++++++-- 4 files changed, 82 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ff4e4b5..b0d45ca9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -224,7 +224,15 @@ public final class Constants { // Outtake Constants public static final int OUTTAKE_PERIOD = 200; + public static final int OUTTAKE_CAN_ID = 60; + public static final double OUTTAKE_REDUCTION = (3 / 1d); //TODO FIND REAL VALUE + public static final int OUTTAKE_CURRENT_LIMIT = 30; + public static final int OUTTAKE_MEASUREMENT_PERIOD = 50; + /** + * Outtake will run for this time (MS) after color sensor trigger + */ + public static final double OUTTAKE_RUN_PERIOD = 50 / 1000d; // Shooter Constants diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cf885173..2a7a3d63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -134,11 +134,10 @@ public class Robot extends TimedRobot { private final SendableChooser autoChooser = new SendableChooser<>(); + @NotNull public static final String RED = "RED"; + @NotNull public static final String BLUE = "BLUE"; - @NotNull private static final String RED = "RED"; - @NotNull private static final String BLUE = "BLUE"; - - private final SendableChooser sideChooser = new SendableChooser<>(); + public static final SendableChooser sideChooser = new SendableChooser<>(); //Subsystems private final RobotTracker robotTracker = RobotTracker.getInstance(); diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 44cd6806..ec3592f0 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -3,7 +3,6 @@ import com.revrobotics.CANSparkMaxLowLevel; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.ColorMatch; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import frc.robot.Constants; import frc.utility.OrangeUtility; @@ -33,7 +32,9 @@ public final class Hopper extends AbstractSubsystem { private final Color blueTarget = new Color(0.184, 0.427, 0.39); private final Color redTarget = new Color(0.38, 0.41, 0.2); - private boolean ballDetected; + public enum ColorSensorStatus {RED, BLUE, NO_BALL} + + private ColorSensorStatus colorSensorStatus = ColorSensorStatus.NO_BALL; public static Hopper getInstance() { return INSTANCE; @@ -80,13 +81,13 @@ public void checkBallColor() { if (colorMatcher.matchClosestColor(detectedColor).color == redTarget) { logData("Ball Color", "Red"); - ballDetected = true; + colorSensorStatus = ColorSensorStatus.RED; } else if (colorMatcher.matchClosestColor(detectedColor).color == blueTarget) { logData("Ball Color", "Blue"); - ballDetected = true; + colorSensorStatus = ColorSensorStatus.BLUE; } else { logData("Ball Color", "No Ball Detected"); - ballDetected = false; + colorSensorStatus = ColorSensorStatus.NO_BALL; } // if (colorMatcher.matchClosestColor(detectedColorBottom).color == redTarget) { @@ -122,8 +123,8 @@ public void setHopperState(HopperState hopperState) { } } - public synchronized boolean isBallDetected() { - return ballDetected; + public synchronized ColorSensorStatus getColorSensorStatus() { + return colorSensorStatus; } @Override @@ -145,7 +146,8 @@ public void selfTest() { @Override public void logData() { - SmartDashboard.putNumber("Hopper Motor Current", hopperMotor.getOutputCurrent()); + logData("Hopper Motor Current", hopperMotor.getOutputCurrent()); + logData("Color Sensor Status", colorSensorStatus); } @Override diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java index f6e96f91..f30382ee 100644 --- a/src/main/java/frc/subsystem/Outtake.java +++ b/src/main/java/frc/subsystem/Outtake.java @@ -1,11 +1,28 @@ package frc.subsystem; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.RelativeEncoder; import frc.robot.Constants; +import frc.robot.Robot; +import frc.subsystem.Hopper.ColorSensorStatus; +import frc.subsystem.Intake.IntakeState; +import frc.utility.Timer; +import frc.utility.controllers.LazyCANSparkMax; import org.jetbrains.annotations.NotNull; public class Outtake extends AbstractSubsystem { private Hopper hopper = Hopper.getInstance(); + private Intake intake = Intake.getInstance(); + + private LazyCANSparkMax outtakeWheels; + private RelativeEncoder outtakeWheelsQuadrature; + + private double lastDetectionTime; + + private ColorSensorStatus allianceColor = ColorSensorStatus.RED; private static @NotNull Outtake instance = new Outtake(); @@ -18,18 +35,54 @@ public static Outtake getInstance() { */ public enum OuttakeState { OFF, - ON + INTAKE, + EJECT, } private OuttakeState outtakeState = OuttakeState.OFF; private Outtake() { super(Constants.OUTTAKE_PERIOD); + + updateAllianceColor(); + + outtakeWheels = new LazyCANSparkMax(Constants.OUTTAKE_CAN_ID, MotorType.kBrushless); + outtakeWheels.setIdleMode(IdleMode.kCoast); + outtakeWheels.setInverted(false); + outtakeWheels.setSmartCurrentLimit(Constants.OUTTAKE_CURRENT_LIMIT); + outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); + outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 200); + outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 25); + outtakeWheels.setControlFramePeriodMs(25); + + outtakeWheelsQuadrature = outtakeWheels.getEncoder(); + outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION); + outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD); + + outtakeWheels.burnFlash(); } + /** + * Uses Sendable Chooser to decide Alliance Color + */ + private void updateAllianceColor() { + if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) { + allianceColor = ColorSensorStatus.BLUE; + } else { + allianceColor = ColorSensorStatus.RED; + } + } + + /** + * Updates state of outtake based on color sensor and intake direction + */ private void updateOuttakeState() { - if (hopper.isBallDetected()) { - outtakeState = OuttakeState.ON; + // If color sensor detects a ball or + if (hopper.getColorSensorStatus() == allianceColor || Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.EJECT; + } else if (intake.wantedIntakeState == IntakeState.INTAKE) { + outtakeState = OuttakeState.INTAKE; } else { outtakeState = OuttakeState.OFF; } @@ -42,7 +95,9 @@ public void update() { switch (outtakeState) { case OFF: break; - case ON: + case EJECT: + break; + case INTAKE: break; } } @@ -54,7 +109,8 @@ public void selfTest() { @Override public void logData() { - + logData("Outtake State", outtakeState); + logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); } @Override From 4fe13bb93269ce5db642de56e43c367380f89f00 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sat, 19 Mar 2022 15:01:17 -0700 Subject: [PATCH 005/108] finished motor control for outtake --- src/main/java/frc/robot/Constants.java | 6 +++++- src/main/java/frc/robot/Robot.java | 6 ++++++ src/main/java/frc/subsystem/Outtake.java | 24 +++++++++++++++++++++++- 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b0d45ca9..1b371452 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -227,7 +227,11 @@ public final class Constants { public static final int OUTTAKE_CAN_ID = 60; public static final double OUTTAKE_REDUCTION = (3 / 1d); //TODO FIND REAL VALUE public static final int OUTTAKE_CURRENT_LIMIT = 30; - public static final int OUTTAKE_MEASUREMENT_PERIOD = 50; + public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 50; + /** + * Maximum Percent Output of Outtake wheels + */ + public static final double OUTTAKE_SPEED_FACTOR = 1d; /** * Outtake will run for this time (MS) after color sensor trigger diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2a7a3d63..84e243b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -149,6 +149,7 @@ public class Robot extends TimedRobot { private final Shooter shooter = Shooter.getInstance(); private final Climber climber = Climber.getInstance(); private final VisionManager visionManager = VisionManager.getInstance(); + private final Outtake outtake = Outtake.getInstance(); //Inputs private final Controller xbox = new Controller(0); @@ -734,6 +735,10 @@ public void testPeriodic() { if (buttonPanel.getRisingEdge(8)) { climber.selfTest(); } + + if (buttonPanel.getRisingEdge(9)) { + outtake.selfTest(); + } } private void startSubsystems() { @@ -745,6 +750,7 @@ private void startSubsystems() { shooter.start(); climber.start(); visionManager.start(); + outtake.start(); } public void killAuto() { diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java index f30382ee..637d8e3b 100644 --- a/src/main/java/frc/subsystem/Outtake.java +++ b/src/main/java/frc/subsystem/Outtake.java @@ -8,6 +8,7 @@ import frc.robot.Robot; import frc.subsystem.Hopper.ColorSensorStatus; import frc.subsystem.Intake.IntakeState; +import frc.utility.OrangeUtility; import frc.utility.Timer; import frc.utility.controllers.LazyCANSparkMax; import org.jetbrains.annotations.NotNull; @@ -57,7 +58,7 @@ private Outtake() { outtakeWheelsQuadrature = outtakeWheels.getEncoder(); outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION); - outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD); + outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD_MS); outtakeWheels.burnFlash(); } @@ -88,16 +89,26 @@ private void updateOuttakeState() { } } + /** + * Sets the percent outtake between 1 and -1 + */ + private void setOuttakePercentOutput(double percentOutput) { + outtakeWheels.setVoltage(12d * percentOutput); + } + @Override public void update() { updateOuttakeState(); switch (outtakeState) { case OFF: + setOuttakePercentOutput(0); break; case EJECT: + setOuttakePercentOutput(1 * Constants.OUTTAKE_SPEED_FACTOR); break; case INTAKE: + setOuttakePercentOutput(-1 * Constants.OUTTAKE_SPEED_FACTOR); break; } } @@ -105,12 +116,23 @@ public void update() { @Override public void selfTest() { + setOuttakePercentOutput(1 * Constants.OUTTAKE_SPEED_FACTOR); + System.out.println("Ejecting"); + OrangeUtility.sleep(Constants.TEST_TIME_MS); + + setOuttakePercentOutput(-1 * Constants.OUTTAKE_SPEED_FACTOR); + System.out.println("Intaking"); + OrangeUtility.sleep(Constants.TEST_TIME_MS); + + System.out.println("Test Finished"); + setOuttakePercentOutput(0); } @Override public void logData() { logData("Outtake State", outtakeState); logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); + logData("Outtake SetVoltage", outtakeWheels.getSetVoltage()); } @Override From fdc08693cf215b600b6b54d4ff100adfd2d8c7ac Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sat, 19 Mar 2022 20:18:05 -0700 Subject: [PATCH 006/108] fix: update alliance color --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/subsystem/Outtake.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 84e243b9..a36bd97a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -262,7 +262,6 @@ public void robotInit() { SmartDashboard.putData("Auto choices", autoChooser); SmartDashboard.putData("Red or Blue", sideChooser); - startSubsystems(); robotTracker.resetGyro(); OrangeUtility.sleep(50); robotTracker.resetPosition(new Pose2d()); @@ -276,6 +275,7 @@ public void robotInit() { NetworkTableInstance.getDefault().setUpdateRate(0.05); Limelight.getInstance().setStreamingMode(StreamingMode.PIP_SECONDARY); + startSubsystems(); limelight.setLedMode(LedMode.OFF); // shooter.homeHood(); // shooter.setHoodPositionMode(HoodPositionMode.RELATIVE_TO_HOME); diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java index 637d8e3b..c10fd290 100644 --- a/src/main/java/frc/subsystem/Outtake.java +++ b/src/main/java/frc/subsystem/Outtake.java @@ -45,8 +45,6 @@ public enum OuttakeState { private Outtake() { super(Constants.OUTTAKE_PERIOD); - updateAllianceColor(); - outtakeWheels = new LazyCANSparkMax(Constants.OUTTAKE_CAN_ID, MotorType.kBrushless); outtakeWheels.setIdleMode(IdleMode.kCoast); outtakeWheels.setInverted(false); @@ -98,6 +96,7 @@ private void setOuttakePercentOutput(double percentOutput) { @Override public void update() { + updateAllianceColor(); updateOuttakeState(); switch (outtakeState) { From 72130a387d4cb7f492680863edce76029c607f38 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 20 Mar 2022 13:33:43 -0700 Subject: [PATCH 007/108] Outtake fixes --- src/main/java/frc/robot/Constants.java | 9 ++++---- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/subsystem/Hopper.java | 7 +++++- src/main/java/frc/subsystem/Outtake.java | 22 +++++++++++++------ .../utility/colorsensor/PicoColorSensor.java | 2 +- 5 files changed, 28 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1b371452..6b54facd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -217,17 +217,18 @@ public final class Constants { //Hopper Constants - public static final int HOPPER_PERIOD = 200; + public static final int HOPPER_PERIOD = 50; public static final double HOPPER_SPEED = 1; public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; // Outtake Constants - public static final int OUTTAKE_PERIOD = 200; + public static final int OUTTAKE_PERIOD = 50; public static final int OUTTAKE_CAN_ID = 60; - public static final double OUTTAKE_REDUCTION = (3 / 1d); //TODO FIND REAL VALUE - public static final int OUTTAKE_CURRENT_LIMIT = 30; + public static final double OUTTAKE_REDUCTION = (3.0 / 1.0); + public static final int OUTTAKE_CURRENT_LIMIT = 20; public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 50; + public static final boolean OUTTAKE_DEBUG_PRINTS = false; /** * Maximum Percent Output of Outtake wheels */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a36bd97a..2f8d3a1c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -736,7 +736,7 @@ public void testPeriodic() { climber.selfTest(); } - if (buttonPanel.getRisingEdge(9)) { + if (buttonPanel.getRisingEdge(10)) { outtake.selfTest(); } } diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index ec3592f0..a86990a3 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -47,7 +47,7 @@ public enum HopperState { HopperState wantedHopperState = HopperState.OFF; private Hopper() { - super(Constants.HOPPER_PERIOD, 5); + super(Constants.HOPPER_PERIOD, 1); hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); hopperMotor.setSmartCurrentLimit(HOPPER_CURRENT_LIMIT); @@ -58,7 +58,9 @@ private Hopper() { colorMatcher.addColorMatch(blueTarget); colorMatcher.addColorMatch(redTarget); + colorSensor = new PicoColorSensor(); + colorSensor.setDebugPrints(Constants.OUTTAKE_DEBUG_PRINTS); } private Color convertRawColor(RawColor rawColor) { @@ -148,6 +150,9 @@ public void selfTest() { public void logData() { logData("Hopper Motor Current", hopperMotor.getOutputCurrent()); logData("Color Sensor Status", colorSensorStatus); + logData("Is Sensor 1 Connected", colorSensor.isSensor0Connected()); + logData("Is Sensor 2 Connected", colorSensor.isSensor1Connected()); + logData("Color Sensor Last Read Time", colorSensor.getLastReadTimestampSeconds()); } @Override diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java index c10fd290..aa08f895 100644 --- a/src/main/java/frc/subsystem/Outtake.java +++ b/src/main/java/frc/subsystem/Outtake.java @@ -7,6 +7,7 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.subsystem.Hopper.ColorSensorStatus; +import frc.subsystem.Intake.IntakeSolState; import frc.subsystem.Intake.IntakeState; import frc.utility.OrangeUtility; import frc.utility.Timer; @@ -23,7 +24,9 @@ public class Outtake extends AbstractSubsystem { private double lastDetectionTime; - private ColorSensorStatus allianceColor = ColorSensorStatus.RED; + private @NotNull ColorSensorStatus allianceColor = ColorSensorStatus.RED; + + private @NotNull ColorSensorStatus opposingAllianceColor = ColorSensorStatus.BLUE; private static @NotNull Outtake instance = new Outtake(); @@ -53,6 +56,7 @@ private Outtake() { outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 200); outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 25); outtakeWheels.setControlFramePeriodMs(25); + outtakeWheels.enableVoltageCompensation(9); outtakeWheelsQuadrature = outtakeWheels.getEncoder(); outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION); @@ -67,8 +71,10 @@ private Outtake() { private void updateAllianceColor() { if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) { allianceColor = ColorSensorStatus.BLUE; + opposingAllianceColor = ColorSensorStatus.RED; } else { allianceColor = ColorSensorStatus.RED; + opposingAllianceColor = ColorSensorStatus.BLUE; } } @@ -77,9 +83,11 @@ private void updateAllianceColor() { */ private void updateOuttakeState() { // If color sensor detects a ball or - if (hopper.getColorSensorStatus() == allianceColor || Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { + if (hopper.getColorSensorStatus() == opposingAllianceColor && intake.getIntakeSolState() == IntakeSolState.OPEN) { lastDetectionTime = Timer.getFPGATimestamp(); outtakeState = OuttakeState.EJECT; + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD && intake.getIntakeSolState() == IntakeSolState.OPEN) { + outtakeState = OuttakeState.EJECT; } else if (intake.wantedIntakeState == IntakeState.INTAKE) { outtakeState = OuttakeState.INTAKE; } else { @@ -91,7 +99,7 @@ private void updateOuttakeState() { * Sets the percent outtake between 1 and -1 */ private void setOuttakePercentOutput(double percentOutput) { - outtakeWheels.setVoltage(12d * percentOutput); + outtakeWheels.setVoltage(9.0 * percentOutput); } @Override @@ -104,10 +112,10 @@ public void update() { setOuttakePercentOutput(0); break; case EJECT: - setOuttakePercentOutput(1 * Constants.OUTTAKE_SPEED_FACTOR); + setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); break; case INTAKE: - setOuttakePercentOutput(-1 * Constants.OUTTAKE_SPEED_FACTOR); + setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); break; } } @@ -115,11 +123,11 @@ public void update() { @Override public void selfTest() { - setOuttakePercentOutput(1 * Constants.OUTTAKE_SPEED_FACTOR); + setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); System.out.println("Ejecting"); OrangeUtility.sleep(Constants.TEST_TIME_MS); - setOuttakePercentOutput(-1 * Constants.OUTTAKE_SPEED_FACTOR); + setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); System.out.println("Intaking"); OrangeUtility.sleep(Constants.TEST_TIME_MS); diff --git a/src/main/java/frc/utility/colorsensor/PicoColorSensor.java b/src/main/java/frc/utility/colorsensor/PicoColorSensor.java index c41129db..8dfcd425 100644 --- a/src/main/java/frc/utility/colorsensor/PicoColorSensor.java +++ b/src/main/java/frc/utility/colorsensor/PicoColorSensor.java @@ -274,7 +274,7 @@ public double getLastReadTimestampSeconds() { } } - void setDebugPrints(boolean debug) { + public void setDebugPrints(boolean debug) { debugPrints.set(debug); } From 8c9e217a8c28cb3861070bfdc0ca92a4ed0e896b Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 20 Mar 2022 14:28:54 -0700 Subject: [PATCH 008/108] Revert "Merge branch 'Logger'" This reverts commit 40f759d6c3b74e5c6c0129ada25710dc7180ad5f, reversing changes made to bf222af04f59f012d6b2ed32723c9c541b4a4653. --- src/main/java/frc/robot/Constants.java | 13 +- .../java/frc/subsystem/AbstractSubsystem.java | 52 ++++- .../java/frc/subsystem/DashboardHandler.java | 207 ------------------ src/main/java/frc/subsystem/Hopper.java | 2 +- src/main/java/frc/utility/Serializer.java | 16 +- .../frc/utility/net/DashboardConnection.java | 27 --- .../java/frc/utility/net/PacketHandler.java | 9 - .../java/frc/utility/net/SendableLog.java | 16 -- 8 files changed, 54 insertions(+), 288 deletions(-) delete mode 100644 src/main/java/frc/subsystem/DashboardHandler.java delete mode 100644 src/main/java/frc/utility/net/DashboardConnection.java delete mode 100644 src/main/java/frc/utility/net/PacketHandler.java delete mode 100644 src/main/java/frc/utility/net/SendableLog.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5285693..6b54facd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -190,6 +190,10 @@ public final class Constants { */ public static final double GOAL_RADIUS = Units.inchesToMeters(GOAL_RADIUS_IN); + /** + * This GOAL RADIUS is used to calculate the turn error + */ + public static final double GOAL_RADIUS_TURN_ERROR_M = GOAL_RADIUS * 3; /** * Maximum speed of the robot when shooting the ball. (Only applies when doing the static shot) Units are in Meters per Second @@ -197,11 +201,6 @@ public final class Constants { */ public static final double MAX_SHOOT_SPEED_SQUARED = Math.pow(MAX_SHOOT_SPEED, 2); - /** - * This GOAL RADIUS is used to calculate the turn error - */ - public static final double GOAL_RADIUS_TURN_ERROR_M = GOAL_RADIUS * 3; - public static final double GRAVITY = 9.80665; /** @@ -280,7 +279,6 @@ public final class Constants { public static final double FEEDER_WHEEL_LOCK_SPEED_RPM = 1000; - // Feeder wheel pidf is unused public static final double FEEDER_WHEEL_P = 0.0002; public static final double FEEDER_WHEEL_I = 0; @@ -498,7 +496,4 @@ public final class Constants { public static final int INTAKE_MOTOR_DEVICE_ID = 40; public static final double INTAKE_MOTOR_SPEED = -1; public static final double INTAKE_OPEN_TIME = 0.0; - - public static final int WEB_DASHBOARD_PORT = 5802; - public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 200; } diff --git a/src/main/java/frc/subsystem/AbstractSubsystem.java b/src/main/java/frc/subsystem/AbstractSubsystem.java index 76c1ede2..a4bea667 100644 --- a/src/main/java/frc/subsystem/AbstractSubsystem.java +++ b/src/main/java/frc/subsystem/AbstractSubsystem.java @@ -1,10 +1,14 @@ package frc.subsystem; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; +import java.util.HashMap; +import java.util.Map; + @SuppressWarnings("unused") public abstract class AbstractSubsystem implements Runnable, AutoCloseable { private final int period; @@ -35,14 +39,6 @@ public AbstractSubsystem(int period) { public abstract void logData(); - public void logData(@NotNull String key, @NotNull Object value) { - DashboardHandler.getInstance().log(key, value); - } - - public void logData(@NotNull String key, @NotNull Object value, boolean logToNetworkTables) { - DashboardHandler.getInstance().log(key, value, logToNetworkTables); - } - public void pause() { signal = ThreadSignal.PAUSED; } @@ -66,8 +62,40 @@ public void start() { public void update() { } + + private final Map logDataMap = new HashMap<>(); + + + public void logData(@NotNull String key, @NotNull Object value) { + //SmartDashboard.putString(key, value.toString()); + logDataMap.put(key, value); + } + int lastLength = 20; + + public void pushLog() { + for (Map.Entry entry : logDataMap.entrySet()) { + Object obj = entry.getValue(); + Class cl = obj.getClass(); + + //@formatter:off + if (cl.equals(Integer.class)) SmartDashboard.putNumber(entry.getKey(), (int) obj); + else if (cl.equals(Double.class)) SmartDashboard.putNumber(entry.getKey(), (double) obj); + else if (cl.equals(Short.class)) SmartDashboard.putNumber(entry.getKey(), (short) obj); + else if (cl.equals(Long.class)) SmartDashboard.putNumber(entry.getKey(), (long) obj); + else if (cl.equals(Float.class)) SmartDashboard.putNumber(entry.getKey(), (float) obj); + else if (cl.equals(Byte.class)) SmartDashboard.putNumber(entry.getKey(), (byte) obj); + else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(entry.getKey(), (boolean) obj); + else if (cl.equals(String.class)) SmartDashboard.putString(entry.getKey(), (String) obj); + else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(entry.getKey(), (Double[]) obj); + else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(entry.getKey(), (Boolean[]) obj); + else if (cl.equals(String[].class)) SmartDashboard.putStringArray(entry.getKey(), (String[]) obj); + else SmartDashboard.putString(entry.getKey(), entry.getValue().toString()); + //@formatter:on + } + } + @Override @SuppressWarnings("BusyWait") public void run() { @@ -75,11 +103,13 @@ public void run() { double startTime = Timer.getFPGATimestamp(); if (signal == ThreadSignal.ALIVE) { update(); - logInterval++; - if (logInterval > loggingInterval) { + if (logInterval >= loggingInterval) { + logData(); - logInterval = 0; + pushLog(); + + logInterval = 1; } } double executionTimeMS = (Timer.getFPGATimestamp() - startTime) * 1000; diff --git a/src/main/java/frc/subsystem/DashboardHandler.java b/src/main/java/frc/subsystem/DashboardHandler.java deleted file mode 100644 index e43c00d6..00000000 --- a/src/main/java/frc/subsystem/DashboardHandler.java +++ /dev/null @@ -1,207 +0,0 @@ -package frc.subsystem; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; -import frc.utility.Serializer; -import frc.utility.Timer; -import frc.utility.net.DashboardConnection; -import frc.utility.net.PacketHandler; -import frc.utility.net.SendableLog; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -import java.io.IOException; -import java.net.*; -import java.nio.charset.StandardCharsets; -import java.util.HashMap; -import java.util.Iterator; -import java.util.Map; -import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.locks.ReadWriteLock; -import java.util.concurrent.locks.ReentrantReadWriteLock; - -import static frc.robot.Constants.WEB_DASHBOARD_PORT; - -public final class DashboardHandler extends AbstractSubsystem { - - private @Nullable DatagramSocket receivingSocket; - private double nextAllowedErrorTime = 0; - - /** - * HashMap of IntetAdress to DashboardConnection - */ - private final HashMap dashboardConnections = new HashMap<>(); - private static final Map LOG_DATA_MAP = new ConcurrentHashMap<>(); - private static final ReadWriteLock LOG_DATA_MAP_LOCK = new ReentrantReadWriteLock(); - private final HashMap packetHandlerMap = new HashMap<>(); - - { - packetHandlerMap.put('k', packet -> { - System.out.println("Received k packet from " + packet.getAddress()); - InetAddress address = packet.getAddress(); - if (dashboardConnections.containsKey(address)) { - synchronized (dashboardConnections) { - dashboardConnections.get(address).keepAlive(); // Update the keepalive time - } - } else { - // This dashboard hasn't been connected too yet. We should add it to the list of connected dashboards - try { - DatagramSocket socket = new DatagramSocket(); - socket.connect(address, WEB_DASHBOARD_PORT); - synchronized (dashboardConnections) { - dashboardConnections.put(address, new DashboardConnection(socket)); - } - } catch (SocketException e) { - DriverStation.reportError("Failed to create socket for " + address, false); - } - } - }); - } - - private static final @NotNull DashboardHandler INSTANCE = new DashboardHandler(Constants.WEB_DASHBOARD_SEND_PERIOD_MS); - - public static @NotNull DashboardHandler getInstance() { - return INSTANCE; - } - - - private DashboardHandler(int period) { - super(period); - - try { - receivingSocket = new DatagramSocket(WEB_DASHBOARD_PORT); - receivingSocket.setSoTimeout(5); - } catch (SocketException e) { - DriverStation.reportError("Could not create socket for listening for data from web dashboard", false); - } - } - - public void log(@NotNull String key, @NotNull Object value) { - log(key, value, false); - } - - public void log(@NotNull String key, @NotNull Object value, boolean logToNetworkTables) { - if (logToNetworkTables) { - //@formatter:off - Class cl = value.getClass(); - if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); - else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); - else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); - else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); - else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); - else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); - else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); - else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); - else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); - else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); - else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); - else SmartDashboard.putString(key, value.toString()); - //@formatter:on - } - LOG_DATA_MAP_LOCK.readLock().lock(); - // We're adding a read lock here because there can only be one writer, but there can be many readers. The - // ConcurrentHashMap is thread-safe and will handle the synchronization with multiple writes for us. We then only put a - // write lock when reading it to prevent any other writes from happening while we're serializing. - try { - LOG_DATA_MAP.put(key, value.toString()); - } finally { - LOG_DATA_MAP_LOCK.readLock().unlock(); - } - } - - public void pushLog() { - try { - String json; - LOG_DATA_MAP_LOCK.writeLock().lock(); // Ensure that no other writes happen while we're serializing - try { - SendableLog log = new SendableLog(LOG_DATA_MAP); - json = 'd' + Serializer.serializeToString(log); - } finally { - LOG_DATA_MAP_LOCK.writeLock().unlock(); - } - - synchronized (dashboardConnections) { - Iterator iterator = dashboardConnections.values().iterator(); - while (iterator.hasNext()) { - DashboardConnection entry = iterator.next(); - if (Timer.getFPGATimestamp() > entry.timeoutTime) { - // If we haven't received a keepalive in a while, remove the connection - entry.close(); - iterator.remove(); - System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " timed out"); - } else { - byte[] bytes = json.getBytes(StandardCharsets.ISO_8859_1); - try { - DatagramPacket packet = new DatagramPacket(bytes, bytes.length); - - entry.datagramSocket.send(packet); - System.out.println( - "sending packet of " + packet.getLength() + " length to " + entry.datagramSocket.getInetAddress()); - } catch (PortUnreachableException e) { - System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " got an error"); - entry.close(); - iterator.remove(); // Remove the connection if it's unreachable - } - } - } - } - } catch (IOException e) { - if (Timer.getFPGATimestamp() > nextAllowedErrorTime) { // Don't spam the driver station - DriverStation.reportError("Could not send data to web dashboard", false); - nextAllowedErrorTime = Timer.getFPGATimestamp() + 5; - } - } - } - - byte[] receivedBytes = new byte[65535]; - - private void handleDashboardPackets() { - if (receivingSocket == null) return; - - for (int i = 0; i < 100; i++) { // Receive up to 100 packets at a time - try { - DatagramPacket receivedPacket = new DatagramPacket(receivedBytes, receivedBytes.length); - try { - receivingSocket.receive(receivedPacket); // Will block for up to 5ms - } catch (SocketTimeoutException ignored) { // We don't care about timeouts - return; - } - char packetType = (char) receivedPacket.getData()[0]; - - if (packetHandlerMap.containsKey(packetType)) { // Find a handler for this packet type and call it - packetHandlerMap.get(packetType).handlePacket(receivedPacket); - } else { - DriverStation.reportWarning("Received packet with unknown packet type: " + packetType, false); - } - } catch (IOException e) { - if (Timer.getFPGATimestamp() > nextAllowedErrorTime) { // Only report errors once per 5 second - DriverStation.reportError("Encountered an error while receiving data from the web dashboard", false); - nextAllowedErrorTime = Timer.getFPGATimestamp() + 5; - } - return; - } - } - } - - @Override - public void update() { - handleDashboardPackets(); - pushLog(); - } - - @Override - public void selfTest() { - - } - - @Override - public void logData() { - - } - - @Override - public void close() throws Exception { - - } -} diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 9ad9a1f4..a86990a3 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -50,7 +50,7 @@ private Hopper() { super(Constants.HOPPER_PERIOD, 1); hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); hopperMotor.setSmartCurrentLimit(HOPPER_CURRENT_LIMIT); - + hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 500); hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 500); diff --git a/src/main/java/frc/utility/Serializer.java b/src/main/java/frc/utility/Serializer.java index c4e38cfd..d8e420ee 100644 --- a/src/main/java/frc/utility/Serializer.java +++ b/src/main/java/frc/utility/Serializer.java @@ -16,19 +16,19 @@ public static String serializeToString(Object obj) throws com.fasterxml.jackson. return OBJECT_MAPPER.writeValueAsString(obj); } - public static void serializeToFile(Object obj, File file) - throws IOException, com.fasterxml.jackson.core.JsonGenerationException, com.fasterxml.jackson.databind.JsonMappingException { + public static void serializeToFile(Object obj, + File file) throws IOException, com.fasterxml.jackson.core.JsonGenerationException, com.fasterxml.jackson.databind.JsonMappingException { OBJECT_MAPPER.configure(SerializationFeature.INDENT_OUTPUT, true); OBJECT_MAPPER.writeValue(file, obj); } - public static Autonomous deserializeAutoFromFile(File file) - throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { + public static Autonomous deserializeAutoFromFile( + File file) throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { return OBJECT_MAPPER.readValue(file, Autonomous.class); } - public static Object deserialize(String object, Class serializableObject) - throws com.fasterxml.jackson.core.JsonProcessingException, com.fasterxml.jackson.databind.JsonMappingException { + public static Object deserialize(String object, + Class serializableObject) throws com.fasterxml.jackson.core.JsonProcessingException, com.fasterxml.jackson.databind.JsonMappingException { return OBJECT_MAPPER.readValue(object, serializableObject); } @@ -37,8 +37,8 @@ public static Autonomous deserializeAuto( return OBJECT_MAPPER.readValue(object, Autonomous.class); } - public static Object deserializeFromFile(File file, Class serializableObject) - throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { + public static Object deserializeFromFile(File file, + Class serializableObject) throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { return OBJECT_MAPPER.readValue(file, serializableObject); } } \ No newline at end of file diff --git a/src/main/java/frc/utility/net/DashboardConnection.java b/src/main/java/frc/utility/net/DashboardConnection.java deleted file mode 100644 index b55ee2dc..00000000 --- a/src/main/java/frc/utility/net/DashboardConnection.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.utility.net; - -import java.io.Closeable; -import java.net.DatagramSocket; - -public class DashboardConnection implements Closeable { - public final DatagramSocket datagramSocket; - public long timeoutTime; - - public DashboardConnection(DatagramSocket datagramSocket, long timeoutTime) { - this.datagramSocket = datagramSocket; - this.timeoutTime = timeoutTime; - } - - public DashboardConnection(DatagramSocket datagramSocket) { - this(datagramSocket, System.currentTimeMillis() + 10000); - } - - public void keepAlive() { - timeoutTime = System.currentTimeMillis() + 10000; - } - - @Override - public void close() { - datagramSocket.close(); - } -} diff --git a/src/main/java/frc/utility/net/PacketHandler.java b/src/main/java/frc/utility/net/PacketHandler.java deleted file mode 100644 index 6058ddc4..00000000 --- a/src/main/java/frc/utility/net/PacketHandler.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.utility.net; - -import java.io.IOException; -import java.net.DatagramPacket; - - -public interface PacketHandler { - void handlePacket(DatagramPacket packet) throws IOException; -} diff --git a/src/main/java/frc/utility/net/SendableLog.java b/src/main/java/frc/utility/net/SendableLog.java deleted file mode 100644 index 265b3840..00000000 --- a/src/main/java/frc/utility/net/SendableLog.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.utility.net; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonProperty; - -import java.util.Map; - -public class SendableLog { - @JsonProperty - public Map log; - - @JsonCreator - public SendableLog(@JsonProperty Map log) { - this.log = log; - } -} From 4a987f97b59c7e3557cef4b3f09119b58eb0b7e3 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 20 Mar 2022 21:08:53 -0700 Subject: [PATCH 009/108] Many outtake fixes --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/subsystem/Hopper.java | 103 ++++++++++++++--------- src/main/java/frc/subsystem/Intake.java | 1 + src/main/java/frc/subsystem/Outtake.java | 49 +++++++---- 4 files changed, 99 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6b54facd..1660b1f5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -218,7 +218,7 @@ public final class Constants { //Hopper Constants public static final int HOPPER_PERIOD = 50; - public static final double HOPPER_SPEED = 1; + public static final double HOPPER_SPEED = .5; public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; @@ -227,7 +227,7 @@ public final class Constants { public static final int OUTTAKE_CAN_ID = 60; public static final double OUTTAKE_REDUCTION = (3.0 / 1.0); public static final int OUTTAKE_CURRENT_LIMIT = 20; - public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 50; + public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 200; public static final boolean OUTTAKE_DEBUG_PRINTS = false; /** * Maximum Percent Output of Outtake wheels diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index a86990a3..bfa517c1 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -3,17 +3,18 @@ import com.revrobotics.CANSparkMaxLowLevel; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.ColorMatch; -import edu.wpi.first.wpilibj.util.Color; import frc.robot.Constants; +import frc.subsystem.Outtake.OuttakeState; import frc.utility.OrangeUtility; import frc.utility.colorsensor.PicoColorSensor; -import frc.utility.colorsensor.PicoColorSensor.RawColor; import frc.utility.controllers.LazyCANSparkMax; import org.jetbrains.annotations.NotNull; import static frc.robot.Constants.HOPPER_CURRENT_LIMIT; public final class Hopper extends AbstractSubsystem { + + private Outtake outtake = Outtake.getInstance(); private static @NotNull Hopper INSTANCE = new Hopper(); private final @NotNull LazyCANSparkMax hopperMotor; private final @NotNull PicoColorSensor colorSensor; @@ -24,13 +25,14 @@ public final class Hopper extends AbstractSubsystem { *

* This object uses a simple euclidian distance to estimate the closest match with given confidence range. */ - private final ColorMatch colorMatcher = new ColorMatch(); /** * Note: Any example colors should be calibrated as the user needs, these are here as a basic example. */ - private final Color blueTarget = new Color(0.184, 0.427, 0.39); - private final Color redTarget = new Color(0.38, 0.41, 0.2); + /*private final Color blueTarget = new Color(101, 246, 255); + private final Color redTarget = new Color(255, 75, 194); */ + + private final ColorMatch colorMatch = new ColorMatch(); public enum ColorSensorStatus {RED, BLUE, NO_BALL} @@ -47,7 +49,7 @@ public enum HopperState { HopperState wantedHopperState = HopperState.OFF; private Hopper() { - super(Constants.HOPPER_PERIOD, 1); + super(10, 1); hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); hopperMotor.setSmartCurrentLimit(HOPPER_CURRENT_LIMIT); @@ -56,55 +58,76 @@ private Hopper() { hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 500); hopperMotor.setControlFramePeriodMs(25); - colorMatcher.addColorMatch(blueTarget); - colorMatcher.addColorMatch(redTarget); - colorSensor = new PicoColorSensor(); colorSensor.setDebugPrints(Constants.OUTTAKE_DEBUG_PRINTS); - } - private Color convertRawColor(RawColor rawColor) { - return new Color(rawColor.red, rawColor.green, rawColor.blue); + /*colorMatch.addColorMatch(blueTarget); + colorMatch.addColorMatch(redTarget); */ } public void checkBallColor() { - Color detectedColor = convertRawColor(colorSensor.getRawColor0()); - //Color detectedColorBottom = bottomBall.getColor(); - logData("Ball Red", detectedColor.red); - logData("Ball Green", detectedColor.green); - logData("Ball Blue", detectedColor.blue); - logData("Color Confidence", colorMatcher.matchClosestColor(detectedColor).confidence); + double red = colorSensor.getRawColor0().red / 309d; + double green = colorSensor.getRawColor0().green; + double blue = colorSensor.getRawColor0().blue / 567d; + double proximity = colorSensor.getProximity0(); -// logData("Bottom Red", detectedColorBottom.red); -// logData("Bottom Green", detectedColorBottom.green); -// logData("Bottom Blue", detectedColorBottom.blue); -// logData("Bottom Confidence", colorMatcher.matchClosestColor(detectedColorBottom).confidence); - - if (colorMatcher.matchClosestColor(detectedColor).color == redTarget) { + // If color sensor is not connected, defaults to no ball + if (proximity < 70 || !colorSensor.isSensor0Connected()) { + logData("Ball Color", "No Ball Detected"); + colorSensorStatus = ColorSensorStatus.NO_BALL; + } else if (red > blue) { logData("Ball Color", "Red"); colorSensorStatus = ColorSensorStatus.RED; - } else if (colorMatcher.matchClosestColor(detectedColor).color == blueTarget) { + } else { logData("Ball Color", "Blue"); colorSensorStatus = ColorSensorStatus.BLUE; + } + + /*double red = colorSensor.getRawColor0().red; + double green = colorSensor.getRawColor0().green; + double blue = colorSensor.getRawColor0().blue; + double proximity = colorSensor.getProximity0(); + + double max = red; + if (green > max) max = green; + if (blue > max) max = blue; + + red = (255 * red) / max; + green = (255 * green) / max; + blue = (255 * blue) / max; */ + + logData("Red", red); + logData("Green", green); + logData("Blue", blue); + logData("Color Sensor Proximity", colorSensor.getProximity0()); + + logData("Raw Red", colorSensor.getRawColor0().red); + logData("Raw Blue", colorSensor.getRawColor0().blue); + + /*Color color = new Color(red, green, blue); + if (proximity > 70) { + if (colorMatch.matchClosestColor(color).color == redTarget) { + logData("Ball Color", "Red"); + colorSensorStatus = ColorSensorStatus.RED; + } else if (colorMatch.matchClosestColor(color).color == blueTarget) { + logData("Ball Color", "Blue"); + colorSensorStatus = ColorSensorStatus.BLUE; + } else { + logData("Bottom Ball Color", "No Ball Detected"); + } } else { logData("Ball Color", "No Ball Detected"); colorSensorStatus = ColorSensorStatus.NO_BALL; - } - -// if (colorMatcher.matchClosestColor(detectedColorBottom).color == redTarget) { -// logData("Bottom Ball Color", "Red"); -// bottomBallDetected = true; -// } else if (colorMatcher.matchClosestColor(detectedColorBottom).color == blueTarget) { -// logData("Bottom Ball Color", "Blue"); -// bottomBallDetected = true; -// } else { -// logData("Bottom Ball Color", "No Ball Detected"); -// bottomBallDetected = false; -// } + } */ } - public void setHopperState(HopperState hopperState) { + private void setHopperStatePrivate(HopperState hopperState) { + // Forces Hopper to run in reverse if outtake is ejecting + if (outtake.getOuttakeState() == OuttakeState.EJECT) { + hopperState = HopperState.OFF; + } + switch (hopperState) { case ON: hopperMotor.set(Constants.HOPPER_SPEED); @@ -131,11 +154,11 @@ public synchronized ColorSensorStatus getColorSensorStatus() { @Override public void update() { - setHopperState(wantedHopperState); + setHopperStatePrivate(wantedHopperState); checkBallColor(); } - public void setWantedHopperState(HopperState hopperState) { + public void setHopperState(HopperState hopperState) { wantedHopperState = hopperState; } diff --git a/src/main/java/frc/subsystem/Intake.java b/src/main/java/frc/subsystem/Intake.java index cc91e51b..11667468 100644 --- a/src/main/java/frc/subsystem/Intake.java +++ b/src/main/java/frc/subsystem/Intake.java @@ -50,6 +50,7 @@ public void logData() { SmartDashboard.putNumber("Intake Motor Speed: ", intakeMotor.get()); SmartDashboard.putNumber("Intake Current", intakeMotor.getOutputCurrent()); SmartDashboard.putBoolean("Intake Solenoid State: ", intakeSol.get()); + logData("Wanted Intake State", wantedIntakeState); } diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java index aa08f895..4ad80806 100644 --- a/src/main/java/frc/subsystem/Outtake.java +++ b/src/main/java/frc/subsystem/Outtake.java @@ -4,6 +4,8 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.RelativeEncoder; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.Constants; import frc.robot.Robot; import frc.subsystem.Hopper.ColorSensorStatus; @@ -16,14 +18,15 @@ public class Outtake extends AbstractSubsystem { - private Hopper hopper = Hopper.getInstance(); - private Intake intake = Intake.getInstance(); + //private Hopper hopper = Hopper.getInstance(); + //private Intake intake = Intake.getInstance(); private LazyCANSparkMax outtakeWheels; - private RelativeEncoder outtakeWheelsQuadrature; private double lastDetectionTime; + private RelativeEncoder outtakeWheelsQuadrature; + private @NotNull ColorSensorStatus allianceColor = ColorSensorStatus.RED; private @NotNull ColorSensorStatus opposingAllianceColor = ColorSensorStatus.BLUE; @@ -46,25 +49,29 @@ public enum OuttakeState { private OuttakeState outtakeState = OuttakeState.OFF; private Outtake() { - super(Constants.OUTTAKE_PERIOD); + super(10); outtakeWheels = new LazyCANSparkMax(Constants.OUTTAKE_CAN_ID, MotorType.kBrushless); outtakeWheels.setIdleMode(IdleMode.kCoast); - outtakeWheels.setInverted(false); + outtakeWheels.setInverted(true); outtakeWheels.setSmartCurrentLimit(Constants.OUTTAKE_CURRENT_LIMIT); outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 200); outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 25); outtakeWheels.setControlFramePeriodMs(25); - outtakeWheels.enableVoltageCompensation(9); outtakeWheelsQuadrature = outtakeWheels.getEncoder(); outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION); outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD_MS); + + //outtakeWheels.enableVoltageCompensation(9); + outtakeWheels.disableVoltageCompensation(); outtakeWheels.burnFlash(); } + NetworkTableEntry sideEntry = NetworkTableInstance.getDefault().getEntry("FMSInfo.IsRedAlliance"); + /** * Uses Sendable Chooser to decide Alliance Color */ @@ -83,13 +90,17 @@ private void updateAllianceColor() { */ private void updateOuttakeState() { // If color sensor detects a ball or - if (hopper.getColorSensorStatus() == opposingAllianceColor && intake.getIntakeSolState() == IntakeSolState.OPEN) { - lastDetectionTime = Timer.getFPGATimestamp(); - outtakeState = OuttakeState.EJECT; - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD && intake.getIntakeSolState() == IntakeSolState.OPEN) { - outtakeState = OuttakeState.EJECT; - } else if (intake.wantedIntakeState == IntakeState.INTAKE) { - outtakeState = OuttakeState.INTAKE; + Hopper hopper = Hopper.getInstance(); + Intake intake = Intake.getInstance(); + if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + if (hopper.getColorSensorStatus() == opposingAllianceColor) { + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.EJECT; + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { + outtakeState = OuttakeState.EJECT; + } else { + outtakeState = OuttakeState.OFF; + } } else { outtakeState = OuttakeState.OFF; } @@ -99,7 +110,12 @@ private void updateOuttakeState() { * Sets the percent outtake between 1 and -1 */ private void setOuttakePercentOutput(double percentOutput) { - outtakeWheels.setVoltage(9.0 * percentOutput); + outtakeWheels.set(percentOutput); + //outtakeWheels.getPIDController().setReference(9 * percentOutput, ControlType.kVoltage); + } + + public OuttakeState getOuttakeState() { + return outtakeState; } @Override @@ -137,9 +153,10 @@ public void selfTest() { @Override public void logData() { - logData("Outtake State", outtakeState); + logData("Outtak" + + "e State", outtakeState); + logData("Outtake SetVoltage", outtakeWheels.getSetpoint()); logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); - logData("Outtake SetVoltage", outtakeWheels.getSetVoltage()); } @Override From eb5007c7b2eb24d24e09ced6f8305779b3ea360e Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 20:05:09 -0700 Subject: [PATCH 010/108] merge hopper and outtake classes. Make Ejection use limelight --- src/main/java/frc/robot/Robot.java | 6 - src/main/java/frc/subsystem/Hopper.java | 219 ++++++++++++++--------- src/main/java/frc/subsystem/Outtake.java | 166 ----------------- 3 files changed, 132 insertions(+), 259 deletions(-) delete mode 100644 src/main/java/frc/subsystem/Outtake.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0b2620d..4f1f5d54 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -151,7 +151,6 @@ public class Robot extends TimedRobot { private final Shooter shooter = Shooter.getInstance(); private final Climber climber = Climber.getInstance(); private final VisionManager visionManager = VisionManager.getInstance(); - private final Outtake outtake = Outtake.getInstance(); //Inputs private final static Controller xbox = new Controller(0); @@ -729,10 +728,6 @@ public void testPeriodic() { if (buttonPanel.getRisingEdge(8)) { climber.selfTest(); } - - if (buttonPanel.getRisingEdge(10)) { - outtake.selfTest(); - } } private void startSubsystems() { @@ -744,7 +739,6 @@ private void startSubsystems() { shooter.start(); climber.start(); visionManager.start(); - outtake.start(); } public void killAuto() { diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index bfa517c1..248a5fe1 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -1,12 +1,19 @@ package frc.subsystem; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; -import com.revrobotics.ColorMatch; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.Constants; -import frc.subsystem.Outtake.OuttakeState; +import frc.robot.Robot; +import frc.subsystem.Intake.IntakeSolState; +import frc.subsystem.Intake.IntakeState; import frc.utility.OrangeUtility; -import frc.utility.colorsensor.PicoColorSensor; +import frc.utility.Timer; import frc.utility.controllers.LazyCANSparkMax; import org.jetbrains.annotations.NotNull; @@ -14,29 +21,16 @@ public final class Hopper extends AbstractSubsystem { - private Outtake outtake = Outtake.getInstance(); private static @NotNull Hopper INSTANCE = new Hopper(); private final @NotNull LazyCANSparkMax hopperMotor; - private final @NotNull PicoColorSensor colorSensor; + private RelativeEncoder outtakeWheelsQuadrature; + private LazyCANSparkMax outtakeWheels; + private double lastDetectionTime; - /** - * A Rev Color Match object is used to register and detect known colors. This can be calibrated ahead of time or during - * operation. - *

- * This object uses a simple euclidian distance to estimate the closest match with given confidence range. - */ - - /** - * Note: Any example colors should be calibrated as the user needs, these are here as a basic example. - */ - /*private final Color blueTarget = new Color(101, 246, 255); - private final Color redTarget = new Color(255, 75, 194); */ - - private final ColorMatch colorMatch = new ColorMatch(); - - public enum ColorSensorStatus {RED, BLUE, NO_BALL} + final @NotNull NetworkTableInstance instance = NetworkTableInstance.getDefault(); + NetworkTable intakeTable = instance.getTable("limelight-intake"); + NetworkTableEntry ta = intakeTable.getEntry("ta"); - private ColorSensorStatus colorSensorStatus = ColorSensorStatus.NO_BALL; public static Hopper getInstance() { return INSTANCE; @@ -48,8 +42,27 @@ public enum HopperState { HopperState wantedHopperState = HopperState.OFF; + /** + * Outtake can either be OFF or INTAKE, or EJECT + */ + public enum OuttakeState { + OFF, + INTAKE, + EJECT, + } + + private OuttakeState outtakeState = OuttakeState.OFF; + + public enum BallColor { + RED, + BLUE, + NO_BALL + } + + BallColor opposingAllianceColor = BallColor.BLUE; + private Hopper() { - super(10, 1); + super(10, 5); hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); hopperMotor.setSmartCurrentLimit(HOPPER_CURRENT_LIMIT); @@ -58,73 +71,81 @@ private Hopper() { hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 500); hopperMotor.setControlFramePeriodMs(25); - colorSensor = new PicoColorSensor(); - colorSensor.setDebugPrints(Constants.OUTTAKE_DEBUG_PRINTS); - - /*colorMatch.addColorMatch(blueTarget); - colorMatch.addColorMatch(redTarget); */ + outtakeWheels = new LazyCANSparkMax(Constants.OUTTAKE_CAN_ID, MotorType.kBrushless); + outtakeWheels.setIdleMode(IdleMode.kCoast); + outtakeWheels.setInverted(true); + outtakeWheels.setSmartCurrentLimit(Constants.OUTTAKE_CURRENT_LIMIT); + outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); + outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 200); + outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 25); + outtakeWheels.setControlFramePeriodMs(25); + + outtakeWheelsQuadrature = outtakeWheels.getEncoder(); + outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION); + outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD_MS); + + outtakeWheels.disableVoltageCompensation(); + outtakeWheels.burnFlash(); } - public void checkBallColor() { - - double red = colorSensor.getRawColor0().red / 309d; - double green = colorSensor.getRawColor0().green; - double blue = colorSensor.getRawColor0().blue / 567d; - double proximity = colorSensor.getProximity0(); - - // If color sensor is not connected, defaults to no ball - if (proximity < 70 || !colorSensor.isSensor0Connected()) { - logData("Ball Color", "No Ball Detected"); - colorSensorStatus = ColorSensorStatus.NO_BALL; - } else if (red > blue) { - logData("Ball Color", "Red"); - colorSensorStatus = ColorSensorStatus.RED; + /** + * Uses Sendable Chooser to decide Alliance Color + */ + private void updateAllianceColor() { + if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) { + opposingAllianceColor = BallColor.RED; } else { - logData("Ball Color", "Blue"); - colorSensorStatus = ColorSensorStatus.BLUE; + opposingAllianceColor = BallColor.BLUE; } + } - /*double red = colorSensor.getRawColor0().red; - double green = colorSensor.getRawColor0().green; - double blue = colorSensor.getRawColor0().blue; - double proximity = colorSensor.getProximity0(); - - double max = red; - if (green > max) max = green; - if (blue > max) max = blue; - - red = (255 * red) / max; - green = (255 * green) / max; - blue = (255 * blue) / max; */ - - logData("Red", red); - logData("Green", green); - logData("Blue", blue); - logData("Color Sensor Proximity", colorSensor.getProximity0()); - - logData("Raw Red", colorSensor.getRawColor0().red); - logData("Raw Blue", colorSensor.getRawColor0().blue); - - /*Color color = new Color(red, green, blue); - if (proximity > 70) { - if (colorMatch.matchClosestColor(color).color == redTarget) { - logData("Ball Color", "Red"); - colorSensorStatus = ColorSensorStatus.RED; - } else if (colorMatch.matchClosestColor(color).color == blueTarget) { - logData("Ball Color", "Blue"); - colorSensorStatus = ColorSensorStatus.BLUE; + /** + * Updates state of outtake based on color sensor and intake direction + */ + private void updateOuttakeState() { + // If color sensor detects a ball or + Intake intake = Intake.getInstance(); + if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + if (getBallColor() == opposingAllianceColor) { + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.EJECT; + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { + outtakeState = OuttakeState.EJECT; } else { - logData("Bottom Ball Color", "No Ball Detected"); + outtakeState = OuttakeState.OFF; } } else { - logData("Ball Color", "No Ball Detected"); - colorSensorStatus = ColorSensorStatus.NO_BALL; - } */ + outtakeState = OuttakeState.OFF; + } + } + + /** + * Sets the percent outtake between 1 and -1 + */ + private void setOuttakePercentOutput(double percentOutput) { + outtakeWheels.set(percentOutput); + //outtakeWheels.getPIDController().setReference(9 * percentOutput, ControlType.kVoltage); + } + + public OuttakeState getOuttakeState() { + return outtakeState; + } + + public BallColor getBallColor() { + @NotNull BallColor currentBallColor; + + if (ta.getDouble(0) > .01) { + currentBallColor = opposingAllianceColor; + } else { + currentBallColor = BallColor.NO_BALL; + } + + return currentBallColor; } private void setHopperStatePrivate(HopperState hopperState) { // Forces Hopper to run in reverse if outtake is ejecting - if (outtake.getOuttakeState() == OuttakeState.EJECT) { + if (getOuttakeState() == OuttakeState.EJECT) { hopperState = HopperState.OFF; } @@ -148,14 +169,25 @@ private void setHopperStatePrivate(HopperState hopperState) { } } - public synchronized ColorSensorStatus getColorSensorStatus() { - return colorSensorStatus; - } - @Override public void update() { + updateAllianceColor(); + getBallColor(); + updateOuttakeState(); + + switch (outtakeState) { + case OFF: + setOuttakePercentOutput(0); + break; + case EJECT: + setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); + break; + case INTAKE: + setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); + break; + } + setHopperStatePrivate(wantedHopperState); - checkBallColor(); } public void setHopperState(HopperState hopperState) { @@ -167,15 +199,28 @@ public void selfTest() { setHopperState(HopperState.ON); OrangeUtility.sleep(5000); setHopperState(HopperState.OFF); + OrangeUtility.sleep(5000); + + setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); + System.out.println("Ejecting"); + OrangeUtility.sleep(Constants.TEST_TIME_MS); + + setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); + System.out.println("Intaking"); + OrangeUtility.sleep(Constants.TEST_TIME_MS); + + System.out.println("Test Finished"); + setOuttakePercentOutput(0); } @Override public void logData() { logData("Hopper Motor Current", hopperMotor.getOutputCurrent()); - logData("Color Sensor Status", colorSensorStatus); - logData("Is Sensor 1 Connected", colorSensor.isSensor0Connected()); - logData("Is Sensor 2 Connected", colorSensor.isSensor1Connected()); - logData("Color Sensor Last Read Time", colorSensor.getLastReadTimestampSeconds()); + + logData("Outtae State", outtakeState); + logData("Outtake SetVoltage", outtakeWheels.getSetpoint()); + logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); + logData("Current Ball Color", getBallColor()); } @Override diff --git a/src/main/java/frc/subsystem/Outtake.java b/src/main/java/frc/subsystem/Outtake.java deleted file mode 100644 index 4ad80806..00000000 --- a/src/main/java/frc/subsystem/Outtake.java +++ /dev/null @@ -1,166 +0,0 @@ -package frc.subsystem; - -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import frc.robot.Constants; -import frc.robot.Robot; -import frc.subsystem.Hopper.ColorSensorStatus; -import frc.subsystem.Intake.IntakeSolState; -import frc.subsystem.Intake.IntakeState; -import frc.utility.OrangeUtility; -import frc.utility.Timer; -import frc.utility.controllers.LazyCANSparkMax; -import org.jetbrains.annotations.NotNull; - -public class Outtake extends AbstractSubsystem { - - //private Hopper hopper = Hopper.getInstance(); - //private Intake intake = Intake.getInstance(); - - private LazyCANSparkMax outtakeWheels; - - private double lastDetectionTime; - - private RelativeEncoder outtakeWheelsQuadrature; - - private @NotNull ColorSensorStatus allianceColor = ColorSensorStatus.RED; - - private @NotNull ColorSensorStatus opposingAllianceColor = ColorSensorStatus.BLUE; - - private static @NotNull Outtake instance = new Outtake(); - - public static Outtake getInstance() { - return instance; - } - - /** - * Outtake can either be OFF or ON - */ - public enum OuttakeState { - OFF, - INTAKE, - EJECT, - } - - private OuttakeState outtakeState = OuttakeState.OFF; - - private Outtake() { - super(10); - - outtakeWheels = new LazyCANSparkMax(Constants.OUTTAKE_CAN_ID, MotorType.kBrushless); - outtakeWheels.setIdleMode(IdleMode.kCoast); - outtakeWheels.setInverted(true); - outtakeWheels.setSmartCurrentLimit(Constants.OUTTAKE_CURRENT_LIMIT); - outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); - outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 200); - outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 25); - outtakeWheels.setControlFramePeriodMs(25); - - outtakeWheelsQuadrature = outtakeWheels.getEncoder(); - outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION); - outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD_MS); - - - //outtakeWheels.enableVoltageCompensation(9); - outtakeWheels.disableVoltageCompensation(); - outtakeWheels.burnFlash(); - } - - NetworkTableEntry sideEntry = NetworkTableInstance.getDefault().getEntry("FMSInfo.IsRedAlliance"); - - /** - * Uses Sendable Chooser to decide Alliance Color - */ - private void updateAllianceColor() { - if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) { - allianceColor = ColorSensorStatus.BLUE; - opposingAllianceColor = ColorSensorStatus.RED; - } else { - allianceColor = ColorSensorStatus.RED; - opposingAllianceColor = ColorSensorStatus.BLUE; - } - } - - /** - * Updates state of outtake based on color sensor and intake direction - */ - private void updateOuttakeState() { - // If color sensor detects a ball or - Hopper hopper = Hopper.getInstance(); - Intake intake = Intake.getInstance(); - if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { - if (hopper.getColorSensorStatus() == opposingAllianceColor) { - lastDetectionTime = Timer.getFPGATimestamp(); - outtakeState = OuttakeState.EJECT; - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { - outtakeState = OuttakeState.EJECT; - } else { - outtakeState = OuttakeState.OFF; - } - } else { - outtakeState = OuttakeState.OFF; - } - } - - /** - * Sets the percent outtake between 1 and -1 - */ - private void setOuttakePercentOutput(double percentOutput) { - outtakeWheels.set(percentOutput); - //outtakeWheels.getPIDController().setReference(9 * percentOutput, ControlType.kVoltage); - } - - public OuttakeState getOuttakeState() { - return outtakeState; - } - - @Override - public void update() { - updateAllianceColor(); - updateOuttakeState(); - - switch (outtakeState) { - case OFF: - setOuttakePercentOutput(0); - break; - case EJECT: - setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); - break; - case INTAKE: - setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); - break; - } - } - - @Override - public void selfTest() { - - setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); - System.out.println("Ejecting"); - OrangeUtility.sleep(Constants.TEST_TIME_MS); - - setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); - System.out.println("Intaking"); - OrangeUtility.sleep(Constants.TEST_TIME_MS); - - System.out.println("Test Finished"); - setOuttakePercentOutput(0); - } - - @Override - public void logData() { - logData("Outtak" + - "e State", outtakeState); - logData("Outtake SetVoltage", outtakeWheels.getSetpoint()); - logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); - } - - @Override - public void close() throws Exception { - - } -} From 4457335959007483b4c5209491391269ebf12516 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 20:21:46 -0700 Subject: [PATCH 011/108] Instantiated a new limelight and started using isTargetVisible in getBallColor --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/subsystem/Hopper.java | 12 +++--------- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 24603c1c..943ec4ad 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -221,6 +221,7 @@ public final class Constants { public static final double HOPPER_SPEED = .5; public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; + public static final String intakeLimelightName = "limelight-intake"; // Outtake Constants public static final int OUTTAKE_PERIOD = 50; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f1f5d54..b99ac1bd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -146,6 +146,7 @@ public class Robot extends TimedRobot { private final Drive drive = Drive.getInstance(); private final BlinkinLED blinkinLED = BlinkinLED.getInstance(); private final Limelight limelight = Limelight.getInstance(); + private final Limelight intakeLimelight = Limelight.getInstance(Constants.intakeLimelightName); private final Hopper hopper = Hopper.getInstance(); private final Intake intake = Intake.getInstance(); private final Shooter shooter = Shooter.getInstance(); @@ -282,6 +283,7 @@ public void robotInit() { Limelight.getInstance().setStreamingMode(StreamingMode.PIP_SECONDARY); startSubsystems(); limelight.setLedMode(LedMode.OFF); + intakeLimelight.setLedMode(LedMode.OFF); // shooter.homeHood(); // shooter.setHoodPositionMode(HoodPositionMode.RELATIVE_TO_HOME); } diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 248a5fe1..2c30bbc1 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -5,13 +5,11 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.Constants; import frc.robot.Robot; import frc.subsystem.Intake.IntakeSolState; import frc.subsystem.Intake.IntakeState; +import frc.utility.Limelight; import frc.utility.OrangeUtility; import frc.utility.Timer; import frc.utility.controllers.LazyCANSparkMax; @@ -26,11 +24,7 @@ public final class Hopper extends AbstractSubsystem { private RelativeEncoder outtakeWheelsQuadrature; private LazyCANSparkMax outtakeWheels; private double lastDetectionTime; - - final @NotNull NetworkTableInstance instance = NetworkTableInstance.getDefault(); - NetworkTable intakeTable = instance.getTable("limelight-intake"); - NetworkTableEntry ta = intakeTable.getEntry("ta"); - + private final Limelight intakeLimelight = Limelight.getInstance(Constants.intakeLimelightName); public static Hopper getInstance() { return INSTANCE; @@ -134,7 +128,7 @@ public OuttakeState getOuttakeState() { public BallColor getBallColor() { @NotNull BallColor currentBallColor; - if (ta.getDouble(0) > .01) { + if (intakeLimelight.isTargetVisible()) { currentBallColor = opposingAllianceColor; } else { currentBallColor = BallColor.NO_BALL; From 65a5837818540f9c794730738dfa1764eab75964 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 21:54:17 -0700 Subject: [PATCH 012/108] Merge branch 'main' into Limelight-Eject-Fixed --- .../utility/colorsensor/PicoColorSensor.java | 286 ------------------ .../frc/utility/net/DashboardConnection.java | 27 ++ .../java/frc/utility/net/PacketHandler.java | 9 + .../java/frc/utility/net/SendableLog.java | 16 + 4 files changed, 52 insertions(+), 286 deletions(-) delete mode 100644 src/main/java/frc/utility/colorsensor/PicoColorSensor.java create mode 100644 src/main/java/frc/utility/net/DashboardConnection.java create mode 100644 src/main/java/frc/utility/net/PacketHandler.java create mode 100644 src/main/java/frc/utility/net/SendableLog.java diff --git a/src/main/java/frc/utility/colorsensor/PicoColorSensor.java b/src/main/java/frc/utility/colorsensor/PicoColorSensor.java deleted file mode 100644 index 8dfcd425..00000000 --- a/src/main/java/frc/utility/colorsensor/PicoColorSensor.java +++ /dev/null @@ -1,286 +0,0 @@ -package frc.utility.colorsensor; - -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.hal.SerialPortJNI; -import edu.wpi.first.wpilibj.Timer; - -import java.nio.charset.StandardCharsets; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.concurrent.locks.ReentrantLock; - -public class PicoColorSensor implements AutoCloseable { - public static class RawColor { - public RawColor(int r, int g, int b, int _ir) { - red = r; - green = g; - blue = b; - ir = _ir; - } - - public RawColor() { - } - - public int red; - public int green; - public int blue; - public int ir; - } - - private static class SingleCharSequence implements CharSequence { - public byte[] data; - - @Override - public int length() { - return data.length; - } - - @Override - public char charAt(int index) { - return (char) data[index]; - } - - @Override - public CharSequence subSequence(int start, int end) { - return new String(data, start, end, StandardCharsets.UTF_8); - } - } - - private static class IntRef { - int value; - } - - int parseIntFromIndex(SingleCharSequence charSeq, int readLen, IntRef lastComma) { - int nextComma = 0; - try { - nextComma = findNextComma(charSeq.data, readLen, lastComma.value); - int value = Integer.parseInt(charSeq, lastComma.value + 1, nextComma, 10); - lastComma.value = nextComma; - return value; - } catch (Exception ex) { - return 0; - } - } - - private int findNextComma(byte[] data, int readLen, int lastComma) { - while (true) { - if (readLen <= lastComma + 1) { - return readLen; - } - lastComma++; - if (data[lastComma] == ',') { - break; - } - } - return lastComma; - } - - private final AtomicBoolean debugPrints = new AtomicBoolean(); - private boolean hasColor0; - private boolean hasColor1; - private int prox0; - private int prox1; - private final RawColor color0 = new RawColor(); - private final RawColor color1 = new RawColor(); - private double lastReadTime; - private final ReentrantLock threadLock = new ReentrantLock(); - private final Thread readThread; - private final AtomicBoolean threadRunning = new AtomicBoolean(true); - - private void threadMain() { - // Using JNI for a non allocating read - int port = SerialPortJNI.serialInitializePort((byte) 1); - SerialPortJNI.serialSetBaudRate(port, 115200); - SerialPortJNI.serialSetDataBits(port, (byte) 8); - SerialPortJNI.serialSetParity(port, (byte) 0); - SerialPortJNI.serialSetStopBits(port, (byte) 10); - - SerialPortJNI.serialSetTimeout(port, 1); - SerialPortJNI.serialEnableTermination(port, '\n'); - - HAL.report(tResourceType.kResourceType_SerialPort, 2); - - byte[] buffer = new byte[257]; - SingleCharSequence charSeq = new SingleCharSequence(); - charSeq.data = buffer; - IntRef lastComma = new IntRef(); - - RawColor color0 = new RawColor(); - RawColor color1 = new RawColor(); - - while (threadRunning.get()) { - int read = SerialPortJNI.serialRead(port, buffer, buffer.length - 1); - if (read <= 0) { - try { - threadLock.lock(); - this.hasColor0 = false; - this.hasColor1 = false; - } finally { - threadLock.unlock(); - } - continue; - } - if (!threadRunning.get()) { - break; - } - - // Trim trailing newline if exists - if (buffer[read - 1] == '\n') { - read--; - } - - if (read == 0) { - continue; - } - - if (debugPrints.get()) { - System.out.println(new String(buffer, 0, read, StandardCharsets.UTF_8)); - } - - lastComma.value = -1; - - boolean hasColor0 = parseIntFromIndex(charSeq, read, lastComma) != 0; - boolean hasColor1 = parseIntFromIndex(charSeq, read, lastComma) != 0; - color0.red = parseIntFromIndex(charSeq, read, lastComma); - color0.green = parseIntFromIndex(charSeq, read, lastComma); - color0.blue = parseIntFromIndex(charSeq, read, lastComma); - color0.ir = parseIntFromIndex(charSeq, read, lastComma); - int prox0 = parseIntFromIndex(charSeq, read, lastComma); - color1.red = parseIntFromIndex(charSeq, read, lastComma); - color1.green = parseIntFromIndex(charSeq, read, lastComma); - color1.blue = parseIntFromIndex(charSeq, read, lastComma); - color1.ir = parseIntFromIndex(charSeq, read, lastComma); - int prox1 = parseIntFromIndex(charSeq, read, lastComma); - - double ts = Timer.getFPGATimestamp(); - - try { - threadLock.lock(); - this.lastReadTime = ts; - this.hasColor0 = hasColor0; - this.hasColor1 = hasColor1; - if (hasColor0) { - this.color0.red = color0.red; - this.color0.green = color0.green; - this.color0.blue = color0.blue; - this.color0.ir = color0.ir; - this.prox0 = prox0; - } - if (hasColor1) { - this.color1.red = color1.red; - this.color1.green = color1.green; - this.color1.blue = color1.blue; - this.color1.ir = color1.ir; - this.prox1 = prox1; - } - } finally { - threadLock.unlock(); - } - } - - SerialPortJNI.serialClose(port); - } - - public PicoColorSensor() { - readThread = new Thread(this::threadMain); - readThread.setName("PicoColorSensorThread"); - readThread.start(); - } - - public boolean isSensor0Connected() { - try { - threadLock.lock(); - return hasColor0; - } finally { - threadLock.unlock(); - } - } - - public boolean isSensor1Connected() { - try { - threadLock.lock(); - return hasColor1; - } finally { - threadLock.unlock(); - } - } - - public RawColor getRawColor0() { - try { - threadLock.lock(); - return new RawColor(color0.red, color0.green, color0.blue, color0.ir); - } finally { - threadLock.unlock(); - } - } - - public void getRawColor0(RawColor rawColor) { - try { - threadLock.lock(); - rawColor.red = color0.red; - rawColor.green = color0.green; - rawColor.blue = color0.blue; - rawColor.ir = color0.ir; - } finally { - threadLock.unlock(); - } - } - - public int getProximity0() { - try { - threadLock.lock(); - return prox0; - } finally { - threadLock.unlock(); - } - } - - public RawColor getRawColor1() { - try { - threadLock.lock(); - return new RawColor(color1.red, color1.green, color1.blue, color1.ir); - } finally { - threadLock.unlock(); - } - } - - public void getRawColor1(RawColor rawColor) { - try { - threadLock.lock(); - rawColor.red = color1.red; - rawColor.green = color1.green; - rawColor.blue = color1.blue; - rawColor.ir = color1.ir; - } finally { - threadLock.unlock(); - } - } - - public int getProximity1() { - try { - threadLock.lock(); - return prox1; - } finally { - threadLock.unlock(); - } - } - - public double getLastReadTimestampSeconds() { - try { - threadLock.lock(); - return lastReadTime; - } finally { - threadLock.unlock(); - } - } - - public void setDebugPrints(boolean debug) { - debugPrints.set(debug); - } - - @Override - public void close() throws Exception { - threadRunning.set(false); - readThread.join(); - } -} diff --git a/src/main/java/frc/utility/net/DashboardConnection.java b/src/main/java/frc/utility/net/DashboardConnection.java new file mode 100644 index 00000000..b55ee2dc --- /dev/null +++ b/src/main/java/frc/utility/net/DashboardConnection.java @@ -0,0 +1,27 @@ +package frc.utility.net; + +import java.io.Closeable; +import java.net.DatagramSocket; + +public class DashboardConnection implements Closeable { + public final DatagramSocket datagramSocket; + public long timeoutTime; + + public DashboardConnection(DatagramSocket datagramSocket, long timeoutTime) { + this.datagramSocket = datagramSocket; + this.timeoutTime = timeoutTime; + } + + public DashboardConnection(DatagramSocket datagramSocket) { + this(datagramSocket, System.currentTimeMillis() + 10000); + } + + public void keepAlive() { + timeoutTime = System.currentTimeMillis() + 10000; + } + + @Override + public void close() { + datagramSocket.close(); + } +} diff --git a/src/main/java/frc/utility/net/PacketHandler.java b/src/main/java/frc/utility/net/PacketHandler.java new file mode 100644 index 00000000..6058ddc4 --- /dev/null +++ b/src/main/java/frc/utility/net/PacketHandler.java @@ -0,0 +1,9 @@ +package frc.utility.net; + +import java.io.IOException; +import java.net.DatagramPacket; + + +public interface PacketHandler { + void handlePacket(DatagramPacket packet) throws IOException; +} diff --git a/src/main/java/frc/utility/net/SendableLog.java b/src/main/java/frc/utility/net/SendableLog.java new file mode 100644 index 00000000..265b3840 --- /dev/null +++ b/src/main/java/frc/utility/net/SendableLog.java @@ -0,0 +1,16 @@ +package frc.utility.net; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; + +import java.util.Map; + +public class SendableLog { + @JsonProperty + public Map log; + + @JsonCreator + public SendableLog(@JsonProperty Map log) { + this.log = log; + } +} From 90512bf12b21bc8e12023fd8644392840881cbf9 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 22:05:50 -0700 Subject: [PATCH 013/108] fixed merge issues --- src/main/java/frc/robot/Constants.java | 4 +- .../java/frc/subsystem/AbstractSubsystem.java | 52 ++++--------------- src/main/java/frc/subsystem/Hopper.java | 2 +- src/main/java/frc/utility/Serializer.java | 16 +++--- 4 files changed, 21 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 943ec4ad..d3b10c80 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -217,19 +217,17 @@ public final class Constants { //Hopper Constants - public static final int HOPPER_PERIOD = 50; + public static final int HOPPER_PERIOD = 10; public static final double HOPPER_SPEED = .5; public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; public static final String intakeLimelightName = "limelight-intake"; // Outtake Constants - public static final int OUTTAKE_PERIOD = 50; public static final int OUTTAKE_CAN_ID = 60; public static final double OUTTAKE_REDUCTION = (3.0 / 1.0); public static final int OUTTAKE_CURRENT_LIMIT = 20; public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 200; - public static final boolean OUTTAKE_DEBUG_PRINTS = false; /** * Maximum Percent Output of Outtake wheels */ diff --git a/src/main/java/frc/subsystem/AbstractSubsystem.java b/src/main/java/frc/subsystem/AbstractSubsystem.java index fb66676e..5a05ae88 100644 --- a/src/main/java/frc/subsystem/AbstractSubsystem.java +++ b/src/main/java/frc/subsystem/AbstractSubsystem.java @@ -1,14 +1,10 @@ package frc.subsystem; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; -import java.util.HashMap; -import java.util.Map; - @SuppressWarnings("unused") public abstract class AbstractSubsystem implements Runnable, AutoCloseable { private final int period; @@ -39,6 +35,14 @@ public AbstractSubsystem(int period) { public abstract void logData(); + public void logData(@NotNull String key, @NotNull Object value) { + DashboardHandler.getInstance().log(key, value); + } + + public void logData(@NotNull String key, @NotNull Object value, boolean logToNetworkTables) { + DashboardHandler.getInstance().log(key, value, logToNetworkTables); + } + public void pause() { signal = ThreadSignal.PAUSED; } @@ -63,38 +67,7 @@ public void update() { } - private final Map logDataMap = new HashMap<>(); - - - public void logData(@NotNull String key, @NotNull Object value) { - //SmartDashboard.putString(key, value.toString()); - logDataMap.put(key, value); - } - int lastLength = 20; - - public void pushLog() { - for (Map.Entry entry : logDataMap.entrySet()) { - Object obj = entry.getValue(); - Class cl = obj.getClass(); - - //@formatter:off - if (cl.equals(Integer.class)) SmartDashboard.putNumber(entry.getKey(), (int) obj); - else if (cl.equals(Double.class)) SmartDashboard.putNumber(entry.getKey(), (double) obj); - else if (cl.equals(Short.class)) SmartDashboard.putNumber(entry.getKey(), (short) obj); - else if (cl.equals(Long.class)) SmartDashboard.putNumber(entry.getKey(), (long) obj); - else if (cl.equals(Float.class)) SmartDashboard.putNumber(entry.getKey(), (float) obj); - else if (cl.equals(Byte.class)) SmartDashboard.putNumber(entry.getKey(), (byte) obj); - else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(entry.getKey(), (boolean) obj); - else if (cl.equals(String.class)) SmartDashboard.putString(entry.getKey(), (String) obj); - else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(entry.getKey(), (Double[]) obj); - else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(entry.getKey(), (Boolean[]) obj); - else if (cl.equals(String[].class)) SmartDashboard.putStringArray(entry.getKey(), (String[]) obj); - else SmartDashboard.putString(entry.getKey(), entry.getValue().toString()); - //@formatter:on - } - } - @Override @SuppressWarnings("BusyWait") public void run() { @@ -104,12 +77,9 @@ public void run() { update(); logInterval++; - if (logInterval >= loggingInterval) { - + if (logInterval > loggingInterval) { logData(); - pushLog(); - - logInterval = 1; + logInterval = 0; } } double executionTimeMS = (Timer.getFPGATimestamp() - startTime) * 1000; @@ -125,4 +95,4 @@ public void run() { logData(subsystemName + " Period Length", (Timer.getFPGATimestamp() - startTime) * 1000); } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 2c30bbc1..7a9457de 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -56,7 +56,7 @@ public enum BallColor { BallColor opposingAllianceColor = BallColor.BLUE; private Hopper() { - super(10, 5); + super(Constants.HOPPER_PERIOD, 5); hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); hopperMotor.setSmartCurrentLimit(HOPPER_CURRENT_LIMIT); diff --git a/src/main/java/frc/utility/Serializer.java b/src/main/java/frc/utility/Serializer.java index d8e420ee..c4e38cfd 100644 --- a/src/main/java/frc/utility/Serializer.java +++ b/src/main/java/frc/utility/Serializer.java @@ -16,19 +16,19 @@ public static String serializeToString(Object obj) throws com.fasterxml.jackson. return OBJECT_MAPPER.writeValueAsString(obj); } - public static void serializeToFile(Object obj, - File file) throws IOException, com.fasterxml.jackson.core.JsonGenerationException, com.fasterxml.jackson.databind.JsonMappingException { + public static void serializeToFile(Object obj, File file) + throws IOException, com.fasterxml.jackson.core.JsonGenerationException, com.fasterxml.jackson.databind.JsonMappingException { OBJECT_MAPPER.configure(SerializationFeature.INDENT_OUTPUT, true); OBJECT_MAPPER.writeValue(file, obj); } - public static Autonomous deserializeAutoFromFile( - File file) throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { + public static Autonomous deserializeAutoFromFile(File file) + throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { return OBJECT_MAPPER.readValue(file, Autonomous.class); } - public static Object deserialize(String object, - Class serializableObject) throws com.fasterxml.jackson.core.JsonProcessingException, com.fasterxml.jackson.databind.JsonMappingException { + public static Object deserialize(String object, Class serializableObject) + throws com.fasterxml.jackson.core.JsonProcessingException, com.fasterxml.jackson.databind.JsonMappingException { return OBJECT_MAPPER.readValue(object, serializableObject); } @@ -37,8 +37,8 @@ public static Autonomous deserializeAuto( return OBJECT_MAPPER.readValue(object, Autonomous.class); } - public static Object deserializeFromFile(File file, - Class serializableObject) throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { + public static Object deserializeFromFile(File file, Class serializableObject) + throws IOException, com.fasterxml.jackson.core.JsonParseException, com.fasterxml.jackson.databind.JsonMappingException { return OBJECT_MAPPER.readValue(file, serializableObject); } } \ No newline at end of file From 06260e9cd87e4d76be9eb596fb50007eebe62b14 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 22:14:19 -0700 Subject: [PATCH 014/108] minor outtake changes --- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/subsystem/AbstractSubsystem.java | 2 +- src/main/java/frc/subsystem/Hopper.java | 51 +++++++++---------- 3 files changed, 25 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d3b10c80..3b9005ee 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -231,7 +231,7 @@ public final class Constants { /** * Maximum Percent Output of Outtake wheels */ - public static final double OUTTAKE_SPEED_FACTOR = 1d; + public static final double OUTTAKE_SPEED = 1d; /** * Outtake will run for this time (MS) after color sensor trigger diff --git a/src/main/java/frc/subsystem/AbstractSubsystem.java b/src/main/java/frc/subsystem/AbstractSubsystem.java index 5a05ae88..43a8c1d1 100644 --- a/src/main/java/frc/subsystem/AbstractSubsystem.java +++ b/src/main/java/frc/subsystem/AbstractSubsystem.java @@ -95,4 +95,4 @@ public void run() { logData(subsystemName + " Period Length", (Timer.getFPGATimestamp() - startTime) * 1000); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 7a9457de..025440a5 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -88,8 +88,10 @@ private Hopper() { private void updateAllianceColor() { if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) { opposingAllianceColor = BallColor.RED; + intakeLimelight.setPipeline(1); } else { opposingAllianceColor = BallColor.BLUE; + intakeLimelight.setPipeline(0); } } @@ -137,13 +139,27 @@ public BallColor getBallColor() { return currentBallColor; } - private void setHopperStatePrivate(HopperState hopperState) { - // Forces Hopper to run in reverse if outtake is ejecting - if (getOuttakeState() == OuttakeState.EJECT) { - hopperState = HopperState.OFF; + @Override + public void update() { + updateAllianceColor(); + getBallColor(); + updateOuttakeState(); + + // Outtake motor control + switch (outtakeState) { + case OFF: + setOuttakePercentOutput(0); + break; + case EJECT: + setOuttakePercentOutput(Constants.OUTTAKE_SPEED); + break; + case INTAKE: + setOuttakePercentOutput(-Constants.OUTTAKE_SPEED); + break; } - switch (hopperState) { + // Hopper Motor Control + switch (wantedHopperState) { case ON: hopperMotor.set(Constants.HOPPER_SPEED); Shooter.getInstance().runFeederWheelReversed = true; @@ -163,27 +179,6 @@ private void setHopperStatePrivate(HopperState hopperState) { } } - @Override - public void update() { - updateAllianceColor(); - getBallColor(); - updateOuttakeState(); - - switch (outtakeState) { - case OFF: - setOuttakePercentOutput(0); - break; - case EJECT: - setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); - break; - case INTAKE: - setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); - break; - } - - setHopperStatePrivate(wantedHopperState); - } - public void setHopperState(HopperState hopperState) { wantedHopperState = hopperState; } @@ -195,11 +190,11 @@ public void selfTest() { setHopperState(HopperState.OFF); OrangeUtility.sleep(5000); - setOuttakePercentOutput(Constants.OUTTAKE_SPEED_FACTOR); + setOuttakePercentOutput(Constants.OUTTAKE_SPEED); System.out.println("Ejecting"); OrangeUtility.sleep(Constants.TEST_TIME_MS); - setOuttakePercentOutput(-Constants.OUTTAKE_SPEED_FACTOR); + setOuttakePercentOutput(-Constants.OUTTAKE_SPEED); System.out.println("Intaking"); OrangeUtility.sleep(Constants.TEST_TIME_MS); From aa1e74547aa3ad4dc9924bd70e25f7170d1fef5c Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 22:27:21 -0700 Subject: [PATCH 015/108] unrevert logger changes --- src/main/java/frc/robot/Constants.java | 13 +++--- .../java/frc/subsystem/AbstractSubsystem.java | 1 + .../java/frc/subsystem/DashboardHandler.java | 44 ++++++++----------- src/main/java/frc/subsystem/Hopper.java | 2 +- 4 files changed, 29 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b9005ee..c7fc8708 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -190,10 +190,6 @@ public final class Constants { */ public static final double GOAL_RADIUS = Units.inchesToMeters(GOAL_RADIUS_IN); - /** - * This GOAL RADIUS is used to calculate the turn error - */ - public static final double GOAL_RADIUS_TURN_ERROR_M = GOAL_RADIUS * 3; /** * Maximum speed of the robot when shooting the ball. (Only applies when doing the static shot) Units are in Meters per Second @@ -201,6 +197,11 @@ public final class Constants { */ public static final double MAX_SHOOT_SPEED_SQUARED = Math.pow(MAX_SHOOT_SPEED, 2); + /** + * This GOAL RADIUS is used to calculate the turn error + */ + public static final double GOAL_RADIUS_TURN_ERROR_M = GOAL_RADIUS * 3; + public static final double GRAVITY = 9.80665; /** @@ -278,6 +279,7 @@ public final class Constants { public static final double FEEDER_WHEEL_LOCK_SPEED_RPM = 1000; + // Feeder wheel pidf is unused public static final double FEEDER_WHEEL_P = 0.0002; public static final double FEEDER_WHEEL_I = 0; @@ -499,5 +501,6 @@ public final class Constants { public static final double INTAKE_OPEN_TIME = 0.0; public static final int WEB_DASHBOARD_PORT = 5802; - public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 50; + + public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 200; } diff --git a/src/main/java/frc/subsystem/AbstractSubsystem.java b/src/main/java/frc/subsystem/AbstractSubsystem.java index 43a8c1d1..c97d0532 100644 --- a/src/main/java/frc/subsystem/AbstractSubsystem.java +++ b/src/main/java/frc/subsystem/AbstractSubsystem.java @@ -68,6 +68,7 @@ public void update() { } int lastLength = 20; + @Override @SuppressWarnings("BusyWait") public void run() { diff --git a/src/main/java/frc/subsystem/DashboardHandler.java b/src/main/java/frc/subsystem/DashboardHandler.java index c63e18cc..859bcdbb 100644 --- a/src/main/java/frc/subsystem/DashboardHandler.java +++ b/src/main/java/frc/subsystem/DashboardHandler.java @@ -33,7 +33,7 @@ public final class DashboardHandler extends AbstractSubsystem { */ private final HashMap dashboardConnections = new HashMap<>(); private static final Map LOG_DATA_MAP = new ConcurrentHashMap<>(); - private static final Map LOG_DATA_MAP_OBJECTS = new ConcurrentHashMap<>(); + private static final ReadWriteLock LOG_DATA_MAP_LOCK = new ReentrantReadWriteLock(); private final HashMap packetHandlerMap = new HashMap<>(); @@ -79,10 +79,27 @@ private DashboardHandler(int period) { } public void log(@NotNull String key, @NotNull Object value) { - log(key, value, true); + log(key, value, false); } public void log(@NotNull String key, @NotNull Object value, boolean logToNetworkTables) { + if (logToNetworkTables) { + //@formatter:off + Class cl = value.getClass(); + if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); + else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); + else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); + else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); + else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); + else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); + else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); + else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); + else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); + else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); + else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); + else SmartDashboard.putString(key, value.toString()); + //@formatter:on + } LOG_DATA_MAP_LOCK.readLock().lock(); // We're adding a read lock here because there can only be one writer, but there can be many readers. The @@ -90,9 +107,6 @@ public void log(@NotNull String key, @NotNull Object value, boolean logToNetwork // write lock when reading it to prevent any other writes from happening while we're serializing. try { LOG_DATA_MAP.put(key, value.toString()); - if (logToNetworkTables) { - LOG_DATA_MAP_OBJECTS.put(key, value); - } } finally { LOG_DATA_MAP_LOCK.readLock().unlock(); } @@ -105,26 +119,6 @@ public void pushLog() { try { SendableLog log = new SendableLog(LOG_DATA_MAP); json = 'd' + Serializer.serializeToString(log); - - for (Map.Entry entry : LOG_DATA_MAP_OBJECTS.entrySet()) { - //@formatter:off - String key = entry.getKey(); - Class cl = entry.getValue().getClass(); - Object value = entry.getValue(); - if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); - else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); - else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); - else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); - else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); - else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); - else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); - else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); - else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); - else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); - else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); - else SmartDashboard.putString(key, value.toString()); - //@formatter:on - } } finally { LOG_DATA_MAP_LOCK.writeLock().unlock(); } diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 025440a5..c72dfb0f 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -59,7 +59,7 @@ private Hopper() { super(Constants.HOPPER_PERIOD, 5); hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); hopperMotor.setSmartCurrentLimit(HOPPER_CURRENT_LIMIT); - + hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 500); hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 500); From 6be7cb88ea47b8333d73caa48b12c91ccd3509de Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 22:30:38 -0700 Subject: [PATCH 016/108] fix bad dashboardHandler merge --- .../java/frc/subsystem/DashboardHandler.java | 46 +++++++++++-------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/subsystem/DashboardHandler.java b/src/main/java/frc/subsystem/DashboardHandler.java index 859bcdbb..c5f32d2d 100644 --- a/src/main/java/frc/subsystem/DashboardHandler.java +++ b/src/main/java/frc/subsystem/DashboardHandler.java @@ -33,7 +33,7 @@ public final class DashboardHandler extends AbstractSubsystem { */ private final HashMap dashboardConnections = new HashMap<>(); private static final Map LOG_DATA_MAP = new ConcurrentHashMap<>(); - + private static final Map LOG_DATA_MAP_OBJECTS = new ConcurrentHashMap<>(); private static final ReadWriteLock LOG_DATA_MAP_LOCK = new ReentrantReadWriteLock(); private final HashMap packetHandlerMap = new HashMap<>(); @@ -79,27 +79,10 @@ private DashboardHandler(int period) { } public void log(@NotNull String key, @NotNull Object value) { - log(key, value, false); + log(key, value, true); } public void log(@NotNull String key, @NotNull Object value, boolean logToNetworkTables) { - if (logToNetworkTables) { - //@formatter:off - Class cl = value.getClass(); - if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); - else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); - else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); - else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); - else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); - else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); - else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); - else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); - else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); - else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); - else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); - else SmartDashboard.putString(key, value.toString()); - //@formatter:on - } LOG_DATA_MAP_LOCK.readLock().lock(); // We're adding a read lock here because there can only be one writer, but there can be many readers. The @@ -107,6 +90,9 @@ public void log(@NotNull String key, @NotNull Object value, boolean logToNetwork // write lock when reading it to prevent any other writes from happening while we're serializing. try { LOG_DATA_MAP.put(key, value.toString()); + if (logToNetworkTables) { + LOG_DATA_MAP_OBJECTS.put(key, value); + } } finally { LOG_DATA_MAP_LOCK.readLock().unlock(); } @@ -119,6 +105,26 @@ public void pushLog() { try { SendableLog log = new SendableLog(LOG_DATA_MAP); json = 'd' + Serializer.serializeToString(log); + + for (Map.Entry entry : LOG_DATA_MAP_OBJECTS.entrySet()) { + //@formatter:off + String key = entry.getKey(); + Class cl = entry.getValue().getClass(); + Object value = entry.getValue(); + if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); + else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); + else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); + else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); + else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); + else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); + else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); + else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); + else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); + else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); + else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); + else SmartDashboard.putString(key, value.toString()); + //@formatter:on + } } finally { LOG_DATA_MAP_LOCK.writeLock().unlock(); } @@ -206,4 +212,4 @@ public void logData() { public void close() throws Exception { } -} +} \ No newline at end of file From 957430025582e77f22ac55175368df0a56824715 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Tue, 22 Mar 2022 22:38:05 -0700 Subject: [PATCH 017/108] style: fixed constant case --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/subsystem/Hopper.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c7fc8708..ff335517 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -222,7 +222,7 @@ public final class Constants { public static final double HOPPER_SPEED = .5; public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; - public static final String intakeLimelightName = "limelight-intake"; + public static final String INTAKE_LIMELIGHT_NAME = "limelight-intake"; // Outtake Constants public static final int OUTTAKE_CAN_ID = 60; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b99ac1bd..7bdbab67 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -146,7 +146,7 @@ public class Robot extends TimedRobot { private final Drive drive = Drive.getInstance(); private final BlinkinLED blinkinLED = BlinkinLED.getInstance(); private final Limelight limelight = Limelight.getInstance(); - private final Limelight intakeLimelight = Limelight.getInstance(Constants.intakeLimelightName); + private final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME); private final Hopper hopper = Hopper.getInstance(); private final Intake intake = Intake.getInstance(); private final Shooter shooter = Shooter.getInstance(); diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index c72dfb0f..895a132e 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -24,7 +24,7 @@ public final class Hopper extends AbstractSubsystem { private RelativeEncoder outtakeWheelsQuadrature; private LazyCANSparkMax outtakeWheels; private double lastDetectionTime; - private final Limelight intakeLimelight = Limelight.getInstance(Constants.intakeLimelightName); + private final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME); public static Hopper getInstance() { return INSTANCE; From 29198736a2d2b7ad155a04a9fe403841185b645e Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 08:51:49 -0700 Subject: [PATCH 018/108] cube/square the vector length of the controller inputs, not the individual components --- .../java/frc/utility/ControllerDriveInputs.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/utility/ControllerDriveInputs.java b/src/main/java/frc/utility/ControllerDriveInputs.java index ae184515..aa8577bf 100644 --- a/src/main/java/frc/utility/ControllerDriveInputs.java +++ b/src/main/java/frc/utility/ControllerDriveInputs.java @@ -74,8 +74,13 @@ public double getRotation() { * @return {@link ControllerDriveInputs} */ public @NotNull ControllerDriveInputs squareInputs() { - x = Math.copySign(x * x, x); - y = Math.copySign(y * y, y); + double dist = Math.hypot(x, y); + double distSquared = dist * dist; + double angle = Math.atan2(y, x); + + + x = distSquared * Math.cos(angle); + y = distSquared * Math.sin(angle); rotation = Math.copySign(rotation * rotation, rotation); return this; } @@ -86,8 +91,12 @@ public double getRotation() { * @return {@link ControllerDriveInputs} */ public @NotNull ControllerDriveInputs cubeInputs() { - x = x * x * x; - y = y * y * y; + double dist = Math.hypot(x, y); + double distCubed = dist * dist * dist; + double angle = Math.atan2(y, x); + + x = distCubed * Math.cos(angle); + y = distCubed * Math.sin(angle); rotation = rotation * rotation * rotation; return this; } From 2f377c0c2c9567d91a0c80ad0ecc1af89d7f261d Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 09:37:07 -0700 Subject: [PATCH 019/108] update the shooter config --- src/main/deploy/shooter/shooterconfig.json | 34 +++++++++++----------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 9e0ddcfe..c1d58560 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -1,51 +1,51 @@ { "shooterConfigs" : [ { - "hoodEjectAngle" : 67.0, - "flywheelSpeed" : 1800.0, + "hoodEjectAngle" : 68.0, + "flywheelSpeed" : 1700.0, "distance" : 40.0 }, { "hoodEjectAngle" : 62.0, - "flywheelSpeed" : 1750.0, + "flywheelSpeed" : 1700.0, "distance" : 60.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 1800.0, + "flywheelSpeed" : 1700.0, "distance" : 82.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1800.0, "distance" : 106.0 }, { "hoodEjectAngle" : 53.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1800.0, "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1900.0, "distance" : 120.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2000.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2150.0, + "flywheelSpeed" : 2100.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2300.0, + "flywheelSpeed" : 2200.0, "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2350.0, + "flywheelSpeed" : 2200.0, "distance" : 175.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2460.0, + "flywheelSpeed" : 2300.0, "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2450.0, + "flywheelSpeed" : 2400.0, "distance" : 200.0 }, { "hoodEjectAngle" : 51.0, @@ -53,19 +53,19 @@ "distance" : 210.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2550.0, + "flywheelSpeed" : 2600.0, "distance" : 225.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2650.0, + "flywheelSpeed" : 2700.0, "distance" : 250.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3000.0, + "flywheelSpeed" : 2800.0, "distance" : 270.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3000.0, + "flywheelSpeed" : 2900.0, "distance" : 299.0 } ] } \ No newline at end of file From b7b211287c44b8cfadcb1166151088a955819221 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 17:15:27 -0700 Subject: [PATCH 020/108] print out the shooter config in simulation init --- .../reflection/ClassInformationSender.java | 9 ++++----- src/main/java/frc/robot/Robot.java | 2 ++ .../utility/shooter/visionlookup/ShooterConfig.java | 10 ++++++++++ .../shooter/visionlookup/VisionLookUpTable.java | 9 +++++++++ 4 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java b/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java index 30419729..8984d0ef 100644 --- a/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java +++ b/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java @@ -2,14 +2,13 @@ import edu.wpi.first.wpilibj.Filesystem; import frc.utility.Serializer; +import org.jetbrains.annotations.Nullable; import java.io.File; import java.io.IOException; import java.util.ArrayList; import java.util.List; -import org.jetbrains.annotations.Nullable; - import static frc.auton.guiauto.serialization.reflection.ReflectionUtils.findClasses; public final class ClassInformationSender { public ClassInformationSender() { @@ -23,10 +22,10 @@ public static void updateReflectionInformation(@Nullable File file) { for (Class aClass : classes) { reflectionClassData.add(new ReflectionClassData(aClass)); } - System.out.println(Serializer.serializeToString(reflectionClassData)); - + //System.out.println(Serializer.serializeToString(reflectionClassData)); + System.out.println("Successfully updated reflection information"); - if(file != null) { + if (file != null) { file.getParentFile().mkdir(); Serializer.serializeToFile(reflectionClassData, file); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e912907..90c35a00 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,6 +25,7 @@ import frc.utility.Limelight.StreamingMode; import frc.utility.shooter.visionlookup.ShooterConfig; import frc.utility.shooter.visionlookup.ShooterPreset; +import frc.utility.shooter.visionlookup.VisionLookUpTable; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; @@ -777,5 +778,6 @@ public void killAuto() { public void simulationInit() { ClassInformationSender.updateReflectionInformation( new File(OsUtil.getUserConfigDirectory("AutoBuilder") + "/robotCodeData.json")); + VisionLookUpTable.getInstance().printShooterConfig(); } } diff --git a/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java b/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java index 6f6f9d26..57a828f9 100644 --- a/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java +++ b/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java @@ -28,4 +28,14 @@ public String toString() { "shooterConfigs=" + shooterConfigs + '}'; } + + public void printCSV() { + StringBuilder sb = new StringBuilder(); + for (ShooterPreset shooterPreset : shooterConfigs) { + sb.append(shooterPreset.getDistance()).append(",") + .append(shooterPreset.getFlywheelSpeed()).append(",") + .append(shooterPreset.getHoodEjectAngle()).append("\n"); + } + System.out.println(sb); + } } diff --git a/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java b/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java index fb485ff7..a1bbd46d 100644 --- a/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java +++ b/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java @@ -110,5 +110,14 @@ public void setShooterConfig(ShooterConfig shooterConfig) { shooterConfigLock.unlock(); } } + + public void printShooterConfig() { + shooterConfigLock.lock(); + try { + shooterConfig.printCSV(); + } finally { + shooterConfigLock.unlock(); + } + } } From a8abf856fced366e30b9cd55e168cfb532b24bdb Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 17:21:29 -0700 Subject: [PATCH 021/108] use the fake goal pos to determine the allowed error of our turn --- .../java/frc/subsystem/VisionManager.java | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 99d96ac1..6cf3d32a 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -61,13 +61,8 @@ public void selfTest() { public void logData() { logData("Distance to Target", Units.metersToInches(getDistanceToTarget())); logData("Rotation Target", getAngleToTarget().getDegrees()); - logData("Allowed Turn Error", getAllowedTurnError()); logData("Allow Shooting Robot Speed", drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED); - logData("Is Robot Allowed Shoot Aiming", - Math.abs((angleOf(getRelativeGoalTranslation()) - .minus(robotTracker.getGyroAngle())).getRadians()) - < getAllowedTurnError()); logData("Is Robot Allowed Shoot Tilt", Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3); @@ -85,6 +80,14 @@ public void logData() { RobotTracker.getInstance().getLastEstimatedPoseMeters().getTranslation().minus(aimToPosition); logData("Calculated Target X", fieldCentricCords.getX()); logData("Calculated Target Y", fieldCentricCords.getY()); + + double allowedTurnError = getAllowedTurnError(aimToPosition.getNorm()); + + logData("Allowed Turn Error", allowedTurnError); + logData("Is Robot Allowed Shoot Aiming", + Math.abs((angleOf(getRelativeGoalTranslation()) + .minus(robotTracker.getGyroAngle())).getRadians()) + < allowedTurnError); } @@ -116,11 +119,14 @@ public void autoTurnRobotToTarget(ControllerDriveInputs controllerDriveInputs, b } private void tryToShoot(Translation2d aimToPosition, double targetAngularSpeed, boolean doSpeedCheck) { - if (Math.abs((angleOf(aimToPosition).minus(robotTracker.getGyroAngle())).getRadians()) < getAllowedTurnError() + //@formatter:off + if (Math.abs((angleOf(aimToPosition).minus(robotTracker.getGyroAngle())).getRadians()) + < getAllowedTurnError(aimToPosition.getNorm()) && Math.abs(robotTracker.getLatencyCompedChassisSpeeds().omegaRadiansPerSecond - targetAngularSpeed) - < Math.toRadians(8) - && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck) && - Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) { + < Math.toRadians(8) + && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck) + && Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) { + //@formatter:on shooter.setFiring(limelight.isTargetVisible() || DriverStation.isAutonomous()); if (shooter.isFiring()) { if (!checksPassedLastTime && lastPrintTime + 0.5 < Timer.getFPGATimestamp()) { @@ -242,7 +248,16 @@ public double getDistanceToTarget() { * @return The allowed turn error in radians */ private double getAllowedTurnError() { - return Math.tan((Constants.GOAL_RADIUS * 0.8) / getDistanceToTarget()); + return getAllowedTurnError(getDistanceToTarget()); + } + + /** + * {@code Math.tan(Constants.GOAL_RADIUS / getDistanceToTarget())} + * + * @return The allowed turn error in radians + */ + private double getAllowedTurnError(double distance) { + return Math.tan((Constants.GOAL_RADIUS * 0.8) / distance); } @Contract(pure = true) From be519a87eb84f27e9d4da6ad734348cabdc319d4 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 17:28:55 -0700 Subject: [PATCH 022/108] smooth out the shooter config a bit --- src/main/deploy/shooter/shooterconfig.json | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index c1d58560..48c47622 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -15,10 +15,6 @@ "hoodEjectAngle" : 56.0, "flywheelSpeed" : 1800.0, "distance" : 106.0 - }, { - "hoodEjectAngle" : 53.0, - "flywheelSpeed" : 1800.0, - "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 1900.0, @@ -37,7 +33,7 @@ "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2200.0, + "flywheelSpeed" : 2250.0, "distance" : 175.0 }, { "hoodEjectAngle" : 51.0, From 64858dbe1e0ac9039887a959c7dfd8ad37dcd09f Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 17:30:06 -0700 Subject: [PATCH 023/108] reduce the allowed turn error when shooting --- src/main/java/frc/subsystem/VisionManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 6cf3d32a..781ac2f1 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -257,7 +257,7 @@ private double getAllowedTurnError() { * @return The allowed turn error in radians */ private double getAllowedTurnError(double distance) { - return Math.tan((Constants.GOAL_RADIUS * 0.8) / distance); + return Math.tan((Constants.GOAL_RADIUS * 0.4) / distance); } @Contract(pure = true) From 6603550b66219d20e9cf815b54afff7af4ac108f Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 25 Mar 2022 14:37:14 -0700 Subject: [PATCH 024/108] detect if the vision target is touching the edge of the screen. If it is we will not use that vision data. --- .../java/frc/subsystem/VisionManager.java | 5 ++-- src/main/java/frc/utility/Limelight.java | 24 +++++++++++++++++++ 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 781ac2f1..750dd177 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -343,10 +343,9 @@ public void update() { logData("Vision Pose Angle", visionPose.getRotation().getRadians()); logData("Vision Pose Time", getLimelightTime()); - //TODO: Check that the contours aren't touching the edge of the screen before using them if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation().plus(robotPositionOffset), - robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) { - + robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED + && !limelight.areCornersTouchingEdge()) { robotTracker.addVisionMeasurement(robotTranslation, getLimelightTime()); diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 7c84d4be..e285f88a 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -5,6 +5,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.Vector2d; import org.jetbrains.annotations.NotNull; import java.util.HashMap; @@ -14,6 +15,7 @@ * This class is used to get data from the limelight network tables */ public final class Limelight { + public static final double[] EMPTY_DOUBLE_ARRAY = new double[0]; final @NotNull NetworkTable limelightTable; final @NotNull NetworkTable limelightGuiTable; @@ -260,4 +262,26 @@ public double getDistance() { return 0; } } + + + public Vector2d[] getCorners() { + Vector2d[] processedCorners = new Vector2d[4]; + double[] corners = limelightTable.getEntry("tcornxy").getDoubleArray(EMPTY_DOUBLE_ARRAY); + for (int i = 0; i < corners.length; i += 2) { + if (i + 1 < corners.length) { + processedCorners[i / 2] = new Vector2d(corners[i], corners[i + 1]); + } + } + return processedCorners; + } + + public boolean areCornersTouchingEdge() { + Vector2d[] corners = getCorners(); + for (Vector2d corner : corners) { + if (corner.x < 2 || corner.x > 318 || corner.y < 2 || corner.y > 238) { + return true; + } + } + return false; + } } \ No newline at end of file From 6692df3e1fb0a64b0bc6b7437a5a0f5421d47002 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Fri, 25 Mar 2022 19:59:21 -0700 Subject: [PATCH 025/108] new: added outtake override for buttonpanel 5 --- src/main/java/frc/robot/Robot.java | 6 ++++++ src/main/java/frc/subsystem/Hopper.java | 27 ++++++++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7bdbab67..68ae673f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -506,12 +506,18 @@ public void teleopPeriodic() { robotTracker.resetPosition(new Pose2d(robotTracker.getLastEstimatedPoseMeters().getTranslation(), new Rotation2d(0))); } + // Override outtake to always intake + if (buttonPanel.getRisingEdge(5)) { + hopper.toggleEjectOverride(); + } + if (xbox.getRisingEdge(Controller.XboxButtons.START)) { useFieldRelative = !useFieldRelative; System.out.println("Field relative: " + useFieldRelative); SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative); } + if (stick.getRawButton(4)) { visionManager.forceVisionOn(resettingPoseVisionOn); visionManager.forceUpdatePose(); diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 895a132e..9ecb2365 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -24,6 +24,7 @@ public final class Hopper extends AbstractSubsystem { private RelativeEncoder outtakeWheelsQuadrature; private LazyCANSparkMax outtakeWheels; private double lastDetectionTime; + private boolean disableEject = false; private final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME); public static Hopper getInstance() { @@ -95,6 +96,13 @@ private void updateAllianceColor() { } } + /** + * Toggles disableEject + */ + public void toggleEjectOverride() { + disableEject = !disableEject; + } + /** * Updates state of outtake based on color sensor and intake direction */ @@ -115,6 +123,15 @@ private void updateOuttakeState() { } } + private void updateOuttakeStateOverrided() { + Intake intake = Intake.getInstance(); + if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + outtakeState = OuttakeState.INTAKE; + } else { + outtakeState = OuttakeState.OFF; + } + } + /** * Sets the percent outtake between 1 and -1 */ @@ -143,9 +160,15 @@ public BallColor getBallColor() { public void update() { updateAllianceColor(); getBallColor(); - updateOuttakeState(); + + if (!disableEject) { + updateOuttakeState(); + } else { + updateOuttakeStateOverrided(); + } // Outtake motor control + switch (outtakeState) { case OFF: setOuttakePercentOutput(0); @@ -157,6 +180,7 @@ public void update() { setOuttakePercentOutput(-Constants.OUTTAKE_SPEED); break; } + } // Hopper Motor Control switch (wantedHopperState) { @@ -210,6 +234,7 @@ public void logData() { logData("Outtake SetVoltage", outtakeWheels.getSetpoint()); logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); logData("Current Ball Color", getBallColor()); + logData("Eject Disabled", disableEject); } @Override From 402b3f492a03285148522c17dce8acb532b7ce93 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Fri, 25 Mar 2022 20:23:03 -0700 Subject: [PATCH 026/108] make an opposite eject mode that looks for friendly balls and only intakes when it sees them --- src/main/java/frc/robot/Constants.java | 6 +++ src/main/java/frc/subsystem/Hopper.java | 72 ++++++++++++++++++++----- 2 files changed, 65 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ff335517..7cde6dde 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -229,6 +229,12 @@ public final class Constants { public static final double OUTTAKE_REDUCTION = (3.0 / 1.0); public static final int OUTTAKE_CURRENT_LIMIT = 20; public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 200; + public static final double OUTTAKE_VERTICAL_OFFSET_THRESHOLD = 0; + /** + * Outtake Always intake has outtake always intaking until opposite ball color is found The other option is to have the + * outtake always outtaking until a friendly ball color is found + */ + public static final boolean OUTTAKE_ALWAYS_INTAKE = true; /** * Maximum Percent Output of Outtake wheels */ diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 9ecb2365..12ba6d8d 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -55,6 +55,7 @@ public enum BallColor { } BallColor opposingAllianceColor = BallColor.BLUE; + BallColor friendlyAllianceColor = BallColor.RED; private Hopper() { super(Constants.HOPPER_PERIOD, 5); @@ -89,10 +90,25 @@ private Hopper() { private void updateAllianceColor() { if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) { opposingAllianceColor = BallColor.RED; - intakeLimelight.setPipeline(1); + friendlyAllianceColor = BallColor.BLUE; + + if (Constants.OUTTAKE_ALWAYS_INTAKE) { + // Will detect opposite color balls and outtake when it sees them + intakeLimelight.setPipeline(1); + } else { + // Will detect same colored balls and intake when it sees them + intakeLimelight.setPipeline(0); + } } else { opposingAllianceColor = BallColor.BLUE; - intakeLimelight.setPipeline(0); + friendlyAllianceColor = BallColor.RED; + if (Constants.OUTTAKE_ALWAYS_INTAKE) { + // Will detect same colored balls and intake when it sees them + intakeLimelight.setPipeline(0); + } else { + // Will detect opposite color balls and outtake when it sees them + intakeLimelight.setPipeline(1); + } } } @@ -109,17 +125,39 @@ public void toggleEjectOverride() { private void updateOuttakeState() { // If color sensor detects a ball or Intake intake = Intake.getInstance(); - if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { - if (getBallColor() == opposingAllianceColor) { - lastDetectionTime = Timer.getFPGATimestamp(); - outtakeState = OuttakeState.EJECT; - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { - outtakeState = OuttakeState.EJECT; + + if (Constants.OUTTAKE_ALWAYS_INTAKE) { + // Intake is running and open + if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + // Ball Color is opposite + if (getBallColor() == opposingAllianceColor) { + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.EJECT; + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { + // Opposite ball color detected within a certain time frame + outtakeState = OuttakeState.EJECT; + } else { + outtakeState = OuttakeState.OFF; + } } else { outtakeState = OuttakeState.OFF; } } else { - outtakeState = OuttakeState.OFF; + // Intake is running and open + if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + // Ball Color is opposite + if (getBallColor() == friendlyAllianceColor) { + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.INTAKE; + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { + // Opposite ball color detected within a certain time frame + outtakeState = OuttakeState.INTAKE; + } else { + outtakeState = OuttakeState.EJECT; + } + } else { + outtakeState = OuttakeState.OFF; + } } } @@ -147,10 +185,18 @@ public OuttakeState getOuttakeState() { public BallColor getBallColor() { @NotNull BallColor currentBallColor; - if (intakeLimelight.isTargetVisible()) { - currentBallColor = opposingAllianceColor; + if (Constants.OUTTAKE_ALWAYS_INTAKE) { + if (intakeLimelight.getVerticalOffset() < Constants.OUTTAKE_VERTICAL_OFFSET_THRESHOLD) { + currentBallColor = opposingAllianceColor; + } else { + currentBallColor = BallColor.NO_BALL; + } } else { - currentBallColor = BallColor.NO_BALL; + if (intakeLimelight.getVerticalOffset() < Constants.OUTTAKE_VERTICAL_OFFSET_THRESHOLD) { + currentBallColor = friendlyAllianceColor; + } else { + currentBallColor = BallColor.NO_BALL; + } } return currentBallColor; @@ -180,7 +226,7 @@ public void update() { setOuttakePercentOutput(-Constants.OUTTAKE_SPEED); break; } - } + // Hopper Motor Control switch (wantedHopperState) { From 4fe00d79a9033d2a27c068cc6642f0d99a69e105 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Fri, 25 Mar 2022 20:28:57 -0700 Subject: [PATCH 027/108] Addressed outtake issues --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/subsystem/Hopper.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7cde6dde..be7bbfa2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -241,7 +241,7 @@ public final class Constants { public static final double OUTTAKE_SPEED = 1d; /** - * Outtake will run for this time (MS) after color sensor trigger + * Outtake will run for this time (Sec) after color sensor trigger */ public static final double OUTTAKE_RUN_PERIOD = 50 / 1000d; diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 12ba6d8d..5915a514 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -137,10 +137,10 @@ private void updateOuttakeState() { // Opposite ball color detected within a certain time frame outtakeState = OuttakeState.EJECT; } else { - outtakeState = OuttakeState.OFF; + outtakeState = OuttakeState.INTAKE; } } else { - outtakeState = OuttakeState.OFF; + outtakeState = OuttakeState.INTAKE; } } else { // Intake is running and open @@ -156,7 +156,7 @@ private void updateOuttakeState() { outtakeState = OuttakeState.EJECT; } } else { - outtakeState = OuttakeState.OFF; + outtakeState = OuttakeState.EJECT; } } } From cc964bf294e255ccfe9c7c32d8be1677d86e9de8 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Fri, 25 Mar 2022 20:36:01 -0700 Subject: [PATCH 028/108] Re-added Hood Eject --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/Robot.java | 6 +++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index be7bbfa2..c5eef417 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -273,6 +273,9 @@ public final class Constants { */ public static final double ALLOWED_SHOOTER_SPEED_ERROR_RPM = 150; + public static final int SHOOTER_EJECT_SPEED = 500; + public static final double HOOD_EJECT_ANGLE = 85; + /** * Conversion from Falcon Sensor Units / 100ms to RPM 2048 is Sensor Units Per Revolution 600 Converts From Time of 100ms to 1 * minute diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5a1de4a9..efc29727 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -601,11 +601,15 @@ private void runShooter() { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(139); } else if (buttonPanel.getRisingEdge(3)) { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(40); + } else if (buttonPanel.getRisingEdge(6)) { + shooter.setSpeed(Constants.SHOOTER_EJECT_SPEED); + shooter.setHoodPosition(Constants.HOOD_EJECT_ANGLE); } } private boolean isTryingToRunShooterFromButtonPanel() { - return buttonPanel.getRawButton(1) || buttonPanel.getRawButton(2) || buttonPanel.getRawButton(3); + return buttonPanel.getRawButton(1) || buttonPanel.getRawButton(2) || buttonPanel.getRawButton( + 3) || buttonPanel.getRawButton(6); } private static final ControllerDriveInputs NO_MOTION_CONTROLLER_INPUTS = new ControllerDriveInputs(0, 0, 0); From b5b2091bb005ec5335dbe9ee240edf7a2eb712a8 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Fri, 25 Mar 2022 21:14:01 -0700 Subject: [PATCH 029/108] Minor outtake changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/subsystem/Hopper.java | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c5eef417..4b2b204e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -238,7 +238,7 @@ public final class Constants { /** * Maximum Percent Output of Outtake wheels */ - public static final double OUTTAKE_SPEED = 1d; + public static final double OUTTAKE_SPEED = .5; /** * Outtake will run for this time (Sec) after color sensor trigger diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 5915a514..1a0b4a99 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -126,6 +126,11 @@ private void updateOuttakeState() { // If color sensor detects a ball or Intake intake = Intake.getInstance(); + if (wantedHopperState == HopperState.REVERSE) { + outtakeState = OuttakeState.EJECT; + return; + } + if (Constants.OUTTAKE_ALWAYS_INTAKE) { // Intake is running and open if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { @@ -140,7 +145,7 @@ private void updateOuttakeState() { outtakeState = OuttakeState.INTAKE; } } else { - outtakeState = OuttakeState.INTAKE; + outtakeState = OuttakeState.OFF; } } else { // Intake is running and open @@ -156,12 +161,12 @@ private void updateOuttakeState() { outtakeState = OuttakeState.EJECT; } } else { - outtakeState = OuttakeState.EJECT; + outtakeState = OuttakeState.OFF; } } } - private void updateOuttakeStateOverrided() { + private void updateOuttakeStateOverridden() { Intake intake = Intake.getInstance(); if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { outtakeState = OuttakeState.INTAKE; @@ -210,7 +215,7 @@ public void update() { if (!disableEject) { updateOuttakeState(); } else { - updateOuttakeStateOverrided(); + updateOuttakeStateOverridden(); } // Outtake motor control @@ -275,10 +280,10 @@ public void selfTest() { @Override public void logData() { logData("Hopper Motor Current", hopperMotor.getOutputCurrent()); - - logData("Outtae State", outtakeState); + logData("Outtake State", outtakeState); logData("Outtake SetVoltage", outtakeWheels.getSetpoint()); logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity()); + logData("Outtake Current", outtakeWheels.getOutputCurrent()); logData("Current Ball Color", getBallColor()); logData("Eject Disabled", disableEject); } From 12cdf10bad5e0cf8c0b085b82b221c7ecd4e49e5 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Fri, 25 Mar 2022 22:00:55 -0700 Subject: [PATCH 030/108] outtake field changes --- src/main/java/frc/robot/Constants.java | 5 +++-- src/main/java/frc/robot/Robot.java | 12 ++++++++++-- src/main/java/frc/subsystem/Hopper.java | 12 +++++++----- src/main/java/frc/subsystem/Intake.java | 7 ++++++- src/main/java/frc/subsystem/RobotTracker.java | 1 + src/main/java/frc/subsystem/VisionManager.java | 7 +++---- 6 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4b2b204e..88c21de6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -238,7 +238,8 @@ public final class Constants { /** * Maximum Percent Output of Outtake wheels */ - public static final double OUTTAKE_SPEED = .5; + public static final double INTAKEING_OUTTAKE_SPEED = .5; + public static final double EJECT_OUTTAKE_SPEED = .5; /** * Outtake will run for this time (Sec) after color sensor trigger @@ -511,5 +512,5 @@ public final class Constants { public static final int WEB_DASHBOARD_PORT = 5802; - public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 200; + public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 50; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efc29727..63fd1175 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -140,6 +140,8 @@ public class Robot extends TimedRobot { @NotNull public static final String RED = "RED"; @NotNull public static final String BLUE = "BLUE"; + private static boolean hoodEject = false; + public static final SendableChooser sideChooser = new SendableChooser<>(); //Subsystems @@ -452,7 +454,9 @@ public void teleopPeriodic() { if (xbox.getRawAxis(2) > 0.1) { if (isTryingToRunShooterFromButtonPanel()) { //If vision is off - shooter.setHoodPosition(shooterPreset.getHoodEjectAngle()); + if (!hoodEject) { + shooter.setHoodPosition(shooterPreset.getHoodEjectAngle()); + } shooter.setSpeed(shooterPreset.getFlywheelSpeed()); shooter.setFiring(true); hopper.setHopperState(Hopper.HopperState.ON); @@ -601,9 +605,12 @@ private void runShooter() { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(139); } else if (buttonPanel.getRisingEdge(3)) { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(40); - } else if (buttonPanel.getRisingEdge(6)) { + } else if (buttonPanel.getRawButton(6)) { shooter.setSpeed(Constants.SHOOTER_EJECT_SPEED); shooter.setHoodPosition(Constants.HOOD_EJECT_ANGLE); + hoodEject = true; + } else { + hoodEject = false; } } @@ -752,6 +759,7 @@ private void startSubsystems() { shooter.start(); climber.start(); visionManager.start(); + DashboardHandler.getInstance().start(); } public void killAuto() { diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 1a0b4a99..b1c3a049 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -168,7 +168,9 @@ private void updateOuttakeState() { private void updateOuttakeStateOverridden() { Intake intake = Intake.getInstance(); - if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + if (wantedHopperState == HopperState.REVERSE) { + outtakeState = OuttakeState.EJECT; + } else if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { outtakeState = OuttakeState.INTAKE; } else { outtakeState = OuttakeState.OFF; @@ -225,10 +227,10 @@ public void update() { setOuttakePercentOutput(0); break; case EJECT: - setOuttakePercentOutput(Constants.OUTTAKE_SPEED); + setOuttakePercentOutput(Constants.EJECT_OUTTAKE_SPEED); break; case INTAKE: - setOuttakePercentOutput(-Constants.OUTTAKE_SPEED); + setOuttakePercentOutput(-Constants.INTAKEING_OUTTAKE_SPEED); break; } @@ -265,11 +267,11 @@ public void selfTest() { setHopperState(HopperState.OFF); OrangeUtility.sleep(5000); - setOuttakePercentOutput(Constants.OUTTAKE_SPEED); + setOuttakePercentOutput(Constants.INTAKEING_OUTTAKE_SPEED); System.out.println("Ejecting"); OrangeUtility.sleep(Constants.TEST_TIME_MS); - setOuttakePercentOutput(-Constants.OUTTAKE_SPEED); + setOuttakePercentOutput(-Constants.INTAKEING_OUTTAKE_SPEED); System.out.println("Intaking"); OrangeUtility.sleep(Constants.TEST_TIME_MS); diff --git a/src/main/java/frc/subsystem/Intake.java b/src/main/java/frc/subsystem/Intake.java index 11667468..d2fd13e6 100644 --- a/src/main/java/frc/subsystem/Intake.java +++ b/src/main/java/frc/subsystem/Intake.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; +import frc.subsystem.Hopper.OuttakeState; import frc.utility.OrangeUtility; import frc.utility.Timer; import frc.utility.controllers.LazyCANSparkMax; @@ -100,7 +101,11 @@ public synchronized void setWantedIntakeState(IntakeState intakeState) { private void setIntakeState(IntakeState intakeState) { switch (intakeState) { case INTAKE: - intakeMotor.set(Constants.INTAKE_MOTOR_SPEED); + if (Hopper.getInstance().getOuttakeState() == OuttakeState.EJECT) { + intakeMotor.set(-Constants.EJECT_OUTTAKE_SPEED); + } else { + intakeMotor.set(Constants.INTAKE_MOTOR_SPEED); + } break; case EJECT: diff --git a/src/main/java/frc/subsystem/RobotTracker.java b/src/main/java/frc/subsystem/RobotTracker.java index 19909a21..2476d9e8 100644 --- a/src/main/java/frc/subsystem/RobotTracker.java +++ b/src/main/java/frc/subsystem/RobotTracker.java @@ -389,6 +389,7 @@ public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle, b gyroOffset = latestEstimatedPose.getRotation().minus(gyroAngle); } swerveDriveOdometry.resetPosition(pose, gyroAngle); + latestEstimatedPose = pose; latencyCompensatedPose = latestEstimatedPose; latestChassisSpeeds = getRotatedSpeeds( diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 750dd177..2608be07 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -213,9 +213,9 @@ private Translation2d getRelativeGoalTranslation() { public void forceUpdatePose() { Optional visionTranslation = getVisionTranslation(); visionTranslation.ifPresent( - mutableTranslation2d -> { + translation2d -> { robotTracker.addVisionMeasurement( - mutableTranslation2d, + translation2d, getLimelightTime()); robotPositionOffset = new Translation2d(); } @@ -344,8 +344,7 @@ public void update() { logData("Vision Pose Time", getLimelightTime()); if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation().plus(robotPositionOffset), - robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED - && !limelight.areCornersTouchingEdge()) { + robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) { robotTracker.addVisionMeasurement(robotTranslation, getLimelightTime()); From 1b1901be172b6e0c9b3d477b2b259e2b1be6a1b8 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 01:56:54 -0700 Subject: [PATCH 031/108] changes to get shooting and moving working (mostly) - fix array allocation in getCorners() being wrong - use corners to detect when the target is touching the edge of the screen - fix needing to reset the position on the robot tracker twice to get it to work - fix robot stuttering due to high cpu usage - update the shooter config - increased allowed error when shooting - optimize lookahead times when shooting and moving --- src/main/deploy/shooter/shooterconfig.json | 16 ++++--- src/main/java/frc/subsystem/Drive.java | 2 +- src/main/java/frc/subsystem/RobotTracker.java | 43 +++---------------- .../java/frc/subsystem/VisionManager.java | 37 +++++++++------- src/main/java/frc/utility/Limelight.java | 5 ++- .../java/frc/subsystem/RobotTrackerTest.java | 2 +- 6 files changed, 44 insertions(+), 61 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 48c47622..42dc5425 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -15,9 +15,13 @@ "hoodEjectAngle" : 56.0, "flywheelSpeed" : 1800.0, "distance" : 106.0 + }, { + "hoodEjectAngle" : 53.0, + "flywheelSpeed" : 1800.0, + "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1900.0, + "flywheelSpeed" : 2000.0, "distance" : 120.0 }, { "hoodEjectAngle" : 51.0, @@ -25,15 +29,15 @@ "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2000.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2200.0, + "flywheelSpeed" : 2100.0, "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2250.0, + "flywheelSpeed" : 2100.0, "distance" : 175.0 }, { "hoodEjectAngle" : 51.0, @@ -45,11 +49,11 @@ "distance" : 200.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2500.0, + "flywheelSpeed" : 2400.0, "distance" : 210.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2600.0, + "flywheelSpeed" : 2500.0, "distance" : 225.0 }, { "hoodEjectAngle" : 51.0, diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 92d713a2..f1d432ce 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -423,7 +423,7 @@ public double getAngleDiff(double targetAngle, double currentAngle) { @Contract(mutates = "param") @NotNull ChassisSpeeds limitAcceleration(@NotNull ChassisSpeeds commandedVelocity) { double dt; - if ((Timer.getFPGATimestamp() - lastLoopTime) > ((double) Constants.DRIVE_PERIOD / 1000) * 4) { + if ((Timer.getFPGATimestamp() - lastLoopTime) > ((double) Constants.DRIVE_PERIOD / 1000) * 20) { // If the dt is a lot greater than our nominal dt reset the acceleration limiting // (ex. we've been disabled for a while) lastRequestedVelocity = RobotTracker.getInstance().getLatencyCompedChassisSpeeds(); diff --git a/src/main/java/frc/subsystem/RobotTracker.java b/src/main/java/frc/subsystem/RobotTracker.java index 2476d9e8..1fda1826 100644 --- a/src/main/java/frc/subsystem/RobotTracker.java +++ b/src/main/java/frc/subsystem/RobotTracker.java @@ -37,7 +37,6 @@ public final class RobotTracker extends AbstractSubsystem { private @NotNull Pose2d latestEstimatedPose = new Pose2d(); private @NotNull ChassisSpeeds latestChassisSpeeds = new ChassisSpeeds(); - private @NotNull Pose2d latencyCompensatedPose = new Pose2d(); private @NotNull ChassisSpeeds latencyCompensatedChassisSpeeds = new ChassisSpeeds(); private final SwerveDriveOdometry swerveDriveOdometry; @@ -119,27 +118,6 @@ public synchronized Rotation2d getAngle() { return latestEstimatedPose.getRotation(); } - /** - * Resets the position on the field to 0,0 with a rotation of 0 degrees - */ - synchronized public void resetOdometry() { - lock.writeLock().lock(); - try { - Rotation2d rawGyroSensor = gyroSensor.getRotation2d(); - swerveDriveOdometry.resetPosition(new Pose2d(), rawGyroSensor); - gyroOffset = latestEstimatedPose.getRotation().minus(rawGyroSensor); - latencyCompensatedPose = latestEstimatedPose; - - latestChassisSpeeds = getRotatedSpeeds( - drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()), - getGyroAngle()); - - latencyCompensatedChassisSpeeds = latestChassisSpeeds; - } finally { - lock.writeLock().unlock(); - } - } - private final List> previousGyroRotations = new ArrayList<>(102); // 1s of data at 10ms per double currentOdometryTime = -1; @@ -178,7 +156,6 @@ public void update() { try { latestEstimatedPose = swerveDriveOdometry.getPoseMeters(); //gyroOffset = latestEstimatedPose.getRotation().minus(rawGyroSensor); - latencyCompensatedPose = latestEstimatedPose; latestChassisSpeeds = getRotatedSpeeds( drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()), @@ -374,23 +351,21 @@ private void updateOdometry(double currentTimeSeconds, Rotation2d gyroAngle, Swe * @param pose The position on the field that your robot is at. */ public void resetPosition(Pose2d pose) { - resetPosition(pose, gyroSensor.getRotation2d(), true); + resetPosition(pose, gyroSensor.getRotation2d()); } public void resetPosition(Translation2d pose) { - resetPosition(new Pose2d(pose, getGyroAngle()), gyroSensor.getRotation2d(), false); + resetPosition(new Pose2d(pose, getGyroAngle()), gyroSensor.getRotation2d()); } - public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle, boolean offsetGyro) { + public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle) { lock.writeLock().lock(); try { - if (offsetGyro) { - gyroOffset = latestEstimatedPose.getRotation().minus(gyroAngle); - } + + gyroOffset = pose.getRotation().minus(gyroAngle); swerveDriveOdometry.resetPosition(pose, gyroAngle); latestEstimatedPose = pose; - latencyCompensatedPose = latestEstimatedPose; latestChassisSpeeds = getRotatedSpeeds( drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()), @@ -430,12 +405,7 @@ public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle, b */ @Contract(pure = true) public @NotNull Pose2d getLatencyCompedPoseMeters() { - lock.readLock().lock(); - try { - return latencyCompensatedPose; - } finally { - lock.readLock().unlock(); - } + return getLastEstimatedPoseMeters(); } /** @@ -486,6 +456,7 @@ public void logData() { logData("Last Estimated Robot Pose X", getLastEstimatedPoseMeters().getX()); logData("Last Estimated Robot Pose Y", getLastEstimatedPoseMeters().getY()); logData("Last Estimated Robot Pose Angle", getLastEstimatedPoseMeters().getRotation().getRadians()); + logData("Gyro Robot Pose Angle", getGyroAngle().getRadians()); logData("Last Estimated Robot Velocity X", getLastChassisSpeeds().vxMetersPerSecond); logData("Last Estimated Robot Velocity Y", getLastChassisSpeeds().vyMetersPerSecond); logData("Last Estimated Robot Velocity Theta", getLastChassisSpeeds().omegaRadiansPerSecond); diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 2608be07..50bfe36e 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -104,10 +104,10 @@ public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean us new State(targetAngle, (futureTargetAngle - targetAngle) * 10), useFieldRelative, 0); + Translation2d aimChecksPosition = getAdjustedTranslation(0.00); - updateShooterState(aimToPosition.getNorm()); - - tryToShoot(aimToPosition, (futureTargetAngle - targetAngle) * 10, false); + updateShooterState(aimChecksPosition.getNorm()); + tryToShoot(aimChecksPosition, (futureTargetAngle - targetAngle) * 10, false); } @@ -257,7 +257,7 @@ private double getAllowedTurnError() { * @return The allowed turn error in radians */ private double getAllowedTurnError(double distance) { - return Math.tan((Constants.GOAL_RADIUS * 0.4) / distance); + return Math.tan((Constants.GOAL_RADIUS * 0.6) / distance); } @Contract(pure = true) @@ -317,6 +317,7 @@ public boolean isVisionForcedOn() { @Override public void update() { + robotPositionOffset = new Translation2d(); Pose2d robotTrackerPose = robotTracker.getLatencyCompedPoseMeters(); Translation2d relativeGoalPos = getRelativeGoalTranslation(); @@ -326,7 +327,7 @@ public void update() { blinkinLED.setStatus(limelightNotConnectedStatus); } - if (Math.abs(angleToTarget - robotTrackerPose.getRotation().getRadians()) < Math.toRadians(50)) { + if (Math.abs(angleToTarget - robotTracker.getGyroAngle().getRadians()) < Math.toRadians(50)) { forceVisionOn(updateLoopSource); } else { unForceVisionOn(updateLoopSource); @@ -343,16 +344,22 @@ public void update() { logData("Vision Pose Angle", visionPose.getRotation().getRadians()); logData("Vision Pose Time", getLimelightTime()); - if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation().plus(robotPositionOffset), - robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) { - - robotTracker.addVisionMeasurement(robotTranslation, - getLimelightTime()); - robotPositionOffset = new Translation2d(); + Translation2d trackerTranslation = robotTracker.getLatencyCompedPoseMeters().getTranslation(); + logData("Tracker Translation X", trackerTranslation.getX()); + logData("Tracker Translation Y", trackerTranslation.getY()); - logData("Using Vision Info", "Using Vision Info"); - blinkinLED.setStatus(limelightUsingVisionStatus); + if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation(), + robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) { + if (limelight.areCornersTouchingEdge()) { + logData("Using Vision Info", "Corners touching edge"); + } else { + robotTracker.addVisionMeasurement(robotTranslation, + getLimelightTime()); + robotPositionOffset = new Translation2d(); + logData("Using Vision Info", "Using Vision Info"); + blinkinLED.setStatus(limelightUsingVisionStatus); + } } else { logData("Using Vision Info", "Position is too far from expected"); blinkinLED.setStatus(limelightTooFarFromExpectedStatus); @@ -458,9 +465,9 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat double timeOfFlightFrames; if (distance < 120) { - timeOfFlightFrames = 28; + timeOfFlightFrames = 25; } else { - timeOfFlightFrames = (0.09 * (distance - 120)) + 28; + timeOfFlightFrames = (0.065 * (distance - 120)) + 25; } //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542; diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index e285f88a..23fecf19 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -265,8 +265,8 @@ public double getDistance() { public Vector2d[] getCorners() { - Vector2d[] processedCorners = new Vector2d[4]; double[] corners = limelightTable.getEntry("tcornxy").getDoubleArray(EMPTY_DOUBLE_ARRAY); + Vector2d[] processedCorners = new Vector2d[corners.length / 2]; for (int i = 0; i < corners.length; i += 2) { if (i + 1 < corners.length) { processedCorners[i / 2] = new Vector2d(corners[i], corners[i + 1]); @@ -278,7 +278,8 @@ public Vector2d[] getCorners() { public boolean areCornersTouchingEdge() { Vector2d[] corners = getCorners(); for (Vector2d corner : corners) { - if (corner.x < 2 || corner.x > 318 || corner.y < 2 || corner.y > 238) { + if (corner.x < 30 || corner.x > 290 || corner.y < 30 || corner.y > 210) { + return true; } } diff --git a/src/test/java/frc/subsystem/RobotTrackerTest.java b/src/test/java/frc/subsystem/RobotTrackerTest.java index ddc3d894..9a1005ae 100644 --- a/src/test/java/frc/subsystem/RobotTrackerTest.java +++ b/src/test/java/frc/subsystem/RobotTrackerTest.java @@ -49,7 +49,7 @@ void robotTrackerTest() throws NoSuchFieldException, IllegalAccessException, NoS double period = 0.05; Timer.setTime(0); - robotTracker.resetPosition(new Pose2d(x, y, new Rotation2d(theta)), new Rotation2d(0), true); + robotTracker.resetPosition(new Pose2d(x, y, new Rotation2d(theta)), new Rotation2d(0)); double largeRandomX = random.nextDouble() * 10 - 5; double largeRandomY = random.nextDouble() * 10 - 5; From 49dc5801229cd1af5c01468a31910d3f6c3363c7 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 12:02:32 -0700 Subject: [PATCH 032/108] replace all the shooter delayed actions vars with a single one Instead of storing the times of when to do many actions we instead just store the time when we last shot. All the other functions then use that time to run their actions --- src/main/java/frc/subsystem/Shooter.java | 70 +++++++----------------- 1 file changed, 21 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java index 7b09f405..6435ab67 100644 --- a/src/main/java/frc/subsystem/Shooter.java +++ b/src/main/java/frc/subsystem/Shooter.java @@ -12,7 +12,6 @@ import com.revrobotics.SparkMaxPIDController; import edu.wpi.first.networktables.EntryListenerFlags; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -26,6 +25,8 @@ import frc.utility.shooter.visionlookup.ShooterPreset; import org.jetbrains.annotations.NotNull; +import static frc.robot.Constants.*; + /** * Shooter class controls the shooter flywheel, feeder wheel, and variable hood Has motor control wrappers for setting velocity * and position for respective parts. @@ -37,9 +38,6 @@ public final class Shooter extends AbstractSubsystem { - // PID TUNING - final @NotNull NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); - final @NotNull NetworkTableEntry shooterP = SmartDashboard.getEntry("ShooterPIDP"); final @NotNull NetworkTableEntry shooterI = SmartDashboard.getEntry("ShooterPIDI"); final @NotNull NetworkTableEntry shooterD = SmartDashboard.getEntry("ShooterPIDD"); @@ -55,7 +53,6 @@ public final class Shooter extends AbstractSubsystem { // Feeder private final LazyTalonFX feederWheel; - private double forceFeederOnTime; // Hood private final LazyCANSparkMax hoodMotor; @@ -84,8 +81,7 @@ public final class Shooter extends AbstractSubsystem { public boolean isFiring() { return shooterState == ShooterState.ON && ((feederWheelState == FeederWheelState.FORWARD) - && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) || feederChecksDisabled) - || Timer.getFPGATimestamp() < forceFeederOnTime); + && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) || feederChecksDisabled)); } // Declarations of Modes and States @@ -556,31 +552,6 @@ public void forceFeederForward() { feederWheel.set(ControlMode.PercentOutput, Constants.FEEDER_WHEEL_SPEED); } - private double feederLockPosition = 0; - - public void lockFeederWheel() { - - // Initializes feederLockPosition if it has not been set before - if (feederLockPosition == 0) { - feederLockPosition = feederWheel.getSelectedSensorPosition(); - } - -// // Will lock feeder to position when locking starts -// if (Math.abs(feederWheel.getSelectedSensorVelocity() * Constants.FALCON_ENCODER_TICKS_PER_100_MS_TO_RPM) < -// Constants.FEEDER_WHEEL_LOCK_SPEED_RPM) { -// feederWheel.set(ControlMode.Position, feederLockPosition); -// } else { -// // Resets the lockPosition when not locking -// feederLockPosition = feederWheel.getSelectedSensorPosition(); -// feederWheel.set(ControlMode.PercentOutput, 0); -// } - - feederWheel.set(ControlMode.Position, feederLockPosition); - logData("Feeder Lock Pos ", feederLockPosition); - logData("Feeder lock pos time ", Timer.getFPGATimestamp()); - //feederWheel.set(ControlMode.PercentOutput, 0); - } - public void setHoodZero() { System.out.println(" Setting Zero " + hoodAbsoluteEncoder.configGetMagnetOffset() + " -> 90"); hoodAbsoluteEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); @@ -588,8 +559,11 @@ public void setHoodZero() { -(-hoodAbsoluteEncoder.getAbsolutePosition() - hoodAbsoluteEncoder.configGetMagnetOffset()) - 90); } - private double nextAllowedShootTime = 0; - private double rumbleTime = 0; + public double getLastShotTime() { + return lastShotTime; + } + + private double lastShotTime = 0; /** * Update Method for Shooter. @@ -623,7 +597,6 @@ public void update() { if (feederWheelState == FeederWheelState.REVERSE) { feederWheel.set(ControlMode.PercentOutput, -Constants.FEEDER_WHEEL_SPEED); - feederLockPosition = feederWheel.getSelectedSensorPosition(); } else { if (runFeederWheelReversed) { feederWheel.set(ControlMode.PercentOutput, -0.3); @@ -644,26 +617,24 @@ public void update() { moveHoodMotor(); // Sets Motor to travel to desired hood angle } - if (nextAllowedShootTime > Timer.getFPGATimestamp() + Constants.SECOND_BALL_SHOOT_DELAY + 0.2) { + if (lastShotTime > Timer.getFPGATimestamp()) { //Check to make sure we don't accidentally do anything dumb and prevent us from shooting. It checks if the //nextAllowedShootTime is greater than the current time plus the delay time which should never happen - nextAllowedShootTime = Timer.getFPGATimestamp(); + lastShotTime = Timer.getFPGATimestamp(); } - if ((feederWheelState == FeederWheelState.FORWARD) - && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) - && (Timer.getFPGATimestamp() > nextAllowedShootTime) - || feederChecksDisabled) + if ( + (feederWheelState == FeederWheelState.FORWARD) + && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) + && (Timer.getFPGATimestamp() > SECOND_BALL_SHOOT_DELAY + lastShotTime) + || feederChecksDisabled) ) { - feederWheel.set(ControlMode.PercentOutput, Constants.FEEDER_WHEEL_SPEED); - feederLockPosition = feederWheel.getSelectedSensorPosition(); - forceFeederOnTime = Timer.getFPGATimestamp() + Constants.FEEDER_CHANGE_STATE_DELAY_SEC; - nextAllowedShootTime = Timer.getFPGATimestamp() + Constants.SECOND_BALL_SHOOT_DELAY; - rumbleTime = Timer.getFPGATimestamp() + Constants.RUMBLE_DELAY; + feederWheel.set(ControlMode.PercentOutput, FEEDER_WHEEL_SPEED); + lastShotTime = Timer.getFPGATimestamp(); Robot.setRumble(RumbleType.kLeftRumble, 0); } else { // Turn OFF Feeder Wheel if feederWheel has not been on in half a second - if (Timer.getFPGATimestamp() > forceFeederOnTime) { + if (Timer.getFPGATimestamp() > lastShotTime + FEEDER_CHANGE_STATE_DELAY_SEC) { if (runFeederWheelReversed) { feederWheel.set(ControlMode.PercentOutput, -0.3); } else { @@ -671,8 +642,9 @@ public void update() { } } - if (Timer.getFPGATimestamp() > rumbleTime && rumbleTime < Timer.getFPGATimestamp() + Constants.RUMBLE_TIME) { - Robot.setRumble(RumbleType.kLeftRumble, 1); + if (Timer.getFPGATimestamp() > lastShotTime + RUMBLE_DELAY + && Timer.getFPGATimestamp() < lastShotTime + RUMBLE_DELAY + RUMBLE_TIME) { + Robot.setRumble(RumbleType.kLeftRumble, 0.25); } else { Robot.setRumble(RumbleType.kLeftRumble, 0); } From b6b262cadaaaf25431c4b8fb41b60dfa644b96cf Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 12:32:24 -0700 Subject: [PATCH 033/108] lock the shooter onto a target if we recently shot --- src/main/java/frc/subsystem/Shooter.java | 2 +- src/main/java/frc/subsystem/VisionManager.java | 18 +++++++++++++++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java index 6435ab67..b7c04c3e 100644 --- a/src/main/java/frc/subsystem/Shooter.java +++ b/src/main/java/frc/subsystem/Shooter.java @@ -563,7 +563,7 @@ public double getLastShotTime() { return lastShotTime; } - private double lastShotTime = 0; + private volatile double lastShotTime = 0; /** * Update Method for Shooter. diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 50bfe36e..36b48bde 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -92,21 +92,33 @@ public void logData() { public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean useFieldRelative) { - Translation2d aimToPosition = getAdjustedTranslation(0.15); + double timeFromLastShoot = shooter.getLastShotTime() - Timer.getFPGATimestamp(); + double shooterLookAheadTime = 0.15 + timeFromLastShoot; + boolean justShot = false; + if (shooterLookAheadTime < 0) { + shooterLookAheadTime = 0.15; + justShot = true; + } + + double turnDelay = 0.15; + + Translation2d aimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay); double targetAngle = angleOf(aimToPosition).getRadians(); // Get the angle that will be used in the future to calculate the end velocity of the turn - Translation2d futureAimToPosition = getAdjustedTranslation(0.25); + Translation2d futureAimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay + (justShot ? 0.1 : 0)); double futureTargetAngle = angleOf(futureAimToPosition).getRadians(); drive.updateTurn(controllerDriveInputs, new State(targetAngle, (futureTargetAngle - targetAngle) * 10), useFieldRelative, 0); - Translation2d aimChecksPosition = getAdjustedTranslation(0.00); + + Translation2d aimChecksPosition = getAdjustedTranslation(shooterLookAheadTime); updateShooterState(aimChecksPosition.getNorm()); + tryToShoot(aimChecksPosition, (futureTargetAngle - targetAngle) * 10, false); } From 82fb83a84ed76bb9969fe8c50af5f458ed3ec3b9 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 14:31:01 -0700 Subject: [PATCH 034/108] optimize getVelocityAdjustedRelativeTranslation() --- .../java/frc/subsystem/VisionManager.java | 31 +++++++++++++------ .../java/frc/subsystem/VisionManagerTest.java | 2 +- 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 36b48bde..6e486257 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -17,6 +17,7 @@ import frc.utility.Limelight.LedMode; import frc.utility.MathUtil; import frc.utility.Timer; +import frc.utility.geometry.MutableTranslation2d; import frc.utility.shooter.visionlookup.ShooterConfig; import frc.utility.shooter.visionlookup.VisionLookUpTable; import org.jetbrains.annotations.Contract; @@ -93,21 +94,21 @@ public void logData() { public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean useFieldRelative) { - double timeFromLastShoot = shooter.getLastShotTime() - Timer.getFPGATimestamp(); - double shooterLookAheadTime = 0.15 + timeFromLastShoot; + double timeFromLastShoot = Timer.getFPGATimestamp() - shooter.getLastShotTime(); + double shooterLookAheadTime = 0.15 - timeFromLastShoot; boolean justShot = false; if (shooterLookAheadTime < 0) { shooterLookAheadTime = 0.15; justShot = true; } - double turnDelay = 0.15; + double turnDelay = 0.00; Translation2d aimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay); double targetAngle = angleOf(aimToPosition).getRadians(); // Get the angle that will be used in the future to calculate the end velocity of the turn - Translation2d futureAimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay + (justShot ? 0.1 : 0)); + Translation2d futureAimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay + (justShot ? 0.0 : 0.1)); double futureTargetAngle = angleOf(futureAimToPosition).getRadians(); drive.updateTurn(controllerDriveInputs, @@ -458,14 +459,24 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat @NotNull Translation2d getVelocityAdjustedRelativeTranslation( @NotNull Translation2d relativeGoalTranslation, @NotNull Translation2d robotVelocity) { - Translation2d fakeGoalPos = relativeGoalTranslation; + MutableTranslation2d fakeGoalPos = new MutableTranslation2d(relativeGoalTranslation); + + double relGoalX = relativeGoalTranslation.getX(); + double relGoalY = relativeGoalTranslation.getY(); + + double velX = robotVelocity.getX(); + double velY = robotVelocity.getY(); for (int i = 0; i < 40; i++) { //System.out.println("Iteration: " + i + " Fake Goal Pos: " + fakeGoalPos); double tof = getTimeOfFlight(fakeGoalPos); - fakeGoalPos = relativeGoalTranslation.plus(robotVelocity.times(tof)); + + fakeGoalPos.set( + relGoalX + (velX * tof), + relGoalY + (velY * tof) + ); } - return fakeGoalPos; + return fakeGoalPos.getTranslation2d(); } /** @@ -477,13 +488,13 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat double timeOfFlightFrames; if (distance < 120) { - timeOfFlightFrames = 25; + timeOfFlightFrames = 25.0 / 30.0; } else { - timeOfFlightFrames = (0.065 * (distance - 120)) + 25; + timeOfFlightFrames = ((0.065 / 30) * (distance - 120)) + (25.0 / 30); } //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542; - return timeOfFlightFrames / 30; + return timeOfFlightFrames; } /** diff --git a/src/test/java/frc/subsystem/VisionManagerTest.java b/src/test/java/frc/subsystem/VisionManagerTest.java index 9029975c..d005f8e7 100644 --- a/src/test/java/frc/subsystem/VisionManagerTest.java +++ b/src/test/java/frc/subsystem/VisionManagerTest.java @@ -13,7 +13,7 @@ class VisionManagerTest { void getVelocityAdjustedRelativeTranslation() { Random random = new Random(719479); - for (int i = 0; i < 1000; i++) { + for (int i = 0; i < 10000000; i++) { Translation2d goalPos = new Translation2d((random.nextDouble() * 20) - 10, (random.nextDouble() * 20) - 10); Translation2d robotVelocity = new Translation2d((random.nextDouble() * 10) - 5, (random.nextDouble() * 10) - 5); Translation2d fakeGoal = VisionManager.getInstance().getVelocityAdjustedRelativeTranslation(goalPos, robotVelocity); From b079307f6e86ccc6eb568a17ba55e798e7d4ef9d Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 15:32:53 -0700 Subject: [PATCH 035/108] tune shoot and moving code --- src/main/deploy/shooter/shooterconfig.json | 4 +- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/subsystem/Drive.java | 39 +++++++++---------- .../java/frc/subsystem/VisionManager.java | 18 +++++++-- 4 files changed, 37 insertions(+), 28 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 42dc5425..29a488d0 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -13,11 +13,11 @@ "distance" : 82.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 1800.0, + "flywheelSpeed" : 1900.0, "distance" : 106.0 }, { "hoodEjectAngle" : 53.0, - "flywheelSpeed" : 1800.0, + "flywheelSpeed" : 1900.0, "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 88c21de6..bbfa2f29 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -164,8 +164,8 @@ public final class Constants { // TurnPID - public static final double DEFAULT_TURN_P = 8.0; - public static final double DEFAULT_TURN_I = 0.0; + public static final double DEFAULT_TURN_P = 12.0; + public static final double DEFAULT_TURN_I = 4.0; public static final double DEFAULT_TURN_D = 0.0; public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0; public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0; diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index f1d432ce..a5ec87c5 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -76,11 +76,10 @@ public enum DriveState { return INSTANCE; } - private final @NotNull ProfiledPIDController turnPID; + private final @NotNull PIDController turnPID; { - turnPID = new ProfiledPIDController(DEFAULT_TURN_P, DEFAULT_TURN_I, DEFAULT_TURN_D, - new TrapezoidProfile.Constraints(DEFAULT_TURN_MAX_VELOCITY, DEFAULT_TURN_MAX_ACCELERATION)); //P=1.0 + turnPID = new PIDController(DEFAULT_TURN_P, DEFAULT_TURN_I, DEFAULT_TURN_D); //P=1.0 // OR 0.8 turnPID.enableContinuousInput(-Math.PI, Math.PI); } @@ -208,16 +207,16 @@ private Drive() { turnD.addListener(event -> turnPID.setD(event.getEntry().getDouble(Constants.DEFAULT_TURN_D)), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - turnMaxVelocity.addListener(event -> turnPID.setConstraints( - new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_VELOCITY), - turnMaxAcceleration.getDouble(6))), - EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - turnMaxAcceleration.addListener( - event -> turnPID.setConstraints( - new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_ACCELERATION), - turnMaxAcceleration.getDouble(6))), - EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); +// turnMaxVelocity.addListener(event -> turnPID.setConstraints( +// new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_VELOCITY), +// turnMaxAcceleration.getDouble(6))), +// EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); +// +// turnMaxAcceleration.addListener( +// event -> turnPID.setConstraints( +// new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_ACCELERATION), +// turnMaxAcceleration.getDouble(6))), +// EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); useFieldRelative = true; @@ -671,13 +670,13 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, } if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate) { - turnPID.reset(RobotTracker.getInstance().getGyroAngle().getRadians(), - RobotTracker.getInstance().getLatencyCompedChassisSpeeds().omegaRadiansPerSecond); + turnPID.reset(); } + turnPID.setSetpoint(goal.position); lastTurnUpdate = Timer.getFPGATimestamp(); - double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians(), goal); + double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()); // System.out.println( // "turn error: " + Math.toDegrees(turnPID.getPositionError()) + " delta speed: " + Math.toDegrees(pidDeltaSpeed)); @@ -685,14 +684,14 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, if (useFieldRelative) { swerveDrive(ChassisSpeeds.fromFieldRelativeSpeeds( - controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.45, - controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.45, + controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.3, + controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.3, pidDeltaSpeed, RobotTracker.getInstance().getGyroAngle())); } else { swerveDrive(new ChassisSpeeds( - controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.45, - controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.45, + controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.3, + controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.3, pidDeltaSpeed)); } diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 6e486257..49fb11d1 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -75,7 +75,17 @@ public void logData() { logData("Vision Robot Velocity X", robotVelocity.getX()); logData("Vision Robot Velocity Y", robotVelocity.getY()); - Translation2d aimToPosition = getAdjustedTranslation(0.15); + double timeFromLastShoot = Timer.getFPGATimestamp() - shooter.getLastShotTime(); + double shooterLookAheadTime = 0.15 - timeFromLastShoot; + boolean justShot = false; + if (shooterLookAheadTime < 0) { + shooterLookAheadTime = 0.15; + justShot = true; + } + + double turnDelay = 0.00; + + Translation2d aimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay); Translation2d fieldCentricCords = RobotTracker.getInstance().getLastEstimatedPoseMeters().getTranslation().minus(aimToPosition); @@ -102,7 +112,7 @@ public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean us justShot = true; } - double turnDelay = 0.00; + double turnDelay = 0.0; Translation2d aimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay); double targetAngle = angleOf(aimToPosition).getRadians(); @@ -488,9 +498,9 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat double timeOfFlightFrames; if (distance < 120) { - timeOfFlightFrames = 25.0 / 30.0; + timeOfFlightFrames = ((0.02 / 30) * (distance - 120)) + (22.5 / 30); } else { - timeOfFlightFrames = ((0.065 / 30) * (distance - 120)) + (25.0 / 30); + timeOfFlightFrames = ((0.077 / 30) * (distance - 120)) + (22.5 / 30); } //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542; From 56a5f1ab91451c79ee75c5a4b7be65c9ee061362 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 19:21:44 -0700 Subject: [PATCH 036/108] create auto that hides the opponent cargo behind the hub --- src/main/deploy/autos/hideballs.json | 3165 ++++++++++++++++++++++++++ 1 file changed, 3165 insertions(+) create mode 100644 src/main/deploy/autos/hideballs.json diff --git a/src/main/deploy/autos/hideballs.json b/src/main/deploy/autos/hideballs.json new file mode 100644 index 00000000..7d69ec29 --- /dev/null +++ b/src/main/deploy/autos/hideballs.json @@ -0,0 +1,3165 @@ +{ + "autonomousSteps" : [ { + "type" : "script", + "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 2.351956220256751, + "pose" : { + "translation" : { + "x" : 6.1293768882751465, + "y" : 1.0937262773513794 + }, + "rotation" : { + "radians" : 2.4178655244299407 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.3188834866654645, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.039938468317359, + "y" : 1.173101702633744 + }, + "rotation" : { + "radians" : 2.411607121974174 + } + }, + "curvature" : -0.10063347960741297 + }, { + "time" : 0.47317666597794406, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.954285601692391, + "y" : 1.2509135147447523 + }, + "rotation" : { + "radians" : 2.395009715228919 + } + }, + "curvature" : -0.1855623376134662 + }, { + "time" : 0.6188308342429313, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.87496604004582, + "y" : 1.3260262403808838 + }, + "rotation" : { + "radians" : 2.3703948601089895 + } + }, + "curvature" : -0.26531768438500725 + }, { + "time" : 0.7539530640358325, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.803342186380178, + "y" : 1.3977211365709081 + }, + "rotation" : { + "radians" : 2.339767645326564 + } + }, + "curvature" : -0.3381985496587611 + }, { + "time" : 0.8779901904625292, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.7397725735722815, + "y" : 1.4656408262000014 + }, + "rotation" : { + "radians" : 2.3056558386592805 + } + }, + "curvature" : -0.3907488038191573 + }, { + "time" : 0.9914536504675615, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.683793342890567, + "y" : 1.529733933533862 + }, + "rotation" : { + "radians" : 2.271678768601324 + } + }, + "curvature" : -0.39766870774643054 + }, { + "time" : 1.095639209373063, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.634299722512424, + "y" : 1.5901997197428273 + }, + "rotation" : { + "radians" : 2.242697343821613 + } + }, + "curvature" : -0.32812227024366547 + }, { + "time" : 1.1923614795366175, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.589727506041527, + "y" : 1.6474327184259892 + }, + "rotation" : { + "radians" : 2.224227521884603 + } + }, + "curvature" : -0.16341344710143665 + }, { + "time" : 1.2837283378298283, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.548234531025173, + "y" : 1.7019673711353107 + }, + "rotation" : { + "radians" : 2.2209763317101947 + } + }, + "curvature" : 0.07956164338822755 + }, { + "time" : 1.3719691365115938, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.507882157471613, + "y" : 1.7544226628997421 + }, + "rotation" : { + "radians" : 2.234944043969041 + } + }, + "curvature" : 0.33953937734428047 + }, { + "time" : 1.4592981567041168, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.466816746367385, + "y" : 1.805446757749337 + }, + "rotation" : { + "radians" : 2.264150133653273 + } + }, + "curvature" : 0.5355105351904276 + }, { + "time" : 1.5477626861241707, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.42345113819465, + "y" : 1.8556616342393681 + }, + "rotation" : { + "radians" : 2.30296870864004 + } + }, + "curvature" : 0.6126486386243994 + }, { + "time" : 1.6390285515859833, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.376646131448524, + "y" : 1.9056077209744444 + }, + "rotation" : { + "radians" : 2.343996330815072 + } + }, + "curvature" : 0.568293999561127 + }, { + "time" : 1.7340988034913118, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.3258919611544115, + "y" : 1.9556885321326263 + }, + "rotation" : { + "radians" : 2.3801609988469874 + } + }, + "curvature" : 0.4354667748411578 + }, { + "time" : 1.8330035326706784, + "velocity" : 0.75, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.271489777385341, + "y" : 2.006115302989542 + }, + "rotation" : { + "radians" : 2.405692727133013 + } + }, + "curvature" : 0.24590943352090008 + }, { + "time" : 2.0360118787648114, + "velocity" : 0.0, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 2.4154850346736763 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ], + "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ] + }, { + "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ], + "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.37094272169677 + } + }, { + "time" : 1.939999956637621, + "rotation" : { + "radians" : 2.370942522371697 + } + } ], + "reversed" : false, + "color" : 209.88235, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ { + "type" : "MaxVelocityConstraint", + "maxVelocity" : 0.75 + } ] + }, { + "type" : "script", + "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "10" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Drive.setAutoRotation 40", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.setAutoRotation", + "args" : [ "40" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 0.9530775920796433 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.22164892589636578, + "velocity" : 0.8865957035854634, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.272283410420641, + "y" : 2.136490201934066 + }, + "rotation" : { + "radians" : 0.93154333853559 + } + }, + "curvature" : -0.3354703724748357 + }, { + "time" : 0.32212126730993046, + "velocity" : 1.2884850692397223, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.339010516181588, + "y" : 2.223017249489203 + }, + "rotation" : { + "radians" : 0.8980578131179813 + } + }, + "curvature" : -0.24297628723974596 + }, { + "time" : 0.40798632695507736, + "velocity" : 1.6319453078203097, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.418256742414087, + "y" : 2.320179727881623 + }, + "rotation" : { + "radians" : 0.8787668506583872 + } + }, + "curvature" : -0.07195280186555318 + }, { + "time" : 0.4482103474158235, + "velocity" : 1.7928413896632942, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.462294308261335, + "y" : 2.373142516471262 + }, + "rotation" : { + "radians" : 0.8763509051285007 + } + }, + "curvature" : -0.0010310805010032027 + }, { + "time" : 0.48701783511066765, + "velocity" : 1.9480713404426708, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.508707731962204, + "y" : 2.428952641785145 + }, + "rotation" : { + "radians" : 0.8784798678297706 + } + }, + "curvature" : 0.057187573384384974 + }, { + "time" : 0.5244749433253648, + "velocity" : 2.0978997733014593, + "acceleration" : -3.006834100609329, + "pose" : { + "translation" : { + "x" : 5.5569134053257585, + "y" : 2.4874170951864016 + }, + "rotation" : { + "radians" : 0.8846611837531144 + } + }, + "curvature" : 0.10384088533571874 + }, { + "time" : 0.5628765788352517, + "velocity" : 1.9824324261311614, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 5.606271566124633, + "y" : 2.5482597037116648 + }, + "rotation" : { + "radians" : 0.8942836471940969 + } + }, + "curvature" : 0.13988861038856043 + }, { + "time" : 0.6051667706790917, + "velocity" : 1.813271658755801, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.656145019875112, + "y" : 2.6111436507878807 + }, + "rotation" : { + "radians" : 0.9066044762714179 + } + }, + "curvature" : 0.16515378657020438 + }, { + "time" : 0.6526161179857626, + "velocity" : 1.6234742695291178, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.7059578616172075, + "y" : 2.6756939969491214 + }, + "rotation" : { + "radians" : 0.9206836126942823 + } + }, + "curvature" : 0.17781422207911715 + }, { + "time" : 0.7069026874415512, + "velocity" : 1.4063279917059635, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.75525419769474, + "y" : 2.7415202005533956 + }, + "rotation" : { + "radians" : 0.9352834691742239 + } + }, + "curvature" : 0.1740904256082478 + }, { + "time" : 0.7714879126896856, + "velocity" : 1.1479870907134255, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.803756867535412, + "y" : 2.8082386384994606 + }, + "rotation" : { + "radians" : 0.9487507880978892 + } + }, + "curvature" : 0.14816445891139096 + }, { + "time" : 0.8556329295191124, + "velocity" : 0.8114070233957184, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.851426165430894, + "y" : 2.8754951269436333 + }, + "rotation" : { + "radians" : 0.9589069733276255 + } + }, + "curvature" : 0.09262519823750537 + }, { + "time" : 1.058484685368042, + "velocity" : 0.0, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : 0.9629949901604784 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ], + "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ] + }, { + "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ], + "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.9599310885968813 + } + }, { + "time" : 0.9699999783188105, + "rotation" : { + "radians" : 0.9599310885968813 + } + } ], + "reversed" : false, + "color" : 148.47061, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : -3.1024027203970066 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2301838401073264, + "velocity" : 0.9207353604293056, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.792630460413637, + "y" : 2.9388424054094813 + }, + "rotation" : { + "radians" : -3.1025922709616696 + } + }, + "curvature" : -0.0034322907229005963 + }, { + "time" : 0.3255946694282438, + "velocity" : 1.302378677712975, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.686655575860993, + "y" : 2.9347317908029744 + }, + "rotation" : { + "radians" : -3.1031002310232063 + } + }, + "curvature" : -0.006005316476928024 + }, { + "time" : 0.3988940171844041, + "velocity" : 1.5955760687376164, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.580523715266813, + "y" : 2.930682060104536 + }, + "rotation" : { + "radians" : -3.103837861955969 + } + }, + "curvature" : -0.007749412153373173 + }, { + "time" : 0.4607866040428335, + "velocity" : 1.843146416171334, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 5.4741821068677154, + "y" : 2.926711325853944 + }, + "rotation" : { + "radians" : -3.104720238709075 + } + }, + "curvature" : -0.008706622259397081 + }, { + "time" : 0.5154157029130185, + "velocity" : 2.0616628116520737, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.367594657212031, + "y" : 2.9228297190306436 + }, + "rotation" : { + "radians" : -3.1056669054798047 + } + }, + "curvature" : -0.00892606605401968 + }, { + "time" : 0.5649014668690089, + "velocity" : 2.259605867476035, + "acceleration" : 4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.260741207842759, + "y" : 2.9190397568610926 + }, + "rotation" : { + "radians" : -3.106602230083352 + } + }, + "curvature" : -0.008459920513905771 + }, { + "time" : 0.6104980484072738, + "velocity" : 2.441992193629095, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 5.153616791980521, + "y" : 2.9153367106261046 + }, + "rotation" : { + "radians" : -3.1074555011063474 + } + }, + "curvature" : -0.007360079879286901 + }, { + "time" : 0.6530171873993678, + "velocity" : 2.6120687495974706, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 5.0462308912065055, + "y" : 2.911708973468194 + }, + "rotation" : { + "radians" : -3.108160812688321 + } + }, + "curvature" : -0.005675482209767876 + }, { + "time" : 0.6930174258760853, + "velocity" : 2.7720697035043407, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.93860669214543, + "y" : 2.9081384281989218 + }, + "rotation" : { + "radians" : -3.108656779571012 + } + }, + "curvature" : -0.003450046167250236 + }, { + "time" : 0.7309003592448649, + "velocity" : 2.923601436979459, + "acceleration" : 2.0944239970255154, + "pose" : { + "translation" : { + "x" : 4.830780343148486, + "y" : 2.904600815106239 + }, + "rotation" : { + "radians" : -3.108886120874728 + } + }, + "curvature" : -7.211325069651598E-4 + }, { + "time" : 0.7673774828497517, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.722800210976294, + "y" : 2.9010660997618305 + }, + "rotation" : { + "radians" : -3.10879514573791 + } + }, + "curvature" : 0.0024815692229093363 + }, { + "time" : 0.8034217930772413, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.614726137481853, + "y" : 2.8974988408284617 + }, + "rotation" : { + "radians" : -3.1083331681482385 + } + }, + "curvature" : 0.0061368314589584135 + }, { + "time" : 0.839474699342444, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.506628696293496, + "y" : 2.8938585578673215 + }, + "rotation" : { + "radians" : -3.107451872483656 + } + }, + "curvature" : 0.010232455610621435 + }, { + "time" : 0.8755098996327016, + "velocity" : 3.0, + "acceleration" : -8.22656662136612E-15, + "pose" : { + "translation" : { + "x" : 4.39858844949784, + "y" : 2.890100099145368 + }, + "rotation" : { + "radians" : -3.1061046457957544 + } + }, + "curvature" : 0.01476570595533646 + }, { + "time" : 0.9114981176657808, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.2906952043227395, + "y" : 2.886174009442671 + }, + "rotation" : { + "radians" : -3.1042458879032835 + } + }, + "curvature" : 0.019743691371007215 + }, { + "time" : 0.9474073803784949, + "velocity" : 2.9999999999999996, + "acceleration" : 8.270759764362102E-15, + "pose" : { + "translation" : { + "x" : 4.183047269820236, + "y" : 2.8820268978597596 + }, + "rotation" : { + "radians" : -3.1018303060188313 + } + }, + "curvature" : 0.025183766010214763 + }, { + "time" : 0.9832033025994578, + "velocity" : 3.0, + "acceleration" : -8.305527617360462E-15, + "pose" : { + "translation" : { + "x" : 4.075750713549512, + "y" : 2.8776018056249644 + }, + "rotation" : { + "radians" : -3.0988121969366995 + } + }, + "curvature" : 0.031114011207342505 + }, { + "time" : 1.0188493791490731, + "velocity" : 2.9999999999999996, + "acceleration" : 8.349604098090194E-15, + "pose" : { + "translation" : { + "x" : 3.9689186182598455, + "y" : 2.8728385739017632 + }, + "rotation" : { + "radians" : -3.095144716752075 + } + }, + "curvature" : 0.03757385368866147 + }, { + "time" : 1.0543072846683412, + "velocity" : 3.0, + "acceleration" : -8.403642908942724E-15, + "pose" : { + "translation" : { + "x" : 3.8626703385735555, + "y" : 2.867674211596125 + }, + "rotation" : { + "radians" : -3.0907791356286958 + } + }, + "curvature" : 0.0446148697110529 + }, { + "time" : 1.0895371815644033, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.7571307576689605, + "y" : 2.862043263163855 + }, + "rotation" : { + "radians" : -3.0856640732526293 + } + }, + "curvature" : 0.05230182094645797 + }, { + "time" : 1.124498036572409, + "velocity" : 2.9999999999999996, + "acceleration" : 8.544307135767894E-15, + "pose" : { + "translation" : { + "x" : 3.6524295439633274, + "y" : 2.855878176417939 + }, + "rotation" : { + "radians" : -3.0797447092896437 + } + }, + "curvature" : 0.06071396531738544 + }, { + "time" : 1.159147946568507, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.548700407795824, + "y" : 2.8491096703358885 + }, + "rotation" : { + "radians" : -3.0729619624239923 + } + }, + "curvature" : 0.0699466838117423 + }, { + "time" : 1.1934444744263788, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.4460803581104718, + "y" : 2.841667102867084 + }, + "rotation" : { + "radians" : -3.0652516314712313 + } + }, + "curvature" : 0.08011346138708186 + }, { + "time" : 1.2273449958888158, + "velocity" : 3.0, + "acceleration" : -0.16623210994932952, + "pose" : { + "translation" : { + "x" : 3.3447089591390977, + "y" : 2.8334788387401204 + }, + "rotation" : { + "radians" : -3.0565434927756256 + } + }, + "curvature" : 0.09134825465286359 + }, { + "time" : 1.2608381382277525, + "velocity" : 2.9944323642801653, + "acceleration" : -0.4439832537480419, + "pose" : { + "translation" : { + "x" : 3.2447275870842858, + "y" : 2.8244726172701524 + }, + "rotation" : { + "radians" : -3.046760349874518 + } + }, + "curvature" : 0.10380826846683365 + }, { + "time" : 1.2939625007283606, + "velocity" : 2.9797257020388157, + "acceleration" : -0.504652005340554, + "pose" : { + "translation" : { + "x" : 3.1462786868023294, + "y" : 2.814575920166238 + }, + "rotation" : { + "radians" : -3.0358170346264064 + } + }, + "curvature" : 0.11767714398773378 + }, { + "time" : 1.3267346665866735, + "velocity" : 2.963187162819065, + "acceleration" : -0.5754544680210432, + "pose" : { + "translation" : { + "x" : 3.049505028486184, + "y" : 2.8037163393386826 + }, + "rotation" : { + "radians" : -3.023619364229805 + } + }, + "curvature" : 0.13316852630405346 + }, { + "time" : 1.3591322565139792, + "velocity" : 2.9445438249422833, + "acceleration" : -0.6578485083677343, + "pose" : { + "translation" : { + "x" : 2.954548964348419, + "y" : 2.791821944706385 + }, + "rotation" : { + "radians" : -3.010063066623661 + } + }, + "curvature" : 0.1505299220368132 + }, { + "time" : 1.391136692414039, + "velocity" : 2.923489754524278, + "acceleration" : -0.7533080854904565, + "pose" : { + "translation" : { + "x" : 2.8615516853041685, + "y" : 2.7788216520041806 + }, + "rotation" : { + "radians" : -2.995032698790502 + } + }, + "curvature" : 0.17004666410797128 + }, { + "time" : 1.4227338667361988, + "velocity" : 2.8996873476287437, + "acceleration" : -0.8632025105219969, + "pose" : { + "translation" : { + "x" : 2.7706524776540853, + "y" : 2.764645590590188 + }, + "rotation" : { + "radians" : -2.9784006000086296 + } + }, + "curvature" : 0.19204565508070712 + }, { + "time" : 1.4539148256972156, + "velocity" : 2.8727718655731107, + "acceleration" : -3.0556739243578237, + "pose" : { + "translation" : { + "x" : 2.681987979767292, + "y" : 2.749225471253151 + }, + "rotation" : { + "radians" : -2.9600259471131536 + } + }, + "curvature" : 0.2168983389890578 + }, { + "time" : 1.4850284729965166, + "velocity" : 2.7776987048289703, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.5956914387643337, + "y" : 2.732494954019785 + }, + "rotation" : { + "radians" : -2.9397540138154308 + } + }, + "curvature" : 0.24502202477503635 + }, { + "time" : 1.516611424846333, + "velocity" : 2.6513668974297047, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.5118919672001288, + "y" : 2.7143900159621204 + }, + "rotation" : { + "radians" : -2.917415783997239 + } + }, + "curvature" : 0.2768782177504529 + }, { + "time" : 1.548889355951825, + "velocity" : 2.5222551730077356, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.430713799746922, + "y" : 2.694849319004849 + }, + "rotation" : { + "radians" : -2.8928281326387415 + } + }, + "curvature" : 0.31296597525025704 + }, { + "time" : 1.5819534964682787, + "velocity" : 2.3899986109419213, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.3522755498772367, + "y" : 2.673814577732666 + }, + "rotation" : { + "radians" : -2.8657948699982057 + } + }, + "curvature" : 0.35380747084108527 + }, { + "time" : 1.6159267882434154, + "velocity" : 2.254105443841374, + "acceleration" : -3.8097016292232664, + "pose" : { + "translation" : { + "x" : 2.276689466546827, + "y" : 2.6512309271976164 + }, + "rotation" : { + "radians" : -2.8361090449991986 + } + }, + "curvature" : 0.3999219560210479 + }, { + "time" : 1.6509216012001868, + "velocity" : 2.120785647905598, + "acceleration" : -3.4461347634327937, + "pose" : { + "translation" : { + "x" : 2.204060690877629, + "y" : 2.62704729072644 + }, + "rotation" : { + "radians" : -2.80355701776569 + } + }, + "curvature" : 0.45178327798480244 + }, { + "time" : 1.686971311060505, + "velocity" : 1.9965534895442902, + "acceleration" : -3.10238739377085, + "pose" : { + "translation" : { + "x" : 2.134486512840713, + "y" : 2.6012167477279142 + }, + "rotation" : { + "radians" : -2.7679249248735176 + } + }, + "curvature" : 0.5097553634159661 + }, { + "time" : 1.7240545520371775, + "velocity" : 1.8815069102180944, + "acceleration" : -2.774924611544726, + "pose" : { + "translation" : { + "x" : 2.0680556279392364, + "y" : 2.5736969015002 + }, + "rotation" : { + "radians" : -2.7290082446458435 + } + }, + "curvature" : 0.5740002323865068 + }, { + "time" : 1.7621406045083514, + "velocity" : 1.7758209858592497, + "acceleration" : -2.460804023554924, + "pose" : { + "translation" : { + "x" : 2.0048473938913958, + "y" : 2.544450247038185 + }, + "rotation" : { + "radians" : -2.6866251713458205 + } + }, + "curvature" : 0.6443551970164934 + }, { + "time" : 1.8011869660776936, + "velocity" : 1.6797355422042324, + "acceleration" : -2.1575333813881774, + "pose" : { + "translation" : { + "x" : 1.9449310873133778, + "y" : 2.5134445388408304 + }, + "rotation" : { + "radians" : -2.640634345688177 + } + }, + "curvature" : 0.7201813658904849 + }, { + "time" : 1.841136715487461, + "velocity" : 1.5935426242745667, + "acceleration" : -1.86294906736094, + "pose" : { + "translation" : { + "x" : 1.8883651604023122, + "y" : 2.480653158718514 + }, + "rotation" : { + "radians" : -2.590957058758506 + } + }, + "curvature" : 0.8001959234416516 + }, { + "time" : 1.8819158229901707, + "velocity" : 1.5175732239845823, + "acceleration" : -1.5751064136624524, + "pose" : { + "translation" : { + "x" : 1.8351964976192239, + "y" : 2.4460554836003747 + }, + "rotation" : { + "radians" : -2.537603239437523 + } + }, + "curvature" : 0.8823164840063602 + }, { + "time" : 1.9234306511360026, + "velocity" : 1.452182951909988, + "acceleration" : -1.292173326268649, + "pose" : { + "translation" : { + "x" : 1.785459672371985, + "y" : 2.409637253341657 + }, + "rotation" : { + "radians" : -2.480699299573502 + } + }, + "curvature" : 0.9635650423352979 + }, { + "time" : 1.9655659800403293, + "velocity" : 1.3977368038062605, + "acceleration" : -1.0123180093738429, + "pose" : { + "translation" : { + "x" : 1.7391762036982668, + "y" : 2.3713909385310568 + }, + "rotation" : { + "radians" : -2.420514345602534 + } + }, + "curvature" : 1.0400947450439444 + }, { + "time" : 2.008183951432712, + "velocity" : 1.3545938638427721, + "acceleration" : -0.733579462931417, + "pose" : { + "translation" : { + "x" : 1.696353812948491, + "y" : 2.331316108298065 + }, + "rotation" : { + "radians" : -2.357479728094081 + } + }, + "curvature" : 1.1074024899538168 + }, { + "time" : 2.051124315824919, + "velocity" : 1.3230936943938578, + "acceleration" : -0.4537052298796883, + "pose" : { + "translation" : { + "x" : 1.6569856804687841, + "y" : 2.2894197981203117 + }, + "rotation" : { + "radians" : -2.2921960475647642 + } + }, + "curvature" : 1.1607601882406264 + }, { + "time" : 2.094206248814145, + "velocity" : 1.3035471960833196, + "acceleration" : -0.16993335972179005, + "pose" : { + "translation" : { + "x" : 1.6210497022839263, + "y" : 2.2457168776309118 + }, + "rotation" : { + "radians" : -2.2254223695416244 + } + }, + "curvature" : 1.1958320363685782 + }, { + "time" : 2.137231762276557, + "velocity" : 1.2962357260268969, + "acceleration" : 0.12131769723435752, + "pose" : { + "translation" : { + "x" : 1.5885077467803068, + "y" : 2.200230418425809 + }, + "rotation" : { + "radians" : -2.1580450690428172 + } + }, + "curvature" : 1.2093603605633507 + }, { + "time" : 2.1799904071245306, + "velocity" : 1.3014231063567148, + "acceleration" : 0.42491135071399705, + "pose" : { + "translation" : { + "x" : 1.5593049113888746, + "y" : 2.152992061871121 + }, + "rotation" : { + "radians" : -2.091028180878493 + } + }, + "curvature" : 1.1997387249442901 + }, { + "time" : 2.2222646301244318, + "velocity" : 1.3193859035519875, + "acceleration" : 0.7477114243725372, + "pose" : { + "translation" : { + "x" : 1.5333687792680886, + "y" : 2.1040423869104825 + }, + "rotation" : { + "radians" : -2.0253520831348943 + } + }, + "curvature" : 1.1672933843552342 + }, { + "time" : 2.2638349143884384, + "velocity" : 1.350468480010599, + "acceleration" : 1.099745716491029, + "pose" : { + "translation" : { + "x" : 1.510608675986873, + "y" : 2.0534312778723915 + }, + "rotation" : { + "radians" : -1.9619508454178411 + } + }, + "curvature" : 1.1141785653908227 + }, { + "time" : 2.3044837782843635, + "velocity" : 1.3951718939603694, + "acceleration" : 1.4961728408552466, + "pose" : { + "translation" : { + "x" : 1.4909149262075676, + "y" : 2.0012182922775534 + }, + "rotation" : { + "radians" : -1.9016590628319923 + } + }, + "curvature" : 1.0439225192180022 + }, { + "time" : 2.343997831668839, + "velocity" : 1.4542917474663264, + "acceleration" : 1.9607687708811097, + "pose" : { + "translation" : { + "x" : 1.4741581103688794, + "y" : 1.9474730286462254 + }, + "rotation" : { + "radians" : -1.8451762474768723 + } + }, + "curvature" : 0.9607726336453983 + }, { + "time" : 2.3821673081822796, + "velocity" : 1.5291332650147609, + "acceleration" : 2.532455282243196, + "pose" : { + "translation" : { + "x" : 1.460188321368836, + "y" : 1.8922754943055606 + }, + "rotation" : { + "radians" : -1.7930521282820115 + } + }, + "curvature" : 0.869026522779364 + }, { + "time" : 2.4187826706759017, + "velocity" : 1.6218600331729836, + "acceleration" : 3.8046696696170534, + "pose" : { + "translation" : { + "x" : 1.4488344212477386, + "y" : 1.8357164731969533 + }, + "rotation" : { + "radians" : -1.745691473047995 + } + }, + "curvature" : 0.7724972819405648 + }, { + "time" : 2.486114901209423, + "velocity" : 1.8780369284715344, + "acceleration" : 3.9999999999999947, + "pose" : { + "translation" : { + "x" : 1.4331791216126497, + "y" : 1.7189331963567618 + }, + "rotation" : { + "radians" : -1.6662825767260925 + } + }, + "curvature" : 0.5761233113953619 + }, { + "time" : 2.546692489588632, + "velocity" : 2.1203472819883706, + "acceleration" : 3.999999999999993, + "pose" : { + "translation" : { + "x" : 1.4253702445836325, + "y" : 1.5980789786207197 + }, + "rotation" : { + "radians" : -1.608229977974804 + } + }, + "curvature" : 0.3843159112832851 + }, { + "time" : 2.602172291072171, + "velocity" : 2.3422664879225272, + "acceleration" : 3.9999999999999893, + "pose" : { + "translation" : { + "x" : 1.4231985572659944, + "y" : 1.4743055659818047 + }, + "rotation" : { + "radians" : -1.5723274201937065 + } + }, + "curvature" : 0.19618933375882694 + }, { + "time" : 2.6534386588593746, + "velocity" : 2.54733195907134, + "acceleration" : 3.220008756110653, + "pose" : { + "translation" : { + "x" : 1.4240437746047974, + "y" : 1.3489724397659302 + }, + "rotation" : { + "radians" : -1.5598679357771585 + } + }, + "curvature" : 0.0 + }, { + "time" : 2.7013005707324047, + "velocity" : 2.7014477343866927, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.4258348567705248, + "y" : 1.2233768946074477 + }, + "rotation" : { + "radians" : -1.550010895685447 + } + }, + "curvature" : 0.15261911450126764 + }, { + "time" : 2.749505539118783, + "velocity" : 2.508627860841179, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.4299988476074077, + "y" : 1.09787018620068 + }, + "rotation" : { + "radians" : -1.5225811069171706 + } + }, + "curvature" : 0.28021498367515013 + }, { + "time" : 2.801724619491527, + "velocity" : 2.2997515393502033, + "acceleration" : -3.6618849413513943, + "pose" : { + "translation" : { + "x" : 1.438542295689782, + "y" : 0.972616643926834 + }, + "rotation" : { + "radians" : -1.4806280826538225 + } + }, + "curvature" : 0.3842039868268945 + }, { + "time" : 2.858949646384186, + "velocity" : 2.090200075103547, + "acceleration" : -1.9233425253606922, + "pose" : { + "translation" : { + "x" : 1.45309450732384, + "y" : 0.8478549124047277 + }, + "rotation" : { + "radians" : -1.4270400417472306 + } + }, + "curvature" : 0.465101762848264 + }, { + "time" : 2.920938902993327, + "velocity" : 1.9709735017516907, + "acceleration" : -0.9820932714498863, + "pose" : { + "translation" : { + "x" : 1.4749322121688557, + "y" : 0.7238891070967739 + }, + "rotation" : { + "radians" : -1.3645961892537415 + } + }, + "curvature" : 0.5230727944268867 + }, { + "time" : 2.9861480086005843, + "velocity" : 1.906932077897538, + "acceleration" : -0.5051437890621265, + "pose" : { + "translation" : { + "x" : 1.505004228858411, + "y" : 0.6010799699149629 + }, + "rotation" : { + "radians" : -1.2959588931506363 + } + }, + "curvature" : 0.5587959564566138 + }, { + "time" : 3.019621045984329, + "velocity" : 1.890023380962095, + "acceleration" : -0.2575060162536636, + "pose" : { + "translation" : { + "x" : 1.5233410168520205, + "y" : 0.5402351909635741 + }, + "rotation" : { + "radians" : -1.2601011467472856 + } + }, + "curvature" : 0.5688389814615953 + }, { + "time" : 3.0534660636487803, + "velocity" : 1.8813080852932873, + "acceleration" : -0.046118696833170436, + "pose" : { + "translation" : { + "x" : 1.5439561306216198, + "y" : 0.4798360248268452 + }, + "rotation" : { + "radians" : -1.2236026771430364 + } + }, + "curvature" : 0.5741215645431921 + }, { + "time" : 3.0875714219887946, + "velocity" : 1.8797351906116175, + "acceleration" : 0.13685239477635067, + "pose" : { + "translation" : { + "x" : 1.5668869314298823, + "y" : 0.41993955191350785 + }, + "rotation" : { + "radians" : -1.1867261461606144 + } + }, + "curvature" : 0.575082775007997 + }, { + "time" : 3.121837246466734, + "velocity" : 1.8844245507504096, + "acceleration" : 0.29657244735757804, + "pose" : { + "translation" : { + "x" : 1.5921549109043553, + "y" : 0.3606047334615141 + }, + "rotation" : { + "radians" : -1.1497095266580952 + } + }, + "curvature" : 0.5722241677945603 + }, { + "time" : 3.15617469824573, + "velocity" : 1.8946080928605293, + "acceleration" : 0.43641554195126636, + "pose" : { + "translation" : { + "x" : 1.619766461838124, + "y" : 0.30189213515072333 + }, + "rotation" : { + "radians" : -1.1127638757054572 + } + }, + "curvature" : 0.566089275718927 + }, { + "time" : 3.1905053847904026, + "velocity" : 1.9095905380344815, + "acceleration" : 0.5585224224331813, + "pose" : { + "translation" : { + "x" : 1.649713648990474, + "y" : 0.24386365071558913 + }, + "rotation" : { + "radians" : -1.076071959756705 + } + }, + "curvature" : 0.5572411700339329 + }, { + "time" : 3.224760834869715, + "velocity" : 1.928722974994318, + "acceleration" : 0.6641677388812759, + "pose" : { + "translation" : { + "x" : 1.6819749798875545, + "y" : 0.18658222555784643 + }, + "rotation" : { + "radians" : -1.039787779550021 + } + }, + "curvature" : 0.5462406244811652 + }, { + "time" : 3.258881991221138, + "velocity" : 1.9513851462562573, + "acceleration" : 0.7540110868217726, + "pose" : { + "translation" : { + "x" : 1.7165161756230418, + "y" : 0.13011158035919834 + }, + "rotation" : { + "radians" : -1.0040369547627968 + } + }, + "curvature" : 0.5336268996944579 + }, { + "time" : 3.2928186950481497, + "velocity" : 1.976973797192011, + "acceleration" : 0.8282759740654535, + "pose" : { + "translation" : { + "x" : 1.7532909416588032, + "y" : 0.0745159346940032 + }, + "rotation" : { + "radians" : -0.9689178606738195 + } + }, + "curvature" : 0.5199024657633008 + }, { + "time" : 3.3265291512596264, + "velocity" : 2.004895358146763, + "acceleration" : 0.8868837310332817, + "pose" : { + "translation" : { + "x" : 1.7922417386255596, + "y" : 0.01985973064196145 + }, + "rotation" : { + "radians" : -0.9345033658181425 + } + }, + "curvature" : 0.5055222588408043 + }, { + "time" : 3.3599793731404124, + "velocity" : 2.0345618157322853, + "acceleration" : 0.9295593121848896, + "pose" : { + "translation" : { + "x" : 1.8333005531235487, + "y" : -0.033792643599197314 + }, + "rotation" : { + "radians" : -0.9008430006823431 + } + }, + "curvature" : 0.4908874449734679 + }, { + "time" : 3.3931426109904206, + "velocity" : 2.065389012297963, + "acceleration" : 0.9559197746209286, + "pose" : { + "translation" : { + "x" : 1.8763896685231884, + "y" : -0.08637713010102743 + }, + "rotation" : { + "radians" : -0.8679653892038623 + } + }, + "curvature" : 0.4763432100815362 + }, { + "time" : 3.425998772114722, + "velocity" : 2.0967968664348136, + "acceleration" : 0.96555227037989, + "pose" : { + "translation" : { + "x" : 1.921422435765741, + "y" : -0.1378302775916802 + }, + "rotation" : { + "radians" : -0.8358807916469844 + } + }, + "curvature" : 0.46217982712431704 + }, { + "time" : 3.4585338402322123, + "velocity" : 2.1282111753226207, + "acceleration" : 0.9580857106502751, + "pose" : { + "translation" : { + "x" : 1.9683040441639754, + "y" : -0.18808951784321692 + }, + "rotation" : { + "radians" : -0.8045836333638371 + } + }, + "curvature" : 0.44863614984773975 + }, { + "time" : 3.4907393016272956, + "velocity" : 2.159066767690149, + "acceleration" : 0.9332583224551695, + "pose" : { + "translation" : { + "x" : 2.0169322922028305, + "y" : -0.2370934420589219 + }, + "rotation" : { + "radians" : -0.774054923708963 + } + }, + "curvature" : 0.43590470633971296 + }, { + "time" : 3.5226115837946828, + "velocity" : 2.1888118402785026, + "acceleration" : 0.8909818249589498, + "pose" : { + "translation" : { + "x" : 2.067198358340079, + "y" : -0.28478207726061555 + }, + "rotation" : { + "radians" : -0.744264498898693 + } + }, + "curvature" : 0.42413766870248887 + }, { + "time" : 3.554151510376562, + "velocity" : 2.2169133416234965, + "acceleration" : 0.831401748795369, + "pose" : { + "translation" : { + "x" : 2.11898757180699, + "y" : -0.33109716267596734 + }, + "rotation" : { + "radians" : -0.7151730492255899 + } + }, + "curvature" : 0.41345311620209624 + }, { + "time" : 3.5853637741835547, + "velocity" : 2.2428632723364927, + "acceleration" : 0.7549524443057307, + "pose" : { + "translation" : { + "x" : 2.1721801834089938, + "y" : -0.3759824261258089 + }, + "rotation" : { + "radians" : -0.6867339133510525 + } + }, + "curvature" : 0.40394115698677635 + }, { + "time" : 3.6162564282292005, + "velocity" : 2.266185757019344, + "acceleration" : 0.6624045708844728, + "pose" : { + "translation" : { + "x" : 2.226652136326343, + "y" : -0.41938386041144704 + }, + "rotation" : { + "radians" : -0.6588946399888784 + } + }, + "curvature" : 0.3956696075854153 + }, { + "time" : 3.646840393102844, + "velocity" : 2.286444715147416, + "acceleration" : 0.5549023669290096, + "pose" : { + "translation" : { + "x" : 2.2822758369147778, + "y" : -0.4612499997019768 + }, + "rotation" : { + "radians" : -0.6315983304138486 + } + }, + "curvature" : 0.38868903913170505 + }, { + "time" : 3.677128977700188, + "velocity" : 2.3032519224314116, + "acceleration" : 0.43398782613727804, + "pose" : { + "translation" : { + "x" : 2.3389209255061876, + "y" : -0.5015321959215944 + }, + "rotation" : { + "radians" : -0.6047847845266212 + } + }, + "curvature" : 0.38303708086136296 + }, { + "time" : 3.7071374093201945, + "velocity" : 2.3162752164359675, + "acceleration" : 0.301609110666151, + "pose" : { + "translation" : { + "x" : 2.396455047209276, + "y" : -0.5401848951369104 + }, + "rotation" : { + "radians" : -0.5783914794951374 + } + }, + "curvature" : 0.37874192561228726 + }, { + "time" : 3.736882368388014, + "velocity" : 2.3252465670872136, + "acceleration" : 0.16011115647032365, + "pose" : { + "translation" : { + "x" : 2.4547446227102228, + "y" : -0.5771659139442626 + }, + "rotation" : { + "radians" : -0.5523544141104476 + } + }, + "curvature" : 0.37582501188792183 + }, { + "time" : 3.766381522526342, + "velocity" : 2.3299697107711976, + "acceleration" : 0.012207476034279642, + "pose" : { + "translation" : { + "x" : 2.5136556190733472, + "y" : -0.6124367158570294 + }, + "rotation" : { + "radians" : -0.5266088547091091 + } + }, + "curvature" : 0.37430286645269983 + }, { + "time" : 3.7956530543152014, + "velocity" : 2.330327042293997, + "acceleration" : -0.13906640622183863, + "pose" : { + "translation" : { + "x" : 2.5730543205417726, + "y" : -0.6459626876929423 + }, + "rotation" : { + "radians" : -0.5010900204761772 + } + }, + "curvature" : 0.37418808431592226 + }, { + "time" : 3.8247151767948284, + "velocity" : 2.326285477363576, + "acceleration" : -0.29041573965154777, + "pose" : { + "translation" : { + "x" : 2.6328080993380887, + "y" : -0.6777134159613993 + }, + "rotation" : { + "radians" : -0.47573374766730814 + } + }, + "curvature" : 0.37548940279405946 + }, { + "time" : 3.8535856305237206, + "velocity" : 2.317901043189824, + "acceleration" : -0.4383615615686928, + "pose" : { + "translation" : { + "x" : 2.6927861864650144, + "y" : -0.7076629632507778 + }, + "rotation" : { + "radians" : -0.45047717414862004 + } + }, + "curvature" : 0.37821079612304137 + }, { + "time" : 3.882281155755939, + "velocity" : 2.305322027938995, + "acceleration" : -0.5793221339027272, + "pose" : { + "translation" : { + "x" : 2.752860442506062, + "y" : -0.7357901446157484 + }, + "rotation" : { + "radians" : -0.42525948787674483 + } + }, + "curvature" : 0.3823494796069915 + }, { + "time" : 3.91081693300839, + "velocity" : 2.288790620568532, + "acceleration" : -0.7096933712661672, + "pose" : { + "translation" : { + "x" : 2.8129061284262007, + "y" : -0.7620788039645863 + }, + "rotation" : { + "radians" : -0.40002278560350163 + } + }, + "curvature" : 0.38789267058227345 + }, { + "time" : 3.9392059849192336, + "velocity" : 2.2686430986108754, + "acceleration" : -0.8749184265202878, + "pose" : { + "translation" : { + "x" : 2.872802676372519, + "y" : -0.786518090446485 + }, + "rotation" : { + "radians" : -0.37471309109137346 + } + }, + "curvature" : 0.394812911527834 + }, { + "time" : 3.9955941671938477, + "velocity" : 2.219308038900831, + "acceleration" : -1.0288336535097118, + "pose" : { + "translation" : { + "x" : 2.991691567646626, + "y" : -0.8298333259347146 + }, + "rotation" : { + "radians" : -0.3236861024053773 + } + }, + "curvature" : 0.4125613419519297 + }, { + "time" : 4.051459900853216, + "velocity" : 2.161831492034062, + "acceleration" : -1.0796480258443715, + "pose" : { + "translation" : { + "x" : 3.108675287572691, + "y" : -0.865765651810257 + }, + "rotation" : { + "radians" : -0.2718791063461188 + } + }, + "curvature" : 0.43479047913732005 + }, { + "time" : 4.10675359376877, + "velocity" : 2.10213376563614, + "acceleration" : -0.9994511460458044, + "pose" : { + "translation" : { + "x" : 3.2230180775513873, + "y" : -0.8944474414456636 + }, + "rotation" : { + "radians" : -0.21916678625150354 + } + }, + "curvature" : 0.4598360409407436 + }, { + "time" : 4.161311871060626, + "velocity" : 2.0476054328705096, + "acceleration" : -0.749178067288782, + "pose" : { + "translation" : { + "x" : 3.334124914760981, + "y" : -0.9161224852502343 + }, + "rotation" : { + "radians" : -0.16568957342253765 + } + }, + "curvature" : 0.48465328108684036 + }, { + "time" : 4.214818370708727, + "velocity" : 2.007519536876757, + "acceleration" : -1.3699445774534182, + "pose" : { + "translation" : { + "x" : 3.4415661777785544, + "y" : -0.9311548350640351 + }, + "rotation" : { + "radians" : -0.11197409892743718 + } + }, + "curvature" : 0.5042015108247184 + }, { + "time" : 4.2675300331068176, + "velocity" : 1.9353074808059383, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.5451023122012337, + "y" : -0.9400376485519146 + }, + "rotation" : { + "radians" : -0.059075560161144215 + } + }, + "curvature" : 0.5109123503903398 + }, { + "time" : 4.322105290852558, + "velocity" : 1.7170064498229765, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.6447084962674126, + "y" : -0.943402033597522 + }, + "rotation" : { + "radians" : -0.008719023872416791 + } + }, + "curvature" : 0.4946090622112777 + }, { + "time" : 4.382159659747878, + "velocity" : 1.4767889742416929, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 3.7405993064779786, + "y" : -0.9420258926973233 + }, + "rotation" : { + "radians" : 0.03660524430653648 + } + }, + "curvature" : 0.4434197350542565 + }, { + "time" : 4.451511716406289, + "velocity" : 1.1993807476080485, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 3.8332533832175386, + "y" : -0.9368427673546194 + }, + "rotation" : { + "radians" : 0.07365896092598179 + } + }, + "curvature" : 0.3462339716324929 + }, { + "time" : 4.540069304785332, + "velocity" : 0.8451503940918763, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 3.9234380963756443, + "y" : -0.9289506824735625 + }, + "rotation" : { + "radians" : 0.09866354717797672 + } + }, + "curvature" : 0.19694019866370868 + }, { + "time" : 4.751356903308301, + "velocity" : 0.0, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 4.012234210968018, + "y" : -0.9196209907531738 + }, + "rotation" : { + "radians" : 0.10776957043787032 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ], + "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ] + }, { + "x" : [ 1.4240437746047974, 0.04392707347869873, 0.0 ], + "y" : [ 1.3489724397659302, -4.019377112388611, 0.0 ] + }, { + "x" : [ 4.012234210968018, 2.833451271057129, 0.0 ], + "y" : [ -0.9196209907531738, 0.30654752254486084, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.2617993877991494 + } + }, { + "time" : 2.6499999407678843, + "rotation" : { + "radians" : 0.2617993877991494 + } + }, { + "time" : 4.659999895840883, + "rotation" : { + "radians" : 0.2617993877991494 + } + } ], + "reversed" : false, + "color" : 52.47058, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 4.012234210968018, + "y" : -0.9196209907531738 + }, + "rotation" : { + "radians" : 0.030602586025410137 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.1839004373900963, + "velocity" : 0.7356017495603853, + "acceleration" : 3.999999999999999, + "pose" : { + "translation" : { + "x" : 4.079839881543424, + "y" : -0.9175061170373492 + }, + "rotation" : { + "radians" : 0.032597811527258144 + } + }, + "curvature" : 0.05820539136138801 + }, { + "time" : 0.2598794478276359, + "velocity" : 1.0395177913105436, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 4.147234073981139, + "y" : -0.9151343674395811 + }, + "rotation" : { + "radians" : 0.03836566081266323 + } + }, + "curvature" : 0.11233457015103718 + }, { + "time" : 0.31790134200340625, + "velocity" : 1.2716053680136252, + "acceleration" : 3.9999999999999982, + "pose" : { + "translation" : { + "x" : 4.2142208005905895, + "y" : -0.912271579263629 + }, + "rotation" : { + "radians" : 0.047629178342962146 + } + }, + "curvature" : 0.1636754126870864 + }, { + "time" : 0.3664818052070989, + "velocity" : 1.4659272208283958, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 4.280620524327787, + "y" : -0.9087076362808375 + }, + "rotation" : { + "radians" : 0.060164770303775844 + } + }, + "curvature" : 0.2132011191407471 + }, { + "time" : 0.40891181812753247, + "velocity" : 1.6356472725101303, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.346269665970026, + "y" : -0.9042556640216365 + }, + "rotation" : { + "radians" : 0.07578809002213045 + } + }, + "curvature" : 0.261623846845801 + }, { + "time" : 0.44687903311679705, + "velocity" : 1.7875161324671887, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.411020111290583, + "y" : -0.8987512250670413 + }, + "rotation" : { + "radians" : 0.09434193350703354 + } + }, + "curvature" : 0.30942902122153493 + }, { + "time" : 0.4813894477163118, + "velocity" : 1.9255577908652477, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 4.474738718233415, + "y" : -0.892051514340153 + }, + "rotation" : { + "radians" : 0.11568561342262434 + } + }, + "curvature" : 0.3568953774098475 + }, { + "time" : 0.5131039023083906, + "velocity" : 2.052415609233563, + "acceleration" : -0.5080487315100024, + "pose" : { + "translation" : { + "x" : 4.537306824087864, + "y" : -0.8840345543976582 + }, + "rotation" : { + "radians" : 0.13968544333222133 + } + }, + "curvature" : 0.404105267004568 + }, { + "time" : 0.5734741207486527, + "velocity" : 2.021744596334006, + "acceleration" : -2.5986497425654487, + "pose" : { + "translation" : { + "x" : 4.658586321464071, + "y" : -0.8636602870095267 + }, + "rotation" : { + "radians" : 0.19510294768348427 + } + }, + "curvature" : 0.4971313158757367 + }, { + "time" : 0.6345428990288139, + "velocity" : 1.8630482313774788, + "acceleration" : -1.782825751736676, + "pose" : { + "translation" : { + "x" : 4.774180161280128, + "y" : -0.8370385771048632 + }, + "rotation" : { + "radians" : 0.2593591749925202 + } + }, + "curvature" : 0.5854307189034433 + }, { + "time" : 0.6978369262592252, + "velocity" : 1.750206009699979, + "acceleration" : -1.1573812389052407, + "pose" : { + "translation" : { + "x" : 4.883610030103824, + "y" : -0.8038613196792674 + }, + "rotation" : { + "radians" : 0.33087764946357995 + } + }, + "curvature" : 0.6633540150470494 + }, { + "time" : 0.7622830361592594, + "velocity" : 1.6756172911812541, + "acceleration" : -0.6206912248410478, + "pose" : { + "translation" : { + "x" : 4.986581972800195, + "y" : -0.7640763998497277 + }, + "rotation" : { + "radians" : 0.40763331940017733 + } + }, + "curvature" : 0.7237257699378088 + }, { + "time" : 0.8268495770863082, + "velocity" : 1.6355414058094946, + "acceleration" : -0.1141447568431195, + "pose" : { + "translation" : { + "x" : 5.0829706221219055, + "y" : -0.7178619421826262 + }, + "rotation" : { + "radians" : 0.48716997806870804 + } + }, + "curvature" : 0.7596273921140029 + }, { + "time" : 0.890535049243441, + "velocity" : 1.6282720430756794, + "acceleration" : 0.4095450005884974, + "pose" : { + "translation" : { + "x" : 5.1728034282996305, + "y" : -0.6656005600217441 + }, + "rotation" : { + "radians" : 0.5667524438791094 + } + }, + "curvature" : 0.7664251916360058 + }, { + "time" : 0.9523748257700176, + "velocity" : 1.6535982143896488, + "acceleration" : 1.0079964321999564, + "pose" : { + "translation" : { + "x" : 5.2562448886324376, + "y" : -0.6078536048162668 + }, + "rotation" : { + "radians" : 0.6436149831528346 + } + }, + "curvature" : 0.7431281524527987 + }, { + "time" : 1.011449874872387, + "velocity" : 1.7131456531168745, + "acceleration" : 1.7790042074143708, + "pose" : { + "translation" : { + "x" : 5.3335807770781685, + "y" : -0.5453354154487897 + }, + "rotation" : { + "radians" : 0.7152240168950873 + } + }, + "curvature" : 0.6923650331342213 + }, { + "time" : 1.066883029265564, + "velocity" : 1.8117614680125869, + "acceleration" : 2.9290544621119077, + "pose" : { + "translation" : { + "x" : 5.405202373843821, + "y" : -0.47888756756332285 + }, + "rotation" : { + "radians" : 0.7794678142970032 + } + }, + "curvature" : 0.6190442095943712 + }, { + "time" : 1.1178097408882126, + "velocity" : 1.960928579931592, + "acceleration" : 4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.47159069497593, + "y" : -0.4094531228932965 + }, + "rotation" : { + "radians" : 0.8347237020762335 + } + }, + "curvature" : 0.5284454355848308 + }, { + "time" : 1.163781325410918, + "velocity" : 2.144814918022414, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.533300721950951, + "y" : -0.3380508785895664 + }, + "rotation" : { + "radians" : 0.879806448814223 + } + }, + "curvature" : 0.4245631719643485 + }, { + "time" : 1.2052874260275384, + "velocity" : 2.3108393204888955, + "acceleration" : 0.9499793055242893, + "pose" : { + "translation" : { + "x" : 5.59094563126564, + "y" : -0.265749616548419 + }, + "rotation" : { + "radians" : 0.9138373619233752 + } + }, + "curvature" : 0.3090988446696365 + }, { + "time" : 1.2440241708410498, + "velocity" : 2.3476384264251067, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 5.645181024027437, + "y" : -0.19364235273957675 + }, + "rotation" : { + "radians" : 0.93608126262559 + } + }, + "curvature" : 0.18105135742527462 + }, { + "time" : 1.2825935410962108, + "velocity" : 2.1933609454044625, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 5.696689155544846, + "y" : -0.12282058653420336 + }, + "rotation" : { + "radians" : 0.9457881291979806 + } + }, + "curvature" : 0.036672195665488225 + }, { + "time" : 1.3225645194975204, + "velocity" : 2.033477031799224, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.746163164917817, + "y" : -0.05434855003290906 + }, + "rotation" : { + "radians" : 0.9420639655569212 + } + }, + "curvature" : -0.13035733884148892 + }, { + "time" : 1.3640767963731968, + "velocity" : 1.8674279242965188, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.794291304628132, + "y" : 0.010762542606244097 + }, + "rotation" : { + "radians" : 0.9237929192972799 + } + }, + "curvature" : -0.3283818138757726 + }, { + "time" : 1.4073934572912177, + "velocity" : 1.6941612806244348, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.8417411701297794, + "y" : 0.07158024583973699 + }, + "rotation" : { + "radians" : 0.8896456491310966 + } + }, + "curvature" : -0.5662511162346159 + }, { + "time" : 1.4530216175559991, + "velocity" : 1.5116486395653088, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.889143929439342, + "y" : 0.12727663341058815 + }, + "rotation" : { + "radians" : 0.8382382104223547 + } + }, + "curvature" : -0.8496047780846073 + }, { + "time" : 1.5019520124723162, + "velocity" : 1.31592705990004, + "acceleration" : -3.2992132129798613, + "pose" : { + "translation" : { + "x" : 5.937078552726376, + "y" : 0.17715404902034848 + }, + "rotation" : { + "radians" : 0.7685396527813694 + } + }, + "curvature" : -1.173437784895902 + }, { + "time" : 1.5280288447019, + "velocity" : 1.2298940304555388, + "acceleration" : -2.595879024435883, + "pose" : { + "translation" : { + "x" : 5.961408087691393, + "y" : 0.19973458251777654 + }, + "rotation" : { + "radians" : 0.7267822918880255 + } + }, + "curvature" : -1.343347329975652 + }, { + "time" : 1.5550966642268162, + "velocity" : 1.1596292455135926, + "acceleration" : -2.0093413111156626, + "pose" : { + "translation" : { + "x" : 5.986056041903794, + "y" : 0.220670857001096 + }, + "rotation" : { + "radians" : 0.6806182852772713 + } + }, + "curvature" : -1.5110727869831724 + }, { + "time" : 1.582993226711798, + "velocity" : 1.103575530074399, + "acceleration" : -1.499480490388413, + "pose" : { + "translation" : { + "x" : 6.011073083229763, + "y" : 0.23992377095496042 + }, + "rotation" : { + "radians" : 0.6303974993218476 + } + }, + "curvature" : -1.6684745220647488 + }, { + "time" : 1.6115420851073832, + "velocity" : 1.0607670738873578, + "acceleration" : -1.0383464958403599, + "pose" : { + "translation" : { + "x" : 6.036503660218244, + "y" : 0.2574671929874306 + }, + "rotation" : { + "radians" : 0.576664005958531 + } + }, + "curvature" : -1.8058581852202884 + }, { + "time" : 1.6405508670810625, + "velocity" : 1.030645906776391, + "acceleration" : -0.6046251421798862, + "pose" : { + "translation" : { + "x" : 6.062385509275637, + "y" : 0.27328876653847445 + }, + "rotation" : { + "radians" : 0.5201672456863837 + } + }, + "curvature" : -1.9129549301403845 + }, { + "time" : 1.6698112663305722, + "velocity" : 1.0129543337199158, + "acceleration" : -0.17989538152775067, + "pose" : { + "translation" : { + "x" : 6.0887491618404965, + "y" : 0.28739071458846865 + }, + "rotation" : { + "radians" : 0.46185036100408955 + } + }, + "curvature" : -1.9803591986041147 + }, { + "time" : 1.6991006131857587, + "velocity" : 1.0076853154927035, + "acceleration" : 0.25454795207597797, + "pose" : { + "translation" : { + "x" : 6.1156174515582356, + "y" : 0.2997906443666931 + }, + "rotation" : { + "radians" : 0.40281210939179324 + } + }, + "curvature" : -2.00112327804307 + }, { + "time" : 1.728184543388813, + "velocity" : 1.0150885703642116, + "acceleration" : 0.7213256391379216, + "pose" : { + "translation" : { + "x" : 6.143005021455821, + "y" : 0.3105223520598379 + }, + "rotation" : { + "radians" : 0.3442451501516796 + } + }, + "curvature" : -1.9720404920384738 + }, { + "time" : 1.756819895841348, + "velocity" : 1.035743984273976, + "acceleration" : 1.2524391885977852, + "pose" : { + "translation" : { + "x" : 6.170917831116474, + "y" : 0.31963662752049693 + }, + "rotation" : { + "radians" : 0.28736056159514967 + } + }, + "curvature" : -1.8941696100081031 + }, { + "time" : 1.784756647104708, + "velocity" : 1.0707330663583168, + "acceleration" : 1.8992599815033173, + "pose" : { + "translation" : { + "x" : 6.19935266385437, + "y" : 0.3272020589756721 + }, + "rotation" : { + "radians" : 0.23331316860807427 + } + }, + "curvature" : -1.7723980930633711 + }, { + "time" : 1.8117374729698128, + "velocity" : 1.1219766691918198, + "acceleration" : 2.7534793106504156, + "pose" : { + "translation" : { + "x" : 6.228296633889339, + "y" : 0.33330583773527067 + }, + "rotation" : { + "radians" : 0.183142361520775 + } + }, + "curvature" : -1.614195199267953 + }, { + "time" : 1.8374933012978958, + "velocity" : 1.1928948096218601, + "acceleration" : -3.5661641751301776, + "pose" : { + "translation" : { + "x" : 6.257726693521562, + "y" : 0.33805456290060576 + }, + "rotation" : { + "radians" : 0.1377385534824891 + } + }, + "curvature" : -1.4279710594799537 + }, { + "time" : 1.8927457727102306, + "velocity" : 0.9958554254837872, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 6.317899124228461, + "y" : 0.3440151160617688 + }, + "rotation" : { + "radians" : 0.06404669180836077 + } + }, + "curvature" : -1.0007588885962067 + }, { + "time" : 1.9651351974193783, + "velocity" : 0.7062977266471963, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.37946360862216, + "y" : 0.3463552460207815 + }, + "rotation" : { + "radians" : 0.01679972270125931 + } + }, + "curvature" : -0.5272785953582695 + }, { + "time" : 2.1417096290811775, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.441819667816162, + "y" : 0.3467085063457489 + }, + "rotation" : { + "radians" : 0.0 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], + "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + }, { + "x" : [ 6.441819667816162, 2.0, 0.0 ], + "y" : [ 0.3467085063457489, 0.0, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 2.0499999541789293, + "rotation" : { + "radians" : 0.0 + } + } ], + "reversed" : false, + "color" : 286.3529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "sleep", + "args" : [ "1000" ], + "argTypes" : [ "long" ], + "reflection" : false + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Intake.setIntakeSolState CLOSE", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.441819667816162, + "y" : 0.3467085063457489 + }, + "rotation" : { + "radians" : 1.6078163234494292 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.1953077076111654, + "velocity" : 0.7812308304446616, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 6.4389816843095105, + "y" : 0.42294590310561375 + }, + "rotation" : { + "radians" : 1.6083198170208068 + } + }, + "curvature" : 0.010007683937884466 + }, { + "time" : 0.28418183429804866, + "velocity" : 1.1367273371921942, + "acceleration" : 3.9999999999999987, + "pose" : { + "translation" : { + "x" : 6.435750551492674, + "y" : 0.5081130612115885 + }, + "rotation" : { + "radians" : 1.609052997763179 + } + }, + "curvature" : 0.006001548474080814 + }, { + "time" : 0.3615636139485796, + "velocity" : 1.446254455794318, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.431910399555818, + "y" : 0.6079771182521938 + }, + "rotation" : { + "radians" : 1.6092943557828885 + } + }, + "curvature" : -8.5488367035683E-4 + }, { + "time" : 0.43523665263811134, + "velocity" : 1.740946610552445, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.427411179058254, + "y" : 0.72529627053882 + }, + "rotation" : { + "radians" : 1.6088589590984765 + } + }, + "curvature" : -0.006111352293260847 + }, { + "time" : 0.47131335599478547, + "velocity" : 1.8852534239791414, + "acceleration" : 3.999999999999987, + "pose" : { + "translation" : { + "x" : 6.42493665439585, + "y" : 0.7906601188879874 + }, + "rotation" : { + "radians" : 1.6083911287059784 + } + }, + "curvature" : -0.008116127399784383 + }, { + "time" : 0.5069348966838761, + "velocity" : 2.0277395867355037, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.422338327934085, + "y" : 0.8603050862099906 + }, + "rotation" : { + "radians" : 1.607763133691658 + } + }, + "curvature" : -0.009853447575726546 + }, { + "time" : 0.5420590363593313, + "velocity" : 2.1682361454373242, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.419643072810146, + "y" : 0.9339457983013029 + }, + "rotation" : { + "radians" : 1.6069775892569418 + } + }, + "curvature" : -0.011436025231544151 + }, { + "time" : 0.5766104955030272, + "velocity" : 2.306441982012108, + "acceleration" : 3.9999999999999947, + "pose" : { + "translation" : { + "x" : 6.416882438497851, + "y" : 1.0111998183356263 + }, + "rotation" : { + "radians" : 1.6060340313214787 + } + }, + "curvature" : -0.012962877357532548 + }, { + "time" : 0.6104960029929839, + "velocity" : 2.4419840119719347, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.4140917029015725, + "y" : 1.0916028128984 + }, + "rotation" : { + "radians" : 1.604928600003804 + } + }, + "curvature" : -0.014520270296573292 + }, { + "time" : 0.6436141193910724, + "velocity" : 2.5744564775642886, + "acceleration" : 3.9999999999999947, + "pose" : { + "translation" : { + "x" : 6.411308924450168, + "y" : 1.1746237180213086 + }, + "rotation" : { + "radians" : 1.603653938746236 + } + }, + "curvature" : -0.01618596901731972 + }, { + "time" : 0.6758618258229525, + "velocity" : 2.703447303291809, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.408573994190903, + "y" : 1.2596799052167897 + }, + "rotation" : { + "radians" : 1.6021991606605406 + } + }, + "curvature" : -0.01803416029898833 + }, { + "time" : 0.7071390741340647, + "velocity" : 2.828556296536258, + "acceleration" : 3.5711423686938364, + "pose" : { + "translation" : { + "x" : 6.405927687883377, + "y" : 1.3461523475125432 + }, + "rotation" : { + "radians" : 1.6005498243279082 + } + }, + "curvature" : -0.020140089228601762 + }, { + "time" : 0.7374186993151032, + "velocity" : 2.936689148928433, + "acceleration" : -3.99999999999999, + "pose" : { + "translation" : { + "x" : 6.403410718093454, + "y" : 1.4334007854860387 + }, + "rotation" : { + "radians" : 1.5986879179972218 + } + }, + "curvature" : -0.02258406827188887 + }, { + "time" : 0.7678125246032664, + "velocity" : 2.8151138477757804, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.401062786287184, + "y" : 1.520778893299024 + }, + "rotation" : { + "radians" : 1.5965918957171965 + } + }, + "curvature" : -0.02545459002718879 + }, { + "time" : 0.7993888917739188, + "velocity" : 2.688808379093171, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.398921634924733, + "y" : 1.6076494447320337 + }, + "rotation" : { + "radians" : 1.59423685765634 + } + }, + "curvature" : -0.028849988627757065 + }, { + "time" : 0.8320832691315886, + "velocity" : 2.558030869662492, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.397022099554306, + "y" : 1.6933994792188969 + }, + "rotation" : { + "radians" : 1.59159503414185 + } + }, + "curvature" : -0.03287744142106298 + }, { + "time" : 0.8658400026260393, + "velocity" : 2.423003935684689, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.395395160906077, + "y" : 1.7774554678812455 + }, + "rotation" : { + "radians" : 1.588636834345054 + } + }, + "curvature" : -0.03764692669916069 + }, { + "time" : 0.9006204438645928, + "velocity" : 2.283882170730475, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.394066996986112, + "y" : 1.859298479563023 + }, + "rotation" : { + "radians" : 1.5853328740324872 + } + }, + "curvature" : -0.04325577926527458 + }, { + "time" : 0.9364146483950042, + "velocity" : 2.1407053526088293, + "acceleration" : -3.999999999999997, + "pose" : { + "translation" : { + "x" : 6.393058035170299, + "y" : 1.938479346864992 + }, + "rotation" : { + "radians" : 1.5816576204489718 + } + }, + "curvature" : -0.04975638894367889 + }, { + "time" : 0.9732587981504898, + "velocity" : 1.993328753586887, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 6.39238200429827, + "y" : 2.0146338321792427 + }, + "rotation" : { + "radians" : 1.5775955933008623 + } + }, + "curvature" : -0.05709527513223549 + }, { + "time" : 1.0112621915316293, + "velocity" : 1.841315180062329, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.392044986767331, + "y" : 2.087497793723702 + }, + "rotation" : { + "radians" : 1.57315140430575 + } + }, + "curvature" : -0.06500723564977115 + }, { + "time" : 1.0506511801697798, + "velocity" : 1.683759225509727, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.392044470626388, + "y" : 2.1569223515766396 + }, + "rotation" : { + "radians" : 1.5683651575191264 + } + }, + "curvature" : -0.07284766972073614 + }, { + "time" : 1.0918455825454725, + "velocity" : 1.5189816160069558, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.39236840166987, + "y" : 2.222889053711179 + }, + "rotation" : { + "radians" : 1.5633344792916735 + } + }, + "curvature" : -0.07936077553696355 + }, { + "time" : 1.1356043581385193, + "velocity" : 1.3439465136347681, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 6.39299423553166, + "y" : 2.2855250420298034 + }, + "rotation" : { + "radians" : 1.5582428882782466 + } + }, + "curvature" : -0.0824337723791933 + }, { + "time" : 1.2379787498796464, + "velocity" : 0.9344489466702602, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.395003296006507, + "y" : 2.4021324106830946 + }, + "rotation" : { + "radians" : 1.549211634448327 + } + }, + "curvature" : -0.06547973929352308 + }, { + "time" : 1.4715909865472114, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.397645473480225, + "y" : 2.5112497806549072 + }, + "rotation" : { + "radians" : 1.5451593331499225 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 6.441819667816162, -0.0441741943359375, 0.0 ], + "y" : [ 0.3467085063457489, 1.1927070915699005, 0.0 ] + }, { + "x" : [ 6.397645473480225, 0.04417705535888672, 0.0 ], + "y" : [ 2.5112497806549072, 1.7227985858917236, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 1.3799999691545963, + "rotation" : { + "radians" : 0.6716053016572916 + } + } ], + "reversed" : false, + "color" : 92.0, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + } ] +} \ No newline at end of file From 181e0171bce589dabf07038f5ef8e36dbda193bf Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 23:32:29 -0700 Subject: [PATCH 037/108] adjust shooter config --- src/main/deploy/shooter/shooterconfig.json | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 29a488d0..f8245b00 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -13,15 +13,15 @@ "distance" : 82.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 1900.0, + "flywheelSpeed" : 1800.0, "distance" : 106.0 }, { "hoodEjectAngle" : 53.0, - "flywheelSpeed" : 1900.0, + "flywheelSpeed" : 1800.0, "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1900.0, "distance" : 120.0 }, { "hoodEjectAngle" : 51.0, @@ -33,7 +33,7 @@ "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2050.0, "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, From 1f6bd2339a3fea7dcea05f6495bed2e004d3a0a6 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 26 Mar 2022 23:32:58 -0700 Subject: [PATCH 038/108] fix intake using the wrong constant --- src/main/java/frc/subsystem/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Intake.java b/src/main/java/frc/subsystem/Intake.java index d2fd13e6..426f4e0b 100644 --- a/src/main/java/frc/subsystem/Intake.java +++ b/src/main/java/frc/subsystem/Intake.java @@ -102,7 +102,7 @@ private void setIntakeState(IntakeState intakeState) { switch (intakeState) { case INTAKE: if (Hopper.getInstance().getOuttakeState() == OuttakeState.EJECT) { - intakeMotor.set(-Constants.EJECT_OUTTAKE_SPEED); + intakeMotor.set(-Constants.INTAKEING_OUTTAKE_SPEED); } else { intakeMotor.set(Constants.INTAKE_MOTOR_SPEED); } From f4f1d5e285a6dded1928eec2829f94e88f6b279a Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 11:15:07 -0700 Subject: [PATCH 039/108] Made it so outtake state changes based on hopper instead of intake + Comment Changes --- src/main/java/frc/subsystem/Hopper.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index b1c3a049..91bbd338 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -126,20 +126,21 @@ private void updateOuttakeState() { // If color sensor detects a ball or Intake intake = Intake.getInstance(); - if (wantedHopperState == HopperState.REVERSE) { + // Override for all other possible states that reverses outtake if hopper is in reverse + if (wantedHopperState == HopperState.REVERSE && intake.getIntakeSolState() == IntakeSolState.OPEN) { outtakeState = OuttakeState.EJECT; return; } if (Constants.OUTTAKE_ALWAYS_INTAKE) { // Intake is running and open - if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + if (wantedHopperState == HopperState.ON && intake.getIntakeSolState() == IntakeSolState.OPEN) { // Ball Color is opposite if (getBallColor() == opposingAllianceColor) { lastDetectionTime = Timer.getFPGATimestamp(); outtakeState = OuttakeState.EJECT; - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { // Opposite ball color detected within a certain time frame + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { outtakeState = OuttakeState.EJECT; } else { outtakeState = OuttakeState.INTAKE; @@ -149,13 +150,13 @@ private void updateOuttakeState() { } } else { // Intake is running and open - if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + if (wantedHopperState == HopperState.ON && intake.getIntakeSolState() == IntakeSolState.OPEN) { // Ball Color is opposite if (getBallColor() == friendlyAllianceColor) { lastDetectionTime = Timer.getFPGATimestamp(); outtakeState = OuttakeState.INTAKE; - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { // Opposite ball color detected within a certain time frame + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { outtakeState = OuttakeState.INTAKE; } else { outtakeState = OuttakeState.EJECT; From a1dab59ad5a4796868b5690ebb9ce2d2aaeafb25 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 11:30:56 -0700 Subject: [PATCH 040/108] style: Outtake code cleanup + Changes various speed constant names to be more consistant and understandable + Added more comments --- src/main/java/frc/robot/Constants.java | 23 +++++++++++++++++---- src/main/java/frc/subsystem/Hopper.java | 9 ++++---- src/main/java/frc/subsystem/Intake.java | 6 +++--- src/test/java/frc/subsystem/IntakeTest.java | 4 ++-- 4 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bbfa2f29..061cdd82 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -235,11 +235,17 @@ public final class Constants { * outtake always outtaking until a friendly ball color is found */ public static final boolean OUTTAKE_ALWAYS_INTAKE = true; + + + /** + * Outtake's speed when ejecting balls + */ + public static final double OUTTAKE_EJECTION_SPEED = .5; + /** - * Maximum Percent Output of Outtake wheels + * Outtake's speed when intaking balls */ - public static final double INTAKEING_OUTTAKE_SPEED = .5; - public static final double EJECT_OUTTAKE_SPEED = .5; + public static final double OUTTAKE_SPEED = -.5; /** * Outtake will run for this time (Sec) after color sensor trigger @@ -507,9 +513,18 @@ public final class Constants { public static final int INTAKE_PERIOD = 50; public static final int INTAKE_SOLENOID_CHANNEL = 3; public static final int INTAKE_MOTOR_DEVICE_ID = 40; - public static final double INTAKE_MOTOR_SPEED = -1; public static final double INTAKE_OPEN_TIME = 0.0; + /** + * Intake Speed when intaking balls + */ + public static final double INTAKE_SPEED = -1; + + /** + * Intake speed when ejecting balls + */ + public static final double INTAKE_EJECTION_SPEED = -.5; + public static final int WEB_DASHBOARD_PORT = 5802; public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 50; diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 91bbd338..55b7ae61 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -215,6 +215,7 @@ public void update() { updateAllianceColor(); getBallColor(); + // Override that makes it so the eject outtake state can never be set if (!disableEject) { updateOuttakeState(); } else { @@ -228,10 +229,10 @@ public void update() { setOuttakePercentOutput(0); break; case EJECT: - setOuttakePercentOutput(Constants.EJECT_OUTTAKE_SPEED); + setOuttakePercentOutput(Constants.OUTTAKE_EJECTION_SPEED); break; case INTAKE: - setOuttakePercentOutput(-Constants.INTAKEING_OUTTAKE_SPEED); + setOuttakePercentOutput(Constants.OUTTAKE_SPEED); break; } @@ -268,11 +269,11 @@ public void selfTest() { setHopperState(HopperState.OFF); OrangeUtility.sleep(5000); - setOuttakePercentOutput(Constants.INTAKEING_OUTTAKE_SPEED); + setOuttakePercentOutput(-Constants.INTAKE_EJECTION_SPEED); System.out.println("Ejecting"); OrangeUtility.sleep(Constants.TEST_TIME_MS); - setOuttakePercentOutput(-Constants.INTAKEING_OUTTAKE_SPEED); + setOuttakePercentOutput(Constants.INTAKE_EJECTION_SPEED); System.out.println("Intaking"); OrangeUtility.sleep(Constants.TEST_TIME_MS); diff --git a/src/main/java/frc/subsystem/Intake.java b/src/main/java/frc/subsystem/Intake.java index 426f4e0b..d9860478 100644 --- a/src/main/java/frc/subsystem/Intake.java +++ b/src/main/java/frc/subsystem/Intake.java @@ -102,14 +102,14 @@ private void setIntakeState(IntakeState intakeState) { switch (intakeState) { case INTAKE: if (Hopper.getInstance().getOuttakeState() == OuttakeState.EJECT) { - intakeMotor.set(-Constants.INTAKEING_OUTTAKE_SPEED); + intakeMotor.set(Constants.INTAKE_EJECTION_SPEED); } else { - intakeMotor.set(Constants.INTAKE_MOTOR_SPEED); + intakeMotor.set(Constants.INTAKE_SPEED); } break; case EJECT: - intakeMotor.set(-Constants.INTAKE_MOTOR_SPEED); + intakeMotor.set(-Constants.INTAKE_SPEED); break; case OFF: diff --git a/src/test/java/frc/subsystem/IntakeTest.java b/src/test/java/frc/subsystem/IntakeTest.java index 60fbe0b1..1e6f59e2 100644 --- a/src/test/java/frc/subsystem/IntakeTest.java +++ b/src/test/java/frc/subsystem/IntakeTest.java @@ -65,7 +65,7 @@ public void intakeDoesRunReversedWhenOpen() throws Exception { intakeMotorField.setAccessible(true); LazyCANSparkMax intakeMotor = (LazyCANSparkMax) intakeMotorField.get(intake); - assertEquals(-Constants.INTAKE_MOTOR_SPEED, intakeMotor.get(), DELTA); + assertEquals(-Constants.INTAKE_SPEED, intakeMotor.get(), DELTA); } @Test @@ -81,6 +81,6 @@ public void intakeDoesRunWhenOpen() throws Exception { intakeMotorField.setAccessible(true); LazyCANSparkMax intakeMotor = (LazyCANSparkMax) intakeMotorField.get(intake); - assertEquals(Constants.INTAKE_MOTOR_SPEED, intakeMotor.get(), DELTA); + assertEquals(Constants.INTAKE_SPEED, intakeMotor.get(), DELTA); } } \ No newline at end of file From d7755b22dcab1a22e9e7a3e541a84b5d33ca3ece Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 13:38:17 -0700 Subject: [PATCH 041/108] Fixed Hood Eject --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/Robot.java | 11 +++++------ src/main/java/frc/subsystem/Shooter.java | 6 ++++-- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 061cdd82..1eb09027 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -280,8 +280,8 @@ public final class Constants { */ public static final double ALLOWED_SHOOTER_SPEED_ERROR_RPM = 150; - public static final int SHOOTER_EJECT_SPEED = 500; - public static final double HOOD_EJECT_ANGLE = 85; + public static final int SHOOTER_EJECT_SPEED = 750; + public static final double HOOD_EJECT_ANGLE = 53; /** * Conversion from Falcon Sensor Units / 100ms to RPM 2048 is Sensor Units Per Revolution 600 Converts From Time of 100ms to 1 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 63fd1175..4da58cd7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -140,7 +140,7 @@ public class Robot extends TimedRobot { @NotNull public static final String RED = "RED"; @NotNull public static final String BLUE = "BLUE"; - private static boolean hoodEject = false; + public static boolean hoodEject = false; public static final SendableChooser sideChooser = new SendableChooser<>(); @@ -452,11 +452,9 @@ public void teleopPeriodic() { stick.update(); buttonPanel.update(); - if (xbox.getRawAxis(2) > 0.1) { + if (xbox.getRawAxis(2) > 0.1 && !hoodEject) { if (isTryingToRunShooterFromButtonPanel()) { //If vision is off - if (!hoodEject) { - shooter.setHoodPosition(shooterPreset.getHoodEjectAngle()); - } + shooter.setHoodPosition(shooterPreset.getHoodEjectAngle()); shooter.setSpeed(shooterPreset.getFlywheelSpeed()); shooter.setFiring(true); hopper.setHopperState(Hopper.HopperState.ON); @@ -466,7 +464,7 @@ public void teleopPeriodic() { visionManager.forceVisionOn(driverForcingVisionOn); visionManager.shootAndMove(getControllerDriveInputs(), useFieldRelative); } - } else { + } else if (!hoodEject) { shooter.setFiring(false); shooter.setSpeed(0); visionManager.unForceVisionOn(driverForcingVisionOn); @@ -608,6 +606,7 @@ private void runShooter() { } else if (buttonPanel.getRawButton(6)) { shooter.setSpeed(Constants.SHOOTER_EJECT_SPEED); shooter.setHoodPosition(Constants.HOOD_EJECT_ANGLE); + shooter.setFiring(true); hoodEject = true; } else { hoodEject = false; diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java index b7c04c3e..e69cbe3c 100644 --- a/src/main/java/frc/subsystem/Shooter.java +++ b/src/main/java/frc/subsystem/Shooter.java @@ -624,10 +624,12 @@ public void update() { } if ( - (feederWheelState == FeederWheelState.FORWARD) + ((feederWheelState == FeederWheelState.FORWARD) && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) && (Timer.getFPGATimestamp() > SECOND_BALL_SHOOT_DELAY + lastShotTime) - || feederChecksDisabled) + || feederChecksDisabled)) + // Doesn't need any of the checks if hood ejecting + || (feederWheelState == FeederWheelState.FORWARD && Robot.hoodEject) ) { feederWheel.set(ControlMode.PercentOutput, FEEDER_WHEEL_SPEED); lastShotTime = Timer.getFPGATimestamp(); From 25e4441668c9c130cc72d12c878e34fe879c306e Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 14:09:11 -0700 Subject: [PATCH 042/108] Reorganized Hood Eject Code --- src/main/java/frc/robot/Robot.java | 21 ++++++++++----------- src/main/java/frc/subsystem/Shooter.java | 8 +++++--- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4da58cd7..693f0900 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -140,8 +140,6 @@ public class Robot extends TimedRobot { @NotNull public static final String RED = "RED"; @NotNull public static final String BLUE = "BLUE"; - public static boolean hoodEject = false; - public static final SendableChooser sideChooser = new SendableChooser<>(); //Subsystems @@ -452,7 +450,14 @@ public void teleopPeriodic() { stick.update(); buttonPanel.update(); - if (xbox.getRawAxis(2) > 0.1 && !hoodEject) { + // Hood Eject + if (buttonPanel.getRawButton(6)) { + shooter.setFeederChecksDisabled(true); + shooter.setSpeed(Constants.SHOOTER_EJECT_SPEED); + shooter.setHoodPosition(Constants.HOOD_EJECT_ANGLE); + shooter.setFiring(true); + } else if (xbox.getRawAxis(2) > 0.1) { + shooter.setFeederChecksDisabled(false); if (isTryingToRunShooterFromButtonPanel()) { //If vision is off shooter.setHoodPosition(shooterPreset.getHoodEjectAngle()); shooter.setSpeed(shooterPreset.getFlywheelSpeed()); @@ -464,7 +469,8 @@ public void teleopPeriodic() { visionManager.forceVisionOn(driverForcingVisionOn); visionManager.shootAndMove(getControllerDriveInputs(), useFieldRelative); } - } else if (!hoodEject) { + } else { + shooter.setFeederChecksDisabled(false); shooter.setFiring(false); shooter.setSpeed(0); visionManager.unForceVisionOn(driverForcingVisionOn); @@ -603,13 +609,6 @@ private void runShooter() { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(139); } else if (buttonPanel.getRisingEdge(3)) { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(40); - } else if (buttonPanel.getRawButton(6)) { - shooter.setSpeed(Constants.SHOOTER_EJECT_SPEED); - shooter.setHoodPosition(Constants.HOOD_EJECT_ANGLE); - shooter.setFiring(true); - hoodEject = true; - } else { - hoodEject = false; } } diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java index e69cbe3c..cf29420f 100644 --- a/src/main/java/frc/subsystem/Shooter.java +++ b/src/main/java/frc/subsystem/Shooter.java @@ -564,7 +564,7 @@ public double getLastShotTime() { } private volatile double lastShotTime = 0; - += /** * Update Method for Shooter. *

@@ -584,6 +584,9 @@ public double getLastShotTime() { */ @Override public void update() { + // Doesn't need any of the checks if hood ejecting + updateEjectChecking(); + // Switch statement only allows certain code to be run for specific states of the robot switch (shooterState) { case OFF: @@ -610,6 +613,7 @@ public void update() { break; case ON: + shooterWheelMaster.set(ControlMode.Velocity, desiredShooterSpeed * Constants.SET_SHOOTER_SPEED_CONVERSION_FACTOR); // Sets shooter motor to desired shooter @@ -628,8 +632,6 @@ public void update() { && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) && (Timer.getFPGATimestamp() > SECOND_BALL_SHOOT_DELAY + lastShotTime) || feederChecksDisabled)) - // Doesn't need any of the checks if hood ejecting - || (feederWheelState == FeederWheelState.FORWARD && Robot.hoodEject) ) { feederWheel.set(ControlMode.PercentOutput, FEEDER_WHEEL_SPEED); lastShotTime = Timer.getFPGATimestamp(); From 6dbbdb6bab75ec71170c330e735e01569989895f Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 14:10:41 -0700 Subject: [PATCH 043/108] Minor syntax fixes to make code run --- src/main/java/frc/subsystem/Shooter.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java index cf29420f..6362fef2 100644 --- a/src/main/java/frc/subsystem/Shooter.java +++ b/src/main/java/frc/subsystem/Shooter.java @@ -564,7 +564,7 @@ public double getLastShotTime() { } private volatile double lastShotTime = 0; -= + /** * Update Method for Shooter. *

@@ -584,9 +584,6 @@ public double getLastShotTime() { */ @Override public void update() { - // Doesn't need any of the checks if hood ejecting - updateEjectChecking(); - // Switch statement only allows certain code to be run for specific states of the robot switch (shooterState) { case OFF: From 4127ab867c85affec817c9afb184fc7a9059eb88 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 27 Mar 2022 14:53:02 -0700 Subject: [PATCH 044/108] Update shooterconfig.json --- src/main/deploy/shooter/shooterconfig.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index f8245b00..9fc36d1b 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -21,11 +21,11 @@ "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1900.0, + "flywheelSpeed" : 1800.0, "distance" : 120.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1900.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, @@ -33,7 +33,7 @@ "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2050.0, + "flywheelSpeed" : 2100.0, "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, From fa812710b49d5620b38bb1f0f8e51ebd9dcc3737 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 27 Mar 2022 14:53:38 -0700 Subject: [PATCH 045/108] improve 5 ball --- src/main/deploy/autos/blue/5ball.json | 1412 ++++++++++++------------- 1 file changed, 664 insertions(+), 748 deletions(-) diff --git a/src/main/deploy/autos/blue/5ball.json b/src/main/deploy/autos/blue/5ball.json index 0b3e67af..85ea1792 100644 --- a/src/main/deploy/autos/blue/5ball.json +++ b/src/main/deploy/autos/blue/5ball.json @@ -250,7 +250,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 3.999999999999999, + "acceleration" : 3.9999999999999987, "pose" : { "translation" : { "x" : 7.626592636108398, @@ -262,419 +262,419 @@ }, "curvature" : -0.0 }, { - "time" : 0.23664586321986603, - "velocity" : 0.9465834528794639, - "acceleration" : 4.000000000000001, + "time" : 0.23693937519075453, + "velocity" : 0.9477575007630178, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 7.52540296068446, - "y" : -3.1488323883245357 + "x" : 7.5252611777282254, + "y" : -3.148483353978918 }, "rotation" : { - "radians" : 2.6953045863269516 + "radians" : 2.6889354034911954 } }, - "curvature" : -0.07577684722310894 + "curvature" : -0.17311628002444718 }, { - "time" : 0.34030220571431735, - "velocity" : 1.3612088228572694, - "acceleration" : 4.0000000000000036, + "time" : 0.34179239143933526, + "velocity" : 1.3671695657573404, + "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 7.417757449569763, - "y" : -3.0966914842865663 + "x" : 7.416731443241588, + "y" : -3.094165712100221 }, "rotation" : { - "radians" : 2.6858123918336316 + "radians" : 2.666000997449467 } }, - "curvature" : -0.07326544503040545 + "curvature" : -0.1843290944729718 }, { - "time" : 0.384629630154288, - "velocity" : 1.5385185206171523, - "acceleration" : 4.0000000000000036, + "time" : 0.3870859500027286, + "velocity" : 1.5483438000109138, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.360111402026817, - "y" : -3.0682766663898775 + "x" : 7.358208917510225, + "y" : -3.0635932230767082 }, "rotation" : { - "radians" : 2.6816220169257763 + "radians" : 2.6546580588501496 } }, - "curvature" : -0.056343210130415364 + "curvature" : -0.1576700182253739 }, { - "time" : 0.42640049414944053, - "velocity" : 1.7056019765977626, + "time" : 0.43004526564064915, + "velocity" : 1.720181062562596, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.299449204304892, - "y" : -3.0380968957629193 + "x" : 7.29633205176151, + "y" : -3.0304232421231063 }, "rotation" : { - "radians" : 2.6784957933752187 + "radians" : 2.644758053448521 } }, - "curvature" : -0.035816849566663374 + "curvature" : -0.12418290422011853 }, { - "time" : 0.4664026360786243, - "velocity" : 1.8656105443144977, - "acceleration" : 3.9999999999999942, + "time" : 0.4714292106726367, + "velocity" : 1.8857168426905462, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 7.235577081185269, - "y" : -3.0061231284680474 + "x" : 7.2308898126944, + "y" : -2.994584240198975 }, "rotation" : { - "radians" : 2.6767024131530928 + "radians" : 2.6367892329110907 } }, - "curvature" : -0.014633323030462457 + "curvature" : -0.08996179996248393 }, { - "time" : 0.5050798369132412, - "velocity" : 2.0203193476529653, - "acceleration" : 4.0, + "time" : 0.5116473910205699, + "velocity" : 2.046589564082279, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 7.168413784820586, - "y" : -2.972412885632366 + "x" : 7.16179740941152, + "y" : -2.956125016324222 }, "rotation" : { - "radians" : 2.676381564257267 + "radians" : 2.6309726297310103 } }, - "curvature" : 0.005723704793670682 + "curvature" : -0.057945967404890976 }, { - "time" : 0.5426791778444782, - "velocity" : 2.170716711377913, - "acceleration" : 3.9999999999999947, + "time" : 0.5509102464902416, + "velocity" : 2.2036409859609662, + "acceleration" : 3.999999999999985, "pose" : { "translation" : { - "x" : 7.097979906595825, - "y" : -2.9371007325542138 + "x" : 7.089084233776916, + "y" : -2.915201800386029 }, "rotation" : { - "radians" : 2.6775963537201526 + "radians" : 2.6273738323394795 } }, - "curvature" : 0.024736031322158278 + "curvature" : -0.029106454170210898 }, { - "time" : 0.579328420101757, - "velocity" : 2.3173136804070285, + "time" : 0.5893084187633256, + "velocity" : 2.357233675053302, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.024387188989294, - "y" : -2.900388757809651 + "x" : 7.012881800773812, + "y" : -2.8720653559457787 }, "rotation" : { - "radians" : 2.6803717421830306 + "radians" : 2.625981856643082 } }, - "curvature" : 0.04244185336482878 + "curvature" : -0.003356981490914264 }, { - "time" : 0.6150805872232928, - "velocity" : 2.460322348893172, - "acceleration" : 4.000000000000005, + "time" : 0.6268587033194142, + "velocity" : 2.5074348132776563, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.947827837433621, - "y" : -2.8625370523589453 + "x" : 6.933411688862364, + "y" : -2.8270480830459803 }, "rotation" : { - "radians" : 2.684720287316673 + "radians" : 2.6267596915335263 } }, - "curvature" : 0.05916509233933032 + "curvature" : 0.01988136359364883 }, { - "time" : 0.6499414309623317, - "velocity" : 2.5997657238493272, - "acceleration" : 3.999999999999995, + "time" : 0.6635324088349698, + "velocity" : 2.6541296353398787, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.868563832176733, - "y" : -2.8238541886530584 + "x" : 6.850973480337416, + "y" : -2.780551121017197 }, "rotation" : { - "radians" : 2.690658352810391 + "radians" : 2.6296746402590294 } }, - "curvature" : 0.07535949039930706 + "curvature" : 0.04137650970352362 }, { - "time" : 0.6838872251295486, - "velocity" : 2.7355489005181948, - "acceleration" : 2.377223272165517, + "time" : 0.6992737012181571, + "velocity" : 2.797094804872628, + "acceleration" : 2.946517671736344, "pose" : { "translation" : { - "x" : 6.786916240142844, - "y" : -2.7846876997401324 + "x" : 6.765932701686253, + "y" : -2.7330314512849725 }, "rotation" : { - "radians" : 2.6982158314592457 + "radians" : 2.634715682627438 } }, - "curvature" : 0.09152947173359463 + "curvature" : 0.0619438539724856 }, { - "time" : 0.7171905270255866, - "velocity" : 2.8147182848254104, - "acceleration" : -3.999999999999995, + "time" : 0.7342311236047477, + "velocity" : 2.900097467693069, + "acceleration" : -3.9999999999999956, "pose" : { "translation" : { - "x" : 6.703254526793444, - "y" : -2.7454145583719765 + "x" : 6.678708763946361, + "y" : -2.6849890001767562 }, "rotation" : { - "radians" : 2.7074416804830337 + "radians" : 2.6419030505364995 } }, - "curvature" : 0.10819628898532277 + "curvature" : 0.08239451314240322 }, { - "time" : 0.7513282074668429, - "velocity" : 2.678167563060385, - "acceleration" : -4.0, + "time" : 0.7699686049800885, + "velocity" : 2.757147542191706, + "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.617985867988281, - "y" : -2.706431656110553 + "x" : 6.589762903063175, + "y" : -2.6369537417288313 }, "rotation" : { - "radians" : 2.7184067558532385 + "radians" : 2.651293318832483 } }, - "curvature" : 0.12588795042125855 + "curvature" : 0.1035289020309025 }, { - "time" : 0.7876117423160344, - "velocity" : 2.5330334236636194, + "time" : 0.8079799488552681, + "velocity" : 2.6051021666909873, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.531544461846352, - "y" : -2.6681462824344635 + "x" : 6.499586120247841, + "y" : -2.5894728004932404 }, "rotation" : { - "radians" : 2.7312047381163698 + "radians" : 2.6629818655214734 } }, - "curvature" : 0.14513907009747126 + "curvature" : 0.126150198530942 }, { - "time" : 0.8261977724199053, - "velocity" : 2.3786893032481355, - "acceleration" : -3.999999999999995, + "time" : 0.8484060299195538, + "velocity" : 2.4433978424338445, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.4443808406068825, - "y" : -2.6309666038454367 + "x" : 6.4086871223349675, + "y" : -2.543097554344712 }, "rotation" : { - "radians" : 2.7459513677847163 + "radians" : 2.6771034833727265 } }, - "curvature" : 0.16649109492446312 + "curvature" : 0.1510827102354415 }, { - "time" : 0.867316787787211, - "velocity" : 2.214213241778913, - "acceleration" : -3.999999999999995, + "time" : 0.8914612758849075, + "velocity" : 2.2711768585724297, + "acceleration" : -3.9999999999999956, "pose" : { "translation" : { - "x" : 6.35695118249032, - "y" : -2.595292142974813 + "x" : 6.317580262140382, + "y" : -2.498370737287587 }, "rotation" : { - "radians" : 2.7627816958449616 + "radians" : 2.6938310626262183 } }, - "curvature" : 0.1904846989403303 + "curvature" : 0.1791841330534715 }, { - "time" : 0.911319406879298, - "velocity" : 2.0382027654105648, - "acceleration" : -3.999999999999993, + "time" : 0.9374815876639159, + "velocity" : 2.0870956114563963, + "acceleration" : -4.000000000000007, "pose" : { "translation" : { - "x" : 6.2697066235593155, - "y" : -2.561504257690032 + "x" : 6.226773478818885, + "y" : -2.455813542262746 }, "rotation" : { - "radians" : 2.781844534128218 + "radians" : 2.713371437701187 } }, - "curvature" : 0.21763495429623342 + "curvature" : 0.2113396676958131 }, { - "time" : 0.9587586936046016, - "velocity" : 1.8484456185093505, + "time" : 0.9870098384364133, + "velocity" : 1.8889826083664063, "acceleration" : -4.000000000000003, "pose" : { "translation" : { - "x" : 6.18308256957971, - "y" : -2.529956620201119 + "x" : 6.136756238222006, + "y" : -2.415912723954534 }, "rotation" : { - "radians" : 2.803292700021926 + "radians" : 2.7359565418118486 } }, - "curvature" : 0.24837648355682493 + "curvature" : 0.24842042272290393 }, { - "time" : 1.0105513298264461, - "velocity" : 1.6412750736219726, - "acceleration" : -4.000000000000001, + "time" : 1.0409639720951336, + "velocity" : 1.6731660737315248, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.097488007881523, - "y" : -2.500965696167171 + "x" : 6.047987473255759, + "y" : -2.3791077015976896 }, "rotation" : { - "radians" : 2.8272669567943085 + "radians" : 2.761826815961614 } }, - "curvature" : 0.28296053676338484 + "curvature" : 0.29117840311887416 }, { - "time" : 1.068337863711064, - "velocity" : 1.4101289380835003, - "acceleration" : -4.0, + "time" : 1.1010149453972793, + "velocity" : 1.4329621805229422, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 6.0132948192199365, - "y" : -2.4748012238028423 + "x" : 5.9608835242383975, + "y" : -2.3457776617842683 }, "rotation" : { - "radians" : 2.853870782635059 + "radians" : 2.79120232287241 } }, - "curvature" : 0.3212799426490462 + "curvature" : 0.34003617310478934 }, { - "time" : 1.135467378593098, - "velocity" : 1.1416108785553647, - "acceleration" : -4.000000000000002, + "time" : 1.1706296628919548, + "velocity" : 1.1545033105442406, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 5.930827089636281, - "y" : -2.451676692984833 + "x" : 5.875806079258169, + "y" : -2.3162286612705714 }, "rotation" : { - "radians" : 2.883132411537973 + "radians" : 2.8242353628684933 } }, - "curvature" : 0.36259430688610855 + "curvature" : 0.3947146209017993 }, { - "time" : 1.2208700982319391, + "time" : 1.2592554905280149, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.850350422319025, - "y" : -2.431739824358374 + "x" : 5.79305011453107, + "y" : -2.2906807297840714 }, "rotation" : { - "radians" : 2.9149503561792365 + "radians" : 2.860937134348083 } }, - "curvature" : 0.40513374346503805 + "curvature" : 0.45363861905641456 }, { - "time" : 1.3209271869450325, + "time" : 1.363043401302631, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.772061249464755, - "y" : -2.415063048443713 + "x" : 5.712831834758603, + "y" : -2.269254972830339 }, "rotation" : { - "radians" : 2.949019566392045 + "radians" : 2.901071430255972 } }, - "curvature" : 0.445587529202387 + "curvature" : 0.5130934535793734 }, { - "time" : 1.41738051101318, + "time" : 1.4623685074995216, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.69607614413917, - "y" : -2.4016339847426025 + "x" : 5.63527661348553, + "y" : -2.251960674499969 }, "rotation" : { - "radians" : 2.9847385905123645 + "radians" : 2.944012822610768 } }, - "curvature" : 0.47855128506289096 + "curvature" : 0.5662163704582089 }, { - "time" : 1.5103430795190167, + "time" : 1.5574160412808196, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.622421132138058, - "y" : -2.3913459208447847 + "x" : 5.560406933457628, + "y" : -2.2386824002755077 }, "rotation" : { - "radians" : 3.0211047999576164 + "radians" : 2.988578192465073 } }, - "curvature" : 0.49612340761474616 + "curvature" : 0.6021348789892561 }, { - "time" : 1.600065855933919, + "time" : 1.648541373811172, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.551021003848291, - "y" : -2.3839882915344788 + "x" : 5.488130326979444, + "y" : -2.229167099838378 }, "rotation" : { - "radians" : 3.056615414973954 + "radians" : 3.0328604090655755 } }, - "curvature" : 0.4879890450440943 + "curvature" : 0.6058749987539385 }, { - "time" : 1.6869345775942237, + "time" : 1.7362583003346943, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.481688626108806, - "y" : -2.379237157896867 + "x" : 5.418227316272052, + "y" : -2.223011209875807 }, "rotation" : { - "radians" : 3.089205177495497 + "radians" : 3.074117872291698 } }, - "curvature" : 0.44243031493165497 + "curvature" : 0.5598690362019735 }, { - "time" : 1.7714646340147757, + "time" : 1.8212223392389115, "velocity" : 0.8, - "acceleration" : -0.8733344992185003, + "acceleration" : -0.8705165062981772, "pose" : { "translation" : { - "x" : 5.414114254071592, - "y" : -2.376645686424581 + "x" : 5.350339353830805, + "y" : -2.2196477568877526 }, "rotation" : { - "radians" : 3.1162624888942854 + "radians" : 3.108791896480375 } }, - "curvature" : 0.3486274106696605 + "curvature" : 0.44767291775872703 }, { - "time" : 1.85842631949868, - "velocity" : 0.7240533599567176, - "acceleration" : -3.999999999999999, + "time" : 1.9083467094330548, + "velocity" : 0.7241567976451654, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.347854843062677, - "y" : -2.375634628124189 + "x" : 5.283956762783092, + "y" : -2.218333459993829 }, "rotation" : { - "radians" : 3.1347683732234373 + "radians" : 3.132720840556557 } }, - "curvature" : 0.20027678859820805 + "curvature" : 0.2597205814128655 }, { - "time" : 2.0394396594878597, + "time" : 2.089385908844346, "velocity" : 0.0, - "acceleration" : -3.999999999999999, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.282323360443115, - "y" : -2.3754827976226807 + "x" : 5.218406677246094, + "y" : -2.2181358337402344 }, "rotation" : { "radians" : 3.141592653589793 @@ -686,8 +686,8 @@ "x" : [ 7.626592636108398, -1.5994892120361328, 0.0 ], "y" : [ -3.196845054626465, 0.7553129196166992, 0.0 ] }, { - "x" : [ 5.282323360443115, -2.0928425788879395, 0.0 ], - "y" : [ -2.3754827976226807, 0.0, 0.0 ] + "x" : [ 5.218406677246094, -2.0928425788879395, 0.0 ], + "y" : [ -2.2181358337402344, 0.0, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -695,7 +695,7 @@ "radians" : -3.1179661408942683 } }, { - "time" : 1.9499999564141035, + "time" : 1.9999999552965164, "rotation" : { "radians" : -3.1066860685499065 } @@ -760,11 +760,11 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 3.9999999999999996, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { - "x" : 5.282323360443115, - "y" : -2.3754827976226807 + "x" : 5.218406677246094, + "y" : -2.2181358337402344 }, "rotation" : { "radians" : -3.134549682568558 @@ -772,797 +772,685 @@ }, "curvature" : 0.0 }, { - "time" : 0.2485183417601868, - "velocity" : 0.9940733670407471, + "time" : 0.24844379078421727, + "velocity" : 0.9937751631368689, "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 5.158801735733213, - "y" : -2.376005907798927 + "x" : 5.094960177412837, + "y" : -2.2188617840741998 }, "rotation" : { - "radians" : 3.1403960142550678 + "radians" : -3.1379555673033197 } }, - "curvature" : -0.1272575305163056 + "curvature" : -0.052504130942746294 }, { - "time" : 0.35252878704229224, - "velocity" : 1.410115148169169, - "acceleration" : 4.000000000000002, + "time" : 0.35211767066746935, + "velocity" : 1.4084706826698772, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 5.033779233153609, - "y" : -2.3746042938932987 + "x" : 4.970435117519401, + "y" : -2.2188032852247943 }, "rotation" : { - "radians" : 3.1185310345528885 + "radians" : 3.136267041691601 } }, - "curvature" : -0.2154550840577468 + "curvature" : -0.08861071555917467 }, { - "time" : 0.3949941940566982, - "velocity" : 1.579976776226793, + "time" : 0.43268308957020346, + "velocity" : 1.7307323582808134, "acceleration" : 3.999999999999993, "pose" : { "translation" : { - "x" : 4.970320515414287, - "y" : -2.372685003625565 + "x" : 4.843987783591622, + "y" : -2.2173570738123303 }, "rotation" : { - "radians" : 3.1038755506483433 + "radians" : 3.123617851022157 } }, - "curvature" : -0.24455484294369947 + "curvature" : -0.10892063513271176 }, { - "time" : 0.43379911025043366, - "velocity" : 1.7351964410017344, - "acceleration" : 3.9999999999999933, - "pose" : { - "translation" : { - "x" : 4.906065515041668, - "y" : -2.3697387947678834 - }, - "rotation" : { - "radians" : 3.0874576477610023 - } - }, - "curvature" : -0.2644121359909958 - }, { - "time" : 0.4699166189846961, - "velocity" : 1.879666475938784, + "time" : 0.4682875420771055, + "velocity" : 1.8731501683084213, "acceleration" : 3.9999999999999964, "pose" : { "translation" : { - "x" : 4.840914741610732, - "y" : -2.3656345444326052 + "x" : 4.779845521340216, + "y" : -2.2159758438113766 }, "rotation" : { - "radians" : 3.069776012638989 + "radians" : 3.1164533918962682 } }, - "curvature" : -0.27597551307938123 + "curvature" : -0.11388685999396095 }, { - "time" : 0.5039785450007895, - "velocity" : 2.0159141800031577, + "time" : 0.5017196993257746, + "velocity" : 2.0068787973030977, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.77478698669438, - "y" : -2.3602638760203263 - }, - "rotation" : { - "radians" : 3.051283717312011 - } - }, - "curvature" : -0.28036017762626425 - }, { - "time" : 0.5364200087481645, - "velocity" : 2.1456800349926577, - "acceleration" : 3.999999999999987, - "pose" : { - "translation" : { - "x" : 4.707618566644907, - "y" : -2.3535402315285237 - }, - "rotation" : { - "radians" : 3.0323821587711612 - } - }, - "curvature" : -0.2787476309858388 - }, { - "time" : 0.5675529868653102, - "velocity" : 2.27021194746124, - "acceleration" : 3.999999999999994, - "pose" : { - "translation" : { - "x" : 4.639362565375471, - "y" : -2.345397943860192 - }, - "rotation" : { - "radians" : 3.0134184909093236 - } - }, - "curvature" : -0.2723037406158022 - }, { - "time" : 0.5976074193052028, - "velocity" : 2.3904296772208102, - "acceleration" : 3.9999999999999813, - "pose" : { - "translation" : { - "x" : 4.569988077141569, - "y" : -2.3357913091324796 - }, - "rotation" : { - "radians" : 2.994685983003478 - } - }, - "curvature" : -0.2621176699160821 - }, { - "time" : 0.6267558907189836, - "velocity" : 2.507023562875933, - "acceleration" : 4.000000000000006, - "pose" : { - "translation" : { - "x" : 4.499479449322507, - "y" : -2.324693658985325 + "x" : 4.715013664295839, + "y" : -2.2141041512441006 }, "rotation" : { - "radians" : 2.9764266585803343 + "radians" : 3.108986265289489 } }, - "curvature" : -0.24916158984645412 + "curvature" : -0.11591011471666389 }, { - "time" : 0.6551293302859829, - "velocity" : 2.62051732114393, - "acceleration" : 4.000000000000006, - "pose" : { - "translation" : { - "x" : 4.427835525202872, - "y" : -2.3120964328900926 - }, - "rotation" : { - "radians" : 2.9588355536058346 - } - }, - "curvature" : -0.23426901034532097 - }, { - "time" : 0.6828274897926779, - "velocity" : 2.7313099591707104, + "time" : 0.5334095522803516, + "velocity" : 2.1336382091214054, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.355068886754005, - "y" : -2.29800825045821 - }, - "rotation" : { - "radians" : 2.942065978126006 - } - }, - "curvature" : -0.21812823894940492 - }, { - "time" : 0.7099262281663108, - "velocity" : 2.839704912665242, - "acceleration" : 2.249078927171693, - "pose" : { - "translation" : { - "x" : 4.281205097415471, - "y" : -2.2824539837498032 + "x" : 4.649450957232703, + "y" : -2.2117159957400556 }, "rotation" : { - "radians" : 2.9262352542189807 + "radians" : 3.101385880033052 } }, - "curvature" : -0.20128694960793697 + "curvature" : -0.1153988635183254 }, { - "time" : 0.7366956832801526, - "velocity" : 2.899911530053652, - "acceleration" : 0.6075869985665846, + "time" : 0.563665676688337, + "velocity" : 2.254662706753347, + "acceleration" : 3.99999999999998, "pose" : { "translation" : { - "x" : 4.206281944876537, - "y" : -2.2654738295823336 + "x" : 4.583128758540219, + "y" : -2.2087950520156383 }, "rotation" : { - "radians" : 2.9114305148203647 + "radians" : 3.093802255722613 } }, - "curvature" : -0.18416398315815855 + "curvature" : -0.11275046849580618 }, { - "time" : 0.7635586220886558, - "velocity" : 2.9162331024169883, - "acceleration" : 0.6121916771139427, + "time" : 0.5927164103455597, + "velocity" : 2.3708656413822373, + "acceleration" : 3.9999999999999805, "pose" : { "translation" : { - "x" : 4.130348683857635, - "y" : -2.2471223818392336 + "x" : 4.51603045608554, + "y" : -2.2053342095085027 }, "rotation" : { - "radians" : 2.8977142638407907 + "radians" : 3.086366518935236 } }, - "curvature" : -0.1670650670986652 + "curvature" : -0.10833530395810449 }, { - "time" : 0.790693133931038, - "velocity" : 2.9328446247294444, - "acceleration" : 0.610901889033365, + "time" : 0.6207345486414533, + "velocity" : 2.4829381945658113, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.053465278891842, - "y" : -2.2274677037785438 + "x" : 4.448150883076096, + "y" : -2.201335112011975 }, "rotation" : { - "radians" : 2.885129504199819 + "radians" : 3.0791919886169734 } }, - "curvature" : -0.1501999014049682 + "curvature" : -0.1024865610528908 }, { - "time" : 0.8180687543276729, - "velocity" : 2.949568442943209, - "acceleration" : 0.6061597220097747, + "time" : 0.647853002436567, + "velocity" : 2.591412009746266, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 3.975701647106349, - "y" : -2.206590400341548 + "x" : 4.379495733922141, + "y" : -2.1968076973094695 }, "rotation" : { - "radians" : 2.873704328992082 + "radians" : 3.0723756619209617 } }, - "curvature" : -0.13369882843621037 + "curvature" : -0.09549473651105347 }, { - "time" : 0.8456518976913671, - "velocity" : 2.9662882334567016, - "acceleration" : 0.6000067617510872, + "time" : 0.6741751852926068, + "velocity" : 2.6967007411704254, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 3.8971369010039325, - "y" : -2.1845826904614114 + "x" : 4.310080980099293, + "y" : -2.191769736808901 }, "rotation" : { - "radians" : 2.8634559390901275 + "radians" : 3.065999941440726 } }, - "curvature" : -0.11762797784051136 + "curvature" : -0.08760573546394629 }, { - "time" : 0.8734057728964819, - "velocity" : 2.9829407462445663, - "acceleration" : 0.5941482062082035, + "time" : 0.69978216896706, + "velocity" : 2.7991286758682383, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 3.817858591244428, - "y" : -2.1615474793718144 + "x" : 4.239932286011077, + "y" : -2.186246375177099 }, "rotation" : { - "radians" : 2.854394098775556 + "radians" : 3.060134476441395 } }, - "curvature" : -0.10200230298518619 + "curvature" : -0.07902157653078037 }, { - "time" : 0.9012903734356116, - "velocity" : 2.9995083316357225, - "acceleration" : 0.017530271310819877, + "time" : 0.7247377728559167, + "velocity" : 2.898951091423665, + "acceleration" : 3.9999999999999876, "pose" : { "translation" : { - "x" : 3.7379619494262, - "y" : -2.1375974309155907 + "x" : 4.169084424851462, + "y" : -2.180269669974223 }, "rotation" : { - "radians" : 2.8465240722371243 + "radians" : 3.054838021258 } }, - "curvature" : -0.08679629840563431 + "curvature" : -0.06990281795605731 }, { - "time" : 0.9293371934510891, - "velocity" : 2.9999999999999996, - "acceleration" : 1.0483666419050847E-14, + "time" : 0.7490922849120007, + "velocity" : 2.996369139648001, + "acceleration" : 0.1503076705961156, "pose" : { "translation" : { - "x" : 3.6575491308676185, - "y" : -2.1128540398533633 + "x" : 4.09758069446741, + "y" : -2.173878131288177 }, "rotation" : { - "radians" : 2.8398491015703136 + "radians" : 3.0501602415106173 } }, - "curvature" : -0.07195242715558157 + "curvature" : -0.06037198639248534 }, { - "time" : 0.9575772634227335, + "time" : 0.7732484729335392, "velocity" : 3.0, - "acceleration" : -1.0429522860436319E-14, - "pose" : { - "translation" : { - "x" : 3.5767284573885263, - "y" : -2.08744670417218 - }, - "rotation" : { - "radians" : 2.834372494847544 - } - }, - "curvature" : -0.05738742170266805 - }, { - "time" : 0.985963938172396, - "velocity" : 2.9999999999999996, - "acceleration" : 1.039470354847096E-14, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.4956136600917134, - "y" : -2.0615117973941506 + "x" : 4.025472333221408, + "y" : -2.1671162613690242 }, "rotation" : { - "radians" : 2.8300993940344488 + "radians" : 3.0461434221192074 } }, - "curvature" : -0.0429966817331539 + "curvature" : -0.05051745629810009 }, { - "time" : 1.014445700240245, + "time" : 0.7975813919887572, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.4143231221443884, - "y" : -2.0351917408850824 + "x" : 3.9528179358540214, + "y" : -2.1600340942633993 }, "rotation" : { - "radians" : 2.8270382894922435 + "radians" : 3.0428240499419688 } }, - "curvature" : -0.028657002421013374 + "curvature" : -0.04039737992759309 }, { - "time" : 1.0429688993490678, + "time" : 0.822082461431673, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.332979121559652, - "y" : -2.008634076163117 + "x" : 3.879682869346425, + "y" : -2.152686735448924 }, "rotation" : { - "radians" : 2.8252023423513197 + "radians" : 3.0402342583591766 } }, - "curvature" : -0.014227847229079386 + "curvature" : -0.030043396377312796 }, { - "time" : 1.0714782036513595, + "time" : 0.8467261259226331, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.2517070739779683, - "y" : -1.9819905372073663 + "x" : 3.8061386887829496, + "y" : -2.145133901468623 }, "rotation" : { - "radians" : 2.824610569391105 + "radians" : 3.038403131725035 } }, - "curvature" : 4.486554553824595E-4 + "curvature" : -0.019463950237647885 }, { - "time" : 1.0999170602694552, + "time" : 0.8714847110618453, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -1.19167595131066E-14, "pose" : { "translation" : { - "x" : 3.1706347754486357, - "y" : -1.955416122766549 + "x" : 3.732262553213623, + "y" : -2.1374394595653357 }, "rotation" : { - "radians" : 2.82528893793973 + "radians" : 3.037357874897631 } }, - "curvature" : 0.015548852554256204 + "curvature" : -0.008647127304545333 }, { - "time" : 1.1282281648158434, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 0.896328669094879, + "velocity" : 2.9999999999999996, + "acceleration" : 1.1890816042388423E-14, "pose" : { "translation" : { - "x" : 3.08989164521126, - "y" : -1.9290681686676265 + "x" : 3.6581366415167116, + "y" : -2.129670967316132 }, "rotation" : { - "radians" : 2.827271411029802 + "radians" : 3.037124857643745 } }, - "curvature" : 0.03126974228812179 + "curvature" : 0.002437030086679833 }, { - "time" : 1.1563539399890645, + "time" : 0.9212268318599895, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.009607968477228, - "y" : -1.9031054201244397 + "x" : 3.5838475682612625, + "y" : -2.121899212266726 }, "rotation" : { - "radians" : 2.8306009755902637 + "radians" : 3.0377305461685578 } }, - "curvature" : 0.04783211564223483 + "curvature" : 0.013834727878406873 }, { - "time" : 1.184237023700504, + "time" : 0.9461466703200341, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -1.1886172423716848E-14, "pose" : { "translation" : { - "x" : 2.929914139211178, - "y" : -1.8776871040463448 + "x" : 3.509485799569643, + "y" : -2.114197751565891 }, "rotation" : { - "radians" : 2.8353306784934555 + "radians" : 3.039202334822319 } }, - "curvature" : 0.06548461110054977 + "curvature" : 0.025607120355229326 }, { - "time" : 1.2118207675049235, - "velocity" : 3.0, - "acceleration" : -0.7663356953505052, + "time" : 0.9710545601502908, + "velocity" : 2.9999999999999996, + "acceleration" : 1.1908354632336836E-14, "pose" : { "translation" : { - "x" : 2.8509399029124705, - "y" : -1.8529720013468496 + "x" : 3.435145068980084, + "y" : -2.1066424515998747 }, "rotation" : { - "radians" : 2.8415246861848655 + "radians" : 3.041569290565269 } }, - "curvature" : 0.08450862897758021 + "curvature" : 0.037829464706338994 }, { - "time" : 1.2391451065801053, - "velocity" : 2.9790603836148275, - "acceleration" : -0.900862157373095, + "time" : 0.9959160529739453, + "velocity" : 3.0, + "acceleration" : -1.1947468879569855E-14, "pose" : { "translation" : { - "x" : 2.772813599396663, - "y" : -1.8291175192522502 + "x" : 3.360921793309222, + "y" : -2.0993110276268103 }, "rotation" : { - "radians" : 2.8492593724376376 + "radians" : 3.0448628212988202 } }, - "curvature" : 0.1052240209046203 + "curvature" : 0.05059067862156035 }, { - "time" : 1.2662653693057482, - "velocity" : 2.95462876522728, - "acceleration" : -1.4327348136338478, + "time" : 1.0206961529447127, + "velocity" : 2.9999999999999996, + "acceleration" : 1.2003978639715924E-14, "pose" : { "translation" : { - "x" : 2.695661405576982, - "y" : -1.8062787636102668 + "x" : 3.2869144885146397, + "y" : -2.0922825834111336 }, "rotation" : { - "radians" : 2.8586244241224126 + "radians" : 3.0491172768608856 } }, - "curvature" : 0.12799539517209996 + "curvature" : 0.063993240610446 }, { - "time" : 1.2932068425100405, - "velocity" : 2.916028778636907, - "acceleration" : -3.9999999999999942, + "time" : 1.04535959846717, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 2.6196065782457936, - "y" : -1.7846076111986804 + "x" : 3.213223185557407, + "y" : -2.0856371508579974 }, "rotation" : { - "radians" : 2.8697239348789982 + "radians" : 3.05437048840413 } }, - "curvature" : 0.15323873749770084 + "curvature" : 0.07815336280211393 }, { - "time" : 1.320307309562304, - "velocity" : 2.8076269104278535, - "acceleration" : -4.000000000000006, + "time" : 1.0698711489159878, + "velocity" : 3.0, + "acceleration" : -0.37676977972820513, "pose" : { "translation" : { - "x" : 2.5447686968560754, - "y" : -1.7642517820339685 + "x" : 3.1399488462646232, + "y" : -2.079455229647685 }, "rotation" : { - "radians" : 2.882677428692841 + "radians" : 3.0606642480050854 } }, - "curvature" : 0.18142780690271726 + "curvature" : 0.09320135214507283 }, { - "time" : 1.3478810613702046, - "velocity" : 2.6973319031962504, - "acceleration" : -3.9999999999999942, + "time" : 1.0942331455034373, + "velocity" : 2.9908211359120074, + "acceleration" : -0.657806238307953, "pose" : { "translation" : { - "x" : 2.47126290630289, - "y" : -1.745353911679942 + "x" : 3.06719277919196, + "y" : -2.0738173268700244 }, "rotation" : { - "radians" : 2.8976207163334644 + "radians" : 3.0680447255833965 } }, - "curvature" : 0.21309938210883872 + "curvature" : 0.10928205530053386 }, { - "time" : 1.3759409012078119, - "velocity" : 2.5850925438458217, - "acceleration" : -4.000000000000006, + "time" : 1.1184753330047406, + "velocity" : 2.974874473743419, + "acceleration" : -0.6954631021109505, "pose" : { "translation" : { - "x" : 2.3991991597048568, - "y" : -1.7280506235563808 + "x" : 2.9950560554862022, + "y" : -2.068803496658802 }, "rotation" : { - "radians" : 2.9147064333256036 + "radians" : 3.076562814361 } }, - "curvature" : 0.2488558447958357 + "curvature" : 0.12655525199728102 }, { - "time" : 1.4045086672921978, - "velocity" : 2.470821479508278, - "acceleration" : -4.0, + "time" : 1.142593788850363, + "velocity" : 2.9581009776228964, + "acceleration" : -0.7372656847547625, "pose" : { "translation" : { - "x" : 2.3286814611856244, - "y" : -1.712471601247671 + "x" : 2.923638924747787, + "y" : -2.0644928798261795 }, "rotation" : { - "radians" : 2.934104033894702 + "radians" : 3.0862743889098336 } }, - "curvature" : 0.2893627194755785 + "curvature" : 0.14519581773365695 }, { - "time" : 1.4336183671333464, - "velocity" : 2.354382680143684, - "acceleration" : -4.0, + "time" : 1.166561406144272, + "velocity" : 2.9404304758467625, + "acceleration" : -0.782997881082271, "pose" : { "translation" : { - "x" : 2.259807108655341, - "y" : -1.6987386608114403 + "x" : 2.853040230893349, + "y" : -2.060963243497106 }, "rotation" : { - "radians" : 2.95599891646069 + "radians" : 3.0972404510187994 } }, - "curvature" : 0.3353375731902151 + "curvature" : 0.1653934156780358 }, { - "time" : 1.463320468012547, - "velocity" : 2.2355742766268825, - "acceleration" : -4.0, + "time" : 1.19035256084492, + "velocity" : 2.9218020521276546, + "acceleration" : -3.3968911862397824, "pose" : { "translation" : { - "x" : 2.192665936592128, - "y" : -1.6869648230871945 + "x" : 2.7833568280182597, + "y" : -2.058290520743733 }, "rotation" : { - "radians" : 2.9805902295653475 + "radians" : 3.1095271278230494 } }, - "curvature" : 0.38752507295857674 + "curvature" : 0.1873513955323033 }, { - "time" : 1.4936878988257565, - "velocity" : 2.1141045533740446, - "acceleration" : -3.9999999999999964, + "time" : 1.2141944862495007, + "velocity" : 2.840813625857848, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.127339558823553, - "y" : -1.677253386004954 + "x" : 2.714682996259171, + "y" : -2.0565483502198276 }, "rotation" : { - "radians" : 3.0080867539704594 + "radians" : 3.1232054735668577 } }, - "curvature" : 0.4466510713484467 + "curvature" : 0.21128447059284522 }, { - "time" : 1.524824670531073, - "velocity" : 1.9895574665527784, - "acceleration" : -3.830124802213549, + "time" : 1.2383947736707137, + "velocity" : 2.7440124761729954, + "acceleration" : -3.9999999999999933, "pose" : { "translation" : { - "x" : 2.0639006113080995, - "y" : -1.6696969968938902 + "x" : 2.647109857656554, + "y" : -2.0558076157951897 }, "rotation" : { - "radians" : 3.0387000883556468 + "radians" : 3.1383510108008927 } }, - "curvature" : 0.513346644844465 + "curvature" : 0.23741461164036604 }, { - "time" : 1.5898426254464024, - "velocity" : 1.7405305848423733, - "acceleration" : -2.9205367698139653, + "time" : 1.263030112690642, + "velocity" : 2.645471120093282, + "acceleration" : -4.000000000000007, "pose" : { "translation" : { - "x" : 1.9429261182139115, - "y" : -1.6613611327495477 + "x" : 2.580724792017243, + "y" : -2.056135986190064 }, "rotation" : { - "radians" : 3.110070421404804 + "radians" : -3.1281423784148634 } }, - "curvature" : 0.6707495427301515 + "curvature" : 0.26596443997087926 }, { - "time" : 1.6586294726277089, - "velocity" : 1.539636068369794, - "acceleration" : -2.312476902063167, + "time" : 1.288125803683672, + "velocity" : 2.5450883561211617, + "acceleration" : -3.999999999999993, "pose" : { "translation" : { - "x" : 1.8301152132917196, - "y" : -1.6624501449987292 + "x" : 2.515610852776976, + "y" : -2.057597454609555 }, "rotation" : { - "radians" : -3.0872540859948607 + "radians" : -3.1098224705430115 } }, - "curvature" : 0.857210634989987 + "curvature" : 0.29714723181388075 }, { - "time" : 1.694295911389866, - "velocity" : 1.4571582525534557, - "acceleration" : -1.935712207829296, + "time" : 1.313716123378181, + "velocity" : 2.4427270773431253, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { - "x" : 1.7768357257042826, - "y" : -1.6666209962559293 + "x" : 2.451846182862937, + "y" : -2.0602518783780397 }, "rotation" : { - "radians" : -3.038795429850023 + "radians" : -3.08979235872868 } }, - "curvature" : 0.9569963097495596 + "curvature" : 0.33115248556629073 }, { - "time" : 1.7305902897747223, - "velocity" : 1.3869027812383132, - "acceleration" : -1.5697273044131261, + "time" : 1.3665755526787362, + "velocity" : 2.2312893601409045, + "acceleration" : -3.7762049239471107, "pose" : { "translation" : { - "x" : 1.7256485446325698, - "y" : -1.6732271661251303 + "x" : 2.3286491653547543, + "y" : -2.069355579662364 }, "rotation" : { - "radians" : -2.9868346157105776 + "radians" : -3.044285665635533 } }, - "curvature" : 1.0564079612638406 + "curvature" : 0.40814258856204794 }, { - "time" : 1.76735655145933, - "velocity" : 1.3291897763907865, - "acceleration" : -1.2076785428926229, + "time" : 1.4220168387612644, + "velocity" : 2.021931702646101, + "acceleration" : -3.01178119185259, "pose" : { "translation" : { - "x" : 1.676542258832705, - "y" : -1.6822607723713734 + "x" : 2.211638475515656, + "y" : -2.083825737131292 }, "rotation" : { - "radians" : -2.9317169913107692 + "radians" : -2.9910714005419226 } }, - "curvature" : 1.1501373820361844 + "curvature" : 0.49703931266591117 }, { - "time" : 1.8044095556051742, - "velocity" : 1.284441658334139, - "acceleration" : -0.8415938937413784, + "time" : 1.4800423556994062, + "velocity" : 1.8471715420842816, + "acceleration" : -2.332000316761054, "pose" : { "translation" : { - "x" : 1.6294904214435064, - "y" : -1.6936958606279404 + "x" : 2.101202896434394, + "y" : -2.1039453603839817 }, "rotation" : { - "radians" : -2.8739898269443005 + "radians" : -2.929853651016197 } }, - "curvature" : 1.231671630794503 + "curvature" : 0.5955376808428134 }, { - "time" : 1.8415337315942304, - "velocity" : 1.253198178511569, - "acceleration" : -0.4610275989179017, + "time" : 1.5401431171375435, + "velocity" : 1.7070165473729648, + "acceleration" : -1.6920514012421186, "pose" : { "translation" : { - "x" : 1.5844507927679574, - "y" : -1.7074874767049923 + "x" : 1.9975965658668429, + "y" : -2.1298870898317546 }, "rotation" : { - "radians" : -2.8144162659479433 + "radians" : -2.860828817960625 } }, - "curvature" : 1.2938507850253793 + "curvature" : 0.6973458833437982 }, { - "time" : 1.8784830733784548, - "velocity" : 1.236163512187191, - "acceleration" : -0.05084780300878941, + "time" : 1.601639333661775, + "velocity" : 1.6029617880320504, + "acceleration" : -1.0471995948613984, "pose" : { "translation" : { - "x" : 1.5413645830546798, - "y" : -1.7235707388982036 + "x" : 1.9009202838373298, + "y" : -2.161698464999354 }, "rotation" : { - "radians" : -2.753967325858055 + "radians" : -2.784982844676026 } }, - "curvature" : 1.3297557068700718 + "curvature" : 0.7908194905212341 }, { - "time" : 1.914982265295949, - "velocity" : 1.2343076084665903, - "acceleration" : 0.4127671665855387, + "time" : 1.6636359764771624, + "velocity" : 1.5380389287930096, + "acceleration" : -0.3329668372610245, "pose" : { "translation" : { - "x" : 1.5001556952794068, - "y" : -1.7418599102973964 + "x" : 1.8111028202399666, + "y" : -2.1992871928262048 }, "rotation" : { - "radians" : -2.6937880813077553 + "radians" : -2.7043823448281255 } }, - "curvature" : 1.333757551972331 + "curvature" : 0.8589918589263511 }, { - "time" : 1.9507281593593455, - "velocity" : 1.2490623398762053, - "acceleration" : 0.9702157730169337, + "time" : 1.7249832824094171, + "velocity" : 1.5176123103622623, + "acceleration" : 0.579231551390755, "pose" : { "translation" : { - "x" : 1.4607299679264543, - "y" : -1.7622474710951792 + "x" : 1.7278822224399804, + "y" : -2.2424064159676718 }, "rotation" : { - "radians" : -2.635140362252442 + "radians" : -2.622338159512215 } }, - "curvature" : 1.3024332506879337 + "curvature" : 0.8822710361525333 }, { - "time" : 1.9853901192083565, - "velocity" : 1.2826919200453955, - "acceleration" : 1.6954572937803598, + "time" : 1.7842365157589062, + "velocity" : 1.5519336526402052, + "acceleration" : -1.7698952228100826, "pose" : { "translation" : { - "x" : 1.4229744177701917, - "y" : -1.7846031908955844 + "x" : 1.6507871228750446, + "y" : -2.2906399810963194 }, "rotation" : { - "radians" : -2.5793313706904004 + "radians" : -2.543313789762975 } }, - "curvature" : 1.2350342042717206 + "curvature" : 0.8436793169541906 }, { - "time" : 2.0186069121127237, - "velocity" : 1.3390095738510968, - "acceleration" : -2.34726952828823, + "time" : 1.8435847320921743, + "velocity" : 1.4468935280696547, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 1.3867564826565157, - "y" : -1.808773201022703 + "x" : 1.5791180466566104, + "y" : -2.3433877072031706 }, "rotation" : { - "radians" : -2.5276425781012595 + "radians" : -2.4725497583473715 } }, - "curvature" : 1.1333298805180545 + "curvature" : 0.7339789593751636 }, { - "time" : 2.0874348859976477, - "velocity" : 1.177451768057196, - "acceleration" : -4.000000000000001, + "time" : 1.9104150039170715, + "velocity" : 1.1795724407700658, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 1.3183007709869798, - "y" : -1.861816860005547 + "x" : 1.5119287191712374, + "y" : -2.399850653898966 }, "rotation" : { - "radians" : -2.441312318087089 + "radians" : -2.415547341031068 } }, - "curvature" : 0.8419545117782058 + "curvature" : 0.5530196676431328 }, { - "time" : 2.1735484520729687, - "velocity" : 0.8329975037559129, + "time" : 1.9969535534435008, + "velocity" : 0.8334182426643494, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 1.2538819828114995, - "y" : -1.919639480765781 + "x" : 1.448007373681925, + "y" : -2.4590163897154227 }, "rotation" : { - "radians" : -2.384418923560335 + "radians" : -2.377658907716917 } }, - "curvature" : 0.4592519163026959 + "curvature" : 0.3063025968420864 }, { - "time" : 2.3817978280119467, + "time" : 2.205308114109588, "velocity" : 0.0, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 1.191657543182373, - "y" : -1.9800645112991333 + "x" : 1.3858580589294434, + "y" : -2.519644260406494 }, "rotation" : { "radians" : -2.3639463990252185 @@ -1571,11 +1459,11 @@ "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 5.282323360443115, -3.944063901901245, 0.0 ], - "y" : [ -2.3754827976226807, -0.02777838706970215, 0.0 ] + "x" : [ 5.218406677246094, -3.944063901901245, 0.0 ], + "y" : [ -2.2181358337402344, -0.02777838706970215, 0.0 ] }, { - "x" : [ 1.191657543182373, -1.9787755012512207, 0.0 ], - "y" : [ -1.9800645112991333, -1.9483323097229004, 0.0 ] + "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ], + "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -1583,7 +1471,7 @@ "radians" : -2.3736477827122884 } }, { - "time" : 2.2899999488145113, + "time" : 2.1199999526143074, "rotation" : { "radians" : -2.3824463294692526 } @@ -1639,11 +1527,11 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.0, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 1.191657543182373, - "y" : -1.9800645112991333 + "x" : 1.3858580589294434, + "y" : -2.519644260406494 }, "rotation" : { "radians" : 0.0 @@ -1651,236 +1539,264 @@ }, "curvature" : 0.0 }, { - "time" : 0.24958615457676173, - "velocity" : 0.9983446183070469, - "acceleration" : 3.9999999999999973, + "time" : 0.24972925414190453, + "velocity" : 0.9989170165676183, + "acceleration" : 3.9999999999999996, "pose" : { "translation" : { - "x" : 1.3162440362680172, - "y" : -1.9800961888311122 + "x" : 1.5105871258381285, + "y" : -2.51993284172886 }, "rotation" : { - "radians" : -7.425481719022404E-4 + "radians" : -0.006712112645976874 } }, - "curvature" : -0.011304718810550897 + "curvature" : -0.10052582439878316 }, { - "time" : 0.3514312438034884, - "velocity" : 1.4057249752139533, - "acceleration" : 4.000000000000001, + "time" : 0.3521794641124606, + "velocity" : 1.4087178564498426, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 1.4386652179964585, - "y" : -1.9802937444110285 + "x" : 1.633905754708394, + "y" : -2.521704127531848 }, "rotation" : { - "radians" : -0.0026512076017559455 + "radians" : -0.023287541793374517 } }, - "curvature" : -0.019459649604384354 + "curvature" : -0.16257509326920988 }, { - "time" : 0.42773237755459936, - "velocity" : 1.7109295102183972, + "time" : 0.4296391530021032, + "velocity" : 1.718556612008413, "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 1.557566435449644, - "y" : -1.9807609539604982 + "x" : 1.7549551743861684, + "y" : -2.5258047027587054 }, "rotation" : { - "radians" : -0.005316401490354412 + "radians" : -0.04499886535467417 } }, - "curvature" : -0.024942408322112766 + "curvature" : -0.19036970718931495 }, { - "time" : 0.49025872163920814, - "velocity" : 1.9610348865568323, - "acceleration" : 4.0, + "time" : 0.49386749238771455, + "velocity" : 1.9754699695508586, + "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 1.6723610279150307, - "y" : -1.9815427598077804 + "x" : 1.8733975745271891, + "y" : -2.5324842440895736 }, "rotation" : { - "radians" : -0.008351502650457828 + "radians" : -0.06754653878366729 } }, - "curvature" : -0.027389037359448042 + "curvature" : -0.18401812984610977 }, { - "time" : 0.5438078316620709, - "velocity" : 2.1752313266482837, - "acceleration" : 4.000000000000004, + "time" : 0.5495862121288385, + "velocity" : 2.1983448485153545, + "acceleration" : 3.9999999999999925, "pose" : { "translation" : { - "x" : 1.7831023281846683, - "y" : -1.9826350762866696 + "x" : 1.989327162226573, + "y" : -2.5415022642969234 }, "rotation" : { - "radians" : -0.011352042966441073 + "radians" : -0.08694366158642766 } }, - "curvature" : -0.026118374329521733 + "curvature" : -0.14408315169979258 }, { - "time" : 0.591065084130195, - "velocity" : 2.36426033652078, - "acceleration" : 3.9999999999999956, + "time" : 0.5993533211792574, + "velocity" : 2.3974132847170297, + "acceleration" : 3.999999999999988, "pose" : { "translation" : { - "x" : 1.8903556638542796, - "y" : -1.983994595335389 + "x" : 2.1031812186483876, + "y" : -2.5522348566009896 }, "rotation" : { - "radians" : -0.013901156721571798 + "radians" : -0.09970719732043364 } }, - "curvature" : -0.020693256448048484 + "curvature" : -0.07464450875370457 }, { - "time" : 0.633814678108913, - "velocity" : 2.535258712435652, - "acceleration" : 3.9999999999999916, + "time" : 0.6447906242370012, + "velocity" : 2.5791624969480043, + "acceleration" : 3.999999999999996, "pose" : { "translation" : { - "x" : 1.9950703586223426, - "y" : -1.9855485920954834 + "x" : 2.215651155655223, + "y" : -2.563781439025206 }, "rotation" : { - "radians" : -0.015616242983820706 + "radians" : -0.10320311301994016 } }, - "curvature" : -0.011492597860093103 + "curvature" : 0.015359194352358661 }, { - "time" : 0.6733634723933186, - "velocity" : 2.693453889573274, - "acceleration" : -4.0, + "time" : 0.6870299154317897, + "velocity" : 2.7481196617271584, + "acceleration" : -3.222032479349847, "pose" : { "translation" : { - "x" : 2.0984517335891724, - "y" : -1.9872047305107117 + "x" : 2.327593572437763, + "y" : -2.5750714987516403 }, "rotation" : { - "radians" : -0.016221646042963344 + "radians" : -0.09599875439359641 } }, - "curvature" : 0.0 + "curvature" : 0.11269067821063196 }, { - "time" : 0.7129122666777242, - "velocity" : 2.5352587124356516, - "acceleration" : -3.9999999999999956, + "time" : 0.7291079502753489, + "velocity" : 2.612542866793996, + "acceleration" : -3.999999999999996, "pose" : { "translation" : { - "x" : 2.201833108556002, - "y" : -1.98886086892594 + "x" : 2.439941312144356, + "y" : -2.5849713364764284 }, "rotation" : { - "radians" : -0.015616242983820706 + "radians" : -0.0780986159927982 } }, - "curvature" : 0.011492597860093103 + "curvature" : 0.20204168868166544 }, { - "time" : 0.7556618606564423, - "velocity" : 2.3642603365207795, + "time" : 0.7742726789750473, + "velocity" : 2.4318839519952027, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.306547803324065, - "y" : -1.9904148656860343 + "x" : 2.553614518510585, + "y" : -2.5923908107652096 }, "rotation" : { - "radians" : -0.013901156721571798 + "radians" : -0.05097712062186278 } }, - "curvature" : 0.020693256448048484 + "curvature" : 0.26929228317845294 }, { - "time" : 0.8029191131245663, - "velocity" : 2.1752313266482832, - "acceleration" : -3.999999999999996, + "time" : 0.8239555505599717, + "velocity" : 2.233152465655505, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.4138011389936764, - "y" : -1.9917743847347538 + "x" : 2.6694316924888426, + "y" : -2.5963900824085613 }, "rotation" : { - "radians" : -0.011352042966441073 + "radians" : -0.017372193426654598 } }, - "curvature" : 0.026118374329521733 + "curvature" : 0.3047342069576041 }, { - "time" : 0.8564682231474292, - "velocity" : 1.9610348865568321, - "acceleration" : -4.0, + "time" : 0.8798582869576448, + "velocity" : 2.0095415200648126, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { - "x" : 2.524542439263314, - "y" : -1.992866701213643 + "x" : 2.788020748877898, + "y" : -2.5962863587774336 }, "rotation" : { - "radians" : -0.008351502650457828 + "radians" : 0.019122127471802145 } }, - "curvature" : 0.027389037359448042 + "curvature" : 0.3048495236191097 }, { - "time" : 0.9189945672320379, - "velocity" : 1.7109295102183972, + "time" : 0.9446429920459655, + "velocity" : 1.7504026997115296, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.639337031728701, - "y" : -1.9936485070609251 + "x" : 2.9097300729524704, + "y" : -2.5917606381785845 + }, + "rotation" : { + "radians" : 0.05453524825539118 + } + }, + "curvature" : 0.2715879998238006 + }, { + "time" : 1.0231687323056105, + "velocity" : 1.43629973867295, + "acceleration" : -3.9999999999999947, + "pose" : { + "translation" : { + "x" : 3.0345395770928008, + "y" : -2.582964454210014 }, "rotation" : { - "radians" : -0.005316401490354412 + "radians" : 0.08488478604076904 } }, - "curvature" : 0.024942408322112766 + "curvature" : 0.20927822730800172 }, { - "time" : 0.9952957009831488, - "velocity" : 1.4057249752139536, + "time" : 1.0706615087143077, + "velocity" : 1.2463286330381615, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.0979779765917996, + "y" : -2.577167223823878 + }, + "rotation" : { + "radians" : 0.09693981571821227 + } + }, + "curvature" : 0.16817339591118868 + }, { + "time" : 1.1274499043473336, + "velocity" : 1.0191750505060577, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.758238249181886, - "y" : -1.9941157166103949 + "x" : 3.1619717574142214, + "y" : -2.5706266201164 }, "rotation" : { - "radians" : -0.0026512076017559455 + "radians" : 0.10625319207386785 } }, - "curvature" : 0.019459649604384354 + "curvature" : 0.12027802306933545 }, { - "time" : 1.0971407902098755, - "velocity" : 0.9983446183070466, + "time" : 1.2018998880559773, + "velocity" : 0.7213751156714835, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.8806594309103275, - "y" : -1.9943132721903112 + "x" : 3.22637516801138, + "y" : -2.5635414574337716 }, "rotation" : { - "radians" : -7.425481719022404E-4 + "radians" : 0.11229090993712704 } }, - "curvature" : 0.011304718810550897 + "curvature" : 0.06477002018967379 }, { - "time" : 1.3467269447866372, + "time" : 1.382243666973848, "velocity" : 0.0, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 3.0052459239959717, - "y" : -1.99434494972229 + "x" : 3.2910027503967285, + "y" : -2.5561599731445312 }, "rotation" : { - "radians" : 0.0 + "radians" : 0.1144532134762521 } }, "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 1.191657543182373, 2.0, 0.0 ], - "y" : [ -1.9800645112991333, 0.0, 0.0 ] + "x" : [ 1.3858580589294434, 2.0, 0.0 ], + "y" : [ -2.519644260406494, 0.0, 0.0 ] }, { - "x" : [ 3.0052459239959717, 2.0, 0.0 ], - "y" : [ -1.99434494972229, 0.0, 0.0 ] + "x" : [ 3.2910027503967285, 2.0693557262420654, 0.0 ], + "y" : [ -2.5561599731445312, 0.2378840446472168, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -1888,7 +1804,7 @@ "radians" : -2.670353755551324 } }, { - "time" : 1.2599999718368053, + "time" : 1.289999971166253, "rotation" : { "radians" : -2.684023492023121 } From 877e4bfbc006001ac8c5c024d886be33e38e8d81 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 27 Mar 2022 14:54:10 -0700 Subject: [PATCH 046/108] imcrease shoot and move speed. Use vision during auto, increased allowed vision error from expected --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/subsystem/Drive.java | 12 ++++++------ src/main/java/frc/subsystem/VisionManager.java | 15 +++++++++++---- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1eb09027..2c969a9f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,7 +42,7 @@ public final class Constants { * Relative position of the limelight from the center of the robot. */ public static final Translation2d LIMELIGHT_CENTER_OFFSET = new Translation2d(-0.684, 0); //TODO: CHANGE - public static final double VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED = Math.pow(1.5, 2); //TODO: CHANGE + public static final double VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED = Math.pow(4, 2); //TODO: CHANGE // This is in inches public static final double VISION_DISTANCE_BEFORE_ERROR_TIGHTENING = 200; diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index a5ec87c5..35574eeb 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -56,12 +56,12 @@ public void resetAuto() { = new ProfiledPIDController(8, 0, 0.01, new TrapezoidProfile.Constraints(4, 4)); autoTurnPIDController.enableContinuousInput(-Math.PI, Math.PI); autoTurnPIDController.setTolerance(Math.toRadians(10)); - - swerveAutoController.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune + swerveAutoController = new HolonomicDriveController( new PIDController(3, 0, 0), new PIDController(3, 0, 0), autoTurnPIDController); + swerveAutoController.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune } public enum DriveState { @@ -684,14 +684,14 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, if (useFieldRelative) { swerveDrive(ChassisSpeeds.fromFieldRelativeSpeeds( - controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.3, - controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.3, + controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.45, + controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.45, pidDeltaSpeed, RobotTracker.getInstance().getGyroAngle())); } else { swerveDrive(new ChassisSpeeds( - controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.3, - controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.3, + controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.45, + controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.45, pidDeltaSpeed)); } diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 49fb11d1..cba04b88 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -27,6 +27,7 @@ import java.util.Optional; import java.util.Set; +import static frc.robot.Constants.GOAL_POSITION; import static frc.robot.Constants.MAX_SHOOT_SPEED; import static frc.utility.geometry.GeometryUtils.angleOf; @@ -135,10 +136,16 @@ public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean us public void autoTurnRobotToTarget(ControllerDriveInputs controllerDriveInputs, boolean fieldRelative) { - drive.updateTurn(controllerDriveInputs, getAngleToTarget(), fieldRelative, getAllowedTurnError()); - - updateShooterState(getDistanceToTarget()); - tryToShoot(getRelativeGoalTranslation(), 0, true); + Optional visionTranslation = getVisionTranslation(); + Translation2d relativeGoalPos; + if (visionTranslation.isPresent()) { + relativeGoalPos = visionTranslation.get().minus(GOAL_POSITION); + } else { + relativeGoalPos = getRelativeGoalTranslation(); + } + drive.updateTurn(controllerDriveInputs, angleOf(relativeGoalPos), fieldRelative, getAllowedTurnError()); + updateShooterState(relativeGoalPos.getNorm()); + tryToShoot(relativeGoalPos, 0, true); } private void tryToShoot(Translation2d aimToPosition, double targetAngularSpeed, boolean doSpeedCheck) { From 377e875f1a9bc0b2bca6dc51cf7e98b8026b48f0 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 15:20:37 -0700 Subject: [PATCH 047/108] Fixed Problem with Outtake wheels not running with hopper when overriding outtake to not eject + Removed Outtake Mode that we don't use --- src/main/java/frc/robot/Constants.java | 6 -- src/main/java/frc/subsystem/Hopper.java | 80 +++++++------------------ 2 files changed, 23 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c969a9f..624da237 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -230,12 +230,6 @@ public final class Constants { public static final int OUTTAKE_CURRENT_LIMIT = 20; public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 200; public static final double OUTTAKE_VERTICAL_OFFSET_THRESHOLD = 0; - /** - * Outtake Always intake has outtake always intaking until opposite ball color is found The other option is to have the - * outtake always outtaking until a friendly ball color is found - */ - public static final boolean OUTTAKE_ALWAYS_INTAKE = true; - /** * Outtake's speed when ejecting balls diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 55b7ae61..1f5503dd 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -8,11 +8,11 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.subsystem.Intake.IntakeSolState; -import frc.subsystem.Intake.IntakeState; import frc.utility.Limelight; import frc.utility.OrangeUtility; import frc.utility.Timer; import frc.utility.controllers.LazyCANSparkMax; +import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; import static frc.robot.Constants.HOPPER_CURRENT_LIMIT; @@ -92,23 +92,14 @@ private void updateAllianceColor() { opposingAllianceColor = BallColor.RED; friendlyAllianceColor = BallColor.BLUE; - if (Constants.OUTTAKE_ALWAYS_INTAKE) { - // Will detect opposite color balls and outtake when it sees them + // Will detect opposite color balls and outtake when it sees them intakeLimelight.setPipeline(1); - } else { - // Will detect same colored balls and intake when it sees them - intakeLimelight.setPipeline(0); - } } else { opposingAllianceColor = BallColor.BLUE; friendlyAllianceColor = BallColor.RED; - if (Constants.OUTTAKE_ALWAYS_INTAKE) { - // Will detect same colored balls and intake when it sees them - intakeLimelight.setPipeline(0); - } else { - // Will detect opposite color balls and outtake when it sees them - intakeLimelight.setPipeline(1); - } + + // Will detect same colored balls and intake when it sees them + intakeLimelight.setPipeline(0); } } @@ -132,46 +123,28 @@ private void updateOuttakeState() { return; } - if (Constants.OUTTAKE_ALWAYS_INTAKE) { - // Intake is running and open - if (wantedHopperState == HopperState.ON && intake.getIntakeSolState() == IntakeSolState.OPEN) { - // Ball Color is opposite - if (getBallColor() == opposingAllianceColor) { - lastDetectionTime = Timer.getFPGATimestamp(); - outtakeState = OuttakeState.EJECT; - // Opposite ball color detected within a certain time frame - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { - outtakeState = OuttakeState.EJECT; - } else { - outtakeState = OuttakeState.INTAKE; - } + // Intake is running and open + if (wantedHopperState == HopperState.ON) { + // Ball Color is opposite + if (getBallColor() == opposingAllianceColor && intake.getIntakeSolState() == IntakeSolState.OPEN) { + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.EJECT; + // Opposite ball color detected within a certain time frame + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD && intake.getIntakeSolState() == IntakeSolState.OPEN) { + outtakeState = OuttakeState.EJECT; } else { - outtakeState = OuttakeState.OFF; + outtakeState = OuttakeState.INTAKE; } } else { - // Intake is running and open - if (wantedHopperState == HopperState.ON && intake.getIntakeSolState() == IntakeSolState.OPEN) { - // Ball Color is opposite - if (getBallColor() == friendlyAllianceColor) { - lastDetectionTime = Timer.getFPGATimestamp(); - outtakeState = OuttakeState.INTAKE; - // Opposite ball color detected within a certain time frame - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD) { - outtakeState = OuttakeState.INTAKE; - } else { - outtakeState = OuttakeState.EJECT; - } - } else { - outtakeState = OuttakeState.OFF; - } + outtakeState = OuttakeState.OFF; } } private void updateOuttakeStateOverridden() { Intake intake = Intake.getInstance(); - if (wantedHopperState == HopperState.REVERSE) { + if (wantedHopperState == HopperState.REVERSE && intake.getIntakeSolState() == IntakeSolState.OPEN) { outtakeState = OuttakeState.EJECT; - } else if (intake.wantedIntakeState == IntakeState.INTAKE && intake.getIntakeSolState() == IntakeSolState.OPEN) { + } else if (wantedHopperState == HopperState.ON) { outtakeState = OuttakeState.INTAKE; } else { outtakeState = OuttakeState.OFF; @@ -190,21 +163,14 @@ public OuttakeState getOuttakeState() { return outtakeState; } + @Contract(pure = true) public BallColor getBallColor() { @NotNull BallColor currentBallColor; - if (Constants.OUTTAKE_ALWAYS_INTAKE) { - if (intakeLimelight.getVerticalOffset() < Constants.OUTTAKE_VERTICAL_OFFSET_THRESHOLD) { - currentBallColor = opposingAllianceColor; - } else { - currentBallColor = BallColor.NO_BALL; - } + if (intakeLimelight.getVerticalOffset() < Constants.OUTTAKE_VERTICAL_OFFSET_THRESHOLD) { + currentBallColor = opposingAllianceColor; } else { - if (intakeLimelight.getVerticalOffset() < Constants.OUTTAKE_VERTICAL_OFFSET_THRESHOLD) { - currentBallColor = friendlyAllianceColor; - } else { - currentBallColor = BallColor.NO_BALL; - } + currentBallColor = BallColor.NO_BALL; } return currentBallColor; @@ -213,7 +179,6 @@ public BallColor getBallColor() { @Override public void update() { updateAllianceColor(); - getBallColor(); // Override that makes it so the eject outtake state can never be set if (!disableEject) { @@ -290,6 +255,7 @@ public void logData() { logData("Outtake Current", outtakeWheels.getOutputCurrent()); logData("Current Ball Color", getBallColor()); logData("Eject Disabled", disableEject); + logData("Hopper State", wantedHopperState); } @Override From dba8ee6cd6bae92c70a031e32be7716bb0ea7d7d Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 27 Mar 2022 15:39:09 -0700 Subject: [PATCH 048/108] create an I zone --- src/main/java/frc/subsystem/Drive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 35574eeb..c6de478f 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -669,12 +669,12 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, if (driveState != DriveState.TURN) setDriveState(DriveState.TELEOP); } - if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate) { - turnPID.reset(); - } - turnPID.setSetpoint(goal.position); + + if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate || turnPID.getPositionError() > Math.toRadians(30)) { + turnPID.reset(); + } lastTurnUpdate = Timer.getFPGATimestamp(); double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()); From 72a860b5f7abc0459f93a5b71db7c475ac8c0a23 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 27 Mar 2022 15:39:42 -0700 Subject: [PATCH 049/108] don't require vision to shoot --- src/main/java/frc/subsystem/VisionManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index cba04b88..d506c39a 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -157,7 +157,7 @@ < getAllowedTurnError(aimToPosition.getNorm()) && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck) && Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) { //@formatter:on - shooter.setFiring(limelight.isTargetVisible() || DriverStation.isAutonomous()); + shooter.setFiring(true); if (shooter.isFiring()) { if (!checksPassedLastTime && lastPrintTime + 0.5 < Timer.getFPGATimestamp()) { lastPrintTime = Timer.getFPGATimestamp(); From 5d150d3d0e90f984d3126a067b5bb1d9461ba8c7 Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Sun, 27 Mar 2022 15:58:06 -0700 Subject: [PATCH 050/108] Added Hideballs auto --- .../deploy/autos/{ => blue}/hideballs.json | 0 src/main/deploy/autos/red/hideballs.json | 3165 +++++++++++++++++ src/main/java/frc/auton/HideBallsBlue.java | 12 + src/main/java/frc/auton/HideBallsRed.java | 12 + src/main/java/frc/robot/Robot.java | 22 +- 5 files changed, 3210 insertions(+), 1 deletion(-) rename src/main/deploy/autos/{ => blue}/hideballs.json (100%) create mode 100644 src/main/deploy/autos/red/hideballs.json create mode 100644 src/main/java/frc/auton/HideBallsBlue.java create mode 100644 src/main/java/frc/auton/HideBallsRed.java diff --git a/src/main/deploy/autos/hideballs.json b/src/main/deploy/autos/blue/hideballs.json similarity index 100% rename from src/main/deploy/autos/hideballs.json rename to src/main/deploy/autos/blue/hideballs.json diff --git a/src/main/deploy/autos/red/hideballs.json b/src/main/deploy/autos/red/hideballs.json new file mode 100644 index 00000000..7d69ec29 --- /dev/null +++ b/src/main/deploy/autos/red/hideballs.json @@ -0,0 +1,3165 @@ +{ + "autonomousSteps" : [ { + "type" : "script", + "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 2.351956220256751, + "pose" : { + "translation" : { + "x" : 6.1293768882751465, + "y" : 1.0937262773513794 + }, + "rotation" : { + "radians" : 2.4178655244299407 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.3188834866654645, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.039938468317359, + "y" : 1.173101702633744 + }, + "rotation" : { + "radians" : 2.411607121974174 + } + }, + "curvature" : -0.10063347960741297 + }, { + "time" : 0.47317666597794406, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.954285601692391, + "y" : 1.2509135147447523 + }, + "rotation" : { + "radians" : 2.395009715228919 + } + }, + "curvature" : -0.1855623376134662 + }, { + "time" : 0.6188308342429313, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.87496604004582, + "y" : 1.3260262403808838 + }, + "rotation" : { + "radians" : 2.3703948601089895 + } + }, + "curvature" : -0.26531768438500725 + }, { + "time" : 0.7539530640358325, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.803342186380178, + "y" : 1.3977211365709081 + }, + "rotation" : { + "radians" : 2.339767645326564 + } + }, + "curvature" : -0.3381985496587611 + }, { + "time" : 0.8779901904625292, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.7397725735722815, + "y" : 1.4656408262000014 + }, + "rotation" : { + "radians" : 2.3056558386592805 + } + }, + "curvature" : -0.3907488038191573 + }, { + "time" : 0.9914536504675615, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.683793342890567, + "y" : 1.529733933533862 + }, + "rotation" : { + "radians" : 2.271678768601324 + } + }, + "curvature" : -0.39766870774643054 + }, { + "time" : 1.095639209373063, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.634299722512424, + "y" : 1.5901997197428273 + }, + "rotation" : { + "radians" : 2.242697343821613 + } + }, + "curvature" : -0.32812227024366547 + }, { + "time" : 1.1923614795366175, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.589727506041527, + "y" : 1.6474327184259892 + }, + "rotation" : { + "radians" : 2.224227521884603 + } + }, + "curvature" : -0.16341344710143665 + }, { + "time" : 1.2837283378298283, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.548234531025173, + "y" : 1.7019673711353107 + }, + "rotation" : { + "radians" : 2.2209763317101947 + } + }, + "curvature" : 0.07956164338822755 + }, { + "time" : 1.3719691365115938, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.507882157471613, + "y" : 1.7544226628997421 + }, + "rotation" : { + "radians" : 2.234944043969041 + } + }, + "curvature" : 0.33953937734428047 + }, { + "time" : 1.4592981567041168, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.466816746367385, + "y" : 1.805446757749337 + }, + "rotation" : { + "radians" : 2.264150133653273 + } + }, + "curvature" : 0.5355105351904276 + }, { + "time" : 1.5477626861241707, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.42345113819465, + "y" : 1.8556616342393681 + }, + "rotation" : { + "radians" : 2.30296870864004 + } + }, + "curvature" : 0.6126486386243994 + }, { + "time" : 1.6390285515859833, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.376646131448524, + "y" : 1.9056077209744444 + }, + "rotation" : { + "radians" : 2.343996330815072 + } + }, + "curvature" : 0.568293999561127 + }, { + "time" : 1.7340988034913118, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.3258919611544115, + "y" : 1.9556885321326263 + }, + "rotation" : { + "radians" : 2.3801609988469874 + } + }, + "curvature" : 0.4354667748411578 + }, { + "time" : 1.8330035326706784, + "velocity" : 0.75, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.271489777385341, + "y" : 2.006115302989542 + }, + "rotation" : { + "radians" : 2.405692727133013 + } + }, + "curvature" : 0.24590943352090008 + }, { + "time" : 2.0360118787648114, + "velocity" : 0.0, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 2.4154850346736763 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ], + "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ] + }, { + "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ], + "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.37094272169677 + } + }, { + "time" : 1.939999956637621, + "rotation" : { + "radians" : 2.370942522371697 + } + } ], + "reversed" : false, + "color" : 209.88235, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ { + "type" : "MaxVelocityConstraint", + "maxVelocity" : 0.75 + } ] + }, { + "type" : "script", + "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "10" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Drive.setAutoRotation 40", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.setAutoRotation", + "args" : [ "40" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 0.9530775920796433 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.22164892589636578, + "velocity" : 0.8865957035854634, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.272283410420641, + "y" : 2.136490201934066 + }, + "rotation" : { + "radians" : 0.93154333853559 + } + }, + "curvature" : -0.3354703724748357 + }, { + "time" : 0.32212126730993046, + "velocity" : 1.2884850692397223, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.339010516181588, + "y" : 2.223017249489203 + }, + "rotation" : { + "radians" : 0.8980578131179813 + } + }, + "curvature" : -0.24297628723974596 + }, { + "time" : 0.40798632695507736, + "velocity" : 1.6319453078203097, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.418256742414087, + "y" : 2.320179727881623 + }, + "rotation" : { + "radians" : 0.8787668506583872 + } + }, + "curvature" : -0.07195280186555318 + }, { + "time" : 0.4482103474158235, + "velocity" : 1.7928413896632942, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.462294308261335, + "y" : 2.373142516471262 + }, + "rotation" : { + "radians" : 0.8763509051285007 + } + }, + "curvature" : -0.0010310805010032027 + }, { + "time" : 0.48701783511066765, + "velocity" : 1.9480713404426708, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.508707731962204, + "y" : 2.428952641785145 + }, + "rotation" : { + "radians" : 0.8784798678297706 + } + }, + "curvature" : 0.057187573384384974 + }, { + "time" : 0.5244749433253648, + "velocity" : 2.0978997733014593, + "acceleration" : -3.006834100609329, + "pose" : { + "translation" : { + "x" : 5.5569134053257585, + "y" : 2.4874170951864016 + }, + "rotation" : { + "radians" : 0.8846611837531144 + } + }, + "curvature" : 0.10384088533571874 + }, { + "time" : 0.5628765788352517, + "velocity" : 1.9824324261311614, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 5.606271566124633, + "y" : 2.5482597037116648 + }, + "rotation" : { + "radians" : 0.8942836471940969 + } + }, + "curvature" : 0.13988861038856043 + }, { + "time" : 0.6051667706790917, + "velocity" : 1.813271658755801, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.656145019875112, + "y" : 2.6111436507878807 + }, + "rotation" : { + "radians" : 0.9066044762714179 + } + }, + "curvature" : 0.16515378657020438 + }, { + "time" : 0.6526161179857626, + "velocity" : 1.6234742695291178, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.7059578616172075, + "y" : 2.6756939969491214 + }, + "rotation" : { + "radians" : 0.9206836126942823 + } + }, + "curvature" : 0.17781422207911715 + }, { + "time" : 0.7069026874415512, + "velocity" : 1.4063279917059635, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.75525419769474, + "y" : 2.7415202005533956 + }, + "rotation" : { + "radians" : 0.9352834691742239 + } + }, + "curvature" : 0.1740904256082478 + }, { + "time" : 0.7714879126896856, + "velocity" : 1.1479870907134255, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.803756867535412, + "y" : 2.8082386384994606 + }, + "rotation" : { + "radians" : 0.9487507880978892 + } + }, + "curvature" : 0.14816445891139096 + }, { + "time" : 0.8556329295191124, + "velocity" : 0.8114070233957184, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.851426165430894, + "y" : 2.8754951269436333 + }, + "rotation" : { + "radians" : 0.9589069733276255 + } + }, + "curvature" : 0.09262519823750537 + }, { + "time" : 1.058484685368042, + "velocity" : 0.0, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : 0.9629949901604784 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ], + "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ] + }, { + "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ], + "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.9599310885968813 + } + }, { + "time" : 0.9699999783188105, + "rotation" : { + "radians" : 0.9599310885968813 + } + } ], + "reversed" : false, + "color" : 148.47061, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : -3.1024027203970066 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2301838401073264, + "velocity" : 0.9207353604293056, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.792630460413637, + "y" : 2.9388424054094813 + }, + "rotation" : { + "radians" : -3.1025922709616696 + } + }, + "curvature" : -0.0034322907229005963 + }, { + "time" : 0.3255946694282438, + "velocity" : 1.302378677712975, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.686655575860993, + "y" : 2.9347317908029744 + }, + "rotation" : { + "radians" : -3.1031002310232063 + } + }, + "curvature" : -0.006005316476928024 + }, { + "time" : 0.3988940171844041, + "velocity" : 1.5955760687376164, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.580523715266813, + "y" : 2.930682060104536 + }, + "rotation" : { + "radians" : -3.103837861955969 + } + }, + "curvature" : -0.007749412153373173 + }, { + "time" : 0.4607866040428335, + "velocity" : 1.843146416171334, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 5.4741821068677154, + "y" : 2.926711325853944 + }, + "rotation" : { + "radians" : -3.104720238709075 + } + }, + "curvature" : -0.008706622259397081 + }, { + "time" : 0.5154157029130185, + "velocity" : 2.0616628116520737, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.367594657212031, + "y" : 2.9228297190306436 + }, + "rotation" : { + "radians" : -3.1056669054798047 + } + }, + "curvature" : -0.00892606605401968 + }, { + "time" : 0.5649014668690089, + "velocity" : 2.259605867476035, + "acceleration" : 4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.260741207842759, + "y" : 2.9190397568610926 + }, + "rotation" : { + "radians" : -3.106602230083352 + } + }, + "curvature" : -0.008459920513905771 + }, { + "time" : 0.6104980484072738, + "velocity" : 2.441992193629095, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 5.153616791980521, + "y" : 2.9153367106261046 + }, + "rotation" : { + "radians" : -3.1074555011063474 + } + }, + "curvature" : -0.007360079879286901 + }, { + "time" : 0.6530171873993678, + "velocity" : 2.6120687495974706, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 5.0462308912065055, + "y" : 2.911708973468194 + }, + "rotation" : { + "radians" : -3.108160812688321 + } + }, + "curvature" : -0.005675482209767876 + }, { + "time" : 0.6930174258760853, + "velocity" : 2.7720697035043407, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.93860669214543, + "y" : 2.9081384281989218 + }, + "rotation" : { + "radians" : -3.108656779571012 + } + }, + "curvature" : -0.003450046167250236 + }, { + "time" : 0.7309003592448649, + "velocity" : 2.923601436979459, + "acceleration" : 2.0944239970255154, + "pose" : { + "translation" : { + "x" : 4.830780343148486, + "y" : 2.904600815106239 + }, + "rotation" : { + "radians" : -3.108886120874728 + } + }, + "curvature" : -7.211325069651598E-4 + }, { + "time" : 0.7673774828497517, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.722800210976294, + "y" : 2.9010660997618305 + }, + "rotation" : { + "radians" : -3.10879514573791 + } + }, + "curvature" : 0.0024815692229093363 + }, { + "time" : 0.8034217930772413, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.614726137481853, + "y" : 2.8974988408284617 + }, + "rotation" : { + "radians" : -3.1083331681482385 + } + }, + "curvature" : 0.0061368314589584135 + }, { + "time" : 0.839474699342444, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.506628696293496, + "y" : 2.8938585578673215 + }, + "rotation" : { + "radians" : -3.107451872483656 + } + }, + "curvature" : 0.010232455610621435 + }, { + "time" : 0.8755098996327016, + "velocity" : 3.0, + "acceleration" : -8.22656662136612E-15, + "pose" : { + "translation" : { + "x" : 4.39858844949784, + "y" : 2.890100099145368 + }, + "rotation" : { + "radians" : -3.1061046457957544 + } + }, + "curvature" : 0.01476570595533646 + }, { + "time" : 0.9114981176657808, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.2906952043227395, + "y" : 2.886174009442671 + }, + "rotation" : { + "radians" : -3.1042458879032835 + } + }, + "curvature" : 0.019743691371007215 + }, { + "time" : 0.9474073803784949, + "velocity" : 2.9999999999999996, + "acceleration" : 8.270759764362102E-15, + "pose" : { + "translation" : { + "x" : 4.183047269820236, + "y" : 2.8820268978597596 + }, + "rotation" : { + "radians" : -3.1018303060188313 + } + }, + "curvature" : 0.025183766010214763 + }, { + "time" : 0.9832033025994578, + "velocity" : 3.0, + "acceleration" : -8.305527617360462E-15, + "pose" : { + "translation" : { + "x" : 4.075750713549512, + "y" : 2.8776018056249644 + }, + "rotation" : { + "radians" : -3.0988121969366995 + } + }, + "curvature" : 0.031114011207342505 + }, { + "time" : 1.0188493791490731, + "velocity" : 2.9999999999999996, + "acceleration" : 8.349604098090194E-15, + "pose" : { + "translation" : { + "x" : 3.9689186182598455, + "y" : 2.8728385739017632 + }, + "rotation" : { + "radians" : -3.095144716752075 + } + }, + "curvature" : 0.03757385368866147 + }, { + "time" : 1.0543072846683412, + "velocity" : 3.0, + "acceleration" : -8.403642908942724E-15, + "pose" : { + "translation" : { + "x" : 3.8626703385735555, + "y" : 2.867674211596125 + }, + "rotation" : { + "radians" : -3.0907791356286958 + } + }, + "curvature" : 0.0446148697110529 + }, { + "time" : 1.0895371815644033, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.7571307576689605, + "y" : 2.862043263163855 + }, + "rotation" : { + "radians" : -3.0856640732526293 + } + }, + "curvature" : 0.05230182094645797 + }, { + "time" : 1.124498036572409, + "velocity" : 2.9999999999999996, + "acceleration" : 8.544307135767894E-15, + "pose" : { + "translation" : { + "x" : 3.6524295439633274, + "y" : 2.855878176417939 + }, + "rotation" : { + "radians" : -3.0797447092896437 + } + }, + "curvature" : 0.06071396531738544 + }, { + "time" : 1.159147946568507, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.548700407795824, + "y" : 2.8491096703358885 + }, + "rotation" : { + "radians" : -3.0729619624239923 + } + }, + "curvature" : 0.0699466838117423 + }, { + "time" : 1.1934444744263788, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.4460803581104718, + "y" : 2.841667102867084 + }, + "rotation" : { + "radians" : -3.0652516314712313 + } + }, + "curvature" : 0.08011346138708186 + }, { + "time" : 1.2273449958888158, + "velocity" : 3.0, + "acceleration" : -0.16623210994932952, + "pose" : { + "translation" : { + "x" : 3.3447089591390977, + "y" : 2.8334788387401204 + }, + "rotation" : { + "radians" : -3.0565434927756256 + } + }, + "curvature" : 0.09134825465286359 + }, { + "time" : 1.2608381382277525, + "velocity" : 2.9944323642801653, + "acceleration" : -0.4439832537480419, + "pose" : { + "translation" : { + "x" : 3.2447275870842858, + "y" : 2.8244726172701524 + }, + "rotation" : { + "radians" : -3.046760349874518 + } + }, + "curvature" : 0.10380826846683365 + }, { + "time" : 1.2939625007283606, + "velocity" : 2.9797257020388157, + "acceleration" : -0.504652005340554, + "pose" : { + "translation" : { + "x" : 3.1462786868023294, + "y" : 2.814575920166238 + }, + "rotation" : { + "radians" : -3.0358170346264064 + } + }, + "curvature" : 0.11767714398773378 + }, { + "time" : 1.3267346665866735, + "velocity" : 2.963187162819065, + "acceleration" : -0.5754544680210432, + "pose" : { + "translation" : { + "x" : 3.049505028486184, + "y" : 2.8037163393386826 + }, + "rotation" : { + "radians" : -3.023619364229805 + } + }, + "curvature" : 0.13316852630405346 + }, { + "time" : 1.3591322565139792, + "velocity" : 2.9445438249422833, + "acceleration" : -0.6578485083677343, + "pose" : { + "translation" : { + "x" : 2.954548964348419, + "y" : 2.791821944706385 + }, + "rotation" : { + "radians" : -3.010063066623661 + } + }, + "curvature" : 0.1505299220368132 + }, { + "time" : 1.391136692414039, + "velocity" : 2.923489754524278, + "acceleration" : -0.7533080854904565, + "pose" : { + "translation" : { + "x" : 2.8615516853041685, + "y" : 2.7788216520041806 + }, + "rotation" : { + "radians" : -2.995032698790502 + } + }, + "curvature" : 0.17004666410797128 + }, { + "time" : 1.4227338667361988, + "velocity" : 2.8996873476287437, + "acceleration" : -0.8632025105219969, + "pose" : { + "translation" : { + "x" : 2.7706524776540853, + "y" : 2.764645590590188 + }, + "rotation" : { + "radians" : -2.9784006000086296 + } + }, + "curvature" : 0.19204565508070712 + }, { + "time" : 1.4539148256972156, + "velocity" : 2.8727718655731107, + "acceleration" : -3.0556739243578237, + "pose" : { + "translation" : { + "x" : 2.681987979767292, + "y" : 2.749225471253151 + }, + "rotation" : { + "radians" : -2.9600259471131536 + } + }, + "curvature" : 0.2168983389890578 + }, { + "time" : 1.4850284729965166, + "velocity" : 2.7776987048289703, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.5956914387643337, + "y" : 2.732494954019785 + }, + "rotation" : { + "radians" : -2.9397540138154308 + } + }, + "curvature" : 0.24502202477503635 + }, { + "time" : 1.516611424846333, + "velocity" : 2.6513668974297047, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.5118919672001288, + "y" : 2.7143900159621204 + }, + "rotation" : { + "radians" : -2.917415783997239 + } + }, + "curvature" : 0.2768782177504529 + }, { + "time" : 1.548889355951825, + "velocity" : 2.5222551730077356, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.430713799746922, + "y" : 2.694849319004849 + }, + "rotation" : { + "radians" : -2.8928281326387415 + } + }, + "curvature" : 0.31296597525025704 + }, { + "time" : 1.5819534964682787, + "velocity" : 2.3899986109419213, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.3522755498772367, + "y" : 2.673814577732666 + }, + "rotation" : { + "radians" : -2.8657948699982057 + } + }, + "curvature" : 0.35380747084108527 + }, { + "time" : 1.6159267882434154, + "velocity" : 2.254105443841374, + "acceleration" : -3.8097016292232664, + "pose" : { + "translation" : { + "x" : 2.276689466546827, + "y" : 2.6512309271976164 + }, + "rotation" : { + "radians" : -2.8361090449991986 + } + }, + "curvature" : 0.3999219560210479 + }, { + "time" : 1.6509216012001868, + "velocity" : 2.120785647905598, + "acceleration" : -3.4461347634327937, + "pose" : { + "translation" : { + "x" : 2.204060690877629, + "y" : 2.62704729072644 + }, + "rotation" : { + "radians" : -2.80355701776569 + } + }, + "curvature" : 0.45178327798480244 + }, { + "time" : 1.686971311060505, + "velocity" : 1.9965534895442902, + "acceleration" : -3.10238739377085, + "pose" : { + "translation" : { + "x" : 2.134486512840713, + "y" : 2.6012167477279142 + }, + "rotation" : { + "radians" : -2.7679249248735176 + } + }, + "curvature" : 0.5097553634159661 + }, { + "time" : 1.7240545520371775, + "velocity" : 1.8815069102180944, + "acceleration" : -2.774924611544726, + "pose" : { + "translation" : { + "x" : 2.0680556279392364, + "y" : 2.5736969015002 + }, + "rotation" : { + "radians" : -2.7290082446458435 + } + }, + "curvature" : 0.5740002323865068 + }, { + "time" : 1.7621406045083514, + "velocity" : 1.7758209858592497, + "acceleration" : -2.460804023554924, + "pose" : { + "translation" : { + "x" : 2.0048473938913958, + "y" : 2.544450247038185 + }, + "rotation" : { + "radians" : -2.6866251713458205 + } + }, + "curvature" : 0.6443551970164934 + }, { + "time" : 1.8011869660776936, + "velocity" : 1.6797355422042324, + "acceleration" : -2.1575333813881774, + "pose" : { + "translation" : { + "x" : 1.9449310873133778, + "y" : 2.5134445388408304 + }, + "rotation" : { + "radians" : -2.640634345688177 + } + }, + "curvature" : 0.7201813658904849 + }, { + "time" : 1.841136715487461, + "velocity" : 1.5935426242745667, + "acceleration" : -1.86294906736094, + "pose" : { + "translation" : { + "x" : 1.8883651604023122, + "y" : 2.480653158718514 + }, + "rotation" : { + "radians" : -2.590957058758506 + } + }, + "curvature" : 0.8001959234416516 + }, { + "time" : 1.8819158229901707, + "velocity" : 1.5175732239845823, + "acceleration" : -1.5751064136624524, + "pose" : { + "translation" : { + "x" : 1.8351964976192239, + "y" : 2.4460554836003747 + }, + "rotation" : { + "radians" : -2.537603239437523 + } + }, + "curvature" : 0.8823164840063602 + }, { + "time" : 1.9234306511360026, + "velocity" : 1.452182951909988, + "acceleration" : -1.292173326268649, + "pose" : { + "translation" : { + "x" : 1.785459672371985, + "y" : 2.409637253341657 + }, + "rotation" : { + "radians" : -2.480699299573502 + } + }, + "curvature" : 0.9635650423352979 + }, { + "time" : 1.9655659800403293, + "velocity" : 1.3977368038062605, + "acceleration" : -1.0123180093738429, + "pose" : { + "translation" : { + "x" : 1.7391762036982668, + "y" : 2.3713909385310568 + }, + "rotation" : { + "radians" : -2.420514345602534 + } + }, + "curvature" : 1.0400947450439444 + }, { + "time" : 2.008183951432712, + "velocity" : 1.3545938638427721, + "acceleration" : -0.733579462931417, + "pose" : { + "translation" : { + "x" : 1.696353812948491, + "y" : 2.331316108298065 + }, + "rotation" : { + "radians" : -2.357479728094081 + } + }, + "curvature" : 1.1074024899538168 + }, { + "time" : 2.051124315824919, + "velocity" : 1.3230936943938578, + "acceleration" : -0.4537052298796883, + "pose" : { + "translation" : { + "x" : 1.6569856804687841, + "y" : 2.2894197981203117 + }, + "rotation" : { + "radians" : -2.2921960475647642 + } + }, + "curvature" : 1.1607601882406264 + }, { + "time" : 2.094206248814145, + "velocity" : 1.3035471960833196, + "acceleration" : -0.16993335972179005, + "pose" : { + "translation" : { + "x" : 1.6210497022839263, + "y" : 2.2457168776309118 + }, + "rotation" : { + "radians" : -2.2254223695416244 + } + }, + "curvature" : 1.1958320363685782 + }, { + "time" : 2.137231762276557, + "velocity" : 1.2962357260268969, + "acceleration" : 0.12131769723435752, + "pose" : { + "translation" : { + "x" : 1.5885077467803068, + "y" : 2.200230418425809 + }, + "rotation" : { + "radians" : -2.1580450690428172 + } + }, + "curvature" : 1.2093603605633507 + }, { + "time" : 2.1799904071245306, + "velocity" : 1.3014231063567148, + "acceleration" : 0.42491135071399705, + "pose" : { + "translation" : { + "x" : 1.5593049113888746, + "y" : 2.152992061871121 + }, + "rotation" : { + "radians" : -2.091028180878493 + } + }, + "curvature" : 1.1997387249442901 + }, { + "time" : 2.2222646301244318, + "velocity" : 1.3193859035519875, + "acceleration" : 0.7477114243725372, + "pose" : { + "translation" : { + "x" : 1.5333687792680886, + "y" : 2.1040423869104825 + }, + "rotation" : { + "radians" : -2.0253520831348943 + } + }, + "curvature" : 1.1672933843552342 + }, { + "time" : 2.2638349143884384, + "velocity" : 1.350468480010599, + "acceleration" : 1.099745716491029, + "pose" : { + "translation" : { + "x" : 1.510608675986873, + "y" : 2.0534312778723915 + }, + "rotation" : { + "radians" : -1.9619508454178411 + } + }, + "curvature" : 1.1141785653908227 + }, { + "time" : 2.3044837782843635, + "velocity" : 1.3951718939603694, + "acceleration" : 1.4961728408552466, + "pose" : { + "translation" : { + "x" : 1.4909149262075676, + "y" : 2.0012182922775534 + }, + "rotation" : { + "radians" : -1.9016590628319923 + } + }, + "curvature" : 1.0439225192180022 + }, { + "time" : 2.343997831668839, + "velocity" : 1.4542917474663264, + "acceleration" : 1.9607687708811097, + "pose" : { + "translation" : { + "x" : 1.4741581103688794, + "y" : 1.9474730286462254 + }, + "rotation" : { + "radians" : -1.8451762474768723 + } + }, + "curvature" : 0.9607726336453983 + }, { + "time" : 2.3821673081822796, + "velocity" : 1.5291332650147609, + "acceleration" : 2.532455282243196, + "pose" : { + "translation" : { + "x" : 1.460188321368836, + "y" : 1.8922754943055606 + }, + "rotation" : { + "radians" : -1.7930521282820115 + } + }, + "curvature" : 0.869026522779364 + }, { + "time" : 2.4187826706759017, + "velocity" : 1.6218600331729836, + "acceleration" : 3.8046696696170534, + "pose" : { + "translation" : { + "x" : 1.4488344212477386, + "y" : 1.8357164731969533 + }, + "rotation" : { + "radians" : -1.745691473047995 + } + }, + "curvature" : 0.7724972819405648 + }, { + "time" : 2.486114901209423, + "velocity" : 1.8780369284715344, + "acceleration" : 3.9999999999999947, + "pose" : { + "translation" : { + "x" : 1.4331791216126497, + "y" : 1.7189331963567618 + }, + "rotation" : { + "radians" : -1.6662825767260925 + } + }, + "curvature" : 0.5761233113953619 + }, { + "time" : 2.546692489588632, + "velocity" : 2.1203472819883706, + "acceleration" : 3.999999999999993, + "pose" : { + "translation" : { + "x" : 1.4253702445836325, + "y" : 1.5980789786207197 + }, + "rotation" : { + "radians" : -1.608229977974804 + } + }, + "curvature" : 0.3843159112832851 + }, { + "time" : 2.602172291072171, + "velocity" : 2.3422664879225272, + "acceleration" : 3.9999999999999893, + "pose" : { + "translation" : { + "x" : 1.4231985572659944, + "y" : 1.4743055659818047 + }, + "rotation" : { + "radians" : -1.5723274201937065 + } + }, + "curvature" : 0.19618933375882694 + }, { + "time" : 2.6534386588593746, + "velocity" : 2.54733195907134, + "acceleration" : 3.220008756110653, + "pose" : { + "translation" : { + "x" : 1.4240437746047974, + "y" : 1.3489724397659302 + }, + "rotation" : { + "radians" : -1.5598679357771585 + } + }, + "curvature" : 0.0 + }, { + "time" : 2.7013005707324047, + "velocity" : 2.7014477343866927, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.4258348567705248, + "y" : 1.2233768946074477 + }, + "rotation" : { + "radians" : -1.550010895685447 + } + }, + "curvature" : 0.15261911450126764 + }, { + "time" : 2.749505539118783, + "velocity" : 2.508627860841179, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.4299988476074077, + "y" : 1.09787018620068 + }, + "rotation" : { + "radians" : -1.5225811069171706 + } + }, + "curvature" : 0.28021498367515013 + }, { + "time" : 2.801724619491527, + "velocity" : 2.2997515393502033, + "acceleration" : -3.6618849413513943, + "pose" : { + "translation" : { + "x" : 1.438542295689782, + "y" : 0.972616643926834 + }, + "rotation" : { + "radians" : -1.4806280826538225 + } + }, + "curvature" : 0.3842039868268945 + }, { + "time" : 2.858949646384186, + "velocity" : 2.090200075103547, + "acceleration" : -1.9233425253606922, + "pose" : { + "translation" : { + "x" : 1.45309450732384, + "y" : 0.8478549124047277 + }, + "rotation" : { + "radians" : -1.4270400417472306 + } + }, + "curvature" : 0.465101762848264 + }, { + "time" : 2.920938902993327, + "velocity" : 1.9709735017516907, + "acceleration" : -0.9820932714498863, + "pose" : { + "translation" : { + "x" : 1.4749322121688557, + "y" : 0.7238891070967739 + }, + "rotation" : { + "radians" : -1.3645961892537415 + } + }, + "curvature" : 0.5230727944268867 + }, { + "time" : 2.9861480086005843, + "velocity" : 1.906932077897538, + "acceleration" : -0.5051437890621265, + "pose" : { + "translation" : { + "x" : 1.505004228858411, + "y" : 0.6010799699149629 + }, + "rotation" : { + "radians" : -1.2959588931506363 + } + }, + "curvature" : 0.5587959564566138 + }, { + "time" : 3.019621045984329, + "velocity" : 1.890023380962095, + "acceleration" : -0.2575060162536636, + "pose" : { + "translation" : { + "x" : 1.5233410168520205, + "y" : 0.5402351909635741 + }, + "rotation" : { + "radians" : -1.2601011467472856 + } + }, + "curvature" : 0.5688389814615953 + }, { + "time" : 3.0534660636487803, + "velocity" : 1.8813080852932873, + "acceleration" : -0.046118696833170436, + "pose" : { + "translation" : { + "x" : 1.5439561306216198, + "y" : 0.4798360248268452 + }, + "rotation" : { + "radians" : -1.2236026771430364 + } + }, + "curvature" : 0.5741215645431921 + }, { + "time" : 3.0875714219887946, + "velocity" : 1.8797351906116175, + "acceleration" : 0.13685239477635067, + "pose" : { + "translation" : { + "x" : 1.5668869314298823, + "y" : 0.41993955191350785 + }, + "rotation" : { + "radians" : -1.1867261461606144 + } + }, + "curvature" : 0.575082775007997 + }, { + "time" : 3.121837246466734, + "velocity" : 1.8844245507504096, + "acceleration" : 0.29657244735757804, + "pose" : { + "translation" : { + "x" : 1.5921549109043553, + "y" : 0.3606047334615141 + }, + "rotation" : { + "radians" : -1.1497095266580952 + } + }, + "curvature" : 0.5722241677945603 + }, { + "time" : 3.15617469824573, + "velocity" : 1.8946080928605293, + "acceleration" : 0.43641554195126636, + "pose" : { + "translation" : { + "x" : 1.619766461838124, + "y" : 0.30189213515072333 + }, + "rotation" : { + "radians" : -1.1127638757054572 + } + }, + "curvature" : 0.566089275718927 + }, { + "time" : 3.1905053847904026, + "velocity" : 1.9095905380344815, + "acceleration" : 0.5585224224331813, + "pose" : { + "translation" : { + "x" : 1.649713648990474, + "y" : 0.24386365071558913 + }, + "rotation" : { + "radians" : -1.076071959756705 + } + }, + "curvature" : 0.5572411700339329 + }, { + "time" : 3.224760834869715, + "velocity" : 1.928722974994318, + "acceleration" : 0.6641677388812759, + "pose" : { + "translation" : { + "x" : 1.6819749798875545, + "y" : 0.18658222555784643 + }, + "rotation" : { + "radians" : -1.039787779550021 + } + }, + "curvature" : 0.5462406244811652 + }, { + "time" : 3.258881991221138, + "velocity" : 1.9513851462562573, + "acceleration" : 0.7540110868217726, + "pose" : { + "translation" : { + "x" : 1.7165161756230418, + "y" : 0.13011158035919834 + }, + "rotation" : { + "radians" : -1.0040369547627968 + } + }, + "curvature" : 0.5336268996944579 + }, { + "time" : 3.2928186950481497, + "velocity" : 1.976973797192011, + "acceleration" : 0.8282759740654535, + "pose" : { + "translation" : { + "x" : 1.7532909416588032, + "y" : 0.0745159346940032 + }, + "rotation" : { + "radians" : -0.9689178606738195 + } + }, + "curvature" : 0.5199024657633008 + }, { + "time" : 3.3265291512596264, + "velocity" : 2.004895358146763, + "acceleration" : 0.8868837310332817, + "pose" : { + "translation" : { + "x" : 1.7922417386255596, + "y" : 0.01985973064196145 + }, + "rotation" : { + "radians" : -0.9345033658181425 + } + }, + "curvature" : 0.5055222588408043 + }, { + "time" : 3.3599793731404124, + "velocity" : 2.0345618157322853, + "acceleration" : 0.9295593121848896, + "pose" : { + "translation" : { + "x" : 1.8333005531235487, + "y" : -0.033792643599197314 + }, + "rotation" : { + "radians" : -0.9008430006823431 + } + }, + "curvature" : 0.4908874449734679 + }, { + "time" : 3.3931426109904206, + "velocity" : 2.065389012297963, + "acceleration" : 0.9559197746209286, + "pose" : { + "translation" : { + "x" : 1.8763896685231884, + "y" : -0.08637713010102743 + }, + "rotation" : { + "radians" : -0.8679653892038623 + } + }, + "curvature" : 0.4763432100815362 + }, { + "time" : 3.425998772114722, + "velocity" : 2.0967968664348136, + "acceleration" : 0.96555227037989, + "pose" : { + "translation" : { + "x" : 1.921422435765741, + "y" : -0.1378302775916802 + }, + "rotation" : { + "radians" : -0.8358807916469844 + } + }, + "curvature" : 0.46217982712431704 + }, { + "time" : 3.4585338402322123, + "velocity" : 2.1282111753226207, + "acceleration" : 0.9580857106502751, + "pose" : { + "translation" : { + "x" : 1.9683040441639754, + "y" : -0.18808951784321692 + }, + "rotation" : { + "radians" : -0.8045836333638371 + } + }, + "curvature" : 0.44863614984773975 + }, { + "time" : 3.4907393016272956, + "velocity" : 2.159066767690149, + "acceleration" : 0.9332583224551695, + "pose" : { + "translation" : { + "x" : 2.0169322922028305, + "y" : -0.2370934420589219 + }, + "rotation" : { + "radians" : -0.774054923708963 + } + }, + "curvature" : 0.43590470633971296 + }, { + "time" : 3.5226115837946828, + "velocity" : 2.1888118402785026, + "acceleration" : 0.8909818249589498, + "pose" : { + "translation" : { + "x" : 2.067198358340079, + "y" : -0.28478207726061555 + }, + "rotation" : { + "radians" : -0.744264498898693 + } + }, + "curvature" : 0.42413766870248887 + }, { + "time" : 3.554151510376562, + "velocity" : 2.2169133416234965, + "acceleration" : 0.831401748795369, + "pose" : { + "translation" : { + "x" : 2.11898757180699, + "y" : -0.33109716267596734 + }, + "rotation" : { + "radians" : -0.7151730492255899 + } + }, + "curvature" : 0.41345311620209624 + }, { + "time" : 3.5853637741835547, + "velocity" : 2.2428632723364927, + "acceleration" : 0.7549524443057307, + "pose" : { + "translation" : { + "x" : 2.1721801834089938, + "y" : -0.3759824261258089 + }, + "rotation" : { + "radians" : -0.6867339133510525 + } + }, + "curvature" : 0.40394115698677635 + }, { + "time" : 3.6162564282292005, + "velocity" : 2.266185757019344, + "acceleration" : 0.6624045708844728, + "pose" : { + "translation" : { + "x" : 2.226652136326343, + "y" : -0.41938386041144704 + }, + "rotation" : { + "radians" : -0.6588946399888784 + } + }, + "curvature" : 0.3956696075854153 + }, { + "time" : 3.646840393102844, + "velocity" : 2.286444715147416, + "acceleration" : 0.5549023669290096, + "pose" : { + "translation" : { + "x" : 2.2822758369147778, + "y" : -0.4612499997019768 + }, + "rotation" : { + "radians" : -0.6315983304138486 + } + }, + "curvature" : 0.38868903913170505 + }, { + "time" : 3.677128977700188, + "velocity" : 2.3032519224314116, + "acceleration" : 0.43398782613727804, + "pose" : { + "translation" : { + "x" : 2.3389209255061876, + "y" : -0.5015321959215944 + }, + "rotation" : { + "radians" : -0.6047847845266212 + } + }, + "curvature" : 0.38303708086136296 + }, { + "time" : 3.7071374093201945, + "velocity" : 2.3162752164359675, + "acceleration" : 0.301609110666151, + "pose" : { + "translation" : { + "x" : 2.396455047209276, + "y" : -0.5401848951369104 + }, + "rotation" : { + "radians" : -0.5783914794951374 + } + }, + "curvature" : 0.37874192561228726 + }, { + "time" : 3.736882368388014, + "velocity" : 2.3252465670872136, + "acceleration" : 0.16011115647032365, + "pose" : { + "translation" : { + "x" : 2.4547446227102228, + "y" : -0.5771659139442626 + }, + "rotation" : { + "radians" : -0.5523544141104476 + } + }, + "curvature" : 0.37582501188792183 + }, { + "time" : 3.766381522526342, + "velocity" : 2.3299697107711976, + "acceleration" : 0.012207476034279642, + "pose" : { + "translation" : { + "x" : 2.5136556190733472, + "y" : -0.6124367158570294 + }, + "rotation" : { + "radians" : -0.5266088547091091 + } + }, + "curvature" : 0.37430286645269983 + }, { + "time" : 3.7956530543152014, + "velocity" : 2.330327042293997, + "acceleration" : -0.13906640622183863, + "pose" : { + "translation" : { + "x" : 2.5730543205417726, + "y" : -0.6459626876929423 + }, + "rotation" : { + "radians" : -0.5010900204761772 + } + }, + "curvature" : 0.37418808431592226 + }, { + "time" : 3.8247151767948284, + "velocity" : 2.326285477363576, + "acceleration" : -0.29041573965154777, + "pose" : { + "translation" : { + "x" : 2.6328080993380887, + "y" : -0.6777134159613993 + }, + "rotation" : { + "radians" : -0.47573374766730814 + } + }, + "curvature" : 0.37548940279405946 + }, { + "time" : 3.8535856305237206, + "velocity" : 2.317901043189824, + "acceleration" : -0.4383615615686928, + "pose" : { + "translation" : { + "x" : 2.6927861864650144, + "y" : -0.7076629632507778 + }, + "rotation" : { + "radians" : -0.45047717414862004 + } + }, + "curvature" : 0.37821079612304137 + }, { + "time" : 3.882281155755939, + "velocity" : 2.305322027938995, + "acceleration" : -0.5793221339027272, + "pose" : { + "translation" : { + "x" : 2.752860442506062, + "y" : -0.7357901446157484 + }, + "rotation" : { + "radians" : -0.42525948787674483 + } + }, + "curvature" : 0.3823494796069915 + }, { + "time" : 3.91081693300839, + "velocity" : 2.288790620568532, + "acceleration" : -0.7096933712661672, + "pose" : { + "translation" : { + "x" : 2.8129061284262007, + "y" : -0.7620788039645863 + }, + "rotation" : { + "radians" : -0.40002278560350163 + } + }, + "curvature" : 0.38789267058227345 + }, { + "time" : 3.9392059849192336, + "velocity" : 2.2686430986108754, + "acceleration" : -0.8749184265202878, + "pose" : { + "translation" : { + "x" : 2.872802676372519, + "y" : -0.786518090446485 + }, + "rotation" : { + "radians" : -0.37471309109137346 + } + }, + "curvature" : 0.394812911527834 + }, { + "time" : 3.9955941671938477, + "velocity" : 2.219308038900831, + "acceleration" : -1.0288336535097118, + "pose" : { + "translation" : { + "x" : 2.991691567646626, + "y" : -0.8298333259347146 + }, + "rotation" : { + "radians" : -0.3236861024053773 + } + }, + "curvature" : 0.4125613419519297 + }, { + "time" : 4.051459900853216, + "velocity" : 2.161831492034062, + "acceleration" : -1.0796480258443715, + "pose" : { + "translation" : { + "x" : 3.108675287572691, + "y" : -0.865765651810257 + }, + "rotation" : { + "radians" : -0.2718791063461188 + } + }, + "curvature" : 0.43479047913732005 + }, { + "time" : 4.10675359376877, + "velocity" : 2.10213376563614, + "acceleration" : -0.9994511460458044, + "pose" : { + "translation" : { + "x" : 3.2230180775513873, + "y" : -0.8944474414456636 + }, + "rotation" : { + "radians" : -0.21916678625150354 + } + }, + "curvature" : 0.4598360409407436 + }, { + "time" : 4.161311871060626, + "velocity" : 2.0476054328705096, + "acceleration" : -0.749178067288782, + "pose" : { + "translation" : { + "x" : 3.334124914760981, + "y" : -0.9161224852502343 + }, + "rotation" : { + "radians" : -0.16568957342253765 + } + }, + "curvature" : 0.48465328108684036 + }, { + "time" : 4.214818370708727, + "velocity" : 2.007519536876757, + "acceleration" : -1.3699445774534182, + "pose" : { + "translation" : { + "x" : 3.4415661777785544, + "y" : -0.9311548350640351 + }, + "rotation" : { + "radians" : -0.11197409892743718 + } + }, + "curvature" : 0.5042015108247184 + }, { + "time" : 4.2675300331068176, + "velocity" : 1.9353074808059383, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.5451023122012337, + "y" : -0.9400376485519146 + }, + "rotation" : { + "radians" : -0.059075560161144215 + } + }, + "curvature" : 0.5109123503903398 + }, { + "time" : 4.322105290852558, + "velocity" : 1.7170064498229765, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.6447084962674126, + "y" : -0.943402033597522 + }, + "rotation" : { + "radians" : -0.008719023872416791 + } + }, + "curvature" : 0.4946090622112777 + }, { + "time" : 4.382159659747878, + "velocity" : 1.4767889742416929, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 3.7405993064779786, + "y" : -0.9420258926973233 + }, + "rotation" : { + "radians" : 0.03660524430653648 + } + }, + "curvature" : 0.4434197350542565 + }, { + "time" : 4.451511716406289, + "velocity" : 1.1993807476080485, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 3.8332533832175386, + "y" : -0.9368427673546194 + }, + "rotation" : { + "radians" : 0.07365896092598179 + } + }, + "curvature" : 0.3462339716324929 + }, { + "time" : 4.540069304785332, + "velocity" : 0.8451503940918763, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 3.9234380963756443, + "y" : -0.9289506824735625 + }, + "rotation" : { + "radians" : 0.09866354717797672 + } + }, + "curvature" : 0.19694019866370868 + }, { + "time" : 4.751356903308301, + "velocity" : 0.0, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 4.012234210968018, + "y" : -0.9196209907531738 + }, + "rotation" : { + "radians" : 0.10776957043787032 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ], + "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ] + }, { + "x" : [ 1.4240437746047974, 0.04392707347869873, 0.0 ], + "y" : [ 1.3489724397659302, -4.019377112388611, 0.0 ] + }, { + "x" : [ 4.012234210968018, 2.833451271057129, 0.0 ], + "y" : [ -0.9196209907531738, 0.30654752254486084, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.2617993877991494 + } + }, { + "time" : 2.6499999407678843, + "rotation" : { + "radians" : 0.2617993877991494 + } + }, { + "time" : 4.659999895840883, + "rotation" : { + "radians" : 0.2617993877991494 + } + } ], + "reversed" : false, + "color" : 52.47058, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 4.012234210968018, + "y" : -0.9196209907531738 + }, + "rotation" : { + "radians" : 0.030602586025410137 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.1839004373900963, + "velocity" : 0.7356017495603853, + "acceleration" : 3.999999999999999, + "pose" : { + "translation" : { + "x" : 4.079839881543424, + "y" : -0.9175061170373492 + }, + "rotation" : { + "radians" : 0.032597811527258144 + } + }, + "curvature" : 0.05820539136138801 + }, { + "time" : 0.2598794478276359, + "velocity" : 1.0395177913105436, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 4.147234073981139, + "y" : -0.9151343674395811 + }, + "rotation" : { + "radians" : 0.03836566081266323 + } + }, + "curvature" : 0.11233457015103718 + }, { + "time" : 0.31790134200340625, + "velocity" : 1.2716053680136252, + "acceleration" : 3.9999999999999982, + "pose" : { + "translation" : { + "x" : 4.2142208005905895, + "y" : -0.912271579263629 + }, + "rotation" : { + "radians" : 0.047629178342962146 + } + }, + "curvature" : 0.1636754126870864 + }, { + "time" : 0.3664818052070989, + "velocity" : 1.4659272208283958, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 4.280620524327787, + "y" : -0.9087076362808375 + }, + "rotation" : { + "radians" : 0.060164770303775844 + } + }, + "curvature" : 0.2132011191407471 + }, { + "time" : 0.40891181812753247, + "velocity" : 1.6356472725101303, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.346269665970026, + "y" : -0.9042556640216365 + }, + "rotation" : { + "radians" : 0.07578809002213045 + } + }, + "curvature" : 0.261623846845801 + }, { + "time" : 0.44687903311679705, + "velocity" : 1.7875161324671887, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.411020111290583, + "y" : -0.8987512250670413 + }, + "rotation" : { + "radians" : 0.09434193350703354 + } + }, + "curvature" : 0.30942902122153493 + }, { + "time" : 0.4813894477163118, + "velocity" : 1.9255577908652477, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 4.474738718233415, + "y" : -0.892051514340153 + }, + "rotation" : { + "radians" : 0.11568561342262434 + } + }, + "curvature" : 0.3568953774098475 + }, { + "time" : 0.5131039023083906, + "velocity" : 2.052415609233563, + "acceleration" : -0.5080487315100024, + "pose" : { + "translation" : { + "x" : 4.537306824087864, + "y" : -0.8840345543976582 + }, + "rotation" : { + "radians" : 0.13968544333222133 + } + }, + "curvature" : 0.404105267004568 + }, { + "time" : 0.5734741207486527, + "velocity" : 2.021744596334006, + "acceleration" : -2.5986497425654487, + "pose" : { + "translation" : { + "x" : 4.658586321464071, + "y" : -0.8636602870095267 + }, + "rotation" : { + "radians" : 0.19510294768348427 + } + }, + "curvature" : 0.4971313158757367 + }, { + "time" : 0.6345428990288139, + "velocity" : 1.8630482313774788, + "acceleration" : -1.782825751736676, + "pose" : { + "translation" : { + "x" : 4.774180161280128, + "y" : -0.8370385771048632 + }, + "rotation" : { + "radians" : 0.2593591749925202 + } + }, + "curvature" : 0.5854307189034433 + }, { + "time" : 0.6978369262592252, + "velocity" : 1.750206009699979, + "acceleration" : -1.1573812389052407, + "pose" : { + "translation" : { + "x" : 4.883610030103824, + "y" : -0.8038613196792674 + }, + "rotation" : { + "radians" : 0.33087764946357995 + } + }, + "curvature" : 0.6633540150470494 + }, { + "time" : 0.7622830361592594, + "velocity" : 1.6756172911812541, + "acceleration" : -0.6206912248410478, + "pose" : { + "translation" : { + "x" : 4.986581972800195, + "y" : -0.7640763998497277 + }, + "rotation" : { + "radians" : 0.40763331940017733 + } + }, + "curvature" : 0.7237257699378088 + }, { + "time" : 0.8268495770863082, + "velocity" : 1.6355414058094946, + "acceleration" : -0.1141447568431195, + "pose" : { + "translation" : { + "x" : 5.0829706221219055, + "y" : -0.7178619421826262 + }, + "rotation" : { + "radians" : 0.48716997806870804 + } + }, + "curvature" : 0.7596273921140029 + }, { + "time" : 0.890535049243441, + "velocity" : 1.6282720430756794, + "acceleration" : 0.4095450005884974, + "pose" : { + "translation" : { + "x" : 5.1728034282996305, + "y" : -0.6656005600217441 + }, + "rotation" : { + "radians" : 0.5667524438791094 + } + }, + "curvature" : 0.7664251916360058 + }, { + "time" : 0.9523748257700176, + "velocity" : 1.6535982143896488, + "acceleration" : 1.0079964321999564, + "pose" : { + "translation" : { + "x" : 5.2562448886324376, + "y" : -0.6078536048162668 + }, + "rotation" : { + "radians" : 0.6436149831528346 + } + }, + "curvature" : 0.7431281524527987 + }, { + "time" : 1.011449874872387, + "velocity" : 1.7131456531168745, + "acceleration" : 1.7790042074143708, + "pose" : { + "translation" : { + "x" : 5.3335807770781685, + "y" : -0.5453354154487897 + }, + "rotation" : { + "radians" : 0.7152240168950873 + } + }, + "curvature" : 0.6923650331342213 + }, { + "time" : 1.066883029265564, + "velocity" : 1.8117614680125869, + "acceleration" : 2.9290544621119077, + "pose" : { + "translation" : { + "x" : 5.405202373843821, + "y" : -0.47888756756332285 + }, + "rotation" : { + "radians" : 0.7794678142970032 + } + }, + "curvature" : 0.6190442095943712 + }, { + "time" : 1.1178097408882126, + "velocity" : 1.960928579931592, + "acceleration" : 4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.47159069497593, + "y" : -0.4094531228932965 + }, + "rotation" : { + "radians" : 0.8347237020762335 + } + }, + "curvature" : 0.5284454355848308 + }, { + "time" : 1.163781325410918, + "velocity" : 2.144814918022414, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.533300721950951, + "y" : -0.3380508785895664 + }, + "rotation" : { + "radians" : 0.879806448814223 + } + }, + "curvature" : 0.4245631719643485 + }, { + "time" : 1.2052874260275384, + "velocity" : 2.3108393204888955, + "acceleration" : 0.9499793055242893, + "pose" : { + "translation" : { + "x" : 5.59094563126564, + "y" : -0.265749616548419 + }, + "rotation" : { + "radians" : 0.9138373619233752 + } + }, + "curvature" : 0.3090988446696365 + }, { + "time" : 1.2440241708410498, + "velocity" : 2.3476384264251067, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 5.645181024027437, + "y" : -0.19364235273957675 + }, + "rotation" : { + "radians" : 0.93608126262559 + } + }, + "curvature" : 0.18105135742527462 + }, { + "time" : 1.2825935410962108, + "velocity" : 2.1933609454044625, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 5.696689155544846, + "y" : -0.12282058653420336 + }, + "rotation" : { + "radians" : 0.9457881291979806 + } + }, + "curvature" : 0.036672195665488225 + }, { + "time" : 1.3225645194975204, + "velocity" : 2.033477031799224, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.746163164917817, + "y" : -0.05434855003290906 + }, + "rotation" : { + "radians" : 0.9420639655569212 + } + }, + "curvature" : -0.13035733884148892 + }, { + "time" : 1.3640767963731968, + "velocity" : 1.8674279242965188, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.794291304628132, + "y" : 0.010762542606244097 + }, + "rotation" : { + "radians" : 0.9237929192972799 + } + }, + "curvature" : -0.3283818138757726 + }, { + "time" : 1.4073934572912177, + "velocity" : 1.6941612806244348, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.8417411701297794, + "y" : 0.07158024583973699 + }, + "rotation" : { + "radians" : 0.8896456491310966 + } + }, + "curvature" : -0.5662511162346159 + }, { + "time" : 1.4530216175559991, + "velocity" : 1.5116486395653088, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.889143929439342, + "y" : 0.12727663341058815 + }, + "rotation" : { + "radians" : 0.8382382104223547 + } + }, + "curvature" : -0.8496047780846073 + }, { + "time" : 1.5019520124723162, + "velocity" : 1.31592705990004, + "acceleration" : -3.2992132129798613, + "pose" : { + "translation" : { + "x" : 5.937078552726376, + "y" : 0.17715404902034848 + }, + "rotation" : { + "radians" : 0.7685396527813694 + } + }, + "curvature" : -1.173437784895902 + }, { + "time" : 1.5280288447019, + "velocity" : 1.2298940304555388, + "acceleration" : -2.595879024435883, + "pose" : { + "translation" : { + "x" : 5.961408087691393, + "y" : 0.19973458251777654 + }, + "rotation" : { + "radians" : 0.7267822918880255 + } + }, + "curvature" : -1.343347329975652 + }, { + "time" : 1.5550966642268162, + "velocity" : 1.1596292455135926, + "acceleration" : -2.0093413111156626, + "pose" : { + "translation" : { + "x" : 5.986056041903794, + "y" : 0.220670857001096 + }, + "rotation" : { + "radians" : 0.6806182852772713 + } + }, + "curvature" : -1.5110727869831724 + }, { + "time" : 1.582993226711798, + "velocity" : 1.103575530074399, + "acceleration" : -1.499480490388413, + "pose" : { + "translation" : { + "x" : 6.011073083229763, + "y" : 0.23992377095496042 + }, + "rotation" : { + "radians" : 0.6303974993218476 + } + }, + "curvature" : -1.6684745220647488 + }, { + "time" : 1.6115420851073832, + "velocity" : 1.0607670738873578, + "acceleration" : -1.0383464958403599, + "pose" : { + "translation" : { + "x" : 6.036503660218244, + "y" : 0.2574671929874306 + }, + "rotation" : { + "radians" : 0.576664005958531 + } + }, + "curvature" : -1.8058581852202884 + }, { + "time" : 1.6405508670810625, + "velocity" : 1.030645906776391, + "acceleration" : -0.6046251421798862, + "pose" : { + "translation" : { + "x" : 6.062385509275637, + "y" : 0.27328876653847445 + }, + "rotation" : { + "radians" : 0.5201672456863837 + } + }, + "curvature" : -1.9129549301403845 + }, { + "time" : 1.6698112663305722, + "velocity" : 1.0129543337199158, + "acceleration" : -0.17989538152775067, + "pose" : { + "translation" : { + "x" : 6.0887491618404965, + "y" : 0.28739071458846865 + }, + "rotation" : { + "radians" : 0.46185036100408955 + } + }, + "curvature" : -1.9803591986041147 + }, { + "time" : 1.6991006131857587, + "velocity" : 1.0076853154927035, + "acceleration" : 0.25454795207597797, + "pose" : { + "translation" : { + "x" : 6.1156174515582356, + "y" : 0.2997906443666931 + }, + "rotation" : { + "radians" : 0.40281210939179324 + } + }, + "curvature" : -2.00112327804307 + }, { + "time" : 1.728184543388813, + "velocity" : 1.0150885703642116, + "acceleration" : 0.7213256391379216, + "pose" : { + "translation" : { + "x" : 6.143005021455821, + "y" : 0.3105223520598379 + }, + "rotation" : { + "radians" : 0.3442451501516796 + } + }, + "curvature" : -1.9720404920384738 + }, { + "time" : 1.756819895841348, + "velocity" : 1.035743984273976, + "acceleration" : 1.2524391885977852, + "pose" : { + "translation" : { + "x" : 6.170917831116474, + "y" : 0.31963662752049693 + }, + "rotation" : { + "radians" : 0.28736056159514967 + } + }, + "curvature" : -1.8941696100081031 + }, { + "time" : 1.784756647104708, + "velocity" : 1.0707330663583168, + "acceleration" : 1.8992599815033173, + "pose" : { + "translation" : { + "x" : 6.19935266385437, + "y" : 0.3272020589756721 + }, + "rotation" : { + "radians" : 0.23331316860807427 + } + }, + "curvature" : -1.7723980930633711 + }, { + "time" : 1.8117374729698128, + "velocity" : 1.1219766691918198, + "acceleration" : 2.7534793106504156, + "pose" : { + "translation" : { + "x" : 6.228296633889339, + "y" : 0.33330583773527067 + }, + "rotation" : { + "radians" : 0.183142361520775 + } + }, + "curvature" : -1.614195199267953 + }, { + "time" : 1.8374933012978958, + "velocity" : 1.1928948096218601, + "acceleration" : -3.5661641751301776, + "pose" : { + "translation" : { + "x" : 6.257726693521562, + "y" : 0.33805456290060576 + }, + "rotation" : { + "radians" : 0.1377385534824891 + } + }, + "curvature" : -1.4279710594799537 + }, { + "time" : 1.8927457727102306, + "velocity" : 0.9958554254837872, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 6.317899124228461, + "y" : 0.3440151160617688 + }, + "rotation" : { + "radians" : 0.06404669180836077 + } + }, + "curvature" : -1.0007588885962067 + }, { + "time" : 1.9651351974193783, + "velocity" : 0.7062977266471963, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.37946360862216, + "y" : 0.3463552460207815 + }, + "rotation" : { + "radians" : 0.01679972270125931 + } + }, + "curvature" : -0.5272785953582695 + }, { + "time" : 2.1417096290811775, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.441819667816162, + "y" : 0.3467085063457489 + }, + "rotation" : { + "radians" : 0.0 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], + "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + }, { + "x" : [ 6.441819667816162, 2.0, 0.0 ], + "y" : [ 0.3467085063457489, 0.0, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 2.0499999541789293, + "rotation" : { + "radians" : 0.0 + } + } ], + "reversed" : false, + "color" : 286.3529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "sleep", + "args" : [ "1000" ], + "argTypes" : [ "long" ], + "reflection" : false + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Intake.setIntakeSolState CLOSE", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.441819667816162, + "y" : 0.3467085063457489 + }, + "rotation" : { + "radians" : 1.6078163234494292 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.1953077076111654, + "velocity" : 0.7812308304446616, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 6.4389816843095105, + "y" : 0.42294590310561375 + }, + "rotation" : { + "radians" : 1.6083198170208068 + } + }, + "curvature" : 0.010007683937884466 + }, { + "time" : 0.28418183429804866, + "velocity" : 1.1367273371921942, + "acceleration" : 3.9999999999999987, + "pose" : { + "translation" : { + "x" : 6.435750551492674, + "y" : 0.5081130612115885 + }, + "rotation" : { + "radians" : 1.609052997763179 + } + }, + "curvature" : 0.006001548474080814 + }, { + "time" : 0.3615636139485796, + "velocity" : 1.446254455794318, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.431910399555818, + "y" : 0.6079771182521938 + }, + "rotation" : { + "radians" : 1.6092943557828885 + } + }, + "curvature" : -8.5488367035683E-4 + }, { + "time" : 0.43523665263811134, + "velocity" : 1.740946610552445, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.427411179058254, + "y" : 0.72529627053882 + }, + "rotation" : { + "radians" : 1.6088589590984765 + } + }, + "curvature" : -0.006111352293260847 + }, { + "time" : 0.47131335599478547, + "velocity" : 1.8852534239791414, + "acceleration" : 3.999999999999987, + "pose" : { + "translation" : { + "x" : 6.42493665439585, + "y" : 0.7906601188879874 + }, + "rotation" : { + "radians" : 1.6083911287059784 + } + }, + "curvature" : -0.008116127399784383 + }, { + "time" : 0.5069348966838761, + "velocity" : 2.0277395867355037, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.422338327934085, + "y" : 0.8603050862099906 + }, + "rotation" : { + "radians" : 1.607763133691658 + } + }, + "curvature" : -0.009853447575726546 + }, { + "time" : 0.5420590363593313, + "velocity" : 2.1682361454373242, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.419643072810146, + "y" : 0.9339457983013029 + }, + "rotation" : { + "radians" : 1.6069775892569418 + } + }, + "curvature" : -0.011436025231544151 + }, { + "time" : 0.5766104955030272, + "velocity" : 2.306441982012108, + "acceleration" : 3.9999999999999947, + "pose" : { + "translation" : { + "x" : 6.416882438497851, + "y" : 1.0111998183356263 + }, + "rotation" : { + "radians" : 1.6060340313214787 + } + }, + "curvature" : -0.012962877357532548 + }, { + "time" : 0.6104960029929839, + "velocity" : 2.4419840119719347, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.4140917029015725, + "y" : 1.0916028128984 + }, + "rotation" : { + "radians" : 1.604928600003804 + } + }, + "curvature" : -0.014520270296573292 + }, { + "time" : 0.6436141193910724, + "velocity" : 2.5744564775642886, + "acceleration" : 3.9999999999999947, + "pose" : { + "translation" : { + "x" : 6.411308924450168, + "y" : 1.1746237180213086 + }, + "rotation" : { + "radians" : 1.603653938746236 + } + }, + "curvature" : -0.01618596901731972 + }, { + "time" : 0.6758618258229525, + "velocity" : 2.703447303291809, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 6.408573994190903, + "y" : 1.2596799052167897 + }, + "rotation" : { + "radians" : 1.6021991606605406 + } + }, + "curvature" : -0.01803416029898833 + }, { + "time" : 0.7071390741340647, + "velocity" : 2.828556296536258, + "acceleration" : 3.5711423686938364, + "pose" : { + "translation" : { + "x" : 6.405927687883377, + "y" : 1.3461523475125432 + }, + "rotation" : { + "radians" : 1.6005498243279082 + } + }, + "curvature" : -0.020140089228601762 + }, { + "time" : 0.7374186993151032, + "velocity" : 2.936689148928433, + "acceleration" : -3.99999999999999, + "pose" : { + "translation" : { + "x" : 6.403410718093454, + "y" : 1.4334007854860387 + }, + "rotation" : { + "radians" : 1.5986879179972218 + } + }, + "curvature" : -0.02258406827188887 + }, { + "time" : 0.7678125246032664, + "velocity" : 2.8151138477757804, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.401062786287184, + "y" : 1.520778893299024 + }, + "rotation" : { + "radians" : 1.5965918957171965 + } + }, + "curvature" : -0.02545459002718879 + }, { + "time" : 0.7993888917739188, + "velocity" : 2.688808379093171, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.398921634924733, + "y" : 1.6076494447320337 + }, + "rotation" : { + "radians" : 1.59423685765634 + } + }, + "curvature" : -0.028849988627757065 + }, { + "time" : 0.8320832691315886, + "velocity" : 2.558030869662492, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.397022099554306, + "y" : 1.6933994792188969 + }, + "rotation" : { + "radians" : 1.59159503414185 + } + }, + "curvature" : -0.03287744142106298 + }, { + "time" : 0.8658400026260393, + "velocity" : 2.423003935684689, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.395395160906077, + "y" : 1.7774554678812455 + }, + "rotation" : { + "radians" : 1.588636834345054 + } + }, + "curvature" : -0.03764692669916069 + }, { + "time" : 0.9006204438645928, + "velocity" : 2.283882170730475, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.394066996986112, + "y" : 1.859298479563023 + }, + "rotation" : { + "radians" : 1.5853328740324872 + } + }, + "curvature" : -0.04325577926527458 + }, { + "time" : 0.9364146483950042, + "velocity" : 2.1407053526088293, + "acceleration" : -3.999999999999997, + "pose" : { + "translation" : { + "x" : 6.393058035170299, + "y" : 1.938479346864992 + }, + "rotation" : { + "radians" : 1.5816576204489718 + } + }, + "curvature" : -0.04975638894367889 + }, { + "time" : 0.9732587981504898, + "velocity" : 1.993328753586887, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 6.39238200429827, + "y" : 2.0146338321792427 + }, + "rotation" : { + "radians" : 1.5775955933008623 + } + }, + "curvature" : -0.05709527513223549 + }, { + "time" : 1.0112621915316293, + "velocity" : 1.841315180062329, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.392044986767331, + "y" : 2.087497793723702 + }, + "rotation" : { + "radians" : 1.57315140430575 + } + }, + "curvature" : -0.06500723564977115 + }, { + "time" : 1.0506511801697798, + "velocity" : 1.683759225509727, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.392044470626388, + "y" : 2.1569223515766396 + }, + "rotation" : { + "radians" : 1.5683651575191264 + } + }, + "curvature" : -0.07284766972073614 + }, { + "time" : 1.0918455825454725, + "velocity" : 1.5189816160069558, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.39236840166987, + "y" : 2.222889053711179 + }, + "rotation" : { + "radians" : 1.5633344792916735 + } + }, + "curvature" : -0.07936077553696355 + }, { + "time" : 1.1356043581385193, + "velocity" : 1.3439465136347681, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 6.39299423553166, + "y" : 2.2855250420298034 + }, + "rotation" : { + "radians" : 1.5582428882782466 + } + }, + "curvature" : -0.0824337723791933 + }, { + "time" : 1.2379787498796464, + "velocity" : 0.9344489466702602, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.395003296006507, + "y" : 2.4021324106830946 + }, + "rotation" : { + "radians" : 1.549211634448327 + } + }, + "curvature" : -0.06547973929352308 + }, { + "time" : 1.4715909865472114, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.397645473480225, + "y" : 2.5112497806549072 + }, + "rotation" : { + "radians" : 1.5451593331499225 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 6.441819667816162, -0.0441741943359375, 0.0 ], + "y" : [ 0.3467085063457489, 1.1927070915699005, 0.0 ] + }, { + "x" : [ 6.397645473480225, 0.04417705535888672, 0.0 ], + "y" : [ 2.5112497806549072, 1.7227985858917236, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 1.3799999691545963, + "rotation" : { + "radians" : 0.6716053016572916 + } + } ], + "reversed" : false, + "color" : 92.0, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + } ] +} \ No newline at end of file diff --git a/src/main/java/frc/auton/HideBallsBlue.java b/src/main/java/frc/auton/HideBallsBlue.java new file mode 100644 index 00000000..4d020950 --- /dev/null +++ b/src/main/java/frc/auton/HideBallsBlue.java @@ -0,0 +1,12 @@ +package frc.auton; + +import edu.wpi.first.wpilibj.Filesystem; +import frc.auton.guiauto.AbstractGuiAuto; + +import java.io.File; + +public class HideBallsBlue extends AbstractGuiAuto { + public HideBallsBlue() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/hideballs.json")); + } +} diff --git a/src/main/java/frc/auton/HideBallsRed.java b/src/main/java/frc/auton/HideBallsRed.java new file mode 100644 index 00000000..cedb3354 --- /dev/null +++ b/src/main/java/frc/auton/HideBallsRed.java @@ -0,0 +1,12 @@ +package frc.auton; + +import edu.wpi.first.wpilibj.Filesystem; +import frc.auton.guiauto.AbstractGuiAuto; + +import java.io.File; + +public class HideBallsRed extends AbstractGuiAuto { + public HideBallsRed() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/hideballs.json")); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 693f0900..54fe564f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -121,6 +121,15 @@ public class Robot extends TimedRobot { @NotNull private BuddyAutoRight buddyAutoRight; + @SuppressWarnings("NotNullFieldNotInitialized") + @NotNull + private HideBallsBlue hideBallsBlue; + + @SuppressWarnings("NotNullFieldNotInitialized") + @NotNull + private HideBallsRed hideBallsRed; + + @NotNull private static final String SHOOT_AND_MOVE_LEFT = "Shoot and Move Left"; @NotNull private static final String SHOOT_AND_MOVE_MID = "Shoot and Move Mid"; @NotNull private static final String SHOOT_AND_MOVE_RIGHT = "Shoot and Move Right"; @@ -132,6 +141,8 @@ public class Robot extends TimedRobot { @NotNull private static final String SHOOT_ONLY_RIGHT = "Shoot Only Right"; @NotNull private static final String SHOOT_ONLY_MID = "Shoot Only Mid"; @NotNull private static final String SHOOT_ONLY_LEFT = "Shoot Only Left"; + @NotNull private static final String HIDE_BALLS = "Hide Balls"; + private static final String RESET_POSE = "Reset Pose"; @@ -250,6 +261,8 @@ public void robotInit() { CompletableFuture.runAsync(() -> fiveBallRed = new FiveBallRed()).thenRun(this::incrementLoadedAutos); CompletableFuture.runAsync(() -> sixBallRed = new SixBallRed()).thenRun(this::incrementLoadedAutos); CompletableFuture.runAsync(() -> buddyAutoLeft = new BuddyAutoLeft()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> hideBallsBlue = new HideBallsBlue()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> hideBallsRed = new HideBallsRed()).thenRun(this::incrementLoadedAutos); SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative); autoChooser.setDefaultOption(SHOOT_AND_MOVE_LEFT, SHOOT_AND_MOVE_LEFT); @@ -262,6 +275,7 @@ public void robotInit() { autoChooser.addOption(SHOOT_ONLY_LEFT, SHOOT_ONLY_LEFT); autoChooser.addOption(SHOOT_ONLY_MID, SHOOT_ONLY_MID); autoChooser.addOption(SHOOT_ONLY_RIGHT, SHOOT_ONLY_RIGHT); + autoChooser.addOption(HIDE_BALLS, HIDE_BALLS); sideChooser.setDefaultOption(BLUE, BLUE); sideChooser.addOption(RED, RED); @@ -294,7 +308,7 @@ public void robotInit() { volatile boolean loadingAutos = true; public void incrementLoadedAutos() { - if (loadedAutos.incrementAndGet() == 10) { + if (loadedAutos.incrementAndGet() == 12) { loadingAutos = false; } } @@ -357,6 +371,9 @@ public void autonomousInit() { case BUDDY_AUTO_LEFT: selectedAuto = buddyAutoLeft; break; + case HIDE_BALLS: + selectedAuto = hideBallsBlue; + break; default: selectedAuto = shootAndMoveRight; break; @@ -393,6 +410,9 @@ public void autonomousInit() { case BUDDY_AUTO_LEFT: selectedAuto = buddyAutoLeft; break; + case HIDE_BALLS: + selectedAuto = hideBallsRed; + break; default: selectedAuto = shootAndMoveRight; break; From d9831532f118b6989fab1f675dbf996f32ee16f9 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 27 Mar 2022 16:06:31 -0700 Subject: [PATCH 051/108] tune shooter config --- src/main/deploy/shooter/shooterconfig.json | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 9fc36d1b..8ebaa834 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -1,7 +1,7 @@ { "shooterConfigs" : [ { - "hoodEjectAngle" : 68.0, - "flywheelSpeed" : 1700.0, + "hoodEjectAngle" : 70.0, + "flywheelSpeed" : 1800.0, "distance" : 40.0 }, { "hoodEjectAngle" : 62.0, @@ -21,7 +21,7 @@ "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1800.0, + "flywheelSpeed" : 1900.0, "distance" : 120.0 }, { "hoodEjectAngle" : 51.0, @@ -33,11 +33,11 @@ "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2000.0, "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2200.0, "distance" : 175.0 }, { "hoodEjectAngle" : 51.0, @@ -45,15 +45,15 @@ "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2400.0, + "flywheelSpeed" : 2420.0, "distance" : 200.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2400.0, + "flywheelSpeed" : 2450.0, "distance" : 210.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2500.0, + "flywheelSpeed" : 2450.0, "distance" : 225.0 }, { "hoodEjectAngle" : 51.0, @@ -61,11 +61,11 @@ "distance" : 250.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2800.0, + "flywheelSpeed" : 3000.0, "distance" : 270.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2900.0, + "flywheelSpeed" : 3300.0, "distance" : 299.0 } ] } \ No newline at end of file From ba1cc7e8dc778a742d8e3dec97a74d86f130bfe5 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 28 Mar 2022 00:18:07 -0700 Subject: [PATCH 052/108] improve distance aprox accuracy as the robot pans left to right --- build.gradle | 2 + src/main/java/frc/robot/Robot.java | 2 + .../java/frc/subsystem/VisionManager.java | 34 ++++- src/main/java/frc/utility/Limelight.java | 137 +++++++++++++++++- 4 files changed, 169 insertions(+), 6 deletions(-) diff --git a/build.gradle b/build.gradle index df2f92a4..a38de3bf 100644 --- a/build.gradle +++ b/build.gradle @@ -54,6 +54,8 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + implementation 'org.apache.commons:commons-math3:3.6.1' + implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 693f0900..30f00be9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -285,6 +285,8 @@ public void robotInit() { startSubsystems(); limelight.setLedMode(LedMode.OFF); intakeLimelight.setLedMode(LedMode.OFF); + + limelight.setStreamingMode(StreamingMode.STANDARD); // shooter.homeHood(); // shooter.setHoodPositionMode(HoodPositionMode.RELATIVE_TO_HOME); } diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index d506c39a..39d4e1eb 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.drive.Vector2d; import frc.robot.Constants; import frc.subsystem.BlinkinLED.BlinkinLedMode; import frc.subsystem.BlinkinLED.LedStatus; @@ -20,6 +21,7 @@ import frc.utility.geometry.MutableTranslation2d; import frc.utility.shooter.visionlookup.ShooterConfig; import frc.utility.shooter.visionlookup.VisionLookUpTable; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; @@ -64,6 +66,18 @@ public void logData() { logData("Distance to Target", Units.metersToInches(getDistanceToTarget())); logData("Rotation Target", getAngleToTarget().getDegrees()); + Vector3D correctVector = limelight.getCorrectTargetVector(); + logData("New Distance", Math.hypot(correctVector.getX(), correctVector.getZ())); + + Vector2d targetPx = limelight.getTargetPosInCameraPixels(); + + logData("py", targetPx.y); + logData("px", targetPx.x); + +// Vector2d newRelGoalPos = limelight.getCorrectGoalPos(); +// logData("New Z", newRelGoalPos.x); +// logData("New X", newRelGoalPos.y); + logData("Allow Shooting Robot Speed", drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED); logData("Is Robot Allowed Shoot Tilt", Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3); @@ -219,10 +233,18 @@ public Translation2d getRobotVel() { Rotation2d currentGyroAngle = getLatencyCompedLimelightRotation(); - double distanceToTarget = limelight.getDistanceM() + Constants.GOAL_RADIUS + Units.inchesToMeters(23); - double angleToTarget = currentGyroAngle.getDegrees() - limelight.getHorizontalOffset(); - return Optional.of(new Translation2d(distanceToTarget * Math.cos(Math.toRadians(angleToTarget)), - distanceToTarget * Math.sin(Math.toRadians(angleToTarget))) + Vector3D offsetVector = limelight.getCorrectTargetVector(); + double angleOffset = Math.atan2(offsetVector.getX(), offsetVector.getZ()); + + + double distanceToTarget = + Units.inchesToMeters( + Math.hypot(offsetVector.getX(), offsetVector.getZ()) + + Constants.GOAL_RADIUS + Units.inchesToMeters(23)); + + double angleToTarget = currentGyroAngle.getRadians() - angleOffset; + return Optional.of(new Translation2d(distanceToTarget * Math.cos(angleToTarget), + distanceToTarget * Math.sin(angleToTarget)) .plus(Constants.GOAL_POSITION)); } @@ -357,12 +379,14 @@ public void update() { blinkinLED.setStatus(limelightNotConnectedStatus); } - if (Math.abs(angleToTarget - robotTracker.getGyroAngle().getRadians()) < Math.toRadians(50)) { + if (Math.abs(new Rotation2d(angleToTarget).minus(robotTracker.getGyroAngle()).getRadians()) < Math.toRadians(50)) { forceVisionOn(updateLoopSource); } else { unForceVisionOn(updateLoopSource); } + logData("Angle To Target", angleToTarget); + Optional robotTranslationOptional = getVisionTranslation(); if (robotTranslationOptional.isPresent()) { diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 23fecf19..6dbc894a 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -1,11 +1,22 @@ package frc.utility; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.EntryListenerFlags; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.drive.Vector2d; +import frc.subsystem.AbstractSubsystem; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; +import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.jetbrains.annotations.NotNull; import java.util.HashMap; @@ -14,7 +25,7 @@ /** * This class is used to get data from the limelight network tables */ -public final class Limelight { +public final class Limelight extends AbstractSubsystem { public static final double[] EMPTY_DOUBLE_ARRAY = new double[0]; final @NotNull NetworkTable limelightTable; final @NotNull NetworkTable limelightGuiTable; @@ -26,6 +37,10 @@ public final class Limelight { return getInstance("limelight"); } + + volatile double angle = -38; + volatile double hOffset = 56; + public static @NotNull Limelight getInstance(String name) { LIMELIGHT_MAP_LOCK.readLock().lock(); try { @@ -50,6 +65,21 @@ public final class Limelight { } } + @Override + public void selfTest() { + + } + + @Override + public void logData() { + + } + + @Override + public void close() throws Exception { + + } + /** * Limelight’s LED states @@ -121,9 +151,26 @@ public enum StreamingMode { } private Limelight(String name) { + super(-1); limelightTable = NetworkTableInstance.getDefault().getTable(name); limelightGuiTable = NetworkTableInstance.getDefault().getTable(name + "gui"); + NetworkTableEntry angleTable = limelightGuiTable.getEntry("angle"); + angleTable.setDouble(angle); + angleTable.addListener(event -> { + angle = event.getEntry().getDouble(angle); + System.out.println("Changing angle to " + hOffset); + }, + EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + + NetworkTableEntry offsetTable = limelightGuiTable.getEntry("hOffset"); + offsetTable.setDouble(hOffset); + offsetTable.addListener(event -> { + hOffset = event.getEntry().getDouble(hOffset); + System.out.println("Changing hOffset to " + hOffset); + }, + EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + limelightTable.getEntry("tl").addListener(event -> lastUpdate = Timer.getFPGATimestamp(), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); @@ -263,6 +310,94 @@ public double getDistance() { } } + public Vector2d getTargetPosInCameraPixels() { + return new Vector2d( + (getHorizontalOffset() / 29.8) * (320.0 / 2) + (320.0 / 2), + (getVerticalOffset() / 24.85) * (240.0 / 2) + (240.0 / 2) + ); + } + + /** + * @return Distance from the limelight to the target in IN + * @see Limelight Docs: Estimating + * Distance + */ + public Vector3D getCorrectTargetVector() { + if (isTargetVisible()) { + + Vector2d targetPosInCameraPixels = getTargetPosInCameraPixels(); + + double py = targetPosInCameraPixels.y; + double px = targetPosInCameraPixels.x; + + + Matrix cameraMatrixInverse = new MatBuilder<>(Nat.N3(), Nat.N3()).fill( + 0.003883, 0, -0.6213, + 0, 0.003901, -0.4681, + 0, 0, 1 + ); + + Matrix cameraUnitVector = cameraMatrixInverse.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(px, py, 1)); + + + Rotation cameraRotation = + new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, Math.toRadians(angle), 0, 0); + + Vector3D vector3D = new Vector3D( + cameraUnitVector.get(0, 0), + cameraUnitVector.get(1, 0), + cameraUnitVector.get(2, 0) + ); + +// return vector3D; + + vector3D = vector3D.normalize(); + +// + Vector3D goalDir = cameraRotation.applyTo(vector3D); + + //logData("Rotated angle", rotatedAngle.toString(), true); + + double k = hOffset / goalDir.getY(); + + return goalDir.scalarMultiply(k).add(new Vector3D(0, 0, 6.0867)); + +// double fx = 257.5129206733333; // focal length x +// double fy = 256.350717159; // focal length y +// + //return 68.728 / (Math.tan(rotatedAngle.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[1])); + } else { + return new Vector3D(0, 0, 0); + } + } + + + // +// Rotation targetRotation = +// new Rotation( +// RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, +// Math.toRadians(getHorizontalOffset()), Math.toRadians(getVerticalOffset()), Math.toRadians(0)); +// +// Rotation changedRotation = cameraRotation.compose(targetRotation, RotationConvention.VECTOR_OPERATOR); + +// double cy = 240.0 / 2; +// double cx = 320.0 / 2; + +// Vector2d targetPosInCameraPixels = getTargetPosInCameraPixels(); +// +// double py = targetPosInCameraPixels.y; +// double px = targetPosInCameraPixels.x; +// +// +// +// +// double theta = Math.toRadians(90 - 57); +// double cos = Math.cos(theta); +// double sin = Math.sin(theta); +// +// Vector3D goalDir = new Vector3D(px, cos * py - sin, sin * py + cos); +// +// goalDir.normalize(); public Vector2d[] getCorners() { double[] corners = limelightTable.getEntry("tcornxy").getDoubleArray(EMPTY_DOUBLE_ARRAY); From ae3aa6252fd79f30f0f229c3c2c70206163d315a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 28 Mar 2022 10:26:03 -0700 Subject: [PATCH 053/108] correctly rotate the distance so that it's angled towards the center of the goal. Also make it so that the distance offset can be live edited --- .../java/frc/subsystem/VisionManager.java | 5 +- src/main/java/frc/utility/Limelight.java | 86 ++++++------------- 2 files changed, 25 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 39d4e1eb..5f9eeffc 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -237,10 +237,7 @@ public Translation2d getRobotVel() { double angleOffset = Math.atan2(offsetVector.getX(), offsetVector.getZ()); - double distanceToTarget = - Units.inchesToMeters( - Math.hypot(offsetVector.getX(), offsetVector.getZ()) - + Constants.GOAL_RADIUS + Units.inchesToMeters(23)); + double distanceToTarget = Units.inchesToMeters(Math.hypot(offsetVector.getX(), offsetVector.getZ())); double angleToTarget = currentGyroAngle.getRadians() - angleOffset; return Optional.of(new Translation2d(distanceToTarget * Math.cos(angleToTarget), diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 6dbc894a..08d7abcb 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -40,6 +40,7 @@ public final class Limelight extends AbstractSubsystem { volatile double angle = -38; volatile double hOffset = 56; + volatile double zOffset = 43; public static @NotNull Limelight getInstance(String name) { LIMELIGHT_MAP_LOCK.readLock().lock(); @@ -157,20 +158,20 @@ private Limelight(String name) { NetworkTableEntry angleTable = limelightGuiTable.getEntry("angle"); angleTable.setDouble(angle); - angleTable.addListener(event -> { - angle = event.getEntry().getDouble(angle); - System.out.println("Changing angle to " + hOffset); - }, + angleTable.addListener(event -> angle = event.getEntry().getDouble(0), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - NetworkTableEntry offsetTable = limelightGuiTable.getEntry("hOffset"); - offsetTable.setDouble(hOffset); - offsetTable.addListener(event -> { - hOffset = event.getEntry().getDouble(hOffset); - System.out.println("Changing hOffset to " + hOffset); - }, + NetworkTableEntry hOffsetTable = limelightGuiTable.getEntry("hOffset"); + hOffsetTable.setDouble(hOffset); + hOffsetTable.addListener(event -> hOffset = event.getEntry().getDouble(0), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + NetworkTableEntry zOffsetTable = limelightGuiTable.getEntry("zOffset"); + zOffsetTable.setDouble(zOffset); + zOffsetTable.addListener(event -> zOffset = event.getEntry().getDouble(0), + EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + + limelightTable.getEntry("tl").addListener(event -> lastUpdate = Timer.getFPGATimestamp(), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); @@ -317,10 +318,15 @@ public Vector2d getTargetPosInCameraPixels() { ); } + private static final Matrix CAMERA_MATRIX_INVERSE = new MatBuilder<>(Nat.N3(), Nat.N3()).fill( + 0.003883, 0, -0.6213, + 0, 0.003901, -0.4681, + 0, 0, 1 + ); + /** - * @return Distance from the limelight to the target in IN - * @see Limelight Docs: Estimating - * Distance + * @return The Correct distance from the limelight to the target in IN. This correctly compensates for the angle of the camera + * to ensure a consistent distance as the robot rotates. */ public Vector3D getCorrectTargetVector() { if (isTargetVisible()) { @@ -330,14 +336,7 @@ public Vector3D getCorrectTargetVector() { double py = targetPosInCameraPixels.y; double px = targetPosInCameraPixels.x; - - Matrix cameraMatrixInverse = new MatBuilder<>(Nat.N3(), Nat.N3()).fill( - 0.003883, 0, -0.6213, - 0, 0.003901, -0.4681, - 0, 0, 1 - ); - - Matrix cameraUnitVector = cameraMatrixInverse.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(px, py, 1)); + Matrix cameraUnitVector = CAMERA_MATRIX_INVERSE.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(px, py, 1)); Rotation cameraRotation = @@ -349,56 +348,19 @@ public Vector3D getCorrectTargetVector() { cameraUnitVector.get(2, 0) ); -// return vector3D; - - vector3D = vector3D.normalize(); - -// Vector3D goalDir = cameraRotation.applyTo(vector3D); - - //logData("Rotated angle", rotatedAngle.toString(), true); + double angle = Math.atan2(goalDir.getX(), goalDir.getZ()); double k = hOffset / goalDir.getY(); - return goalDir.scalarMultiply(k).add(new Vector3D(0, 0, 6.0867)); - -// double fx = 257.5129206733333; // focal length x -// double fy = 256.350717159; // focal length y -// - //return 68.728 / (Math.tan(rotatedAngle.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[1])); + return goalDir.scalarMultiply(k) + .add(new Vector3D(0, 0, 6.0867) + .add(new Vector3D(Math.sin(angle) * zOffset, 0, Math.cos(angle) * zOffset))); } else { return new Vector3D(0, 0, 0); } } - - // -// Rotation targetRotation = -// new Rotation( -// RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, -// Math.toRadians(getHorizontalOffset()), Math.toRadians(getVerticalOffset()), Math.toRadians(0)); -// -// Rotation changedRotation = cameraRotation.compose(targetRotation, RotationConvention.VECTOR_OPERATOR); - -// double cy = 240.0 / 2; -// double cx = 320.0 / 2; - -// Vector2d targetPosInCameraPixels = getTargetPosInCameraPixels(); -// -// double py = targetPosInCameraPixels.y; -// double px = targetPosInCameraPixels.x; -// -// -// -// -// double theta = Math.toRadians(90 - 57); -// double cos = Math.cos(theta); -// double sin = Math.sin(theta); -// -// Vector3D goalDir = new Vector3D(px, cos * py - sin, sin * py + cos); -// -// goalDir.normalize(); - public Vector2d[] getCorners() { double[] corners = limelightTable.getEntry("tcornxy").getDoubleArray(EMPTY_DOUBLE_ARRAY); Vector2d[] processedCorners = new Vector2d[corners.length / 2]; From 6ae17aaecec551458c67fd163a09f5792efa9085 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 28 Mar 2022 10:34:58 -0700 Subject: [PATCH 054/108] make it so that we can reset the robot position when the robot is disabled --- src/main/java/frc/robot/Robot.java | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7a0b2e13..966c7270 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -325,6 +325,18 @@ public void incrementLoadedAutos() { @Override public void robotPeriodic() { SmartDashboard.putNumber("Match Timestamp", DriverStation.getMatchTime()); + if (!DriverStation.isEnabled()) { + xbox.update(); + stick.update(); + buttonPanel.update(); + } + + if (stick.getRawButton(4)) { + visionManager.forceVisionOn(resettingPoseVisionOn); + visionManager.forceUpdatePose(); + } else { + visionManager.unForceVisionOn(resettingPoseVisionOn); + } } @@ -548,14 +560,6 @@ public void teleopPeriodic() { SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative); } - - if (stick.getRawButton(4)) { - visionManager.forceVisionOn(resettingPoseVisionOn); - visionManager.forceUpdatePose(); - } else { - visionManager.unForceVisionOn(resettingPoseVisionOn); - } - // if (xbox.getRisingEdge(XboxButtons.LEFT_BUMPER)) { // visionManager.adjustShooterHoodBias(-0.5); // } else if (xbox.getRisingEdge(XboxButtons.RIGHT_BUMPER)) { From fd934f618c8862d1cad3284d4d44e68176ed7dce Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 28 Mar 2022 13:39:37 -0700 Subject: [PATCH 055/108] adjust the CAMERA_MATRIX_INVERSE to improve accuracy refactor: rename zOffset -> depthOffset --- src/main/java/frc/utility/Limelight.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 08d7abcb..86c29462 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -40,7 +40,7 @@ public final class Limelight extends AbstractSubsystem { volatile double angle = -38; volatile double hOffset = 56; - volatile double zOffset = 43; + volatile double depthOffset = 24; public static @NotNull Limelight getInstance(String name) { LIMELIGHT_MAP_LOCK.readLock().lock(); @@ -167,8 +167,8 @@ private Limelight(String name) { EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); NetworkTableEntry zOffsetTable = limelightGuiTable.getEntry("zOffset"); - zOffsetTable.setDouble(zOffset); - zOffsetTable.addListener(event -> zOffset = event.getEntry().getDouble(0), + zOffsetTable.setDouble(depthOffset); + zOffsetTable.addListener(event -> depthOffset = event.getEntry().getDouble(0), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); @@ -319,8 +319,8 @@ public Vector2d getTargetPosInCameraPixels() { } private static final Matrix CAMERA_MATRIX_INVERSE = new MatBuilder<>(Nat.N3(), Nat.N3()).fill( - 0.003883, 0, -0.6213, - 0, 0.003901, -0.4681, + 0.00392173, 0, -0.6274771, + 0, 0.00389782, -0.46773827, 0, 0, 1 ); @@ -355,7 +355,7 @@ public Vector3D getCorrectTargetVector() { return goalDir.scalarMultiply(k) .add(new Vector3D(0, 0, 6.0867) - .add(new Vector3D(Math.sin(angle) * zOffset, 0, Math.cos(angle) * zOffset))); + .add(new Vector3D(Math.sin(angle) * depthOffset, 0, Math.cos(angle) * depthOffset))); } else { return new Vector3D(0, 0, 0); } From 76fdf19bd8f877a20a4e85cd9f66c303bf7f7fdc Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Mon, 28 Mar 2022 20:02:10 -0700 Subject: [PATCH 056/108] Make it so manual eject runs outtake full speed backwards --- src/main/java/frc/robot/Constants.java | 9 ++++-- src/main/java/frc/subsystem/Hopper.java | 37 ++++++++++++++++--------- 2 files changed, 31 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 061cdd82..0720e502 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -238,9 +238,14 @@ public final class Constants { /** - * Outtake's speed when ejecting balls + * Outtake's speed when automatically ejecting balls */ - public static final double OUTTAKE_EJECTION_SPEED = .5; + public static final double OUTTAKE_AUTO_EJECTION_SPEED = .5; + + /** + * Outtake's speed when manually ejecting balls + */ + public static final double OUTTAKE_MANUAL_EJECTION_SPEED = 1; /** * Outtake's speed when intaking balls diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 55b7ae61..6a135bd0 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -218,22 +218,33 @@ public void update() { // Override that makes it so the eject outtake state can never be set if (!disableEject) { updateOuttakeState(); + + // Outtake motor control + + switch (outtakeState) { + case OFF: + setOuttakePercentOutput(0); + break; + case EJECT: + setOuttakePercentOutput(Constants.OUTTAKE_AUTO_EJECTION_SPEED); + break; + case INTAKE: + setOuttakePercentOutput(Constants.OUTTAKE_SPEED); + break; + } } else { + // Modified outtakeState update to reflect manual ejecting updateOuttakeStateOverridden(); - } - // Outtake motor control - - switch (outtakeState) { - case OFF: - setOuttakePercentOutput(0); - break; - case EJECT: - setOuttakePercentOutput(Constants.OUTTAKE_EJECTION_SPEED); - break; - case INTAKE: - setOuttakePercentOutput(Constants.OUTTAKE_SPEED); - break; + switch (outtakeState) { + case OFF: + setOuttakePercentOutput(0); + break; + case EJECT: + // Outtake runs at max speed when manual ejecting (trigger eject) + setOuttakePercentOutput(Constants.OUTTAKE_MANUAL_EJECTION_SPEED); + break; + } } From 986f377dac4d6a05ccdf089bc7824448a8a51146 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 28 Mar 2022 22:21:36 -0700 Subject: [PATCH 057/108] disable logging data to the web dashboard (was leaking memory) --- src/main/java/frc/subsystem/DashboardHandler.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/subsystem/DashboardHandler.java b/src/main/java/frc/subsystem/DashboardHandler.java index c5f32d2d..c71ba86a 100644 --- a/src/main/java/frc/subsystem/DashboardHandler.java +++ b/src/main/java/frc/subsystem/DashboardHandler.java @@ -69,13 +69,14 @@ public final class DashboardHandler extends AbstractSubsystem { private DashboardHandler(int period) { super(period); - - try { - receivingSocket = new DatagramSocket(WEB_DASHBOARD_PORT); - receivingSocket.setSoTimeout(5); - } catch (SocketException e) { - DriverStation.reportError("Could not create socket for listening for data from web dashboard", false); - } + receivingSocket = null; //Disables networking code + +// try { +// receivingSocket = new DatagramSocket(WEB_DASHBOARD_PORT); +// receivingSocket.setSoTimeout(5); +// } catch (SocketException e) { +// DriverStation.reportError("Could not create socket for listening for data from web dashboard", false); +// } } public void log(@NotNull String key, @NotNull Object value) { From 999b4a58a1ef909b8269d2e07bcbbc682efaf0b2 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 28 Mar 2022 22:24:33 -0700 Subject: [PATCH 058/108] refactor the hopper class and outtake to make the code more concise and readable This commit should not change any behavior --- src/main/java/frc/subsystem/Hopper.java | 93 +++++++++---------------- src/main/java/frc/subsystem/Intake.java | 4 +- 2 files changed, 36 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 7e494243..362be1b1 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -38,12 +38,13 @@ public enum HopperState { HopperState wantedHopperState = HopperState.OFF; /** - * Outtake can either be OFF or INTAKE, or EJECT + * Outtake can either be OFF or INTAKE, or AUTO_EJECT */ public enum OuttakeState { OFF, INTAKE, - EJECT, + AUTO_EJECT, + MANUAL_EJECT } private OuttakeState outtakeState = OuttakeState.OFF; @@ -93,7 +94,7 @@ private void updateAllianceColor() { friendlyAllianceColor = BallColor.BLUE; // Will detect opposite color balls and outtake when it sees them - intakeLimelight.setPipeline(1); + intakeLimelight.setPipeline(1); } else { opposingAllianceColor = BallColor.BLUE; friendlyAllianceColor = BallColor.RED; @@ -114,40 +115,28 @@ public void toggleEjectOverride() { * Updates state of outtake based on color sensor and intake direction */ private void updateOuttakeState() { - // If color sensor detects a ball or Intake intake = Intake.getInstance(); - // Override for all other possible states that reverses outtake if hopper is in reverse - if (wantedHopperState == HopperState.REVERSE && intake.getIntakeSolState() == IntakeSolState.OPEN) { - outtakeState = OuttakeState.EJECT; + if (wantedHopperState == HopperState.REVERSE) { + outtakeState = intake.getIntakeSolState() == IntakeSolState.OPEN ? OuttakeState.MANUAL_EJECT : OuttakeState.OFF; return; } - - // Intake is running and open - if (wantedHopperState == HopperState.ON) { - // Ball Color is opposite - if (getBallColor() == opposingAllianceColor && intake.getIntakeSolState() == IntakeSolState.OPEN) { - lastDetectionTime = Timer.getFPGATimestamp(); - outtakeState = OuttakeState.EJECT; - // Opposite ball color detected within a certain time frame - } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD && intake.getIntakeSolState() == IntakeSolState.OPEN) { - outtakeState = OuttakeState.EJECT; - } else { - outtakeState = OuttakeState.INTAKE; - } - } else { + if (wantedHopperState == HopperState.OFF) { outtakeState = OuttakeState.OFF; + return; } - } - private void updateOuttakeStateOverridden() { - Intake intake = Intake.getInstance(); - if (wantedHopperState == HopperState.REVERSE && intake.getIntakeSolState() == IntakeSolState.OPEN) { - outtakeState = OuttakeState.EJECT; - } else if (wantedHopperState == HopperState.ON) { - outtakeState = OuttakeState.INTAKE; + if (getBallColor() == opposingAllianceColor + && intake.getIntakeSolState() == IntakeSolState.OPEN + && !disableEject) { // If disable eject is on, it will not outtake + lastDetectionTime = Timer.getFPGATimestamp(); + outtakeState = OuttakeState.AUTO_EJECT; + } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD + && intake.getIntakeSolState() == IntakeSolState.OPEN) { + // Ensure that we keep outtaking for a minimum amount of time + outtakeState = OuttakeState.AUTO_EJECT; } else { - outtakeState = OuttakeState.OFF; + outtakeState = OuttakeState.INTAKE; } } @@ -179,37 +168,23 @@ public BallColor getBallColor() { @Override public void update() { updateAllianceColor(); + updateOuttakeState(); - // Override that makes it so the eject outtake state can never be set - if (!disableEject) { - updateOuttakeState(); - - // Outtake motor control - - switch (outtakeState) { - case OFF: - setOuttakePercentOutput(0); - break; - case EJECT: - setOuttakePercentOutput(Constants.OUTTAKE_AUTO_EJECTION_SPEED); - break; - case INTAKE: - setOuttakePercentOutput(Constants.OUTTAKE_SPEED); - break; - } - } else { - // Modified outtakeState update to reflect manual ejecting - updateOuttakeStateOverridden(); - - switch (outtakeState) { - case OFF: - setOuttakePercentOutput(0); - break; - case EJECT: - // Outtake runs at max speed when manual ejecting (trigger eject) - setOuttakePercentOutput(Constants.OUTTAKE_MANUAL_EJECTION_SPEED); - break; - } + // Outtake motor control + + switch (outtakeState) { + case OFF: + setOuttakePercentOutput(0); + break; + case AUTO_EJECT: + setOuttakePercentOutput(Constants.OUTTAKE_AUTO_EJECTION_SPEED); + break; + case INTAKE: + setOuttakePercentOutput(Constants.OUTTAKE_SPEED); + break; + case MANUAL_EJECT: + setOuttakePercentOutput(Constants.OUTTAKE_MANUAL_EJECTION_SPEED); + break; } diff --git a/src/main/java/frc/subsystem/Intake.java b/src/main/java/frc/subsystem/Intake.java index d9860478..6b396cc8 100644 --- a/src/main/java/frc/subsystem/Intake.java +++ b/src/main/java/frc/subsystem/Intake.java @@ -92,7 +92,7 @@ public enum IntakeState { INTAKE, EJECT, OFF } - IntakeState wantedIntakeState = IntakeState.OFF; + private IntakeState wantedIntakeState = IntakeState.OFF; public synchronized void setWantedIntakeState(IntakeState intakeState) { wantedIntakeState = intakeState; @@ -101,7 +101,7 @@ public synchronized void setWantedIntakeState(IntakeState intakeState) { private void setIntakeState(IntakeState intakeState) { switch (intakeState) { case INTAKE: - if (Hopper.getInstance().getOuttakeState() == OuttakeState.EJECT) { + if (Hopper.getInstance().getOuttakeState() == OuttakeState.AUTO_EJECT) { intakeMotor.set(Constants.INTAKE_EJECTION_SPEED); } else { intakeMotor.set(Constants.INTAKE_SPEED); From 0fb29c1a286db1f2136976b44ed2d872e7361abd Mon Sep 17 00:00:00 2001 From: CoolPolyester Date: Mon, 28 Mar 2022 23:13:58 -0700 Subject: [PATCH 059/108] Added limelight pipelines to the github --- limelight/IntakePipeline 3_27_2022.vpr | 68 +++++++++++++++++++++++++ limelight/ShooterPipeline 3_27_2022.vpr | 68 +++++++++++++++++++++++++ 2 files changed, 136 insertions(+) create mode 100644 limelight/IntakePipeline 3_27_2022.vpr create mode 100644 limelight/ShooterPipeline 3_27_2022.vpr diff --git a/limelight/IntakePipeline 3_27_2022.vpr b/limelight/IntakePipeline 3_27_2022.vpr new file mode 100644 index 00000000..bca6d64f --- /dev/null +++ b/limelight/IntakePipeline 3_27_2022.vpr @@ -0,0 +1,68 @@ +area_max:100 +area_min:1.874161 +area_similarity:0 +aspect_max:3.681249012500001 +aspect_min:0.07206001250000002 +black_level:0 +blue_balance:2500 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:100 +convexity_min:58.6 +corner_approx:5.000000 +crop_x_max:1 +crop_x_min:-1 +crop_y_max:-0.07 +crop_y_min:-1 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:1200 +force_convex:1 +hue_max:154 +hue_min:4 +image_flip:1 +image_source:0 +img_to_show:0 +intersection_filter:0 +invert_hue:1 +multigroup_max:7 +multigroup_min:1 +multigroup_rejector:0 +pipeline_led_enabled:0 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1260 +roi_x:0.000000 +roi_y:0.000000 +sat_max:255 +sat_min:229 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0.000000 +val_max:255 +val_min:132 +x_outlier_miqr:1.5 +y_max:1.000000 +y_min:-1.000000 +y_outlier_miqr:1.5 diff --git a/limelight/ShooterPipeline 3_27_2022.vpr b/limelight/ShooterPipeline 3_27_2022.vpr new file mode 100644 index 00000000..8c796f7a --- /dev/null +++ b/limelight/ShooterPipeline 3_27_2022.vpr @@ -0,0 +1,68 @@ +area_max:100 +area_min:0.0017850625000000004 +area_similarity:15 +aspect_max:20.000000 +aspect_min:0.000000 +black_level:5 +blue_balance:1331 +calibration_type:0 +contour_grouping:3 +contour_sort_final:0 +convexity_max:100 +convexity_min:10 +corner_approx:5.000000 +crop_x_max:1.000000 +crop_x_min:-1.000000 +crop_y_max:1.000000 +crop_y_min:-1.000000 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:2 +force_convex:1 +hue_max:89 +hue_min:55 +image_flip:1 +image_source:0 +img_to_show:0 +intersection_filter:0 +invert_hue:0 +multigroup_max:5 +multigroup_min:3 +multigroup_rejector:1 +pipeline_led_enabled:0 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1599 +roi_x:0.000000 +roi_y:0.000000 +sat_max:255 +sat_min:81 +send_corners:1 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0.000000 +val_max:255 +val_min:85 +x_outlier_miqr:0.7 +y_max:1.000000 +y_min:-1.000000 +y_outlier_miqr:1.6 From aec58d6ca1516a19f9544135dfffc0b303dec80f Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 15:19:30 -0700 Subject: [PATCH 060/108] create a flag to tell if we're on the practice bot --- src/main/java/frc/robot/Robot.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 966c7270..b322ec36 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,6 +32,7 @@ import java.io.File; import java.io.IOException; import java.lang.Thread.State; +import java.nio.file.Files; import java.util.Collections; import java.util.concurrent.CompletableFuture; import java.util.concurrent.ExecutorService; @@ -51,6 +52,8 @@ public class Robot extends TimedRobot { public boolean useFieldRelative = false; + public static final boolean IS_PRACTICE = Files.exists(new File("/home/lvuser/practice").toPath()); + //GUI final @NotNull NetworkTableInstance instance = NetworkTableInstance.getDefault(); final @NotNull NetworkTable autoDataTable = instance.getTable("autodata"); From 4735c1e8ea4ebbb562726910c5586daa042b4018 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 15:37:45 -0700 Subject: [PATCH 061/108] add move autos that move opponent cargo around --- .../deploy/autos/blue/buddyautoleftsteal.json | 3806 +++++++++++++++++ .../blue/{hideballs.json => hidecargo.json} | 1073 +++-- .../deploy/autos/blue/movecargotohanger.json | 3458 +++++++++++++++ .../deploy/autos/red/buddyautoleftsteal.json | 3806 +++++++++++++++++ .../red/{hideballs.json => hidecargo.json} | 1073 +++-- .../deploy/autos/red/movecargotohanger.json | 3458 +++++++++++++++ .../java/frc/auton/BuddyAutoLeftHideBlue.java | 12 + .../java/frc/auton/BuddyAutoLeftHideRed.java | 12 + src/main/java/frc/auton/HangerCargoBlue.java | 12 + src/main/java/frc/auton/HangerCargoRed.java | 12 + ...{HideBallsBlue.java => HideCargoBlue.java} | 6 +- .../{HideBallsRed.java => HideCargoRed.java} | 6 +- src/main/java/frc/robot/Robot.java | 59 +- src/main/java/frc/subsystem/Drive.java | 1 + 14 files changed, 15629 insertions(+), 1165 deletions(-) create mode 100644 src/main/deploy/autos/blue/buddyautoleftsteal.json rename src/main/deploy/autos/blue/{hideballs.json => hidecargo.json} (77%) create mode 100644 src/main/deploy/autos/blue/movecargotohanger.json create mode 100644 src/main/deploy/autos/red/buddyautoleftsteal.json rename src/main/deploy/autos/red/{hideballs.json => hidecargo.json} (77%) create mode 100644 src/main/deploy/autos/red/movecargotohanger.json create mode 100644 src/main/java/frc/auton/BuddyAutoLeftHideBlue.java create mode 100644 src/main/java/frc/auton/BuddyAutoLeftHideRed.java create mode 100644 src/main/java/frc/auton/HangerCargoBlue.java create mode 100644 src/main/java/frc/auton/HangerCargoRed.java rename src/main/java/frc/auton/{HideBallsBlue.java => HideCargoBlue.java} (62%) rename src/main/java/frc/auton/{HideBallsRed.java => HideCargoRed.java} (63%) diff --git a/src/main/deploy/autos/blue/buddyautoleftsteal.json b/src/main/deploy/autos/blue/buddyautoleftsteal.json new file mode 100644 index 00000000..5264679f --- /dev/null +++ b/src/main/deploy/autos/blue/buddyautoleftsteal.json @@ -0,0 +1,3806 @@ +{ + "autonomousSteps" : [ { + "type" : "script", + "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON\nsleep 1500", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { + "methodName" : "sleep", + "args" : [ "1500" ], + "argTypes" : [ "long" ], + "reflection" : false + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.979461669921875, + "y" : 0.4386838674545288 + }, + "rotation" : { + "radians" : 2.4670512351406453 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.2218186293424793, + "velocity" : 0.8872745173699175, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.902607691762569, + "y" : 0.5001444677761242 + }, + "rotation" : { + "radians" : 2.4669815013720786 + } + }, + "curvature" : -0.001770057444385252 + }, { + "time" : 0.3128823124020749, + "velocity" : 1.2515292496083, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 6.826564423769014, + "y" : 0.5609803238967288 + }, + "rotation" : { + "radians" : 2.46664012448608 + } + }, + "curvature" : -0.005584153832447457 + }, { + "time" : 0.38171187830481973, + "velocity" : 1.526847513219279, + "acceleration" : 3.999999999999998, + "pose" : { + "translation" : { + "x" : 6.751933529499183, + "y" : 0.6207539377647322 + }, + "rotation" : { + "radians" : 2.465845563577437 + } + }, + "curvature" : -0.011384177747344891 + }, { + "time" : 0.4386276460816924, + "velocity" : 1.7545105843267694, + "acceleration" : 3.999999999999998, + "pose" : { + "translation" : { + "x" : 6.679111945442855, + "y" : 0.6792085023480467 + }, + "rotation" : { + "radians" : 2.4644380166116457 + } + }, + "curvature" : -0.019103896941460693 + }, { + "time" : 0.4876979714419739, + "velocity" : 1.9507918857678952, + "acceleration" : 3.9999999999999925, + "pose" : { + "translation" : { + "x" : 6.608317537175935, + "y" : 0.7362419663763262 + }, + "rotation" : { + "radians" : 2.4622863748597443 + } + }, + "curvature" : -0.028529142584546568 + }, { + "time" : 0.5310864114411233, + "velocity" : 2.1243456457644925, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.539614755514776, + "y" : 0.7918810990831844 + }, + "rotation" : { + "radians" : 2.459302059694302 + } + }, + "curvature" : -0.03918060333359783 + }, { + "time" : 0.5701494584010045, + "velocity" : 2.2805978336040176, + "acceleration" : -0.47832492111012465, + "pose" : { + "translation" : { + "x" : 6.472940292670501, + "y" : 0.8462555549484136 + }, + "rotation" : { + "radians" : 2.455456663103906 + } + }, + "curvature" : -0.05024047921606022 + }, { + "time" : 0.6070915310824326, + "velocity" : 2.262927519603029, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.40812873840332, + "y" : 0.8995719384402037 + }, + "rotation" : { + "radians" : 2.4507991475637327 + } + }, + "curvature" : -0.06056066057312981 + }, { + "time" : 0.6446468880225168, + "velocity" : 2.1127060918426923, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.3449382361768585, + "y" : 0.95208786875736 + }, + "rotation" : { + "radians" : 2.445467378655361 + } + }, + "curvature" : -0.06878202666638442 + }, { + "time" : 0.6843932978096723, + "velocity" : 1.9537204526940704, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.28307613931247, + "y" : 1.0040860445715225 + }, + "rotation" : { + "radians" : 2.4396891056204377 + } + }, + "curvature" : -0.07355656872184071 + }, { + "time" : 0.7271558960672502, + "velocity" : 1.782670059663759, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.222224667143564, + "y" : 1.0558483087693844 + }, + "rotation" : { + "radians" : 2.4337699158796475 + } + }, + "curvature" : -0.07380896893317555 + }, { + "time" : 0.7741602914447601, + "velocity" : 1.5946524781537195, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.162066561169922, + "y" : 1.1076297131949104 + }, + "rotation" : { + "radians" : 2.428069867169106 + } + }, + "curvature" : -0.06893533742465202 + }, { + "time" : 0.8273894003721558, + "velocity" : 1.3817360424441365, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 6.1023107412120225, + "y" : 1.1596325833915557 + }, + "rotation" : { + "radians" : 2.422974921973857 + } + }, + "curvature" : -0.058846523386872546 + }, { + "time" : 0.8905738529525908, + "velocity" : 1.1289982321223961, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 6.042717961565359, + "y" : 1.2119805833444843 + }, + "rotation" : { + "radians" : 2.418872019409643 + } + }, + "curvature" : -0.043821739722789785 + }, { + "time" : 0.9731110220523553, + "velocity" : 0.7988495557233383, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.9831264671547615, + "y" : 1.2646927802227879 + }, + "rotation" : { + "radians" : 2.4161366104589406 + } + }, + "curvature" : -0.02420493001126614 + }, { + "time" : 1.1728234109831899, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.923477649688721, + "y" : 1.317657709121704 + }, + "rotation" : { + "radians" : 2.4151395686141304 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 6.979461669921875, -1.232039451599121, 0.0 ], + "y" : [ 0.4386838674545288, 0.9852284789085388, 0.0 ] + }, { + "x" : [ 5.923477649688721, -0.9546575546264648, 0.0 ], + "y" : [ 1.317657709121704, 0.8482611179351807, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.7759904509715376 + } + }, { + "time" : 1.0799999758601189, + "rotation" : { + "radians" : 2.3681701999637936 + } + } ], + "reversed" : false, + "color" : 209.88235, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "VisionManager.shootBalls 13", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "13" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.923477649688721, + "y" : 1.317657709121704 + }, + "rotation" : { + "radians" : 2.4280629259945457 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.1916862579332135, + "velocity" : 0.766745031732854, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.8679256060540865, + "y" : 1.3657653522359396 + }, + "rotation" : { + "radians" : 2.427546282399253 + } + }, + "curvature" : -0.013552982296289647 + }, { + "time" : 0.26883282720714063, + "velocity" : 1.0753313088285628, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.814256962185027, + "y" : 1.4123322981758974 + }, + "rotation" : { + "radians" : 2.426170125310746 + } + }, + "curvature" : -0.025078420554477936 + }, { + "time" : 0.3253598004362359, + "velocity" : 1.3014392017449439, + "acceleration" : 3.999999999999993, + "pose" : { + "translation" : { + "x" : 5.763593902279354, + "y" : 1.4564440353524333 + }, + "rotation" : { + "radians" : 2.424133609238276 + } + }, + "curvature" : -0.035413495196781865 + }, { + "time" : 0.37047014804781875, + "velocity" : 1.4818805921912748, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.716341427527368, + "y" : 1.4977757879532874 + }, + "rotation" : { + "radians" : 2.4216494598565745 + } + }, + "curvature" : -0.043163158740255655 + }, { + "time" : 0.44071473447231035, + "velocity" : 1.7628589378892412, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.630849667446455, + "y" : 1.57313263497781 + }, + "rotation" : { + "radians" : 2.4166846345884783 + } + }, + "curvature" : -0.03766210226111181 + }, { + "time" : 0.4973489273055233, + "velocity" : 1.989395709222093, + "acceleration" : -1.536593152344523, + "pose" : { + "translation" : { + "x" : 5.551411211490631, + "y" : 1.643696241080761 + }, + "rotation" : { + "radians" : 2.4146239525620357 + } + }, + "curvature" : 0.002386794122151864 + }, { + "time" : 0.5545817819308377, + "velocity" : 1.9014520967157054, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.468152332963655, + "y" : 1.7176221262780018 + }, + "rotation" : { + "radians" : 2.416957846275337 + } + }, + "curvature" : 0.03338570540861069 + }, { + "time" : 0.5881329413084252, + "velocity" : 1.7672474592053555, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.422027663326844, + "y" : 1.7583682663707805 + }, + "rotation" : { + "radians" : 2.419124204295016 + } + }, + "curvature" : 0.03568476607637792 + }, { + "time" : 0.6278550666121283, + "velocity" : 1.6083589579905428, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.3716818848624825, + "y" : 1.8026411016471684 + }, + "rotation" : { + "radians" : 2.42141292546337 + } + }, + "curvature" : 0.03192840803852446 + }, { + "time" : 0.67622301229786, + "velocity" : 1.4148871752476162, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.316670433480795, + "y" : 1.850801377645439 + }, + "rotation" : { + "radians" : 2.4235110036015075 + } + }, + "curvature" : 0.025280758123603746 + }, { + "time" : 0.7373987455586821, + "velocity" : 1.1701842422043276, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.257077135815052, + "y" : 1.9027724422048777 + }, + "rotation" : { + "radians" : 2.4252082067772336 + } + }, + "curvature" : 0.017656926445273908 + }, { + "time" : 0.8212616808414924, + "velocity" : 0.8347325010730865, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.193638766594631, + "y" : 1.9579375321181942 + }, + "rotation" : { + "radians" : 2.4263532239110557 + } + }, + "curvature" : 0.009537673600406624 + }, { + "time" : 1.029944806109764, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.127869606018066, + "y" : 2.0150370597839355 + }, + "rotation" : { + "radians" : 2.4267848465547592 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 5.923477649688721, -0.8946475982666016, 0.0 ], + "y" : [ 1.317657709121704, 0.7744828462600708, 0.0 ] + }, { + "x" : [ 5.127869606018066, -1.0593681335449219, 0.0 ], + "y" : [ 2.0150370597839355, 0.9194501638412476, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.356194490192345 + } + }, { + "time" : 0.9399999789893627, + "rotation" : { + "radians" : 2.4532338595176353 + } + } ], + "reversed" : false, + "color" : 154.35294, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "VisionManager.shootBalls 8\nDrive.turnToAngle 40", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "8" ], + "argTypes" : [ "double" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Drive.turnToAngle", + "args" : [ "40" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.127869606018066, + "y" : 2.0150370597839355 + }, + "rotation" : { + "radians" : 0.9676638940223135 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.23907872328411964, + "velocity" : 0.9563148931364785, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.192705269680573, + "y" : 2.109189995806446 + }, + "rotation" : { + "radians" : 0.9679091836027879 + } + }, + "curvature" : 0.004044048851551997 + }, { + "time" : 0.3327971604973431, + "velocity" : 1.3311886419893724, + "acceleration" : 3.9999999999999907, + "pose" : { + "translation" : { + "x" : 5.253460748921498, + "y" : 2.197499692440033 + }, + "rotation" : { + "radians" : 0.9685084969782757 + } + }, + "curvature" : 0.007076805090390186 + }, { + "time" : 0.39816082847441153, + "velocity" : 1.5926433138976455, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.307566488200337, + "y" : 2.2762623392827663 + }, + "rotation" : { + "radians" : 0.9692958679676649 + } + }, + "curvature" : 0.009223838838357203 + }, { + "time" : 0.44663324990958686, + "velocity" : 1.7865329996383468, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.353885251563042, + "y" : 2.343804362230003 + }, + "rotation" : { + "radians" : 0.9700562639818262 + } + }, + "curvature" : 0.008519844053710474 + }, { + "time" : 0.5116917209128728, + "velocity" : 2.0467668836514905, + "acceleration" : 1.992648464590459, + "pose" : { + "translation" : { + "x" : 5.424351285982993, + "y" : 2.4466790923615918 + }, + "rotation" : { + "radians" : 0.9696596521756633 + } + }, + "curvature" : -0.030593782175814987 + }, { + "time" : 0.5546990819250267, + "velocity" : 2.1324654355384465, + "acceleration" : -3.9999999999999925, + "pose" : { + "translation" : { + "x" : 5.475393638014793, + "y" : 2.5206459313631058 + }, + "rotation" : { + "radians" : 0.9621486944770179 + } + }, + "curvature" : -0.13823598332150094 + }, { + "time" : 0.5969447671310003, + "velocity" : 1.9634826947145523, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.5252832076948835, + "y" : 2.591331197647378 + }, + "rotation" : { + "radians" : 0.9510157851137995 + } + }, + "curvature" : -0.09101992339026437 + }, { + "time" : 0.6595418386244202, + "velocity" : 1.7130944087408728, + "acceleration" : -3.999999999999997, + "pose" : { + "translation" : { + "x" : 5.592436509672552, + "y" : 2.6847757240757346 + }, + "rotation" : { + "radians" : 0.9456880198624937 + } + }, + "curvature" : -0.018285197310074868 + }, { + "time" : 0.7058887043411002, + "velocity" : 1.527706945874153, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.636414063088068, + "y" : 2.745653131905783 + }, + "rotation" : { + "radians" : 0.9448592549816716 + } + }, + "curvature" : -0.005896965233362419 + }, { + "time" : 0.768439061104125, + "velocity" : 1.2775055188220539, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 5.687825173619785, + "y" : 2.8167451025219634 + }, + "rotation" : { + "radians" : 0.9445964731587643 + } + }, + "curvature" : -0.001026058794484897 + }, { + "time" : 0.8582844674175891, + "velocity" : 0.9181238935681972, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.745632715359989, + "y" : 2.896663040502972 + }, + "rotation" : { + "radians" : 0.9445853727793603 + } + }, + "curvature" : 3.9777751022351305E-4 + }, { + "time" : 1.0878154408096383, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.807385444641113, + "y" : 2.9820399284362793 + }, + "rotation" : { + "radians" : 0.9446200461231519 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.127869606018066, 1.0498194694519043, 0.0 ], + "y" : [ 2.0150370597839355, 1.5242493152618408, 0.0 ] + }, { + "x" : [ 5.807385444641113, 1.0001163482666016, 0.0 ], + "y" : [ 2.9820399284362793, 1.3827617168426514, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.9380478872771981 + } + }, { + "time" : 0.9999999776482582, + "rotation" : { + "radians" : 0.9272952180016122 + } + } ], + "reversed" : false, + "color" : 59.52942, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@time 1.4\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.4, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.807385444641113, + "y" : 2.9820399284362793 + }, + "rotation" : { + "radians" : -3.1077984471976654 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2180776573035808, + "velocity" : 0.8723106292143229, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.712328077295783, + "y" : 2.978708433225936 + }, + "rotation" : { + "radians" : -3.1041624869647366 + } + }, + "curvature" : 0.07273634738038368 + }, { + "time" : 0.3111557355200276, + "velocity" : 1.2446229420801103, + "acceleration" : 3.9999999999999956, + "pose" : { + "translation" : { + "x" : 5.613894489667587, + "y" : 2.974581701005377 + }, + "rotation" : { + "radians" : -3.094472230094453 + } + }, + "curvature" : 0.11857740286674891 + }, { + "time" : 0.38620859133114593, + "velocity" : 1.5448343653245833, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.509367328174033, + "y" : 2.9689573476464197 + }, + "rotation" : { + "radians" : -3.0808888420166256 + } + }, + "curvature" : 0.1367542913886183 + }, { + "time" : 0.4534555009735825, + "velocity" : 1.8138220038943296, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 5.39670240160558, + "y" : 2.961229747164907 + }, + "rotation" : { + "radians" : -3.065345218180051 + } + }, + "curvature" : 0.13629877278960334 + }, { + "time" : 0.5166742966413302, + "velocity" : 2.0666971865653205, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 5.274478519695425, + "y" : 2.950885042220694 + }, + "rotation" : { + "radians" : -3.0491647636938963 + } + }, + "curvature" : 0.1267933963841827 + }, { + "time" : 0.5473532125193565, + "velocity" : 2.189412850077426, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 5.209495855934646, + "y" : 2.9445945602601076 + }, + "rotation" : { + "radians" : -3.041082387571697 + } + }, + "curvature" : 0.12079034959194605 + }, { + "time" : 0.5775859258575893, + "velocity" : 2.310343703430357, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 5.141847331689291, + "y" : 2.9374961546176337 + }, + "rotation" : { + "radians" : -3.0330761640829413 + } + }, + "curvature" : 0.11467075614396673 + }, { + "time" : 0.607452738767687, + "velocity" : 2.429810955070748, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 5.071508179679264, + "y" : 2.929549181170158 + }, + "rotation" : { + "radians" : -3.025171418955131 + } + }, + "curvature" : 0.10876301194249897 + }, { + "time" : 0.6370065753804001, + "velocity" : 2.5480263015216003, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.998483164915225, + "y" : 2.920717795803565 + }, + "rotation" : { + "radians" : -3.017377231431235 + } + }, + "curvature" : 0.10327528635802412 + }, { + "time" : 0.6662799728523379, + "velocity" : 2.6651198914093515, + "acceleration" : 3.9999999999999942, + "pose" : { + "translation" : { + "x" : 4.922805017153899, + "y" : 2.910970798490861 + }, + "rotation" : { + "radians" : -3.0096904819137515 + } + }, + "curvature" : 0.09832969802645626 + }, { + "time" : 0.6952901141770801, + "velocity" : 2.7811604567083203, + "acceleration" : 3.9999999999999836, + "pose" : { + "translation" : { + "x" : 4.844532863353379, + "y" : 2.9002814773702994 + }, + "rotation" : { + "radians" : -3.0020990455222543 + } + }, + "curvature" : 0.09398961737092937 + }, { + "time" : 0.724042537599304, + "velocity" : 2.8961701503972157, + "acceleration" : 3.6380002628705683, + "pose" : { + "translation" : { + "x" : 4.763750660128433, + "y" : 2.888627452823507 + }, + "rotation" : { + "radians" : -2.994584224283115 + } + }, + "curvature" : 0.09028031417064307 + }, { + "time" : 0.7525829010133674, + "velocity" : 3.0, + "acceleration" : -1.0263248734009206E-14, + "pose" : { + "translation" : { + "x" : 4.680565626205809, + "y" : 2.8759905215536072 + }, + "rotation" : { + "radians" : -2.9871225391208203 + } + }, + "curvature" : 0.08720396777763022 + }, { + "time" : 0.7814294661611385, + "velocity" : 2.9999999999999996, + "acceleration" : 1.000209901006119E-14, + "pose" : { + "translation" : { + "x" : 4.595106674879542, + "y" : 2.8623565006633434 + }, + "rotation" : { + "radians" : -2.979687003342993 + } + }, + "curvature" : 0.08475025940246926 + }, { + "time" : 0.8110292004704637, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.507522846466259, + "y" : 2.847715071733205 + }, + "rotation" : { + "radians" : -2.9722479862521185 + } + }, + "curvature" : 0.08290368133012417 + }, { + "time" : 0.8413290035988551, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.417981740760487, + "y" : 2.832059624899553 + }, + "rotation" : { + "radians" : -2.9647737573971904 + } + }, + "curvature" : 0.08164850595350744 + }, { + "time" : 0.872270133120958, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.326667949489956, + "y" : 2.8153871029327426 + }, + "rotation" : { + "radians" : -2.9572307833972085 + } + }, + "curvature" : 0.08097214961252051 + }, { + "time" : 0.9037887421215142, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.233781488770907, + "y" : 2.7976978453152483 + }, + "rotation" : { + "radians" : -2.9495838324025407 + } + }, + "curvature" : 0.08086748247871706 + }, { + "time" : 0.9358164189685482, + "velocity" : 3.0, + "acceleration" : -9.119536798471233E-15, + "pose" : { + "translation" : { + "x" : 4.1395362315633975, + "y" : 2.778995432319789 + }, + "rotation" : { + "radians" : -2.941795926901371 + } + }, + "curvature" : 0.08133448893648988 + }, { + "time" : 0.9682807293579756, + "velocity" : 2.9999999999999996, + "acceleration" : 9.019320323200669E-15, + "pose" : { + "translation" : { + "x" : 4.044158340126607, + "y" : 2.7592865290874524 + }, + "rotation" : { + "radians" : -2.9338281737918814 + } + }, + "curvature" : 0.08238157300892571 + }, { + "time" : 1.0011057608042824, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.9478846984741445, + "y" : 2.73858072970582 + }, + "rotation" : { + "radians" : -2.925639491155511 + } + }, + "curvature" : 0.08402672524453093 + }, { + "time" : 1.0342126698325704, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.850961344829351, + "y" : 2.7168904012870905 + }, + "rotation" : { + "radians" : -2.91718624359335 + } + }, + "curvature" : 0.08629871484426285 + }, { + "time" : 1.0675202322086716, + "velocity" : 3.0, + "acceleration" : -0.1315579405706849, + "pose" : { + "translation" : { + "x" : 3.7536419040806095, + "y" : 2.694230528046205 + }, + "rotation" : { + "radians" : -2.9084217919318283 + } + }, + "curvature" : 0.08923843792521573 + }, { + "time" : 1.1009699296037385, + "velocity" : 2.9955994266979924, + "acceleration" : -0.16210586852590414, + "pose" : { + "translation" : { + "x" : 3.6561860202366483, + "y" : 2.6706185553789727 + }, + "rotation" : { + "radians" : -2.899295958219869 + } + }, + "curvature" : 0.09290053504803716 + }, { + "time" : 1.1345079603171326, + "velocity" : 2.990162715100549, + "acceleration" : -0.19242213116635215, + "pose" : { + "translation" : { + "x" : 3.5588577888818484, + "y" : 2.646074233940194 + }, + "rotation" : { + "radians" : -2.889754402955238 + } + }, + "curvature" : 0.0973553849716613 + }, { + "time" : 1.1680607870231765, + "velocity" : 2.983706408679117, + "acceleration" : -0.22706531667766747, + "pose" : { + "translation" : { + "x" : 3.4619241896315485, + "y" : 2.620619463721785 + }, + "rotation" : { + "radians" : -2.8797379082054317 + } + }, + "curvature" : 0.10269158445623167 + }, { + "time" : 1.2015549415534306, + "velocity" : 2.976101047873854, + "acceleration" : -0.2670814149888477, + "pose" : { + "translation" : { + "x" : 3.365653518587351, + "y" : 2.5942781381309032 + }, + "rotation" : { + "radians" : -2.869181557635527 + } + }, + "curvature" : 0.10901903383045562 + }, { + "time" : 1.2349184127471249, + "velocity" : 2.9671902847785026, + "acceleration" : -0.31371465342134314, + "pose" : { + "translation" : { + "x" : 3.2703138207924285, + "y" : 2.567075988068072 + }, + "rotation" : { + "radians" : -2.858013802441017 + } + }, + "curvature" : 0.11647276318602535 + }, { + "time" : 1.2680813779246811, + "velocity" : 2.9567865766514014, + "acceleration" : -0.3684468103258239, + "pose" : { + "translation" : { + "x" : 3.1761713226868302, + "y" : 2.539040426005304 + }, + "rotation" : { + "radians" : -2.8461554009678793 + } + }, + "curvature" : 0.12521765233260732 + }, { + "time" : 1.3009769666485926, + "velocity" : 2.944666301912286, + "acceleration" : -0.43303740193218687, + "pose" : { + "translation" : { + "x" : 3.0834888645627863, + "y" : 2.510200390064227 + }, + "rotation" : { + "radians" : -2.833518219736519 + } + }, + "curvature" : 0.1354542158098308 + }, { + "time" : 1.3335420599902073, + "velocity" : 2.930564398497954, + "acceleration" : -0.5095598286786093, + "pose" : { + "translation" : { + "x" : 2.9925243330200146, + "y" : 2.4805861880942075 + }, + "rotation" : { + "radians" : -2.820003885293717 + } + }, + "curvature" : 0.14742563679773568 + }, { + "time" : 1.365718127787587, + "velocity" : 2.91416876690357, + "acceleration" : -0.6004245148597813, + "pose" : { + "translation" : { + "x" : 2.9035290934210276, + "y" : 2.4502293417504766 + }, + "rotation" : { + "radians" : -2.8055022808143084 + } + }, + "curvature" : 0.16142623105857898 + }, { + "time" : 1.3974521047697568, + "velocity" : 2.8951149091694792, + "acceleration" : -0.7083738286934076, + "pose" : { + "translation" : { + "x" : 2.816746422346437, + "y" : 2.4191624305722534 + }, + "rotation" : { + "radians" : -2.7898898902549565 + } + }, + "curvature" : 0.17781148738953634 + }, { + "time" : 1.4286973034286823, + "velocity" : 2.87298162816717, + "acceleration" : -0.8769242761272262, + "pose" : { + "translation" : { + "x" : 2.7324099400502604, + "y" : 2.38741893606087 + }, + "rotation" : { + "radians" : -2.7730280085554613 + } + }, + "curvature" : 0.1970097361358105 + }, { + "time" : 1.4594210697670953, + "velocity" : 2.846039211610955, + "acceleration" : -3.9999999999999947, + "pose" : { + "translation" : { + "x" : 2.650742042915226, + "y" : 2.3550330857578956 + }, + "rotation" : { + "radians" : -2.7547608625141673 + } + }, + "curvature" : 0.2195352951517829 + }, { + "time" : 1.4900955256707504, + "velocity" : 2.723341387996335, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.5719523359080814, + "y" : 2.3220396973232624 + }, + "rotation" : { + "radians" : -2.7349137288234022 + } + }, + "curvature" : 0.24600255746489014 + }, { + "time" : 1.5212190951177167, + "velocity" : 2.5988471102084696, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.4962360650348963, + "y" : 2.2884740226133893 + }, + "rotation" : { + "radians" : -2.7132912008461836 + } + }, + "curvature" : 0.2771397985751853 + }, { + "time" : 1.5528031633912385, + "velocity" : 2.4725108371143825, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.42377254979637, + "y" : 2.254371591759306 + }, + "rotation" : { + "radians" : -2.689675854298644 + } + }, + "curvature" : 0.3138003172075707 + }, { + "time" : 1.5848723080728448, + "velocity" : 2.3442342583879574, + "acceleration" : -3.9999999999999942, + "pose" : { + "translation" : { + "x" : 2.354723615643138, + "y" : 2.219768057244778 + }, + "rotation" : { + "radians" : -2.6638277072718006 + } + }, + "curvature" : 0.3569666341755263 + }, { + "time" : 1.6174693044153108, + "velocity" : 2.213846273018094, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.289232026431076, + "y" : 2.184699037984432 + }, + "rotation" : { + "radians" : -2.6354850773300993 + } + }, + "curvature" : 0.4077405496577045 + }, { + "time" : 1.6506622716676704, + "velocity" : 2.0810744040086555, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 2.2274199168766096, + "y" : 2.1491999634018804 + }, + "rotation" : { + "radians" : -2.6043677221311743 + } + }, + "curvature" : 0.4673075916967568 + }, { + "time" : 1.6845551653471331, + "velocity" : 1.9455028292908045, + "acceleration" : -3.7890145173659073, + "pose" : { + "translation" : { + "x" : 2.169387225012014, + "y" : 2.1133059175078435 + }, + "rotation" : { + "radians" : -2.5701835156594286 + } + }, + "curvature" : 0.5368586687945079 + }, { + "time" : 1.7192335134624979, + "velocity" : 1.8141060648434189, + "acceleration" : -3.2696015880426708, + "pose" : { + "translation" : { + "x" : 2.1152101246407256, + "y" : 2.077051482978277 + }, + "rotation" : { + "radians" : -2.532640340224557 + } + }, + "curvature" : 0.6174451066520994 + }, { + "time" : 1.7907419934854965, + "velocity" : 1.5803018250017051, + "acceleration" : -2.726779669291407, + "pose" : { + "translation" : { + "x" : 2.018599167179449, + "y" : 2.003596336511297 + }, + "rotation" : { + "radians" : -2.446433526852606 + } + }, + "curvature" : 0.8136612252229765 + }, { + "time" : 1.8275860774684607, + "velocity" : 1.479836125863293, + "acceleration" : -2.359563803315625, + "pose" : { + "translation" : { + "x" : 1.9761847286498835, + "y" : 1.9664608799550876 + }, + "rotation" : { + "radians" : -2.397408828393692 + } + }, + "curvature" : 0.9278899124682813 + }, { + "time" : 1.8649660152071963, + "velocity" : 1.391635777804781, + "acceleration" : -1.9838468623386898, + "pose" : { + "translation" : { + "x" : 1.937661583645081, + "y" : 1.9290952336820055 + }, + "rotation" : { + "radians" : -2.3443966957035403 + } + }, + "curvature" : 1.0492344280773838 + }, { + "time" : 1.902729666191217, + "velocity" : 1.3167184772896783, + "acceleration" : -1.5953761316272526, + "pose" : { + "translation" : { + "x" : 1.9029635716538653, + "y" : 1.891529134866047 + }, + "rotation" : { + "radians" : -2.2876080290002703 + } + }, + "curvature" : 1.1720276125742721 + }, { + "time" : 1.940679658288589, + "velocity" : 1.256173965702088, + "acceleration" : -1.186040735740344, + "pose" : { + "translation" : { + "x" : 1.8719913626680516, + "y" : 1.85379088381519 + }, + "rotation" : { + "radians" : -2.227526882083615 + } + }, + "curvature" : 1.2877279640741648 + }, { + "time" : 1.9785675742743187, + "velocity" : 1.2112373539507049, + "acceleration" : -0.7410587042667358, + "pose" : { + "translation" : { + "x" : 1.8446108896377584, + "y" : 1.8159071880495183 + }, + "rotation" : { + "radians" : -2.164970018044204 + } + }, + "curvature" : 1.3850491721536529 + }, { + "time" : 2.0160893592543716, + "velocity" : 1.1834315085916118, + "acceleration" : -0.23332707042705494, + "pose" : { + "translation" : { + "x" : 1.8206517809267098, + "y" : 1.777903006379347 + }, + "rotation" : { + "radians" : -2.1011212361539733 + } + }, + "curvature" : 1.4508998888713995 + }, { + "time" : 2.052881886984702, + "velocity" : 1.1748468158826877, + "acceleration" : 0.3888458638588137, + "pose" : { + "translation" : { + "x" : 1.799905792767543, + "y" : 1.7398013929833471 + }, + "rotation" : { + "radians" : -2.037523308037849 + } + }, + "curvature" : 1.4721810231732262 + }, { + "time" : 2.088519327343306, + "velocity" : 1.188704287164646, + "acceleration" : 1.231210997720917, + "pose" : { + "translation" : { + "x" : 1.7821252417171145, + "y" : 1.7016233414866697 + }, + "rotation" : { + "radians" : -1.976018179939325 + } + }, + "curvature" : 1.4380568182608686 + }, { + "time" : 2.1225056544060728, + "velocity" : 1.2305486268164643, + "acceleration" : 2.534651579871874, + "pose" : { + "translation" : { + "x" : 1.7670214371118043, + "y" : 1.6633876290390703 + }, + "rotation" : { + "radians" : -1.918641555195968 + } + }, + "curvature" : 1.341918509745125 + }, { + "time" : 2.1542554976740123, + "velocity" : 1.311023417216232, + "acceleration" : 3.9999999999999845, + "pose" : { + "translation" : { + "x" : 1.7542631135228248, + "y" : 1.6251106603930339 + }, + "rotation" : { + "radians" : -1.8674949603224105 + } + }, + "curvature" : 1.182232258965127 + }, { + "time" : 2.2099359231623428, + "velocity" : 1.5337451191695526, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 1.7342355685846904, + "y" : 1.5484857759979853 + }, + "rotation" : { + "radians" : -1.7919535583318533 + } + }, + "curvature" : 0.6868485079079413 + }, { + "time" : 2.2579553489086845, + "velocity" : 1.7258228221549206, + "acceleration" : -1.2499750022631253, + "pose" : { + "translation" : { + "x" : 1.718481421470642, + "y" : 1.4718265533447266 + }, + "rotation" : { + "radians" : -1.7639859651971548 + } + }, + "curvature" : 0.0 + }, { + "time" : 2.3039131087144136, + "velocity" : 1.6683767712377462, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.7040171629614953, + "y" : 1.3951845909212786 + }, + "rotation" : { + "radians" : -1.744243059159777 + } + }, + "curvature" : 0.4949885871870906 + }, { + "time" : 2.353201866781118, + "velocity" : 1.4712217389709286, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 1.692568655125001, + "y" : 1.3186628076501847 + }, + "rotation" : { + "radians" : -1.688734274061912 + } + }, + "curvature" : 0.9318135483719395 + }, { + "time" : 2.409519830422982, + "velocity" : 1.2459498844034724, + "acceleration" : -2.1690899330402935, + "pose" : { + "translation" : { + "x" : 1.6866636401594342, + "y" : 1.2423782268174186 + }, + "rotation" : { + "radians" : -1.6025452180114088 + } + }, + "curvature" : 1.3089484866875387 + }, { + "time" : 2.4408900706331065, + "velocity" : 1.1779050121666355, + "acceleration" : -1.4371594032530797, + "pose" : { + "translation" : { + "x" : 1.6864417690519602, + "y" : 1.204360419059464 + }, + "rotation" : { + "radians" : -1.5497323811772583 + } + }, + "curvature" : 1.4645464958895065 + }, { + "time" : 2.4737794652080054, + "velocity" : 1.130637709486019, + "acceleration" : -0.882454393290906, + "pose" : { + "translation" : { + "x" : 1.6883282148664875, + "y" : 1.1664440317872504 + }, + "rotation" : { + "radians" : -1.4916448185226532 + } + }, + "curvature" : 1.589559435549557 + }, { + "time" : 2.5078700922418036, + "velocity" : 1.100554285890002, + "acceleration" : -0.4285512346859183, + "pose" : { + "translation" : { + "x" : 1.6925034029607406, + "y" : 1.1286425412588699 + }, + "rotation" : { + "radians" : -1.4293799219170706 + } + }, + "curvature" : 1.677647696189635 + }, { + "time" : 2.542863859363714, + "velocity" : 1.0855576637835957, + "acceleration" : -0.03409957451987376, + "pose" : { + "translation" : { + "x" : 1.6991206308842841, + "y" : 1.0909691291280303 + }, + "rotation" : { + "radians" : -1.3641679929709554 + } + }, + "curvature" : 1.7243201694504038 + }, { + "time" : 2.578478719165117, + "velocity" : 1.084343212217783, + "acceleration" : 0.32480160779729494, + "pose" : { + "translation" : { + "x" : 1.708307124635804, + "y" : 1.0534366687917367 + }, + "rotation" : { + "radians" : -1.2973160315953416 + } + }, + "curvature" : 1.72818476876106 + }, { + "time" : 2.6144494673900835, + "velocity" : 1.0960265690749238, + "acceleration" : 0.662753960787673, + "pose" : { + "translation" : { + "x" : 1.7201650949203895, + "y" : 1.0160577117379717 + }, + "rotation" : { + "radians" : -1.2301287631472986 + } + }, + "curvature" : 1.691537146255751 + }, { + "time" : 2.6505308994153998, + "velocity" : 1.1199396810605933, + "acceleration" : 0.9891536088281452, + "pose" : { + "translation" : { + "x" : 1.7347727934068158, + "y" : 0.9788444738933771 + }, + "rotation" : { + "radians" : -1.163822987935704 + } + }, + "curvature" : 1.6200724563638298 + }, { + "time" : 2.686501528745972, + "velocity" : 1.1555201588747483, + "acceleration" : 1.310397118703277, + "pose" : { + "translation" : { + "x" : 1.7521855689848245, + "y" : 0.941808821970934 + }, + "rotation" : { + "radians" : -1.0994531238813727 + } + }, + "curvature" : 1.52183879385189 + }, { + "time" : 2.7221665450113437, + "velocity" : 1.202255493427397, + "acceleration" : 1.6311499809238765, + "pose" : { + "translation" : { + "x" : 1.7724369240224052, + "y" : 0.9049622598176431 + }, + "rotation" : { + "radians" : -1.0378615567095117 + } + }, + "curvature" : 1.405821442899369 + }, { + "time" : 2.757359258068479, + "velocity" : 1.2596600866592031, + "acceleration" : 1.9551274988150928, + "pose" : { + "translation" : { + "x" : 1.7955395706230775, + "y" : 0.8683159147622064 + }, + "rotation" : { + "radians" : -0.9796592961829077 + } + }, + "curvature" : 1.2806102287854289 + }, { + "time" : 2.7919408310448324, + "velocity" : 1.3272714709375524, + "acceleration" : 2.2856166223411245, + "pose" : { + "translation" : { + "x" : 1.821486486883173, + "y" : 0.8318805239627071 + }, + "rotation" : { + "radians" : -0.9252342480030827 + } + }, + "curvature" : 1.1534643716591353 + }, { + "time" : 2.825798523286439, + "velocity" : 1.4046571751190786, + "acceleration" : 2.6258637992019844, + "pose" : { + "translation" : { + "x" : 1.850251973149117, + "y" : 0.7956664207542907 + }, + "rotation" : { + "radians" : -0.8747791350162617 + } + }, + "curvature" : 1.0298714520728465 + }, { + "time" : 2.8588428851625496, + "velocity" : 1.4914271687372875, + "acceleration" : 2.9794041928936728, + "pose" : { + "translation" : { + "x" : 1.8817927082747086, + "y" : 0.7596835209968459 + }, + "rotation" : { + "radians" : -0.8283296056482383 + } + }, + "curvature" : 0.9135232462092671 + }, { + "time" : 2.891004389345329, + "velocity" : 1.5872492891492276, + "acceleration" : 3.5509647371659865, + "pose" : { + "translation" : { + "x" : 1.916048805878404, + "y" : 0.7239413094226848 + }, + "rotation" : { + "radians" : -0.7858044305587907 + } + }, + "curvature" : 0.8065539475387932 + }, { + "time" : 2.9523592966906644, + "velocity" : 1.8051184015845994, + "acceleration" : 3.999999999999984, + "pose" : { + "translation" : { + "x" : 1.9923910543609047, + "y" : 0.6532146522016653 + }, + "rotation" : { + "radians" : -0.7118327324730775 + } + }, + "curvature" : 0.6236089171423088 + }, { + "time" : 3.0100353656283176, + "velocity" : 2.035822677335212, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 2.0785084606141027, + "y" : 0.5835531856100715 + }, + "rotation" : { + "radians" : -0.6511181043270622 + } + }, + "curvature" : 0.4800010235812804 + }, { + "time" : 3.0646190962309157, + "velocity" : 2.2541575997456036, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 2.1734333233560115, + "y" : 0.5150158743770881 + }, + "rotation" : { + "radians" : -0.6017399678104041 + } + }, + "curvature" : 0.3691740752256128 + }, { + "time" : 3.1166653761479624, + "velocity" : 2.46234271941379, + "acceleration" : 4.000000000000028, + "pose" : { + "translation" : { + "x" : 2.276034298238187, + "y" : 0.4476534745680283 + }, + "rotation" : { + "radians" : -0.5619346801385068 + } + }, + "curvature" : 0.2836399510576229 + }, { + "time" : 3.1418275733370513, + "velocity" : 2.562991508170146, + "acceleration" : 4.0000000000000275, + "pose" : { + "translation" : { + "x" : 2.3298233544980946, + "y" : 0.4144263756493385 + }, + "rotation" : { + "radians" : -0.5451464563946322 + } + }, + "curvature" : 0.24824311811855138 + }, { + "time" : 3.1664400864835103, + "velocity" : 2.6614415607559834, + "acceleration" : 3.999999999999966, + "pose" : { + "translation" : { + "x" : 2.3850501980787406, + "y" : 0.38150809671011743 + }, + "rotation" : { + "radians" : -0.5302189330250398 + } + }, + "curvature" : 0.21679631372622093 + }, { + "time" : 3.1905125871491076, + "velocity" : 2.757731563418372, + "acceleration" : 4.000000000000013, + "pose" : { + "translation" : { + "x" : 2.4415424379903916, + "y" : 0.3489024790262656 + }, + "rotation" : { + "radians" : -0.5170134828220253 + } + }, + "curvature" : 0.1886809876230122 + }, { + "time" : 3.214049672205038, + "velocity" : 2.851879903642094, + "acceleration" : 3.303657668196632, + "pose" : { + "translation" : { + "x" : 2.4991237930953503, + "y" : 0.31661276891827583 + }, + "rotation" : { + "radians" : -0.5054087566305369 + } + }, + "curvature" : 0.16335410873820064 + }, { + "time" : 3.2371151553370474, + "velocity" : 2.928080363861817, + "acceleration" : 1.0968430910701312, + "pose" : { + "translation" : { + "x" : 2.5576151483652376, + "y" : 0.2846416040989146 + }, + "rotation" : { + "radians" : -0.49529950641493536 + } + }, + "curvature" : 0.14033935814094875 + }, { + "time" : 3.259949848690555, + "velocity" : 2.953126439503317, + "acceleration" : 1.0318522973125155, + "pose" : { + "translation" : { + "x" : 2.6168356111382742, + "y" : 0.25299100002090213 + }, + "rotation" : { + "radians" : -0.48659537038691486 + } + }, + "curvature" : 0.11921786782590763 + }, { + "time" : 3.2827100656165435, + "velocity" : 2.9766116216257297, + "acceleration" : 0.9835246105005508, + "pose" : { + "translation" : { + "x" : 2.6766035673765614, + "y" : 0.22166233622459286 + }, + "rotation" : { + "radians" : -0.47921968759438144 + } + }, + "curvature" : 0.09961923590719055 + }, { + "time" : 3.305354934644453, + "velocity" : 2.99888340761624, + "acceleration" : 0.04948814588093301, + "pose" : { + "translation" : { + "x" : 2.736737737923363, + "y" : 0.19065634268565645 + }, + "rotation" : { + "radians" : -0.4731083784610581 + } + }, + "curvature" : 0.08121320610983931 + }, { + "time" : 3.3279177598197176, + "velocity" : 2.9999999999999996, + "acceleration" : 1.3150721678562816E-14, + "pose" : { + "translation" : { + "x" : 2.7970582347603883, + "y" : 0.15997308616275907 + }, + "rotation" : { + "radians" : -0.4682089061577221 + } + }, + "curvature" : 0.063702197976732 + }, { + "time" : 3.3504305526891756, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 2.857387617265072, + "y" : 0.1296119565452436 + }, + "rotation" : { + "radians" : -0.4644793182582315 + } + }, + "curvature" : 0.0468147679013892 + }, { + "time" : 3.3728462381053794, + "velocity" : 3.0, + "acceleration" : -1.3294976180711803E-14, + "pose" : { + "translation" : { + "x" : 2.9175519484678567, + "y" : 0.09957165320080996 + }, + "rotation" : { + "radians" : -0.461887356526069 + } + }, + "curvature" : 0.03030003960646033 + }, { + "time" : 3.395114760380951, + "velocity" : 2.9999999999999996, + "acceleration" : 1.3412784420541495E-14, + "pose" : { + "translation" : { + "x" : 2.977381851309474, + "y" : 0.06985017132319626 + }, + "rotation" : { + "radians" : -0.4604096131005984 + } + }, + "curvature" : 0.013923147393524328 + }, { + "time" : 3.4171876919923743, + "velocity" : 3.0, + "acceleration" : -0.04926038986669166, + "pose" : { + "translation" : { + "x" : 3.0367135648982266, + "y" : 0.04044478827985998 + }, + "rotation" : { + "radians" : -0.46003070242576016 + } + }, + "curvature" : -0.0025382237303508883 + }, { + "time" : 3.43902255151928, + "velocity" : 2.99892440630702, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.0953900007672708, + "y" : 0.011352049959658217 + }, + "rotation" : { + "radians" : -0.4607424088911207 + } + }, + "curvature" : -0.019296044170462233 + }, { + "time" : 3.460894301999556, + "velocity" : 2.911437404385916, + "acceleration" : -4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.153261799131896, + "y" : -0.017432242879471982 + }, + "rotation" : { + "radians" : -0.46254275944377354 + } + }, + "curvature" : -0.03655263042494752 + }, { + "time" : 3.483096236841796, + "velocity" : 2.8226296650169553, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.2101883851468074, + "y" : -0.04591304826283249 + }, + "rotation" : { + "radians" : -0.4654349576602489 + } + }, + "curvature" : -0.05449941944261404 + }, { + "time" : 3.5284559791470897, + "velocity" : 2.6411906957957814, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.320693882987083, + "y" : -0.10198790459356033 + }, + "rotation" : { + "radians" : -0.4745255802104961 + } + }, + "curvature" : -0.09314935868314721 + }, { + "time" : 3.5750713755207792, + "velocity" : 2.454729110301023, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 3.4259977320907637, + "y" : -0.1569278776878491 + }, + "rotation" : { + "radians" : -0.488085849209704 + } + }, + "curvature" : -0.13632788196047352 + }, { + "time" : 3.6230020187291068, + "velocity" : 2.2630065374677133, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 3.525399329757196, + "y" : -0.21080134048127874 + }, + "rotation" : { + "radians" : -0.5061441065978821 + } + }, + "curvature" : -0.18426344503119224 + }, { + "time" : 3.6724555712310893, + "velocity" : 2.065192327459783, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.6184400330154176, + "y" : -0.2636901170639021 + }, + "rotation" : { + "radians" : -0.528561557547075 + } + }, + "curvature" : -0.2354062697850754 + }, { + "time" : 3.723886359469485, + "velocity" : 1.8594691745062002, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.7049369588571714, + "y" : -0.3156899195544618 + }, + "rotation" : { + "radians" : -0.5548405336431187 + } + }, + "curvature" : -0.28492058176951146 + }, { + "time" : 3.7781787791469426, + "velocity" : 1.6422994957963697, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 3.785016784469917, + "y" : -0.36691078497460694 + }, + "rotation" : { + "radians" : -0.5838549927785415 + } + }, + "curvature" : -0.32275632767074536 + }, { + "time" : 3.837038685288918, + "velocity" : 1.4068598712284692, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.859149547469844, + "y" : -0.41747751212311 + }, + "rotation" : { + "radians" : -0.6135313524400181 + } + }, + "curvature" : -0.3322823649480369 + }, { + "time" : 3.9040276125177047, + "velocity" : 1.1389041623133211, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 3.9281824461348833, + "y" : -0.46753009845008364 + }, + "rotation" : { + "radians" : -0.6405875389886441 + } + }, + "curvature" : -0.29180251710931904 + }, { + "time" : 3.988546345848521, + "velocity" : 0.800829228990057, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 3.9933736396377206, + "y" : -0.5172241769311974 + }, + "rotation" : { + "radians" : -0.6605456517130821 + } + }, + "curvature" : -0.1820147647738938 + }, { + "time" : 4.188753653096035, + "velocity" : 0.0, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 4.056426048278809, + "y" : -0.5667314529418945 + }, + "rotation" : { + "radians" : -0.6682894149758353 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.807385444641113, -3.022507667541504, 0.0 ], + "y" : [ 2.9820399284362793, -0.10218214988708496, 0.0 ] + }, { + "x" : [ 1.718481421470642, -0.47991621494293213, 0.0 ], + "y" : [ 1.4718265533447266, -2.453189492225647, 0.0 ] + }, { + "x" : [ 4.056426048278809, 2.0053954124450684, 0.0 ], + "y" : [ -0.5667314529418945, -1.5832068920135498, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : -3.127725471555593 + } + }, { + "time" : 2.249999949708581, + "rotation" : { + "radians" : -0.798910986801039 + } + }, { + "time" : 4.099999908357859, + "rotation" : { + "radians" : -0.6659689899224528 + } + } ], + "reversed" : false, + "color" : 192.47057, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 2\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 2.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "@t 2.7\nDrive.setAutoRotation 35", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 2.7, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.setAutoRotation", + "args" : [ "35" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.056426048278809, + "y" : -0.5667314529418945 + }, + "rotation" : { + "radians" : 2.4520537478581654 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.18493870993194386, + "velocity" : 0.7397548397277754, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 4.003633991609843, + "y" : -0.5232320818440126 + }, + "rotation" : { + "radians" : 2.4530901867155106 + } + }, + "curvature" : 0.029752937013657187 + }, { + "time" : 0.26148282578134485, + "velocity" : 1.0459313031253796, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.9508028739061523, + "y" : -0.47987904225311695 + }, + "rotation" : { + "radians" : 2.4560494170073883 + } + }, + "curvature" : 0.05635458173870567 + }, { + "time" : 0.32012947576533146, + "velocity" : 1.2805179030613263, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 3.897901180103951, + "y" : -0.4368061550801574 + }, + "rotation" : { + "radians" : 2.4607189045800832 + } + }, + "curvature" : 0.08009487969635327 + }, { + "time" : 0.36946312936714043, + "velocity" : 1.477852517468562, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.8449053191515077, + "y" : -0.39413398946635425 + }, + "rotation" : { + "radians" : 2.466900851787993 + } + }, + "curvature" : 0.10120927083782304 + }, { + "time" : 0.4128042913826585, + "velocity" : 1.6512171655306345, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.791799285257279, + "y" : -0.35197029855173945 + }, + "rotation" : { + "radians" : 2.474409887161352 + } + }, + "curvature" : 0.1198860351486836 + }, { + "time" : 0.451853631575856, + "velocity" : 1.8074145263034247, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.738574319138042, + "y" : -0.3104104552436979 + }, + "rotation" : { + "radians" : 2.483071079673296 + } + }, + "curvature" : 0.13627342891926963 + }, { + "time" : 0.4876202915697968, + "velocity" : 1.9504811662791879, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.6852285692670304, + "y" : -0.2695378879855088 + }, + "rotation" : { + "radians" : 2.4927182713379636 + } + }, + "curvature" : 0.15048631581735405 + }, { + "time" : 0.5207612575253416, + "velocity" : 2.0830450301013674, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.631766753122065, + "y" : -0.2294245165248867 + }, + "rotation" : { + "radians" : 2.5031927075423317 + } + }, + "curvature" : 0.16261213661992346 + }, { + "time" : 0.5517326643887606, + "velocity" : 2.206930657555043, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 3.5781998184336885, + "y" : -0.1901311876825229 + }, + "rotation" : { + "radians" : 2.514341935981025 + } + }, + "curvature" : 0.1727161388917002 + }, { + "time" : 0.5808665583284028, + "velocity" : 2.3234662333136122, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.5245446044332986, + "y" : -0.15170811112062665 + }, + "rotation" : { + "radians" : 2.526018940159826 + } + }, + "curvature" : 0.18084582936508686 + }, { + "time" : 0.6084135883193114, + "velocity" : 2.4336543532772463, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.470823503101282, + "y" : -0.11419529511146642 + }, + "rotation" : { + "radians" : 2.538081470866874 + } + }, + "curvature" : 0.18703463399877743 + }, { + "time" : 0.6345684464707756, + "velocity" : 2.5382737858831033, + "acceleration" : 3.9999999999999862, + "pose" : { + "translation" : { + "x" : 3.417064120415148, + "y" : -0.07762298230591114 + }, + "rotation" : { + "radians" : 2.5503915380457647 + } + }, + "curvature" : 0.19130476386228212 + }, { + "time" : 0.6594858696162618, + "velocity" : 2.6379434784650475, + "acceleration" : 3.999999999999993, + "pose" : { + "translation" : { + "x" : 3.36329893759766, + "y" : -0.04201208550197144 + }, + "rotation" : { + "radians" : 2.5628150257457696 + } + }, + "curvature" : 0.19366929489891171 + }, { + "time" : 0.6832911528298411, + "velocity" : 2.733164611319365, + "acceleration" : 3.0356954614896483, + "pose" : { + "translation" : { + "x" : 3.3095649723649707, + "y" : -0.007374623413340942 + }, + "rotation" : { + "radians" : 2.575221394062975 + } + }, + "curvature" : 0.1941334791085683 + }, { + "time" : 0.7282945981321051, + "velocity" : 2.8697813659748452, + "acceleration" : 0.35602362885687006, + "pose" : { + "translation" : { + "x" : 3.2023594154743478, + "y" : 0.05897377757355571 + }, + "rotation" : { + "radians" : 2.5994770448888582 + } + }, + "curvature" : 0.18934541322883708 + }, { + "time" : 0.7712234425204774, + "velocity" : 2.8850650489366254, + "acceleration" : 0.5853616532790817, + "pose" : { + "translation" : { + "x" : 3.095821448769353, + "y" : 0.1214873934270031 + }, + "rotation" : { + "radians" : 2.622176713767235 + } + }, + "curvature" : 0.17683058489309955 + }, { + "time" : 0.8129042620605731, + "velocity" : 2.909463402372643, + "acceleration" : 0.8532903307335525, + "pose" : { + "translation" : { + "x" : 2.9903759750508243, + "y" : 0.18034581308165798 + }, + "rotation" : { + "radians" : 2.6423801962411266 + } + }, + "curvature" : 0.156321766875571 + }, { + "time" : 0.8531469929208557, + "velocity" : 2.943802135498035, + "acceleration" : 0.5959644267737211, + "pose" : { + "translation" : { + "x" : 2.886487960834632, + "y" : 0.23582909704128951 + }, + "rotation" : { + "radians" : 2.659177929985617 + } + }, + "curvature" : 0.12732676169969878 + }, { + "time" : 0.8919109189015986, + "velocity" : 2.966904056424647, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 2.784651596291951, + "y" : 0.28830383278545924 + }, + "rotation" : { + "radians" : 2.6716758173179893 + } + }, + "curvature" : 0.0890361198334152 + }, { + "time" : 0.9303572438648844, + "velocity" : 2.813118756571504, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.68537945518953, + "y" : 0.338209190176201 + }, + "rotation" : { + "radians" : 2.6789757213651453 + } + }, + "curvature" : 0.04021801225466915 + }, { + "time" : 0.9696416444039297, + "velocity" : 2.6559811544153225, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 2.5891916548299605, + "y" : 0.3860429768647009 + }, + "rotation" : { + "radians" : 2.680153087048729 + } + }, + "curvature" : -0.02088202984869535 + }, { + "time" : 1.0098343280634516, + "velocity" : 2.4952104197772345, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.4966050159919497, + "y" : 0.4323476936979773 + }, + "rotation" : { + "radians" : 2.6742351029390625 + } + }, + "curvature" : -0.09660379275555367 + }, { + "time" : 1.0510425689247183, + "velocity" : 2.3303774563321675, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.4081222228705883, + "y" : 0.47769659012556076 + }, + "rotation" : { + "radians" : 2.660185719014542 + } + }, + "curvature" : -0.18983275500917693 + }, { + "time" : 1.0934364510155294, + "velocity" : 2.160801927968923, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 2.3242209830176215, + "y" : 0.5226797196061739 + }, + "rotation" : { + "radians" : 2.636908345303076 + } + }, + "curvature" : -0.30373535138801905 + }, { + "time" : 1.1372916096013064, + "velocity" : 1.9853812936258146, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 2.245343187281719, + "y" : 0.5678899950144114 + }, + "rotation" : { + "radians" : 2.6032836790059384 + } + }, + "curvature" : -0.4410606681722093 + }, { + "time" : 1.1830628813303603, + "velocity" : 1.8022962067095987, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.171884069748746, + "y" : 0.6139092440474201 + }, + "rotation" : { + "radians" : 2.5582681034202466 + } + }, + "curvature" : -0.6027205654193771 + }, { + "time" : 1.2315199026124195, + "velocity" : 1.6084681215813614, + "acceleration" : -3.2340381044624498, + "pose" : { + "translation" : { + "x" : 2.104181367682031, + "y" : 0.6612942646315787 + }, + "rotation" : { + "radians" : 2.501083096666535 + } + }, + "curvature" : -0.7854142700643647 + }, { + "time" : 1.2832918933759534, + "velocity" : 1.441035530708215, + "acceleration" : -2.161144532997304, + "pose" : { + "translation" : { + "x" : 2.0425044814626396, + "y" : 0.7105628803291779 + }, + "rotation" : { + "radians" : 2.431516535185916 + } + }, + "curvature" : -0.978530406846595 + }, { + "time" : 1.3381225717901073, + "velocity" : 1.322538509812933, + "acceleration" : -1.509248244590621, + "pose" : { + "translation" : { + "x" : 1.9870436345296412, + "y" : 0.7621799957451003 + }, + "rotation" : { + "radians" : 2.3503134945481206 + } + }, + "curvature" : -1.161734937054165 + }, { + "time" : 1.3664915633080867, + "velocity" : 1.2797226591636164, + "acceleration" : -1.1263537161568526, + "pose" : { + "translation" : { + "x" : 1.961679353004223, + "y" : 0.7889966218317799 + }, + "rotation" : { + "radians" : 2.3059373339142457 + } + }, + "curvature" : -1.24077199916201 + }, { + "time" : 1.3952936681311403, + "velocity" : 1.2472813013630308, + "acceleration" : -0.7667596602916469, + "pose" : { + "translation" : { + "x" : 1.937899033320381, + "y" : 0.8165436519335003 + }, + "rotation" : { + "radians" : 2.2595451461663565 + } + }, + "curvature" : -1.306155490343675 + }, { + "time" : 1.424400522054752, + "velocity" : 1.2249633399364037, + "acceleration" : -0.42132507119357915, + "pose" : { + "translation" : { + "x" : 1.9156993576789647, + "y" : 0.8448581059673881 + }, + "rotation" : { + "radians" : 2.2116259567064747 + } + }, + "curvature" : -1.3541835082656821 + }, { + "time" : 1.4536761435963022, + "velocity" : 1.2126287866061738, + "acceleration" : -0.08170141033781976, + "pose" : { + "translation" : { + "x" : 1.8950700272107497, + "y" : 0.8739710818044841 + }, + "rotation" : { + "radians" : 2.162745065781283 + } + }, + "curvature" : -1.3818724424188715 + }, { + "time" : 1.4829785112196618, + "velocity" : 1.210234741845108, + "acceleration" : 0.26045790812859687, + "pose" : { + "translation" : { + "x" : 1.8759934232245703, + "y" : 0.9039073195012026 + }, + "rotation" : { + "radians" : 2.113522302547328 + } + }, + "curvature" : -1.387344994997751 + }, { + "time" : 1.5121614711143974, + "velocity" : 1.2178356745322916, + "acceleration" : 0.6143648008907316, + "pose" : { + "translation" : { + "x" : 1.8584442684554539, + "y" : 0.9346847655307897 + }, + "rotation" : { + "radians" : 2.064604229359281 + } + }, + "curvature" : -1.3700812404747664 + }, { + "time" : 1.5410766974782304, + "velocity" : 1.2356001718200182, + "acceleration" : 0.9912142506014653, + "pose" : { + "translation" : { + "x" : 1.8423892883127544, + "y" : 0.9663141370147823 + }, + "rotation" : { + "radians" : 2.0166336726291316 + } + }, + "curvature" : -1.3309685196438428 + }, { + "time" : 1.5695753786663202, + "velocity" : 1.2638484707370008, + "acceleration" : 1.6439611484263257, + "pose" : { + "translation" : { + "x" : 1.8277868721282857, + "y" : 0.9987984859544667 + }, + "rotation" : { + "radians" : 1.9702206136785245 + } + }, + "curvature" : -1.2721364283154866 + }, { + "time" : 1.6245807356379516, + "velocity" : 1.3542751405536837, + "acceleration" : 2.798992607878158, + "pose" : { + "translation" : { + "x" : 1.8027295760623936, + "y" : 1.0663033839935565 + }, + "rotation" : { + "radians" : 1.8842065669188885 + } + }, + "curvature" : -1.1079237968679712 + }, { + "time" : 1.676119381405686, + "velocity" : 1.4985314290776237, + "acceleration" : 3.999999999999997, + "pose" : { + "translation" : { + "x" : 1.782759900790552, + "y" : 1.137054014048772 + }, + "rotation" : { + "radians" : 1.8100759100562795 + } + }, + "curvature" : -0.904882088448944 + }, { + "time" : 1.7234002817652252, + "velocity" : 1.6876550305157805, + "acceleration" : 3.9999999999999885, + "pose" : { + "translation" : { + "x" : 1.7672103094854315, + "y" : 1.2107543994101775 + }, + "rotation" : { + "radians" : 1.7501344274699886 + } + }, + "curvature" : -0.6861541138888728 + }, { + "time" : 1.7668603468362696, + "velocity" : 1.8614952907999578, + "acceleration" : -2.6118583184812723, + "pose" : { + "translation" : { + "x" : 1.7552473678998695, + "y" : 1.2869440876638691 + }, + "rotation" : { + "radians" : 1.7057998025006624 + } + }, + "curvature" : -0.4641078115451259 + }, { + "time" : 1.8104168065342312, + "velocity" : 1.7477319892142427, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 1.7458609043071398, + "y" : 1.3649842060986543 + }, + "rotation" : { + "radians" : 1.6781427516970726 + } + }, + "curvature" : -0.23909570910678915 + }, { + "time" : 1.858532964177138, + "velocity" : 1.5552673586426153, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.7378531694412231, + "y" : 1.444043517112732 + }, + "rotation" : { + "radians" : 1.668497763241333 + } + }, + "curvature" : -0.0 + }, { + "time" : 1.9135617600953347, + "velocity" : 1.3351521749698287, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 1.731150510518038, + "y" : 1.5232887159155766 + }, + "rotation" : { + "radians" : 1.6290693879713087 + } + }, + "curvature" : -0.9651764780400796 + }, { + "time" : 1.9447141490181088, + "velocity" : 1.2105426192787325, + "acceleration" : -3.8758391033227886, + "pose" : { + "translation" : { + "x" : 1.7297123504678038, + "y" : 1.5629148639371933 + }, + "rotation" : { + "radians" : 1.582290663183503 + } + }, + "curvature" : -1.3866393975350608 + }, { + "time" : 1.9793752955290544, + "velocity" : 1.0762015922656092, + "acceleration" : -2.0744901253079546, + "pose" : { + "translation" : { + "x" : 1.7304462942776127, + "y" : 1.6025386552453256 + }, + "rotation" : { + "radians" : 1.5198280388303091 + } + }, + "curvature" : -1.7544316082551803 + }, { + "time" : 2.0177456093542387, + "velocity" : 0.9966027551302967, + "acceleration" : -1.0463551078296154, + "pose" : { + "translation" : { + "x" : 1.7339390283538525, + "y" : 1.6421520518968291 + }, + "rotation" : { + "radians" : 1.4439430669964353 + } + }, + "curvature" : -2.0458770787946707 + }, { + "time" : 2.0589400093906947, + "velocity" : 0.9534987842381746, + "acceleration" : -0.4880905611008773, + "pose" : { + "translation" : { + "x" : 1.7407113486278938, + "y" : 1.6817436414219813 + }, + "rotation" : { + "radians" : 1.3575512036532698 + } + }, + "curvature" : -2.2350303059988965 + }, { + "time" : 2.0803982879163243, + "velocity" : 0.9430252010323411, + "acceleration" : -0.17603090665285, + "pose" : { + "translation" : { + "x" : 1.7454726503711389, + "y" : 1.70152681499276 + }, + "rotation" : { + "radians" : 1.3115071881305223 + } + }, + "curvature" : -2.2849521300818747 + }, { + "time" : 2.1022775055735607, + "velocity" : 0.9391737825112827, + "acceleration" : 0.10804204147343682, + "pose" : { + "translation" : { + "x" : 1.751220456348885, + "y" : 1.7212988221176222 + }, + "rotation" : { + "radians" : 1.2642073316351676 + } + }, + "curvature" : -2.3037310856346584 + }, { + "time" : 2.124492132931374, + "velocity" : 0.9415738962015925, + "acceleration" : 0.3726592944849579, + "pose" : { + "translation" : { + "x" : 1.7580023567016838, + "y" : 1.7410573696656044 + }, + "rotation" : { + "radians" : 1.2161486950907356 + } + }, + "curvature" : -2.2920014286625774 + }, { + "time" : 2.146960488972026, + "velocity" : 0.9499469379119386, + "acceleration" : 0.6237338591278402, + "pose" : { + "translation" : { + "x" : 1.7658622538765485, + "y" : 1.7607999883402954 + }, + "rotation" : { + "radians" : 1.1678204422268519 + } + }, + "curvature" : -2.2517750837082873 + }, { + "time" : 2.1696053252037935, + "velocity" : 0.9640712890040968, + "acceleration" : 0.8654335988347971, + "pose" : { + "translation" : { + "x" : 1.7748404343704771, + "y" : 1.780524038470248 + }, + "rotation" : { + "radians" : 1.119686904181982 + } + }, + "curvature" : -2.1862781024111237 + }, { + "time" : 2.192354484026944, + "velocity" : 0.9837591753948806, + "acceleration" : 1.1007322784603935, + "pose" : { + "translation" : { + "x" : 1.7849736404739778, + "y" : 1.8002267157993888 + }, + "rotation" : { + "radians" : 1.0721723951266355 + } + }, + "curvature" : -2.0996461580977543 + }, { + "time" : 2.215141507868605, + "velocity" : 1.008841588067443, + "acceleration" : 1.3317674781680595, + "pose" : { + "translation" : { + "x" : 1.796295142014595, + "y" : 1.819905057277429 + }, + "rotation" : { + "radians" : 1.025649276755208 + } + }, + "curvature" : -1.996538776374713 + }, { + "time" : 2.2379061099913464, + "velocity" : 1.0391587448279456, + "acceleration" : 1.6754933878837617, + "pose" : { + "translation" : { + "x" : 1.808834808100432, + "y" : 1.8395559468502753 + }, + "rotation" : { + "radians" : 0.9804301798041166 + } + }, + "curvature" : -1.8817412716281585 + }, { + "time" : 2.2830950022778635, + "velocity" : 1.1148724350597967, + "acceleration" : 2.127602917951782, + "pose" : { + "translation" : { + "x" : 1.8376715372041317, + "y" : 1.8787621757874542 + }, + "rotation" : { + "radians" : 0.8948396405471469 + } + }, + "curvature" : -1.6348328210818963 + }, { + "time" : 2.3276395725958383, + "velocity" : 1.2096455928472285, + "acceleration" : 2.5789601207010184, + "pose" : { + "translation" : { + "x" : 1.8716574925150553, + "y" : 1.9178176341376911 + }, + "rotation" : { + "radians" : 0.8166729724145454 + } + }, + "curvature" : -1.3886967164626418 + }, { + "time" : 2.371283333319735, + "velocity" : 1.3222011112715757, + "acceleration" : 3.0313394226052046, + "pose" : { + "translation" : { + "x" : 1.9109165188377353, + "y" : 1.9566924759531596 + }, + "rotation" : { + "radians" : 0.7463747869451778 + } + }, + "curvature" : -1.1623279144475585 + }, { + "time" : 2.4138523222192685, + "velocity" : 1.4512421655031749, + "acceleration" : 3.4847988479105942, + "pose" : { + "translation" : { + "x" : 1.9555249368440535, + "y" : 1.9953549631045817 + }, + "rotation" : { + "radians" : 0.683796860013437 + } + }, + "curvature" : -0.9648147341420715 + }, { + "time" : 2.455238313142531, + "velocity" : 1.5954640189921991, + "acceleration" : 3.93825151072416, + "pose" : { + "translation" : { + "x" : 2.0055138388660367, + "y" : 2.0337716505743684 + }, + "rotation" : { + "radians" : 0.6284298787308873 + } + }, + "curvature" : -0.7982697547383403 + }, { + "time" : 2.49538256040239, + "velocity" : 1.7535621614102237, + "acceleration" : 4.000000000000021, + "pose" : { + "translation" : { + "x" : 2.060871384688653, + "y" : 2.0719075717497617 + }, + "rotation" : { + "radians" : 0.5795857801198268 + } + }, + "curvature" : -0.6608172515589942 + }, { + "time" : 2.53441622407071, + "velocity" : 1.9096968160835055, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.121545097342608, + "y" : 2.1097264237159736 + }, + "rotation" : { + "radians" : 0.5365211642557431 + } + }, + "curvature" : -0.5487920239178083 + }, { + "time" : 2.5725848078202302, + "velocity" : 2.062371151081586, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.18744415889714, + "y" : 2.147190752549328 + }, + "rotation" : { + "radians" : 0.4985109954494367 + } + }, + "curvature" : -0.45807197306180775 + }, { + "time" : 2.6100585451602245, + "velocity" : 2.212266100441563, + "acceleration" : 3.9999999999999893, + "pose" : { + "translation" : { + "x" : 2.258441706252816, + "y" : 2.1842621386104017 + }, + "rotation" : { + "radians" : 0.4648869468393921 + } + }, + "curvature" : -0.3847621795776758 + }, { + "time" : 2.646940241630883, + "velocity" : 2.359792886324197, + "acceleration" : 4.00000000000001, + "pose" : { + "translation" : { + "x" : 2.334377126934328, + "y" : 2.220901381837166 + }, + "rotation" : { + "radians" : 0.43505326862724697 + } + }, + "curvature" : -0.32547741802200536 + }, { + "time" : 2.683288527639372, + "velocity" : 2.505186030358152, + "acceleration" : 4.000000000000014, + "pose" : { + "translation" : { + "x" : 2.415058354883288, + "y" : 2.2570686870381245 + }, + "rotation" : { + "radians" : 0.4084896240286113 + } + }, + "curvature" : -0.2774076507783822 + }, { + "time" : 2.719132405668665, + "velocity" : 2.6485615424753246, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 2.500264166251025, + "y" : 2.2927238491854567 + }, + "rotation" : { + "radians" : 0.384747078396271 + } + }, + "curvature" : -0.23828130083939184 + }, { + "time" : 2.754480704601807, + "velocity" : 2.789954738207894, + "acceleration" : 2.9488324318429173, + "pose" : { + "translation" : { + "x" : 2.5897464751913803, + "y" : 2.3278264387081586 + }, + "rotation" : { + "radians" : 0.3634409880883115 + } + }, + "curvature" : -0.20628975468482194 + }, { + "time" : 2.7895490251950776, + "velocity" : 2.8933653393035956, + "acceleration" : 0.7037966720327286, + "pose" : { + "translation" : { + "x" : 2.683232629653503, + "y" : 2.362335986785183 + }, + "rotation" : { + "radians" : 0.34424291191521855 + } + }, + "curvature" : -0.18000474642406875 + }, { + "time" : 2.8249707390558063, + "velocity" : 2.918295023636472, + "acceleration" : 0.5763378030245723, + "pose" : { + "translation" : { + "x" : 2.7804277071746464, + "y" : 2.39621217063858 + }, + "rotation" : { + "radians" : 0.32687266278130106 + } + }, + "curvature" : -0.15830253678418688 + }, { + "time" : 2.861139234917162, + "velocity" : 2.939140295079909, + "acceleration" : 0.4740600472954994, + "pose" : { + "translation" : { + "x" : 2.881016810672964, + "y" : 2.429414998826637 + }, + "rotation" : { + "radians" : 0.31109102748658707 + } + }, + "curvature" : -0.1402996898246936 + }, { + "time" : 2.8979872681123133, + "velocity" : 2.9566084754391486, + "acceleration" : 0.3917399621578322, + "pose" : { + "translation" : { + "x" : 2.984667364240305, + "y" : 2.4619049965370223 + }, + "rotation" : { + "radians" : 0.2966933532809512 + } + }, + "curvature" : -0.1253009626312453 + }, { + "time" : 2.935436818258982, + "velocity" : 2.9712789607964325, + "acceleration" : 0.3251659903474371, + "pose" : { + "translation" : { + "x" : 3.0910314089350095, + "y" : 2.4936433908799245 + }, + "rotation" : { + "radians" : 0.28350402582744755 + } + }, + "curvature" : -0.11275805306566417 + }, { + "time" : 2.9734007910904325, + "velocity" : 2.9836235536196942, + "acceleration" : 0.2709852112638229, + "pose" : { + "translation" : { + "x" : 3.199747898574706, + "y" : 2.5245922961811917 + }, + "rotation" : { + "radians" : 0.2713717786654788 + } + }, + "curvature" : -0.10223741084606355 + }, { + "time" : 3.0117845646161134, + "velocity" : 2.9940249885976535, + "acceleration" : 0.1543098408303897, + "pose" : { + "translation" : { + "x" : 3.3104449955291066, + "y" : 2.5547148992754742 + }, + "rotation" : { + "radians" : 0.2601657392085518 + } + }, + "curvature" : -0.09339533006984713 + }, { + "time" : 3.0505054353667576, + "velocity" : 2.9999999999999996, + "acceleration" : 7.591176208555741E-15, + "pose" : { + "translation" : { + "x" : 3.422742366512802, + "y" : 2.5839756447993656 + }, + "rotation" : { + "radians" : 0.24977210747664394 + } + }, + "curvature" : -0.08595876803031433 + }, { + "time" : 3.0895059097464324, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.5362534783780575, + "y" : 2.612340420484543 + }, + "rotation" : { + "radians" : 0.24009136790995844 + } + }, + "curvature" : -0.07971061642849785 + }, { + "time" : 3.1286993195347663, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.650587893907611, + "y" : 2.6397767424509064 + }, + "rotation" : { + "radians" : 0.23103594442528902 + } + }, + "curvature" : -0.07447841843016513 + }, { + "time" : 3.16795942350856, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.765353567607466, + "y" : 2.666253940499722 + }, + "rotation" : { + "radians" : 0.22252822046257315 + } + }, + "curvature" : -0.07012575415648367 + }, { + "time" : 3.2071598053906003, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.8801591414996897, + "y" : 2.6917433434067615 + }, + "rotation" : { + "radians" : 0.21449885716390527 + } + }, + "curvature" : -0.06654570394142899 + }, { + "time" : 3.2461747016130165, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.9946162409152066, + "y" : 2.7162184642154443 + }, + "rotation" : { + "radians" : 0.20688535313560402 + } + }, + "curvature" : -0.06365594712905223 + }, { + "time" : 3.2848798203338982, + "velocity" : 3.0, + "acceleration" : -7.735398279518994E-15, + "pose" : { + "translation" : { + "x" : 4.1083417702865965, + "y" : 2.7396551855299762 + }, + "rotation" : { + "radians" : 0.19963079814758483 + } + }, + "curvature" : -0.06139517108104972 + }, { + "time" : 3.32315315320381, + "velocity" : 2.9999999999999996, + "acceleration" : 7.84832578379943E-15, + "pose" : { + "translation" : { + "x" : 4.220960208940891, + "y" : 2.7620319448084913 + }, + "rotation" : { + "radians" : 0.19268278060685093 + } + }, + "curvature" : -0.05972055737308964 + }, { + "time" : 3.3608757811105803, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.332105906892365, + "y" : 2.783329919656194 + }, + "rotation" : { + "radians" : 0.18599241484195372 + } + }, + "curvature" : -0.0586061860352566 + }, { + "time" : 3.3979326749154137, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.441425380635337, + "y" : 2.8035332131184987 + }, + "rotation" : { + "radians" : 0.17951345937534083 + } + }, + "curvature" : -0.05804225916920248 + }, { + "time" : 3.43421349202006, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.548579608936964, + "y" : 2.8226290389741697 + }, + "rotation" : { + "radians" : 0.17320150172377563 + } + }, + "curvature" : -0.058035096135246704 + }, { + "time" : 3.4696133694646405, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.653246328630036, + "y" : 2.8406079070284633 + }, + "rotation" : { + "radians" : 0.16701318920823915 + } + }, + "curvature" : -0.05860789609869524 + }, { + "time" : 3.5040337141415066, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.755122330405774, + "y" : 2.857463808406269 + }, + "rotation" : { + "radians" : 0.16090548924261228 + } + }, + "curvature" : -0.05980230049917739 + }, { + "time" : 3.5373829906159067, + "velocity" : 3.0, + "acceleration" : -1.3000955620508416, + "pose" : { + "translation" : { + "x" : 4.853925754606621, + "y" : 2.87319440084525 + }, + "rotation" : { + "radians" : 0.15483496726687765 + } + }, + "curvature" : -0.06168081541885887 + }, { + "time" : 3.5698052848244366, + "velocity" : 2.9578479191879836, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 4.9493983870190466, + "y" : 2.8878011939889827 + }, + "rotation" : { + "radians" : 0.14875707688110845 + } + }, + "curvature" : -0.0643301641863426 + }, { + "time" : 3.601908104387069, + "velocity" : 2.8294366409374536, + "acceleration" : -3.999999999999995, + "pose" : { + "translation" : { + "x" : 5.041307954666333, + "y" : 2.9012897346800983 + }, + "rotation" : { + "radians" : 0.1426254663359473 + } + }, + "curvature" : -0.06786561693146706 + }, { + "time" : 3.634098277881151, + "velocity" : 2.700675946961127, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.12945042160138, + "y" : 2.913669792253425 + }, + "rotation" : { + "radians" : 0.13639132070467594 + } + }, + "curvature" : -0.07243625256641337 + }, { + "time" : 3.6663242273354184, + "velocity" : 2.5717721491440564, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.213652284699492, + "y" : 2.924955543829128 + }, + "rotation" : { + "radians" : 0.13000278348350505 + } + }, + "curvature" : -0.07823088630580577 + }, { + "time" : 3.6985369799811316, + "velocity" : 2.4429211385612026, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 5.293772869451182, + "y" : 2.935165759605849 + }, + "rotation" : { + "radians" : 0.12340454072534943 + } + }, + "curvature" : -0.08548392661118162 + }, { + "time" : 3.730691893404645, + "velocity" : 2.314301484867149, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.3697066257549615, + "y" : 2.9443239881538474 + }, + "rotation" : { + "radians" : 0.11653771369919076 + } + }, + "curvature" : -0.09447950544669352 + }, { + "time" : 3.7627510266183024, + "velocity" : 2.1860649520125195, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.441385423710139, + "y" : 2.952458741708144 + }, + "rotation" : { + "radians" : 0.10934030498899684 + } + }, + "curvature" : -0.10555051064633172 + }, { + "time" : 3.7946864190689498, + "velocity" : 2.058323382209931, + "acceleration" : -3.9999999999999982, + "pose" : { + "translation" : { + "x" : 5.508780849409618, + "y" : 2.959603681461658 + }, + "rotation" : { + "radians" : 0.10174859495289124 + } + }, + "curvature" : -0.1190660934873409 + }, { + "time" : 3.8581528018765208, + "velocity" : 1.8044578509796478, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.630820283137819, + "y" : 2.971085620886363 + }, + "rotation" : { + "radians" : 0.08513910857652777 + } + }, + "curvature" : -0.15483239704424728 + }, { + "time" : 3.9212966580627038, + "velocity" : 1.5518824262349158, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.73647917568087, + "y" : 2.9791491162686725 + }, + "rotation" : { + "radians" : 0.0663518340621249 + } + }, + "curvature" : -0.20277252875109172 + }, { + "time" : 3.985088994027884, + "velocity" : 1.296713082374195, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.827194162416493, + "y" : 2.98426771953671 + }, + "rotation" : { + "radians" : 0.045585623619075005 + } + }, + "curvature" : -0.2546275827711898 + }, { + "time" : 4.0522933348683985, + "velocity" : 1.027895719012139, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.9052577307282945, + "y" : 2.987015154086521 + }, + "rotation" : { + "radians" : 0.02445962652264226 + } + }, + "curvature" : -0.27725843604659994 + }, { + "time" : 4.131181440085891, + "velocity" : 0.7123432981421679, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.973891685375232, + "y" : 2.988071244162576 + }, + "rotation" : { + "radians" : 0.007123366020833881 + } + }, + "curvature" : -0.20476533171945074 + }, { + "time" : 4.309267264621433, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.037320613861084, + "y" : 2.9882278442382812 + }, + "rotation" : { + "radians" : 0.0 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.056426048278809, -3.3782451152801514, 0.0 ], + "y" : [ -0.5667314529418945, 2.785569906234741, 0.0 ] + }, { + "x" : [ 1.7378531694412231, -0.24853384494781494, 0.0 ], + "y" : [ 1.444043517112732, 2.535710334777832, 0.0 ] + }, { + "x" : [ 6.037320613861084, 2.0, 0.0 ], + "y" : [ 2.9882278442382812, 0.0, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.2873733127254927 + } + }, { + "time" : 1.8499999586492777, + "rotation" : { + "radians" : 2.3228735180023055 + } + }, { + "time" : 4.21999990567565, + "rotation" : { + "radians" : 0.6128208180837543 + } + } ], + "reversed" : false, + "color" : 124.23529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + } ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/blue/hideballs.json b/src/main/deploy/autos/blue/hidecargo.json similarity index 77% rename from src/main/deploy/autos/blue/hideballs.json rename to src/main/deploy/autos/blue/hidecargo.json index 7d69ec29..9932a2f5 100644 --- a/src/main/deploy/autos/blue/hideballs.json +++ b/src/main/deploy/autos/blue/hidecargo.json @@ -2109,1057 +2109,982 @@ }, "curvature" : 0.0 }, { - "time" : 0.1839004373900963, - "velocity" : 0.7356017495603853, - "acceleration" : 3.999999999999999, - "pose" : { - "translation" : { - "x" : 4.079839881543424, - "y" : -0.9175061170373492 - }, - "rotation" : { - "radians" : 0.032597811527258144 - } - }, - "curvature" : 0.05820539136138801 - }, { - "time" : 0.2598794478276359, - "velocity" : 1.0395177913105436, - "acceleration" : 4.000000000000002, + "time" : 0.18393581920253443, + "velocity" : 0.7357432768101378, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 4.147234073981139, - "y" : -0.9151343674395811 + "x" : 4.079866022458708, + "y" : -0.9175092776647826 }, "rotation" : { - "radians" : 0.03836566081266323 + "radians" : 0.032422495075255596 } }, - "curvature" : 0.11233457015103718 + "curvature" : 0.05303783852412645 }, { - "time" : 0.31790134200340625, - "velocity" : 1.2716053680136252, - "acceleration" : 3.9999999999999982, + "time" : 0.2600740573066539, + "velocity" : 1.0402962292266158, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 4.2142208005905895, - "y" : -0.912271579263629 + "x" : 4.147437401812738, + "y" : -0.9151584665515866 }, "rotation" : { - "radians" : 0.047629178342962146 + "radians" : 0.03767860106119578 } }, - "curvature" : 0.1636754126870864 + "curvature" : 0.10177004653056979 }, { - "time" : 0.3664818052070989, - "velocity" : 1.4659272208283958, - "acceleration" : 4.0000000000000036, + "time" : 0.31842255095187355, + "velocity" : 1.2736902038074942, + "acceleration" : 3.9999999999999902, "pose" : { "translation" : { - "x" : 4.280620524327787, - "y" : -0.9087076362808375 + "x" : 4.214887726988865, + "y" : -0.9123489750712632 }, "rotation" : { - "radians" : 0.060164770303775844 + "radians" : 0.046083444431288076 } }, - "curvature" : 0.2132011191407471 + "curvature" : 0.14663006175024168 }, { - "time" : 0.40891181812753247, - "velocity" : 1.6356472725101303, - "acceleration" : 4.0, + "time" : 0.36752120065207927, + "velocity" : 1.4700848026083166, + "acceleration" : 3.9999999999999933, "pose" : { "translation" : { - "x" : 4.346269665970026, - "y" : -0.9042556640216365 + "x" : 4.28215626266865, + "y" : -0.9088819079360633 }, "rotation" : { - "radians" : 0.07578809002213045 + "radians" : 0.05737092549763 } }, - "curvature" : 0.261623846845801 + "curvature" : 0.18797575110518114 }, { - "time" : 0.44687903311679705, - "velocity" : 1.7875161324671887, - "acceleration" : 4.0, + "time" : 0.41067486026855743, + "velocity" : 1.642699441074229, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.411020111290583, - "y" : -0.8987512250670413 + "x" : 4.349182263537044, + "y" : -0.904578393853863 }, "rotation" : { - "radians" : 0.09434193350703354 + "radians" : 0.0712929378915065 } }, - "curvature" : 0.30942902122153493 + "curvature" : 0.22608984719455819 }, { - "time" : 0.4813894477163118, - "velocity" : 1.9255577908652477, - "acceleration" : 3.9999999999999964, + "time" : 0.4495779246156969, + "velocity" : 1.7983116984627867, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.474738718233415, - "y" : -0.892051514340153 + "x" : 4.415905077562968, + "y" : -0.8992789224692164 }, "rotation" : { - "radians" : 0.11568561342262434 + "radians" : 0.08761614166764932 } }, - "curvature" : 0.3568953774098475 + "curvature" : 0.2611858070311432 }, { - "time" : 0.5131039023083906, - "velocity" : 2.052415609233563, - "acceleration" : -0.5080487315100024, + "time" : 0.48523766875483854, + "velocity" : 1.940950675019353, + "acceleration" : 3.9999999999999867, "pose" : { "translation" : { - "x" : 4.537306824087864, - "y" : -0.8840345543976582 + "x" : 4.482264249279902, + "y" : -0.8928426813044079 }, "rotation" : { - "radians" : 0.13968544333222133 + "radians" : 0.10611908836229655 } }, - "curvature" : 0.404105267004568 + "curvature" : 0.29341692608996234 }, { - "time" : 0.5734741207486527, - "velocity" : 2.021744596334006, - "acceleration" : -2.5986497425654487, + "time" : 0.5183117595380216, + "velocity" : 2.0732470381520844, + "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.658586321464071, - "y" : -0.8636602870095267 + "x" : 4.548199623066466, + "y" : -0.8851468927005044 }, "rotation" : { - "radians" : 0.19510294768348427 + "radians" : 0.12658982254096487 } }, - "curvature" : 0.4971313158757367 + "curvature" : 0.32288731915441804 }, { - "time" : 0.6345428990288139, - "velocity" : 1.8630482313774788, - "acceleration" : -1.782825751736676, + "time" : 0.5492586656367637, + "velocity" : 2.197034662547053, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.774180161280128, - "y" : -0.8370385771048632 + "x" : 4.613651446427005, + "y" : -0.876086150758407 }, "rotation" : { - "radians" : 0.2593591749925202 + "radians" : 0.14882402397746977 } }, - "curvature" : 0.5854307189034433 + "curvature" : 0.3496638342427625 }, { - "time" : 0.6978369262592252, - "velocity" : 1.750206009699979, - "acceleration" : -1.1573812389052407, + "time" : 0.5784138965550384, + "velocity" : 2.3136555862201518, + "acceleration" : -1.6236735964798095, "pose" : { "translation" : { - "x" : 4.883610030103824, - "y" : -0.8038613196792674 + "x" : 4.678560473272171, + "y" : -0.8655717582799038 }, "rotation" : { - "radians" : 0.33087764946357995 + "radians" : 0.17262371101994242 } }, - "curvature" : 0.6633540150470494 + "curvature" : 0.3737882085974883 }, { - "time" : 0.7622830361592594, - "velocity" : 1.6756172911812541, - "acceleration" : -0.6206912248410478, + "time" : 0.6069779983301187, + "velocity" : 2.267276808360792, + "acceleration" : -1.8021731001922658, "pose" : { "translation" : { - "x" : 4.986581972800195, - "y" : -0.7640763998497277 + "x" : 4.742868067199511, + "y" : -0.8535310637087223 }, "rotation" : { - "radians" : 0.40763331940017733 + "radians" : 0.19779649220341822 } }, - "curvature" : 0.7237257699378088 + "curvature" : 0.3952888936073791 }, { - "time" : 0.8268495770863082, - "velocity" : 1.6355414058094946, - "acceleration" : -0.1141447568431195, + "time" : 0.6360217316741164, + "velocity" : 2.214934973399082, + "acceleration" : -1.4376529363116377, "pose" : { "translation" : { - "x" : 5.0829706221219055, - "y" : -0.7178619421826262 + "x" : 4.80651630477405, + "y" : -0.8399067980715813 }, "rotation" : { - "radians" : 0.48716997806870804 + "radians" : 0.22415532600922183 } }, - "curvature" : 0.7596273921140029 + "curvature" : 0.41419203433477625 }, { - "time" : 0.890535049243441, - "velocity" : 1.6282720430756794, - "acceleration" : 0.4095450005884974, + "time" : 0.6655393241600903, + "velocity" : 2.1724989198887714, + "acceleration" : -1.1396966079827222, "pose" : { "translation" : { - "x" : 5.1728034282996305, - "y" : -0.6656005600217441 + "x" : 4.869448078808871, + "y" : -0.824656411919243 }, "rotation" : { - "radians" : 0.5667524438791094 + "radians" : 0.25151872624934885 } }, - "curvature" : 0.7664251916360058 + "curvature" : 0.4305311361191539 }, { - "time" : 0.9523748257700176, - "velocity" : 1.6535982143896488, - "acceleration" : 1.0079964321999564, + "time" : 0.6954246504047454, + "velocity" : 2.138438714939281, + "acceleration" : -0.8908554566969, "pose" : { "translation" : { - "x" : 5.2562448886324376, - "y" : -0.6078536048162668 + "x" : 4.931607201645704, + "y" : -0.8077514122675655 }, "rotation" : { - "radians" : 0.6436149831528346 + "radians" : 0.2797113326941941 } }, - "curvature" : 0.7431281524527987 + "curvature" : 0.4443550158057545 }, { - "time" : 1.011449874872387, - "velocity" : 1.7131456531168745, - "acceleration" : 1.7790042074143708, + "time" : 0.725580967378834, + "velocity" : 2.111573795409033, + "acceleration" : -0.6793465022751951, "pose" : { "translation" : { - "x" : 5.3335807770781685, - "y" : -0.5453354154487897 + "x" : 4.992938508435509, + "y" : -0.7891766995385554 }, "rotation" : { - "radians" : 0.7152240168950873 + "radians" : 0.3085647538618645 } }, - "curvature" : 0.6923650331342213 + "curvature" : 0.45573373321158034 }, { - "time" : 1.066883029265564, - "velocity" : 1.8117614680125869, - "acceleration" : 2.9290544621119077, + "time" : 0.7559198081982444, + "velocity" : 2.0909632100152824, + "acceleration" : -0.4179681793139011, "pose" : { "translation" : { - "x" : 5.405202373843821, - "y" : -0.47888756756332285 + "x" : 5.053387960419059, + "y" : -0.7689299045014195 }, "rotation" : { - "radians" : 0.7794678142970032 + "radians" : 0.33791858215085874 } }, - "curvature" : 0.6190442095943712 + "curvature" : 0.4647623301686379 }, { - "time" : 1.1178097408882126, - "velocity" : 1.960928579931592, - "acceleration" : 4.000000000000004, + "time" : 0.8167858290266697, + "velocity" : 2.0655231501075435, + "acceleration" : -0.1384063526866644, "pose" : { "translation" : { - "x" : 5.47159069497593, - "y" : -0.4094531228932965 + "x" : 5.171431395063053, + "y" : -0.7234702639619126 }, "rotation" : { - "radians" : 0.8347237020762335 + "radians" : 0.39753225232864264 } }, - "curvature" : 0.5284454355848308 + "curvature" : 0.47628134337229905 }, { - "time" : 1.163781325410918, - "velocity" : 2.144814918022414, - "acceleration" : 4.0, + "time" : 0.8775052043910777, + "velocity" : 2.0571192028259433, + "acceleration" : 0.07651708161584311, "pose" : { "translation" : { - "x" : 5.533300721950951, - "y" : -0.3380508785895664 + "x" : 5.28533164196233, + "y" : -0.6715829475066926 }, "rotation" : { - "radians" : 0.879806448814223 + "radians" : 0.457468911728272 } }, - "curvature" : 0.4245631719643485 + "curvature" : 0.4801807958619656 }, { - "time" : 1.2052874260275384, - "velocity" : 2.3108393204888955, - "acceleration" : 0.9499793055242893, + "time" : 0.9376075969896146, + "velocity" : 2.061718062505713, + "acceleration" : 0.23719713417019256, "pose" : { "translation" : { - "x" : 5.59094563126564, - "y" : -0.265749616548419 + "x" : 5.39470746489593, + "y" : -0.6136396617759576 }, "rotation" : { - "radians" : 0.9138373619233752 + "radians" : 0.51683377374029 } }, - "curvature" : 0.3090988446696365 + "curvature" : 0.47804100648283726 }, { - "time" : 1.2440241708410498, - "velocity" : 2.3476384264251067, - "acceleration" : -4.000000000000005, + "time" : 0.9967126677163254, + "velocity" : 2.0757376158970153, + "acceleration" : 0.3466981374198625, "pose" : { "translation" : { - "x" : 5.645181024027437, - "y" : -0.19364235273957675 + "x" : 5.49920556001598, + "y" : -0.5501521453061287 }, "rotation" : { - "radians" : 0.93608126262559 + "radians" : 0.5749352590162321 } }, - "curvature" : 0.18105135742527462 + "curvature" : 0.471605425281159 }, { - "time" : 1.2825935410962108, - "velocity" : 2.1933609454044625, - "acceleration" : -4.000000000000005, + "time" : 1.0545224472486359, + "velocity" : 2.0957801587855203, + "acceleration" : 0.40356633939226144, "pose" : { "translation" : { - "x" : 5.696689155544846, - "y" : -0.12282058653420336 + "x" : 5.5985038608263835, + "y" : -0.48175095064352114 }, "rotation" : { - "radians" : 0.9457881291979806 + "radians" : 0.6312847326123074 } }, - "curvature" : 0.036672195665488225 + "curvature" : 0.46262836247598543 }, { - "time" : 1.3225645194975204, - "velocity" : 2.033477031799224, - "acceleration" : -4.000000000000003, + "time" : 1.1108140478616002, + "velocity" : 2.1184975539834254, + "acceleration" : 0.40345921603853396, "pose" : { "translation" : { - "x" : 5.746163164917817, - "y" : -0.05434855003290906 + "x" : 5.692314843161512, + "y" : -0.4091642264580173 }, "rotation" : { - "radians" : 0.9420639655569212 + "radians" : 0.6855833217165795 } }, - "curvature" : -0.13035733884148892 + "curvature" : 0.4527597065949554 }, { - "time" : 1.3640767963731968, - "velocity" : 1.8674279242965188, - "acceleration" : -4.0, + "time" : 1.1654323160372946, + "velocity" : 2.1405337976429735, + "acceleration" : 0.34034527303420153, "pose" : { "translation" : { - "x" : 5.794291304628132, - "y" : 0.010762542606244097 + "x" : 5.780388830164895, + "y" : -0.33319649965673825 }, "rotation" : { - "radians" : 0.9237929192972799 + "radians" : 0.7377000747034669 } }, - "curvature" : -0.3283818138757726 + "curvature" : 0.443485601912384 }, { - "time" : 1.4073934572912177, - "velocity" : 1.6941612806244348, - "acceleration" : -4.000000000000003, + "time" : 1.2182824818871505, + "velocity" : 2.1585211017690455, + "acceleration" : 0.20771430856564294, "pose" : { "translation" : { - "x" : 5.8417411701297794, - "y" : 0.07158024583973699 + "x" : 5.862517297267914, + "y" : -0.25470745749771595 }, "rotation" : { - "radians" : 0.8896456491310966 + "radians" : 0.7876462908989643 } }, - "curvature" : -0.5662511162346159 + "curvature" : 0.4361251242987918 }, { - "time" : 1.4530216175559991, - "velocity" : 1.5116486395653088, - "acceleration" : -4.000000000000002, + "time" : 1.2693229851547776, + "velocity" : 2.169122944614123, + "acceleration" : 1.0817549407792827E-4, "pose" : { "translation" : { - "x" : 5.889143929439342, - "y" : 0.12727663341058815 + "x" : 5.938536177168487, + "y" : -0.17459072970356537 }, "rotation" : { - "radians" : 0.8382382104223547 + "radians" : 0.8355501360445428 } }, - "curvature" : -0.8496047780846073 + "curvature" : 0.431872317394874 }, { - "time" : 1.5019520124723162, - "velocity" : 1.31592705990004, - "acceleration" : -3.2992132129798613, + "time" : 1.3185585791987948, + "velocity" : 2.169128270698835, + "acceleration" : -0.2848041321796146, "pose" : { "translation" : { - "x" : 5.937078552726376, - "y" : 0.17715404902034848 + "x" : 6.008329164809766, + "y" : -0.09375267057515657 }, "rotation" : { - "radians" : 0.7685396527813694 + "radians" : 0.8816343285891601 } }, - "curvature" : -1.173437784895902 + "curvature" : 0.431870196556052 }, { - "time" : 1.5280288447019, - "velocity" : 1.2298940304555388, - "acceleration" : -2.595879024435883, + "time" : 1.3660336232443555, + "velocity" : 2.15560718197925, + "acceleration" : -0.6435155746795979, "pose" : { "translation" : { - "x" : 5.961408087691393, - "y" : 0.19973458251777654 + "x" : 6.071831022358822, + "y" : -0.013091141105286752 }, "rotation" : { - "radians" : 0.7267822918880255 + "radians" : 0.9261982414140092 } }, - "curvature" : -1.343347329975652 + "curvature" : 0.43730501698866603 }, { - "time" : 1.5550966642268162, - "velocity" : 1.1596292455135926, - "acceleration" : -2.0093413111156626, + "time" : 1.4118252194909213, + "velocity" : 2.126139576605145, + "acceleration" : -1.063489692084961, "pose" : { "translation" : { - "x" : 5.986056041903794, - "y" : 0.220670857001096 + "x" : 6.12903088418534, + "y" : 0.06652570890764764 }, "rotation" : { - "radians" : 0.6806182852772713 + "radians" : 0.9696044259991614 } }, - "curvature" : -1.5110727869831724 + "curvature" : 0.4495108307569809 }, { - "time" : 1.582993226711798, - "velocity" : 1.103575530074399, - "acceleration" : -1.499480490388413, + "time" : 1.4560355521525066, + "velocity" : 2.0791223435359023, + "acceleration" : -1.52002157710405, "pose" : { "translation" : { - "x" : 6.011073083229763, - "y" : 0.23992377095496042 + "x" : 6.179975561840308, + "y" : 0.14428065874597884 }, "rotation" : { - "radians" : 0.6303974993218476 + "radians" : 1.0122682566684933 } }, - "curvature" : -1.6684745220647488 + "curvature" : 0.4700711657871874 }, { - "time" : 1.6115420851073832, - "velocity" : 1.0607670738873578, - "acceleration" : -1.0383464958403599, + "time" : 1.4987824321910335, + "velocity" : 2.014146163523463, + "acceleration" : -1.974071718041697, "pose" : { "translation" : { - "x" : 6.036503660218244, - "y" : 0.2574671929874306 + "x" : 6.224772849034707, + "y" : 0.21942863465909568 }, "rotation" : { - "radians" : 0.576664005958531 + "radians" : 1.0546477790856124 } }, - "curvature" : -1.8058581852202884 + "curvature" : 0.500889279611352 }, { - "time" : 1.6405508670810625, - "velocity" : 1.030645906776391, - "acceleration" : -0.6046251421798862, + "time" : 1.540186616781137, + "velocity" : 1.9324113337155617, + "acceleration" : -2.3720307789417294, "pose" : { "translation" : { - "x" : 6.062385509275637, - "y" : 0.27328876653847445 + "x" : 6.263594826618203, + "y" : 0.2913179277497715 }, "rotation" : { - "radians" : 0.5201672456863837 + "radians" : 1.0972283316685527 } }, - "curvature" : -1.9129549301403845 + "curvature" : 0.5441574151816366 }, { - "time" : 1.6698112663305722, - "velocity" : 1.0129543337199158, - "acceleration" : -0.17989538152775067, + "time" : 1.580353908329549, + "velocity" : 1.8371332818560024, + "acceleration" : -2.6474197214125605, "pose" : { "translation" : { - "x" : 6.0887491618404965, - "y" : 0.28739071458846865 + "x" : 6.296681167557836, + "y" : 0.3594114118604921 }, "rotation" : { - "radians" : 0.46185036100408955 + "radians" : 1.140492186777639 } }, - "curvature" : -1.9803591986041147 + "curvature" : 0.6020636034649972 }, { - "time" : 1.6991006131857587, - "velocity" : 1.0076853154927035, - "acceleration" : 0.25454795207597797, + "time" : 1.6193492670845173, + "velocity" : 1.7338962000445413, + "acceleration" : -2.721672977384236, "pose" : { "translation" : { - "x" : 6.1156174515582356, - "y" : 0.2997906443666931 + "x" : 6.324342441916713, + "y" : 0.42330776145978355 }, "rotation" : { - "radians" : 0.40281210939179324 + "radians" : 1.1848561982155952 } }, - "curvature" : -2.00112327804307 + "curvature" : 0.6758923235708024 }, { - "time" : 1.728184543388813, - "velocity" : 1.0150885703642116, - "acceleration" : 0.7213256391379216, + "time" : 1.6571590723066265, + "velocity" : 1.6309902748913652, + "acceleration" : -2.4945738768301835, "pose" : { "translation" : { - "x" : 6.143005021455821, - "y" : 0.3105223520598379 + "x" : 6.346963421832697, + "y" : 0.4827626695285403 }, "rotation" : { - "radians" : 0.3442451501516796 + "radians" : 1.2305496048186084 } }, - "curvature" : -1.9720404920384738 + "curvature" : 0.7638726499983753 }, { - "time" : 1.756819895841348, - "velocity" : 1.035743984273976, - "acceleration" : 1.2524391885977852, + "time" : 1.6936360168640041, + "velocity" : 1.539995841891948, + "acceleration" : -1.7970788234851456, "pose" : { "translation" : { - "x" : 6.170917831116474, - "y" : 0.31963662752049693 + "x" : 6.3650063864971, + "y" : 0.5377100654463529 }, "rotation" : { - "radians" : 0.28736056159514967 + "radians" : 1.2773923275414163 } }, - "curvature" : -1.8941696100081031 + "curvature" : 0.8568101590190533 }, { - "time" : 1.784756647104708, - "velocity" : 1.0707330663583168, - "acceleration" : 1.8992599815033173, + "time" : 1.7284182440412232, + "velocity" : 1.477489437998118, + "acceleration" : 1.705632879833736, "pose" : { "translation" : { - "x" : 6.19935266385437, - "y" : 0.3272020589756721 + "x" : 6.37901442713337, + "y" : 0.5882833328778361 }, "rotation" : { - "radians" : 0.23331316860807427 + "radians" : 1.3244344674414545 } }, - "curvature" : -1.7723980930633711 + "curvature" : 0.9308397775185885 }, { - "time" : 1.8117374729698128, - "velocity" : 1.1219766691918198, - "acceleration" : 2.7534793106504156, + "time" : 1.7883248954688422, + "velocity" : 1.5796681923938034, + "acceleration" : 3.9999999999999862, "pose" : { "translation" : { - "x" : 6.228296633889339, - "y" : 0.33330583773527067 + "x" : 6.397521991248141, + "y" : 0.6779655956833608 }, "rotation" : { - "radians" : 0.183142361520775 + "radians" : 1.4085741509062624 } }, - "curvature" : -1.614195199267953 + "curvature" : 0.8143141037035398 }, { - "time" : 1.8374933012978958, - "velocity" : 1.1928948096218601, - "acceleration" : -3.5661641751301776, + "time" : 1.8363455674677351, + "velocity" : 1.7717508803893742, + "acceleration" : 4.000000000000008, "pose" : { "translation" : { - "x" : 6.257726693521562, - "y" : 0.33805456290060576 + "x" : 6.408572673797607, + "y" : 0.7576718926429749 }, "rotation" : { - "radians" : 0.1377385534824891 + "radians" : 1.4464418628084068 } }, - "curvature" : -1.4279710594799537 - }, { - "time" : 1.8927457727102306, - "velocity" : 0.9958554254837872, - "acceleration" : -3.999999999999999, - "pose" : { - "translation" : { - "x" : 6.317899124228461, - "y" : 0.3440151160617688 - }, - "rotation" : { - "radians" : 0.06404669180836077 - } - }, - "curvature" : -1.0007588885962067 + "curvature" : 0.0 }, { - "time" : 1.9651351974193783, - "velocity" : 0.7062977266471963, - "acceleration" : -4.0, + "time" : 1.879584211551695, + "velocity" : 1.9447054567252142, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 6.37946360862216, - "y" : 0.3463552460207815 + "x" : 6.417938666279497, + "y" : 0.8374714001847678 }, "rotation" : { - "radians" : 0.01679972270125931 + "radians" : 1.4673635124320494 } }, - "curvature" : -0.5272785953582695 + "curvature" : 0.4361537758239235 }, { - "time" : 2.1417096290811775, - "velocity" : 0.0, - "acceleration" : -4.0, + "time" : 1.9241999105096688, + "velocity" : 2.123168252557109, + "acceleration" : 4.000000000000008, "pose" : { "translation" : { - "x" : 6.441819667816162, - "y" : 0.3467085063457489 + "x" : 6.425398940205923, + "y" : 0.9279097366234055 }, "rotation" : { - "radians" : 0.0 + "radians" : 1.509659326369691 } }, - "curvature" : 0.0 - } ], - "pointList" : [ { - "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], - "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + "curvature" : 0.44722076145528444 }, { - "x" : [ 6.441819667816162, 2.0, 0.0 ], - "y" : [ 0.3467085063457489, 0.0, 0.0 ] - } ], - "rotations" : [ { - "time" : 0.0, - "rotation" : { - "radians" : 0.0 - } - }, { - "time" : 2.0499999541789293, - "rotation" : { - "radians" : 0.0 - } - } ], - "reversed" : false, - "color" : 286.3529, - "closed" : false, - "velocityStart" : 0.0, - "velocityEnd" : 0.0, - "constraints" : [ ] - }, { - "type" : "script", - "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "sleep", - "args" : [ "1000" ], - "argTypes" : [ "long" ], - "reflection" : false - }, { - "methodName" : "frc.subsystem.Intake.setWantedIntakeState", - "args" : [ "EJECT" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], - "reflection" : true - } ] - } - }, { - "type" : "script", - "script" : "Intake.setIntakeSolState CLOSE", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "frc.subsystem.Intake.setIntakeSolState", - "args" : [ "CLOSE" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], - "reflection" : true - } ] - } - }, { - "type" : "trajectory", - "states" : [ { - "time" : 0.0, - "velocity" : 0.0, - "acceleration" : 4.0, + "time" : 1.9728110085852033, + "velocity" : 2.317612644859248, + "acceleration" : 3.999999999999993, "pose" : { "translation" : { - "x" : 6.441819667816162, - "y" : 0.3467085063457489 + "x" : 6.429642048575715, + "y" : 1.035761920819823 }, "rotation" : { - "radians" : 1.6078163234494292 + "radians" : 1.550908753938559 } }, - "curvature" : -0.0 + "curvature" : 0.31730031892680094 }, { - "time" : 0.1953077076111654, - "velocity" : 0.7812308304446616, - "acceleration" : 3.999999999999996, + "time" : 1.9987893246112054, + "velocity" : 2.421525908963256, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 6.4389816843095105, - "y" : 0.42294590310561375 + "x" : 6.430305899986379, + "y" : 1.0973157606626955 }, "rotation" : { - "radians" : 1.6083198170208068 + "radians" : 1.5684899825119734 } }, - "curvature" : 0.010007683937884466 + "curvature" : 0.25637867502172557 }, { - "time" : 0.28418183429804866, - "velocity" : 1.1367273371921942, - "acceleration" : 3.9999999999999987, + "time" : 2.025781495555629, + "velocity" : 2.5294945927409493, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 6.435750551492674, - "y" : 0.5081130612115885 + "x" : 6.429927970748395, + "y" : 1.1641340877395123 }, "rotation" : { - "radians" : 1.609052997763179 + "radians" : 1.583853790503598 } }, - "curvature" : 0.006001548474080814 + "curvature" : 0.20594813300808948 }, { - "time" : 0.3615636139485796, - "velocity" : 1.446254455794318, - "acceleration" : 4.0, + "time" : 2.0536364380792893, + "velocity" : 2.6409143628355918, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.431910399555818, - "y" : 0.6079771182521938 + "x" : 6.42849117103934, + "y" : 1.2361304745226107 }, "rotation" : { - "radians" : 1.6092943557828885 + "radians" : 1.597166880767735 } }, - "curvature" : -8.5488367035683E-4 + "curvature" : 0.16589805557058687 }, { - "time" : 0.43523665263811134, - "velocity" : 1.740946610552445, - "acceleration" : 4.0, + "time" : 2.082166732992299, + "velocity" : 2.7550355424876307, + "acceleration" : 3.999999999999967, "pose" : { "translation" : { - "x" : 6.427411179058254, - "y" : 0.72529627053882 + "x" : 6.4260035555757895, + "y" : 1.3130642882106258 }, "rotation" : { - "radians" : 1.6088589590984765 + "radians" : 1.6086729537639617 } }, - "curvature" : -0.006111352293260847 + "curvature" : 0.13471207633826426 }, { - "time" : 0.47131335599478547, - "velocity" : 1.8852534239791414, - "acceleration" : 3.999999999999987, + "time" : 2.1111641583855323, + "velocity" : 2.8710252440605633, + "acceleration" : 4.000000000000011, "pose" : { "translation" : { - "x" : 6.42493665439585, - "y" : 0.7906601188879874 + "x" : 6.422495681211174, + "y" : 1.3945594657209313 }, "rotation" : { - "radians" : 1.6083911287059784 + "radians" : 1.6186275742658227 } }, - "curvature" : -0.008116127399784383 + "curvature" : 0.11061622885252656 }, { - "time" : 0.5069348966838761, - "velocity" : 2.0277395867355037, - "acceleration" : 4.0, + "time" : 2.1404115791378824, + "velocity" : 2.988014927069964, + "acceleration" : 0.40226107984247866, "pose" : { "translation" : { - "x" : 6.422338327934085, - "y" : 0.8603050862099906 + "x" : 6.418017964533647, + "y" : 1.4801232886820799 }, "rotation" : { - "radians" : 1.607763133691658 + "radians" : 1.6272684320752597 } }, - "curvature" : -0.009853447575726546 + "curvature" : 0.09202002758878106 }, { - "time" : 0.5420590363593313, - "velocity" : 2.1682361454373242, - "acceleration" : 4.000000000000005, + "time" : 2.170205843436901, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.419643072810146, - "y" : 0.9339457983013029 + "x" : 6.412638039463943, + "y" : 1.5691651584262445 }, "rotation" : { - "radians" : 1.6069775892569418 + "radians" : 1.6348043576150604 } }, - "curvature" : -0.011436025231544151 + "curvature" : 0.07763517137648186 }, { - "time" : 0.5766104955030272, - "velocity" : 2.306441982012108, - "acceleration" : 3.9999999999999947, + "time" : 2.200892251234442, + "velocity" : 2.9999999999999996, + "acceleration" : 9.430296455303456E-15, "pose" : { "translation" : { - "x" : 6.416882438497851, - "y" : 1.0111998183356263 + "x" : 6.406438114853245, + "y" : 1.6610153709816586 }, "rotation" : { - "radians" : 1.6060340313214787 + "radians" : 1.6414135047463148 } }, - "curvature" : -0.012962877357532548 + "curvature" : 0.06646668401976608 }, { - "time" : 0.6104960029929839, - "velocity" : 2.4419840119719347, - "acceleration" : 4.0, + "time" : 2.232286754539466, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.4140917029015725, - "y" : 1.0916028128984 + "x" : 6.399512332081045, + "y" : 1.7549438920650573 }, "rotation" : { - "radians" : 1.604928600003804 + "radians" : 1.6472455716072028 } }, - "curvature" : -0.014520270296573292 + "curvature" : 0.05776403436142682 }, { - "time" : 0.6436141193910724, - "velocity" : 2.5744564775642886, - "acceleration" : 3.9999999999999947, + "time" : 2.2641313884937704, + "velocity" : 3.0, + "acceleration" : -0.5220281507380499, "pose" : { "translation" : { - "x" : 6.411308924450168, - "y" : 1.1746237180213086 + "x" : 6.3919641226530075, + "y" : 1.8501791320741177 }, "rotation" : { - "radians" : 1.603653938746236 + "radians" : 1.6524254678653565 } }, - "curvature" : -0.01618596901731972 + "curvature" : 0.050967461408882746 }, { - "time" : 0.6758618258229525, - "velocity" : 2.703447303291809, - "acceleration" : 4.0, + "time" : 2.2962499027431713, + "velocity" : 2.9832332314019316, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.408573994190903, - "y" : 1.2596799052167897 + "x" : 6.383903565798832, + "y" : 1.9459267210798998 }, "rotation" : { - "radians" : 1.6021991606605406 + "radians" : 1.6570571945158592 } }, - "curvature" : -0.01803416029898833 + "curvature" : 0.04566147685699515 }, { - "time" : 0.7071390741340647, - "velocity" : 2.828556296536258, - "acceleration" : 3.5711423686938364, + "time" : 2.329098019757857, + "velocity" : 2.851840763343189, + "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.405927687883377, - "y" : 1.3461523475125432 + "x" : 6.375444746070116, + "y" : 2.0413882838192876 }, "rotation" : { - "radians" : 1.6005498243279082 + "radians" : 1.6612273952864063 } }, - "curvature" : -0.020140089228601762 + "curvature" : 0.04153801107443314 }, { - "time" : 0.7374186993151032, - "velocity" : 2.936689148928433, - "acceleration" : -3.99999999999999, + "time" : 2.3631515088169963, + "velocity" : 2.7156268071066307, + "acceleration" : -3.999999999999995, "pose" : { "translation" : { - "x" : 6.403410718093454, - "y" : 1.4334007854860387 + "x" : 6.366703110938218, + "y" : 2.135780214687429 }, "rotation" : { - "radians" : 1.5986879179972218 + "radians" : 1.6650083738352117 } }, - "curvature" : -0.02258406827188887 + "curvature" : 0.038368183750837316 }, { - "time" : 0.7678125246032664, - "velocity" : 2.8151138477757804, + "time" : 2.3983080444520524, + "velocity" : 2.5750006645664074, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.401062786287184, - "y" : 1.520778893299024 + "x" : 6.357792828392121, + "y" : 2.228352452730178 }, "rotation" : { - "radians" : 1.5965918957171965 + "radians" : 1.6684605177848264 } }, - "curvature" : -0.02545459002718879 + "curvature" : 0.03598061733215407 }, { - "time" : 0.7993888917739188, - "velocity" : 2.688808379093171, - "acceleration" : -4.0, + "time" : 2.4344694314979898, + "velocity" : 2.4303551163826573, + "acceleration" : -3.999999999999995, "pose" : { "translation" : { - "x" : 6.398921634924733, - "y" : 1.6076494447320337 + "x" : 6.3488241445362945, + "y" : 2.318407256636533 }, "rotation" : { - "radians" : 1.59423685765634 + "radians" : 1.6716341182037762 } }, - "curvature" : -0.028849988627757065 + "curvature" : 0.03424396145033817 }, { - "time" : 0.8320832691315886, - "velocity" : 2.558030869662492, - "acceleration" : -4.0, + "time" : 2.4715493896338248, + "velocity" : 2.2820352838393174, + "acceleration" : -4.000000000000005, "pose" : { "translation" : { - "x" : 6.397022099554306, - "y" : 1.6933994792188969 + "x" : 6.339900741188558, + "y" : 2.4053179797310804 }, "rotation" : { - "radians" : 1.59159503414185 + "radians" : 1.6745705658893675 } }, - "curvature" : -0.03287744142106298 + "curvature" : 0.03305114421605748 }, { - "time" : 0.8658400026260393, - "velocity" : 2.423003935684689, + "time" : 2.509484949213048, + "velocity" : 2.1302930455224227, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.395395160906077, - "y" : 1.7774554678812455 + "x" : 6.331117093477943, + "y" : 2.4885478449664333 }, "rotation" : { - "radians" : 1.588636834345054 + "radians" : 1.6773028650946311 } }, - "curvature" : -0.03764692669916069 + "curvature" : 0.03230248453046758 }, { - "time" : 0.9006204438645928, - "velocity" : 2.283882170730475, + "time" : 2.5482536718462243, + "velocity" : 1.9752181549897188, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.394066996986112, - "y" : 1.859298479563023 + "x" : 6.322555827442557, + "y" : 2.567668719915673 }, "rotation" : { - "radians" : 1.5853328740324872 + "radians" : 1.6798553438041104 } }, - "curvature" : -0.04325577926527458 + "curvature" : 0.03188409115100577 }, { - "time" : 0.9364146483950042, - "velocity" : 2.1407053526088293, + "time" : 2.587900600923266, + "velocity" : 1.8166304386815513, "acceleration" : -3.999999999999997, "pose" : { "translation" : { - "x" : 6.393058035170299, - "y" : 1.938479346864992 - }, - "rotation" : { - "radians" : 1.5816576204489718 - } - }, - "curvature" : -0.04975638894367889 - }, { - "time" : 0.9732587981504898, - "velocity" : 1.993328753586887, - "acceleration" : -4.000000000000003, - "pose" : { - "translation" : { - "x" : 6.39238200429827, - "y" : 2.0146338321792427 - }, - "rotation" : { - "radians" : 1.5775955933008623 - } - }, - "curvature" : -0.05709527513223549 - }, { - "time" : 1.0112621915316293, - "velocity" : 1.841315180062329, - "acceleration" : -4.0, - "pose" : { - "translation" : { - "x" : 6.392044986767331, - "y" : 2.087497793723702 + "x" : 6.314285077627446, + "y" : 2.64237989176479 }, "rotation" : { - "radians" : 1.57315140430575 + "radians" : 1.682242375809382 } }, - "curvature" : -0.06500723564977115 + "curvature" : 0.031637223157170435 }, { - "time" : 1.0506511801697798, - "velocity" : 1.683759225509727, + "time" : 2.6285823403067847, + "velocity" : 1.6539034811474767, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.392044470626388, - "y" : 2.1569223515766396 + "x" : 6.306355844682457, + "y" : 2.712526842305124 }, "rotation" : { - "radians" : 1.5683651575191264 + "radians" : 1.6844659116451925 } }, - "curvature" : -0.07284766972073614 + "curvature" : 0.03131479782522635 }, { - "time" : 1.0918455825454725, - "velocity" : 1.5189816160069558, - "acceleration" : -4.0, + "time" : 2.6706436297897875, + "velocity" : 1.4856583232154645, + "acceleration" : -4.000000000000005, "pose" : { "translation" : { - "x" : 6.39236840166987, - "y" : 2.222889053711179 + "x" : 6.298799352960103, + "y" : 2.7781200229258047 }, "rotation" : { - "radians" : 1.5633344792916735 + "radians" : 1.6865117620775738 } }, - "curvature" : -0.07936077553696355 + "curvature" : 0.03052674983936547 }, { - "time" : 1.1356043581385193, - "velocity" : 1.3439465136347681, + "time" : 2.71476243308551, + "velocity" : 1.3091831100325741, "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.39299423553166, - "y" : 2.2855250420298034 + "x" : 6.291624408113421, + "y" : 2.839353629606194 }, "rotation" : { - "radians" : 1.5582428882782466 + "radians" : 1.6883451359021284 } }, - "curvature" : -0.0824337723791933 + "curvature" : 0.028694423143857225 }, { - "time" : 1.2379787498796464, - "velocity" : 0.9344489466702602, - "acceleration" : -4.000000000000001, + "time" : 2.815943111560705, + "velocity" : 0.904460396131795, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.395003296006507, - "y" : 2.4021324106830946 + "x" : 6.278326433749044, + "y" : 2.9505502779693416 }, "rotation" : { - "radians" : 1.549211634448327 + "radians" : 1.6911174188705012 } }, - "curvature" : -0.06547973929352308 + "curvature" : 0.018977039717131546 }, { - "time" : 1.4715909865472114, + "time" : 3.0420582105936536, "velocity" : 0.0, - "acceleration" : -4.000000000000001, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.397645473480225, - "y" : 2.5112497806549072 + "x" : 6.265983581542969, + "y" : 3.052058696746826 }, "rotation" : { - "radians" : 1.5451593331499225 + "radians" : 1.6921482303347304 } }, - "curvature" : 0.0 + "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 6.441819667816162, -0.0441741943359375, 0.0 ], - "y" : [ 0.3467085063457489, 1.1927070915699005, 0.0 ] + "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], + "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + }, { + "x" : [ 6.408572673797607, 0.15555095672607422, 0.0 ], + "y" : [ 0.7576718926429749, 1.2444130182266235, 0.0 ] }, { - "x" : [ 6.397645473480225, 0.04417705535888672, 0.0 ], - "y" : [ 2.5112497806549072, 1.7227985858917236, 0.0 ] + "x" : [ 6.265983581542969, -0.19443941116333008, 0.0 ], + "y" : [ 3.052058696746826, 1.5944044589996338, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : 0.0 + "radians" : -0.13962634015954636 } }, { - "time" : 1.3799999691545963, + "time" : 1.8299999590963125, "rotation" : { - "radians" : 0.6716053016572916 + "radians" : 0.4537856055185257 + } + }, { + "time" : 2.9499999340623617, + "rotation" : { + "radians" : 0.4537856055185257 } } ], "reversed" : false, - "color" : 92.0, + "color" : 286.3529, "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] + }, { + "type" : "script", + "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "sleep", + "args" : [ "1000" ], + "argTypes" : [ "long" ], + "reflection" : false + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Intake.setIntakeSolState CLOSE", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + } ] + } } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/blue/movecargotohanger.json b/src/main/deploy/autos/blue/movecargotohanger.json new file mode 100644 index 00000000..8eb15ca5 --- /dev/null +++ b/src/main/deploy/autos/blue/movecargotohanger.json @@ -0,0 +1,3458 @@ +{ + "autonomousSteps" : [ { + "type" : "script", + "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 2.351956220256751, + "pose" : { + "translation" : { + "x" : 6.1293768882751465, + "y" : 1.0937262773513794 + }, + "rotation" : { + "radians" : 2.4178655244299407 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.3188834866654645, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.039938468317359, + "y" : 1.173101702633744 + }, + "rotation" : { + "radians" : 2.411607121974174 + } + }, + "curvature" : -0.10063347960741297 + }, { + "time" : 0.47317666597794406, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.954285601692391, + "y" : 1.2509135147447523 + }, + "rotation" : { + "radians" : 2.395009715228919 + } + }, + "curvature" : -0.1855623376134662 + }, { + "time" : 0.6188308342429313, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.87496604004582, + "y" : 1.3260262403808838 + }, + "rotation" : { + "radians" : 2.3703948601089895 + } + }, + "curvature" : -0.26531768438500725 + }, { + "time" : 0.7539530640358325, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.803342186380178, + "y" : 1.3977211365709081 + }, + "rotation" : { + "radians" : 2.339767645326564 + } + }, + "curvature" : -0.3381985496587611 + }, { + "time" : 0.8779901904625292, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.7397725735722815, + "y" : 1.4656408262000014 + }, + "rotation" : { + "radians" : 2.3056558386592805 + } + }, + "curvature" : -0.3907488038191573 + }, { + "time" : 0.9914536504675615, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.683793342890567, + "y" : 1.529733933533862 + }, + "rotation" : { + "radians" : 2.271678768601324 + } + }, + "curvature" : -0.39766870774643054 + }, { + "time" : 1.095639209373063, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.634299722512424, + "y" : 1.5901997197428273 + }, + "rotation" : { + "radians" : 2.242697343821613 + } + }, + "curvature" : -0.32812227024366547 + }, { + "time" : 1.1923614795366175, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.589727506041527, + "y" : 1.6474327184259892 + }, + "rotation" : { + "radians" : 2.224227521884603 + } + }, + "curvature" : -0.16341344710143665 + }, { + "time" : 1.2837283378298283, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.548234531025173, + "y" : 1.7019673711353107 + }, + "rotation" : { + "radians" : 2.2209763317101947 + } + }, + "curvature" : 0.07956164338822755 + }, { + "time" : 1.3719691365115938, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.507882157471613, + "y" : 1.7544226628997421 + }, + "rotation" : { + "radians" : 2.234944043969041 + } + }, + "curvature" : 0.33953937734428047 + }, { + "time" : 1.4592981567041168, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.466816746367385, + "y" : 1.805446757749337 + }, + "rotation" : { + "radians" : 2.264150133653273 + } + }, + "curvature" : 0.5355105351904276 + }, { + "time" : 1.5477626861241707, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.42345113819465, + "y" : 1.8556616342393681 + }, + "rotation" : { + "radians" : 2.30296870864004 + } + }, + "curvature" : 0.6126486386243994 + }, { + "time" : 1.6390285515859833, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.376646131448524, + "y" : 1.9056077209744444 + }, + "rotation" : { + "radians" : 2.343996330815072 + } + }, + "curvature" : 0.568293999561127 + }, { + "time" : 1.7340988034913118, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.3258919611544115, + "y" : 1.9556885321326263 + }, + "rotation" : { + "radians" : 2.3801609988469874 + } + }, + "curvature" : 0.4354667748411578 + }, { + "time" : 1.8330035326706784, + "velocity" : 0.75, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.271489777385341, + "y" : 2.006115302989542 + }, + "rotation" : { + "radians" : 2.405692727133013 + } + }, + "curvature" : 0.24590943352090008 + }, { + "time" : 2.0360118787648114, + "velocity" : 0.0, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 2.4154850346736763 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ], + "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ] + }, { + "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ], + "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.37094272169677 + } + }, { + "time" : 1.939999956637621, + "rotation" : { + "radians" : 2.370942522371697 + } + } ], + "reversed" : false, + "color" : 209.88235, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ { + "type" : "MaxVelocityConstraint", + "maxVelocity" : 0.75 + } ] + }, { + "type" : "script", + "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "10" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Drive.turnToAngle 40", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.turnToAngle", + "args" : [ "40" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 0.9530775920796433 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.22164892589636578, + "velocity" : 0.8865957035854634, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.272283410420641, + "y" : 2.136490201934066 + }, + "rotation" : { + "radians" : 0.93154333853559 + } + }, + "curvature" : -0.3354703724748357 + }, { + "time" : 0.32212126730993046, + "velocity" : 1.2884850692397223, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.339010516181588, + "y" : 2.223017249489203 + }, + "rotation" : { + "radians" : 0.8980578131179813 + } + }, + "curvature" : -0.24297628723974596 + }, { + "time" : 0.40798632695507736, + "velocity" : 1.6319453078203097, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.418256742414087, + "y" : 2.320179727881623 + }, + "rotation" : { + "radians" : 0.8787668506583872 + } + }, + "curvature" : -0.07195280186555318 + }, { + "time" : 0.4482103474158235, + "velocity" : 1.7928413896632942, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.462294308261335, + "y" : 2.373142516471262 + }, + "rotation" : { + "radians" : 0.8763509051285007 + } + }, + "curvature" : -0.0010310805010032027 + }, { + "time" : 0.48701783511066765, + "velocity" : 1.9480713404426708, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.508707731962204, + "y" : 2.428952641785145 + }, + "rotation" : { + "radians" : 0.8784798678297706 + } + }, + "curvature" : 0.057187573384384974 + }, { + "time" : 0.5244749433253648, + "velocity" : 2.0978997733014593, + "acceleration" : -3.006834100609329, + "pose" : { + "translation" : { + "x" : 5.5569134053257585, + "y" : 2.4874170951864016 + }, + "rotation" : { + "radians" : 0.8846611837531144 + } + }, + "curvature" : 0.10384088533571874 + }, { + "time" : 0.5628765788352517, + "velocity" : 1.9824324261311614, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 5.606271566124633, + "y" : 2.5482597037116648 + }, + "rotation" : { + "radians" : 0.8942836471940969 + } + }, + "curvature" : 0.13988861038856043 + }, { + "time" : 0.6051667706790917, + "velocity" : 1.813271658755801, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.656145019875112, + "y" : 2.6111436507878807 + }, + "rotation" : { + "radians" : 0.9066044762714179 + } + }, + "curvature" : 0.16515378657020438 + }, { + "time" : 0.6526161179857626, + "velocity" : 1.6234742695291178, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.7059578616172075, + "y" : 2.6756939969491214 + }, + "rotation" : { + "radians" : 0.9206836126942823 + } + }, + "curvature" : 0.17781422207911715 + }, { + "time" : 0.7069026874415512, + "velocity" : 1.4063279917059635, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.75525419769474, + "y" : 2.7415202005533956 + }, + "rotation" : { + "radians" : 0.9352834691742239 + } + }, + "curvature" : 0.1740904256082478 + }, { + "time" : 0.7714879126896856, + "velocity" : 1.1479870907134255, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.803756867535412, + "y" : 2.8082386384994606 + }, + "rotation" : { + "radians" : 0.9487507880978892 + } + }, + "curvature" : 0.14816445891139096 + }, { + "time" : 0.8556329295191124, + "velocity" : 0.8114070233957184, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.851426165430894, + "y" : 2.8754951269436333 + }, + "rotation" : { + "radians" : 0.9589069733276255 + } + }, + "curvature" : 0.09262519823750537 + }, { + "time" : 1.058484685368042, + "velocity" : 0.0, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : 0.9629949901604784 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ], + "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ] + }, { + "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ], + "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.9599310885968813 + } + }, { + "time" : 0.9699999783188105, + "rotation" : { + "radians" : 0.9599310885968813 + } + } ], + "reversed" : false, + "color" : 148.47061, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 1.3\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.3, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "@t 1.6\nIntake.setWantedIntakeState INTAKE", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.6, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : -3.1024027203970066 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2301659590513677, + "velocity" : 0.9206638362054708, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.792647884092066, + "y" : 2.9388182625618144 + }, + "rotation" : { + "radians" : -3.1018966911012167 + } + }, + "curvature" : 0.009441605420194805 + }, { + "time" : 0.32549598090329745, + "velocity" : 1.3019839236131898, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.686791604118646, + "y" : 2.9345442011023373 + }, + "rotation" : { + "radians" : -3.100425032373633 + } + }, + "curvature" : 0.01823558838667193 + }, { + "time" : 0.3986288451113676, + "velocity" : 1.5945153804454704, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 5.580971612904613, + "y" : 2.9300674260512527 + }, + "rotation" : { + "radians" : -3.098055153290077 + } + }, + "curvature" : 0.026416758928169817 + }, { + "time" : 0.4602561477805154, + "velocity" : 1.8410245911220613, + "acceleration" : 3.9999999999999916, + "pose" : { + "translation" : { + "x" : 5.475217593972275, + "y" : 2.9252976053729753 + }, + "rotation" : { + "radians" : -3.094851059223684 + } + }, + "curvature" : 0.0340260625248394 + }, { + "time" : 0.5145132056228292, + "velocity" : 2.0580528224913164, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.369566605466148, + "y" : 2.9201516955883897 + }, + "rotation" : { + "radians" : -3.090872945078929 + } + }, + "curvature" : 0.04110875647986678 + }, { + "time" : 0.563515762369635, + "velocity" : 2.2540630494785394, + "acceleration" : 4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.264062661760839, + "y" : 2.914553730779989 + }, + "rotation" : { + "radians" : -3.086176913297493 + } + }, + "curvature" : 0.047712969032265094 + }, { + "time" : 0.6085162323222143, + "velocity" : 2.4340649292888568, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.158756315068919, + "y" : 2.9084346115970146 + }, + "rotation" : { + "radians" : -3.080814792156677 + } + }, + "curvature" : 0.05388859471718488 + }, { + "time" : 0.6503267256223321, + "velocity" : 2.601306902489328, + "acceleration" : 3.9999999999999956, + "pose" : { + "translation" : { + "x" : 5.053704237048805, + "y" : 2.9017318942605925 + }, + "rotation" : { + "radians" : -3.0748340330236092 + } + }, + "curvature" : 0.059686480108153514 + }, { + "time" : 0.6895078614844453, + "velocity" : 2.758031445937781, + "acceleration" : 3.9999999999999956, + "pose" : { + "translation" : { + "x" : 4.948968800412642, + "y" : 2.894389579568873 + }, + "rotation" : { + "radians" : -3.0682776682148387 + } + }, + "curvature" : 0.06515785747811133 + }, { + "time" : 0.7264647120959763, + "velocity" : 2.9058588483839047, + "acceleration" : 2.666249486168943, + "pose" : { + "translation" : { + "x" : 4.844617660534176, + "y" : 2.8863579019021692 + }, + "rotation" : { + "radians" : -3.061184313833844 + } + }, + "curvature" : 0.07035398883055903 + }, { + "time" : 0.7617731677016681, + "velocity" : 2.9999999999999996, + "acceleration" : 8.556674489449135E-15, + "pose" : { + "translation" : { + "x" : 4.740723337056639, + "y" : 2.8775931182280945 + }, + "rotation" : { + "radians" : -3.0535882043682183 + } + }, + "curvature" : 0.07532598833602047 + }, { + "time" : 0.7963729965952918, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.637362795500621, + "y" : 2.868057297106702 + }, + "rotation" : { + "radians" : -3.045519247910405 + } + }, + "curvature" : 0.08012479693513168 + }, { + "time" : 0.8307945521766903, + "velocity" : 3.0, + "acceleration" : -8.652000136404901E-15, + "pose" : { + "translation" : { + "x" : 4.534617028871959, + "y" : 2.857718107695623 + }, + "rotation" : { + "radians" : -3.0370030926303286 + } + }, + "curvature" : 0.08480128843727726 + }, { + "time" : 0.8650131685157781, + "velocity" : 2.9999999999999996, + "acceleration" : 8.710165382862835E-15, + "pose" : { + "translation" : { + "x" : 4.432570639269606, + "y" : 2.846548608755203 + }, + "rotation" : { + "radians" : -3.028061196597019 + } + }, + "curvature" : 0.08940649170847131 + }, { + "time" : 0.899003277798418, + "velocity" : 3.0, + "acceleration" : -0.09574616288107782, + "pose" : { + "translation" : { + "x" : 4.331311419493517, + "y" : 2.834527037653644 + }, + "rotation" : { + "radians" : -3.018710894247568 + } + }, + "curvature" : 0.09399191846371814 + }, { + "time" : 0.9327567117085453, + "velocity" : 2.9967682382190453, + "acceleration" : -0.16603740413403223, + "pose" : { + "translation" : { + "x" : 4.230929934652522, + "y" : 2.821636599372141 + }, + "rotation" : { + "radians" : -3.0089654537653083 + } + }, + "curvature" : 0.0986099907832933 + }, { + "time" : 0.9662773051518138, + "velocity" : 2.9912025658986927, + "acceleration" : -0.17374271650320738, + "pose" : { + "translation" : { + "x" : 4.131519103772213, + "y" : 2.807865255510019 + }, + "rotation" : { + "radians" : -2.9988341203843856 + } + }, + "curvature" : 0.10331456682545719 + }, { + "time" : 0.9995509183178184, + "velocity" : 2.985421517959354, + "acceleration" : -0.18358691510613726, + "pose" : { + "translation" : { + "x" : 4.033173781402816, + "y" : 2.793205513289873 + }, + "rotation" : { + "radians" : -2.9883221412125427 + } + }, + "curvature" : 0.10816156739888892 + }, { + "time" : 1.0325512170508016, + "velocity" : 2.979363094917385, + "acceleration" : -0.1957881485667769, + "pose" : { + "translation" : { + "x" : 3.9359903392270725, + "y" : 2.7776542145627077 + }, + "rotation" : { + "radians" : -2.977430767583505 + } + }, + "curvature" : 0.11320971018612586 + }, { + "time" : 1.0652520605900002, + "velocity" : 2.9729606573042733, + "acceleration" : -0.2106132308438905, + "pose" : { + "translation" : { + "x" : 3.840066247668119, + "y" : 2.761212324813073 + }, + "rotation" : { + "radians" : -2.9661572312387867 + } + }, + "curvature" : 0.11852136257591563 + }, { + "time" : 1.0976276482842051, + "velocity" : 2.966141930179527, + "acceleration" : -0.22838513501532773, + "pose" : { + "translation" : { + "x" : 3.7454996574973665, + "y" : 2.743884722164204 + }, + "rotation" : { + "radians" : -2.9544946908190806 + } + }, + "curvature" : 0.1241635283632123 + }, { + "time" : 1.1296526738305466, + "velocity" : 2.9588278903962566, + "acceleration" : -0.2494917017630424, + "pose" : { + "translation" : { + "x" : 3.6523889814423782, + "y" : 2.7256799863831596 + }, + "rotation" : { + "radians" : -2.942432145241093 + } + }, + "curvature" : 0.1302089880914559 + }, { + "time" : 1.1613024877721105, + "velocity" : 2.950931524455492, + "acceleration" : -0.27439562802342504, + "pose" : { + "translation" : { + "x" : 3.560832475794749, + "y" : 2.7066101878859605 + }, + "rotation" : { + "radians" : -2.9299543105725916 + } + }, + "curvature" : 0.13673761759648564 + }, { + "time" : 1.1925532689948115, + "velocity" : 2.9423564467156664, + "acceleration" : -0.30364572614284985, + "pose" : { + "translation" : { + "x" : 3.470927822017984, + "y" : 2.686690676742728 + }, + "rotation" : { + "radians" : -2.9170414570270506 + } + }, + "curvature" : 0.1438379143723059 + }, { + "time" : 1.2233822059663813, + "velocity" : 2.932995371762722, + "acceleration" : -0.3378893144869457, + "pose" : { + "translation" : { + "x" : 3.3827717083553797, + "y" : 2.6659398716828218 + }, + "rotation" : { + "radians" : -2.9036692027182647 + } + }, + "curvature" : 0.15160876664672418 + }, { + "time" : 1.253767688448565, + "velocity" : 2.922728441916462, + "acceleration" : -0.377885389511768, + "pose" : { + "translation" : { + "x" : 3.296459411437901, + "y" : 2.644379049099978 + }, + "rotation" : { + "radians" : -2.8898082608960753 + } + }, + "curvature" : 0.1601615053363712 + }, { + "time" : 1.2836895103731156, + "velocity" : 2.9114214225836013, + "acceleration" : -0.4245178992880957, + "pose" : { + "translation" : { + "x" : 3.2120843778920607, + "y" : 2.62203213205745 + }, + "rotation" : { + "radians" : -2.875424137597093 + } + }, + "curvature" : 0.1696222839550847 + }, { + "time" : 1.313129084481963, + "velocity" : 2.898923796426977, + "acceleration" : -0.47880793939403443, + "pose" : { + "translation" : { + "x" : 3.129737805947798, + "y" : 2.5989254792931433 + }, + "rotation" : { + "radians" : -2.860476777086651 + } + }, + "curvature" : 0.180134835376471 + }, { + "time" : 1.3420696691632106, + "velocity" : 2.8850668147108904, + "acceleration" : -0.5419229606175602, + "pose" : { + "translation" : { + "x" : 3.049508227046359, + "y" : 2.5750876742247573 + }, + "rotation" : { + "radians" : -2.844920153276735 + } + }, + "curvature" : 0.19186365592042148 + }, { + "time" : 1.37049660762701, + "velocity" : 2.869661604057295, + "acceleration" : -0.6151800394568323, + "pose" : { + "translation" : { + "x" : 2.9714810874481756, + "y" : 2.550549313954921 + }, + "rotation" : { + "radians" : -2.828701806670222 + } + }, + "curvature" : 0.2049976646319762 + }, { + "time" : 1.3983975791037442, + "velocity" : 2.852497483323354, + "acceleration" : -0.7000388399543165, + "pose" : { + "translation" : { + "x" : 2.895738329840743, + "y" : 2.5253427982763332 + }, + "rotation" : { + "radians" : -2.8117623285706195 + } + }, + "curvature" : 0.21975437583412769 + }, { + "time" : 1.4257628610363704, + "velocity" : 2.8333407231042154, + "acceleration" : -0.8320723157548291, + "pose" : { + "translation" : { + "x" : 2.822357974946499, + "y" : 2.499502118676901 + }, + "rotation" : { + "radians" : -2.7940347976772673 + } + }, + "curvature" : 0.23638460141375914 + }, { + "time" : 1.4525899504690478, + "velocity" : 2.8110186446750056, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.751413703130705, + "y" : 2.473062647344876 + }, + "rotation" : { + "radians" : -2.7754441792642495 + } + }, + "curvature" : 0.25517765880922155 + }, { + "time" : 1.4792695579561794, + "velocity" : 2.7043002147264787, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.6829744360093244, + "y" : 2.4460609261739954 + }, + "rotation" : { + "radians" : -2.7559067046024177 + } + }, + "curvature" : 0.27646699088034715 + }, { + "time" : 1.506205081648366, + "velocity" : 2.5965581199577317, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.617103918056899, + "y" : 2.418534455768619 + }, + "rotation" : { + "radians" : -2.7353292590414324 + } + }, + "curvature" : 0.30063598962569904 + }, { + "time" : 1.533414427222619, + "velocity" : 2.4877207376607204, + "acceleration" : -3.9999999999999933, + "pose" : { + "translation" : { + "x" : 2.553860298214431, + "y" : 2.3905214844488683 + }, + "rotation" : { + "radians" : -2.713608822413696 + } + }, + "curvature" : 0.32812363568576425 + }, { + "time" : 1.5609222653091537, + "velocity" : 2.3776893853145813, + "acceleration" : -3.949364309865745, + "pose" : { + "translation" : { + "x" : 2.493295711497262, + "y" : 2.3620607972557637 + }, + "rotation" : { + "radians" : -2.690632026659688 + } + }, + "curvature" : 0.35942929054988465 + }, { + "time" : 1.5887534138112722, + "velocity" : 2.267774040717741, + "acceleration" : -3.7932782770182274, + "pose" : { + "translation" : { + "x" : 2.43545586060295, + "y" : 2.333191504956364 + }, + "rotation" : { + "radians" : -2.6662749246206054 + } + }, + "curvature" : 0.3951155704482967 + }, { + "time" : 1.6454161870589727, + "velocity" : 2.0528363738416293, + "acceleration" : -3.524370787870329, + "pose" : { + "translation" : { + "x" : 2.328098505131493, + "y" : 2.2743839107679378 + }, + "rotation" : { + "radians" : -2.612872321869423 + } + }, + "curvature" : 0.4821864866479269 + }, { + "time" : 1.7034881688217829, + "velocity" : 1.8481691777230431, + "acceleration" : -3.1886854801463445, + "pose" : { + "translation" : { + "x" : 2.232009308124283, + "y" : 2.214410084736084 + }, + "rotation" : { + "radians" : -2.552217385705653 + } + }, + "curvature" : 0.5948949158111395 + }, { + "time" : 1.7629801786134969, + "velocity" : 1.658467869915481, + "acceleration" : -2.793593360246646, + "pose" : { + "translation" : { + "x" : 2.1472796517252846, + "y" : 2.153573117658766 + }, + "rotation" : { + "radians" : -2.4830442955743517 + } + }, + "curvature" : 0.7387705578516988 + }, { + "time" : 1.823812432610913, + "velocity" : 1.4885272890594612, + "acceleration" : -2.4614261655202934, + "pose" : { + "translation" : { + "x" : 2.0738578353503314, + "y" : 2.092161056443846 + }, + "rotation" : { + "radians" : -2.404174055212181 + } + }, + "curvature" : 0.9170860803614409 + }, { + "time" : 1.8547141636761173, + "velocity" : 1.4124649596556962, + "acceleration" : -2.226093041455955, + "pose" : { + "translation" : { + "x" : 2.041328791717895, + "y" : 2.0613236050452572 + }, + "rotation" : { + "radians" : -2.3608351537787935 + } + }, + "curvature" : 1.0185171315348982 + }, { + "time" : 1.8858592963049856, + "velocity" : 1.3431329966353498, + "acceleration" : -1.9794032204108298, + "pose" : { + "translation" : { + "x" : 2.011535687139258, + "y" : 2.0304401522735134 + }, + "rotation" : { + "radians" : -2.314846284252715 + } + }, + "curvature" : 1.1263819090275555 + }, { + "time" : 1.9171879104709784, + "velocity" : 1.2811210368641754, + "acceleration" : -1.7210458088826468, + "pose" : { + "translation" : { + "x" : 1.98442585493639, + "y" : 1.9995394662212282 + }, + "rotation" : { + "radians" : -2.26626721249112 + } + }, + "curvature" : 1.2380648061508586 + }, { + "time" : 1.9486228942778938, + "velocity" : 1.22701998973099, + "acceleration" : -1.4497718123734797, + "pose" : { + "translation" : { + "x" : 1.9599351754080274, + "y" : 1.9686481087687149 + }, + "rotation" : { + "radians" : -2.2152625603384335 + } + }, + "curvature" : 1.3496477269860494 + }, { + "time" : 1.9800679118911164, + "velocity" : 1.1814318895557523, + "acceleration" : -1.1629063029917064, + "pose" : { + "translation" : { + "x" : 1.9379876574375503, + "y" : 1.9377902245891243 + }, + "rotation" : { + "radians" : -2.162125525962242 + } + }, + "curvature" : 1.4558154532790182 + }, { + "time" : 2.0114057081537027, + "velocity" : 1.144988968760121, + "acceleration" : -0.8555700128978933, + "pose" : { + "translation" : { + "x" : 1.9184950201008633, + "y" : 1.9069873301535836 + }, + "rotation" : { + "radians" : -2.1072976365024556 + } + }, + "curvature" : 1.5499621702421391 + }, { + "time" : 2.042496890239119, + "velocity" : 1.1183882857022904, + "acceleration" : -0.5193738693932407, + "pose" : { + "translation" : { + "x" : 1.9013562742742751, + "y" : 1.8762581027363336 + }, + "rotation" : { + "radians" : -2.051380200524095 + } + }, + "curvature" : 1.6245702075644999 + }, { + "time" : 2.0731792244008695, + "velocity" : 1.1024526830866856, + "acceleration" : -0.1401133909627551, + "pose" : { + "translation" : { + "x" : 1.8864573042423771, + "y" : 1.8456181694198683 + }, + "rotation" : { + "radians" : -1.9951332453181105 + } + }, + "curvature" : 1.6718749322032591 + }, { + "time" : 2.103267289985402, + "velocity" : 1.098236942190127, + "acceleration" : 0.3065421531438961, + "pose" : { + "translation" : { + "x" : 1.873670449305921, + "y" : 1.8150798961000731 + }, + "rotation" : { + "radians" : -1.939459195747904 + } + }, + "curvature" : 1.6847350337062899 + }, { + "time" : 2.132552032689131, + "velocity" : 1.1072139502727931, + "acceleration" : 0.8636627394205474, + "pose" : { + "translation" : { + "x" : 1.8628540853896993, + "y" : 1.7846521764913632 + }, + "rotation" : { + "radians" : -1.885371407123151 + } + }, + "curvature" : 1.6575269774282888 + }, { + "time" : 2.1607993444095768, + "velocity" : 1.1316101008945396, + "acceleration" : 1.6117144791019087, + "pose" : { + "translation" : { + "x" : 1.8538522066504246, + "y" : 1.7543402211318213 + }, + "rotation" : { + "radians" : -1.8339512599110672 + } + }, + "curvature" : 1.5868287956600426 + }, { + "time" : 2.1877462109747206, + "velocity" : 1.175040755904009, + "acceleration" : 3.6208105431392568, + "pose" : { + "translation" : { + "x" : 1.8464940070846083, + "y" : 1.7241453463883367 + }, + "rotation" : { + "radians" : -1.7863006275868372 + } + }, + "curvature" : 1.4716950974629326 + }, { + "time" : 2.236042382237465, + "velocity" : 1.349912042005413, + "acceleration" : 3.9999999999999813, + "pose" : { + "translation" : { + "x" : 1.8359489103056603, + "y" : 1.6640913673919613 + }, + "rotation" : { + "radians" : -1.7065641844274897 + } + }, + "curvature" : 1.11509729054641 + }, { + "time" : 2.277907500803531, + "velocity" : 1.517372516269675, + "acceleration" : 3.9833148393409696, + "pose" : { + "translation" : { + "x" : 1.829540444920319, + "y" : 1.6044148692087425 + }, + "rotation" : { + "radians" : -1.654002070934546 + } + }, + "curvature" : 0.613882276060416 + }, { + "time" : 2.315345763310788, + "velocity" : 1.6665009028739752, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.825326681137085, + "y" : 1.5449646711349487 + }, + "rotation" : { + "radians" : -1.6352154187320553 + } + }, + "curvature" : 0.0 + }, { + "time" : 2.3528636610794607, + "velocity" : 1.5164293117992842, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.8219009519815899, + "y" : 1.485354601351787 + }, + "rotation" : { + "radians" : -1.6145927519610666 + } + }, + "curvature" : 0.6613132762682022 + }, { + "time" : 2.3953009609475626, + "velocity" : 1.3466801123268766, + "acceleration" : -2.447765685928921, + "pose" : { + "translation" : { + "x" : 1.8207798104572248, + "y" : 1.4246136307510824 + }, + "rotation" : { + "radians" : -1.559258462845717 + } + }, + "curvature" : 1.12045601019587 + }, { + "time" : 2.4441469639679103, + "velocity" : 1.2271165422388892, + "acceleration" : -0.5720634881939421, + "pose" : { + "translation" : { + "x" : 1.8239080891303416, + "y" : 1.361831680281739 + }, + "rotation" : { + "radians" : -1.480370605655411 + } + }, + "curvature" : 1.349435348223791 + }, { + "time" : 2.4708676658243456, + "velocity" : 1.2118306043279066, + "acceleration" : 0.15979441326309798, + "pose" : { + "translation" : { + "x" : 1.8275716032411027, + "y" : 1.3294530877098598 + }, + "rotation" : { + "radians" : -1.43570297822473 + } + }, + "curvature" : 1.3836934054080776 + }, { + "time" : 2.498501305753145, + "velocity" : 1.2162463056066528, + "acceleration" : 0.7244111612153562, + "pose" : { + "translation" : { + "x" : 1.8328603306872537, + "y" : 1.2963242800469743 + }, + "rotation" : { + "radians" : -1.389335893805699 + } + }, + "curvature" : 1.3736643754703268 + }, { + "time" : 2.526759486377394, + "velocity" : 1.2367168470464982, + "acceleration" : 1.1926306446746133, + "pose" : { + "translation" : { + "x" : 1.839917232537418, + "y" : 1.2623921911031442 + }, + "rotation" : { + "radians" : -1.3424216946735859 + } + }, + "curvature" : 1.3285660480051897 + }, { + "time" : 2.5553998339251613, + "velocity" : 1.270874203206097, + "acceleration" : 1.601933018243381, + "pose" : { + "translation" : { + "x" : 1.8488651751439278, + "y" : 1.2276157532543266 + }, + "rotation" : { + "radians" : -1.2959209814340706 + } + }, + "curvature" : 1.258109886245163 + }, { + "time" : 2.5842198818567117, + "velocity" : 1.317041989575004, + "acceleration" : 1.9730587889589195, + "pose" : { + "translation" : { + "x" : 1.8598076922431273, + "y" : 1.191965371940812 + }, + "rotation" : { + "radians" : -1.2505887693826183 + } + }, + "curvature" : 1.171451900081923 + }, { + "time" : 2.613052469764473, + "velocity" : 1.3739303805548424, + "acceleration" : 2.4837574247785343, + "pose" : { + "translation" : { + "x" : 1.8728297470556754, + "y" : 1.1554224001656621 + }, + "rotation" : { + "radians" : -1.206979322113377 + } + }, + "curvature" : 1.0764509613846949 + }, { + "time" : 2.670135599245487, + "velocity" : 1.5157110272329066, + "acceleration" : 3.0990658562394877, + "pose" : { + "translation" : { + "x" : 1.9053640427268448, + "y" : 1.0796356820471829 + }, + "rotation" : { + "radians" : -1.1262654169222006 + } + }, + "curvature" : 0.8844858371105864 + }, { + "time" : 2.7259954044656856, + "velocity" : 1.6888242423270121, + "acceleration" : 3.649016814110955, + "pose" : { + "translation" : { + "x" : 1.9468053174205124, + "y" : 1.000305405119434 + }, + "rotation" : { + "radians" : -1.0550981178089267 + } + }, + "curvature" : 0.7124506625993454 + }, { + "time" : 2.7801731094475235, + "velocity" : 1.8865195987556824, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 1.9972424725681748, + "y" : 0.9176229045073114 + }, + "rotation" : { + "radians" : -0.9932815877369497 + } + }, + "curvature" : 0.5709539223900668 + }, { + "time" : 2.8325182827204705, + "velocity" : 2.0959002918474696, + "acceleration" : 3.999999999999984, + "pose" : { + "translation" : { + "x" : 2.05654044297944, + "y" : 0.8319042281900693 + }, + "rotation" : { + "radians" : -0.939869543584731 + } + }, + "curvature" : 0.45987023815540634 + }, { + "time" : 2.8832022831916913, + "velocity" : 2.298636293732353, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 2.124364584051719, + "y" : 0.7435733209513273 + }, + "rotation" : { + "radians" : -0.8936544133402136 + } + }, + "curvature" : 0.37479587351455695 + }, { + "time" : 2.9324371813867773, + "velocity" : 2.4955758865126962, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 2.2002050589799182, + "y" : 0.6531452083290787 + }, + "rotation" : { + "radians" : -0.853427174289811 + } + }, + "curvature" : 0.3104667355759126 + }, { + "time" : 2.980286705128464, + "velocity" : 2.686973981479444, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 2.2834012259661307, + "y" : 0.5612091805656974 + }, + "rotation" : { + "radians" : -0.8180911393767909 + } + }, + "curvature" : 0.26216926405981567 + }, { + "time" : 3.0036886028665157, + "velocity" : 2.7805815724316503, + "acceleration" : 1.7834018615548795, + "pose" : { + "translation" : { + "x" : 2.3275160092188085, + "y" : 0.514876020137125 + }, + "rotation" : { + "radians" : -0.8019547141012324 + } + }, + "curvature" : 0.24281777874977586 + }, { + "time" : 3.026940898356585, + "velocity" : 2.822049759494064, + "acceleration" : 0.6908039852530397, + "pose" : { + "translation" : { + "x" : 2.3731660254293274, + "y" : 0.4684119765579453 + }, + "rotation" : { + "radians" : -0.7866964853329266 + } + }, + "curvature" : 0.22613732914460744 + }, { + "time" : 3.050322026787958, + "velocity" : 2.8382015361941697, + "acceleration" : 0.5944336707391082, + "pose" : { + "translation" : { + "x" : 2.420237140398036, + "y" : 0.4219041096592999 + }, + "rotation" : { + "radians" : -0.772220104481907 + } + }, + "curvature" : 0.2118009615897061 + }, { + "time" : 3.0738960629383696, + "velocity" : 2.8522147370371953, + "acceleration" : 0.5085625308709665, + "pose" : { + "translation" : { + "x" : 2.4686103672150495, + "y" : 0.37544096780698055 + }, + "rotation" : { + "radians" : -0.7584370845254595 + } + }, + "curvature" : 0.19952992215767287 + }, { + "time" : 3.097629651885521, + "velocity" : 2.86428475109881, + "acceleration" : 0.4316967127192361, + "pose" : { + "translation" : { + "x" : 2.518162628360552, + "y" : 0.3291120623998669 + }, + "rotation" : { + "radians" : -0.7452660870351325 + } + }, + "curvature" : 0.18908811609138015 + }, { + "time" : 3.1214873211538543, + "velocity" : 2.874584028495092, + "acceleration" : 0.3624475659523509, + "pose" : { + "translation" : { + "x" : 2.5687675178050995, + "y" : 0.28300734236836433 + }, + "rotation" : { + "radians" : -0.7326322058073411 + } + }, + "curvature" : 0.1802768862425938 + }, { + "time" : 3.145431891692516, + "velocity" : 2.8832626798046044, + "acceleration" : 0.2995436811273533, + "pose" : { + "translation" : { + "x" : 2.6202960631099237, + "y" : 0.23721666867284164 + }, + "rotation" : { + "radians" : -0.7204662745977262 + } + }, + "curvature" : 0.17293027048351933 + }, { + "time" : 3.1694248840625208, + "velocity" : 2.890449629060376, + "acceleration" : 0.24183189107987368, + "pose" : { + "translation" : { + "x" : 2.6726174875272335, + "y" : 0.19182928880206873 + }, + "rotation" : { + "radians" : -0.708704216367118 + } + }, + "curvature" : 0.16691079279926538 + }, { + "time" : 3.193426917481795, + "velocity" : 2.8962540861919215, + "acceleration" : 0.18827218906255389, + "pose" : { + "translation" : { + "x" : 2.7255999721005186, + "y" : 0.14693331127165443 + }, + "rotation" : { + "radians" : -0.6972864449360306 + } + }, + "curvature" : 0.16210577857562763 + }, { + "time" : 3.217398100152016, + "velocity" : 2.9007671932276624, + "acceleration" : 0.13793017248481926, + "pose" : { + "translation" : { + "x" : 2.7791114177648524, + "y" : 0.10261518012248416 + }, + "rotation" : { + "radians" : -0.6861573260641398 + } + }, + "curvature" : 0.15842414582671857 + }, { + "time" : 3.241298409734759, + "velocity" : 2.9040637670508507, + "acceleration" : 0.08996972588049867, + "pose" : { + "translation" : { + "x" : 2.8330202074471944, + "y" : 0.0589591494191577 + }, + "rotation" : { + "radians" : -0.6752647030503031 + } + }, + "curvature" : 0.1557936000148212 + }, { + "time" : 3.2650880630305976, + "velocity" : 2.9062041156366694, + "acceleration" : 0.04364814484804298, + "pose" : { + "translation" : { + "x" : 2.8871959681666937, + "y" : 0.0160467577484269 + }, + "rotation" : { + "radians" : -0.6645594915329975 + } + }, + "curvature" : 0.15415814328341032 + }, { + "time" : 3.288727873894095, + "velocity" : 2.9072359495254196, + "acceleration" : -0.0016842852381663946, + "pose" : { + "translation" : { + "x" : 2.941510333134991, + "y" : -0.02604369728236655 + }, + "rotation" : { + "radians" : -0.6539953489714027 + } + }, + "curvature" : 0.15347579410401332 + }, { + "time" : 3.3121795982222406, + "velocity" : 2.907196450132324, + "acceleration" : -0.046578151707487234, + "pose" : { + "translation" : { + "x" : 2.995837703856523, + "y" : -0.06723768454685342 + }, + "rotation" : { + "radians" : -0.6435284261390796 + } + }, + "curvature" : 0.15371639662570263 + }, { + "time" : 3.335406264503977, + "velocity" : 2.9061145949465943, + "acceleration" : -0.09146977902103462, + "pose" : { + "translation" : { + "x" : 3.0500560122288234, + "y" : -0.10746496490119917 + }, + "rotation" : { + "radians" : -0.6331172107927046 + } + }, + "curvature" : 0.15485937730919708 + }, { + "time" : 3.358372487917268, + "velocity" : 2.904013879566033, + "acceleration" : -0.13665410312300072, + "pose" : { + "translation" : { + "x" : 3.1040474826428266, + "y" : -0.14666011668566625 + }, + "rotation" : { + "radians" : -0.6227224774786422 + } + }, + "curvature" : 0.15689127710744225 + }, { + "time" : 3.381044765324835, + "velocity" : 2.900915619831146, + "acceleration" : -2.1491915946775815, + "pose" : { + "translation" : { + "x" : 3.157699394083171, + "y" : -0.18476306122617636 + }, + "rotation" : { + "radians" : -0.6123073622363067 + } + }, + "curvature" : 0.15980284830484542 + }, { + "time" : 3.403563912604914, + "velocity" : 2.8525176577774944, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.2109048422285014, + "y" : -0.22171958833587269 + }, + "rotation" : { + "radians" : -0.6018375867750714 + } + }, + "curvature" : 0.16358545453108617 + }, { + "time" : 3.4492280218552347, + "velocity" : 2.6698612207762125, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.315582387420548, + "y" : -0.2920090449608779 + }, + "rotation" : { + "radians" : -0.5806125205758887 + } + }, + "curvature" : 0.1737031415493062 + }, { + "time" : 3.4961580129617453, + "velocity" : 2.48214125635017, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 3.4173701773397624, + "y" : -0.35723214386962354 + }, + "rotation" : { + "radians" : -0.558846041472928 + } + }, + "curvature" : 0.18697261071178092 + }, { + "time" : 3.544441517737483, + "velocity" : 2.2890072372472194, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 3.5157001126304266, + "y" : -0.4172200702665805 + }, + "rotation" : { + "radians" : -0.5364315370742367 + } + }, + "curvature" : 0.20264494363905447 + }, { + "time" : 3.594311089348581, + "velocity" : 2.0895289508028285, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.610170322670001, + "y" : -0.471948353301741 + }, + "rotation" : { + "radians" : -0.5134162602944821 + } + }, + "curvature" : 0.2190045005586516 + }, { + "time" : 3.6462406863199543, + "velocity" : 1.8818105629173347, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.7005695527788163, + "y" : -0.5215536821206115 + }, + "rotation" : { + "radians" : -0.4900919671398013 + } + }, + "curvature" : 0.23265913487493842 + }, { + "time" : 3.701128109083174, + "velocity" : 1.6622608718644565, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.7869015514297644, + "y" : -0.5663507219142048 + }, + "rotation" : { + "radians" : -0.4671173179465116 + } + }, + "curvature" : 0.23773186828932136 + }, { + "time" : 3.7606892883012333, + "velocity" : 1.4240161549922183, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 3.8694094574579907, + "y" : -0.6068489299690327 + }, + "rotation" : { + "radians" : -0.4456529747120997 + } + }, + "curvature" : 0.2254664901452733 + }, { + "time" : 3.8285064753472717, + "velocity" : 1.1527474068080645, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 3.9486001872705856, + "y" : -0.6437693717170987 + }, + "rotation" : { + "radians" : -0.42745091338906643 + } + }, + "curvature" : 0.1853952813106193 + }, { + "time" : 3.9140667774226117, + "velocity" : 0.8105061985067056, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 4.025268822056276, + "y" : -0.67806153678589 + }, + "rotation" : { + "radians" : -0.414785534453574 + } + }, + "curvature" : 0.10957339292803865 + }, { + "time" : 4.116693327049288, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 4.100522994995117, + "y" : -0.7109201550483704 + }, + "rotation" : { + "radians" : -0.41008727651812055 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ], + "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ] + }, { + "x" : [ 1.825326681137085, -0.12263345718383789, 0.0 ], + "y" : [ 1.5449646711349487, -1.9010478258132935, 0.0 ] + }, { + "x" : [ 4.100522994995117, 2.3999733924865723, 0.0 ], + "y" : [ -0.7109201550483704, -1.0433523654937744, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : -3.1364759703203093 + } + }, { + "time" : 2.3099999483674765, + "rotation" : { + "radians" : 0.022477488801708422 + } + }, { + "time" : 4.029999909922481, + "rotation" : { + "radians" : -0.44615049906220317 + } + } ], + "reversed" : false, + "color" : 52.47058, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 2\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 2.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 4.100522994995117, + "y" : -0.7109201550483704 + }, + "rotation" : { + "radians" : 2.5857927494599444 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.17905554113279049, + "velocity" : 0.716222164531162, + "acceleration" : 3.999999999999999, + "pose" : { + "translation" : { + "x" : 4.046051351866051, + "y" : -0.6770905534088945 + }, + "rotation" : { + "radians" : 2.5859290584771175 + } + }, + "curvature" : 0.004100780417876861 + }, { + "time" : 0.2532222147861238, + "velocity" : 1.0128888591444951, + "acceleration" : 3.9999999999999982, + "pose" : { + "translation" : { + "x" : 3.9915714570599867, + "y" : -0.6432753174175616 + }, + "rotation" : { + "radians" : 2.5862997183209484 + } + }, + "curvature" : 0.007316179708346309 + }, { + "time" : 0.3101283388167845, + "velocity" : 1.2405133552671377, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.9370801190687295, + "y" : -0.6094880375241134 + }, + "rotation" : { + "radians" : 2.5868491896599624 + } + }, + "curvature" : 0.009686266454747228 + }, { + "time" : 0.35809346841854367, + "velocity" : 1.4323738736741745, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.8825794348939553, + "y" : -0.5757414756226353 + }, + "rotation" : { + "radians" : 2.5875244773924204 + } + }, + "curvature" : 0.011252124346097734 + }, { + "time" : 0.4003365631482469, + "velocity" : 1.6013462525929871, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.828076535205502, + "y" : -0.5420475838784169 + }, + "rotation" : { + "radians" : 2.588275148867047 + } + }, + "curvature" : 0.012054310762550804 + }, { + "time" : 0.43850530180719144, + "velocity" : 1.7540212072287653, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.7735833294996617, + "y" : -0.5084175235548116 + }, + "rotation" : { + "radians" : 2.5890532903104244 + } + }, + "curvature" : 0.0121315702156112 + }, { + "time" : 0.4735755889182925, + "velocity" : 1.8943023556731697, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.719116251257474, + "y" : -0.4748616838400972 + }, + "rotation" : { + "radians" : 2.589813411890699 + } + }, + "curvature" : 0.01151977337607476 + }, { + "time" : 0.5061806573270884, + "velocity" : 2.024722629308353, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.6646960031030176, + "y" : -0.4413897006743355 + }, + "rotation" : { + "radians" : 2.590512310722669 + } + }, + "curvature" : 0.010251050076436605 + }, { + "time" : 0.5367579667986577, + "velocity" : 2.147031867194631, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.610347301961702, + "y" : -0.4080104755762326 + }, + "rotation" : { + "radians" : 2.591108899964241 + } + }, + "curvature" : 0.008353084081511222 + }, { + "time" : 0.5656238557211262, + "velocity" : 2.2624954228845047, + "acceleration" : 3.9999999999999893, + "pose" : { + "translation" : { + "x" : 3.55609862421856, + "y" : -0.37473219446999906 + }, + "rotation" : { + "radians" : 2.5915640110153895 + } + }, + "curvature" : 0.0058485380718137905 + }, { + "time" : 0.6191140364610012, + "velocity" : 2.476456145844004, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.4480325127148035, + "y" : -0.308507742918664 + }, + "rotation" : { + "radians" : 2.5919013866336735 + } + }, + "curvature" : -9.17525897781337E-4 + }, { + "time" : 0.6679788967836321, + "velocity" : 2.671915587134528, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.340790984879593, + "y" : -0.24276823694478367 + }, + "rotation" : { + "radians" : 2.591240769502497 + } + }, + "curvature" : -0.009982710275132317 + }, { + "time" : 0.7130614555741249, + "velocity" : 2.8522458222964993, + "acceleration" : 3.5172385867566596, + "pose" : { + "translation" : { + "x" : 3.2347111984854564, + "y" : -0.17755532287992537 + }, + "rotation" : { + "radians" : 2.5893138286617203 + } + }, + "curvature" : -0.02138799605984592 + }, { + "time" : 0.7550700296469314, + "velocity" : 2.9999999999999996, + "acceleration" : 7.342027890777618E-15, + "pose" : { + "translation" : { + "x" : 3.130165997854956, + "y" : -0.11290100492229271 + }, + "rotation" : { + "radians" : 2.5858591271774563 + } + }, + "curvature" : -0.035290971114166826 + }, { + "time" : 0.7953939670709853, + "velocity" : 3.0, + "acceleration" : -7.485000429419822E-15, + "pose" : { + "translation" : { + "x" : 3.0275557589260416, + "y" : -0.04882824759624782 + }, + "rotation" : { + "radians" : 2.5806134504141105 + } + }, + "curvature" : -0.05197909891742402 + }, { + "time" : 0.8349476686405554, + "velocity" : 2.9999999999999996, + "acceleration" : -0.5537636550923151, + "pose" : { + "translation" : { + "x" : 2.9273002343174035, + "y" : 0.014648421788166388 + }, + "rotation" : { + "radians" : 2.573303027054985 + } + }, + "curvature" : -0.07188677132820555 + }, { + "time" : 0.87374969323478, + "velocity" : 2.97851284903572, + "acceleration" : -0.9070822146479386, + "pose" : { + "translation" : { + "x" : 2.829830398393824, + "y" : 0.07752231067570392 + }, + "rotation" : { + "radians" : 2.563634738472427 + } + }, + "curvature" : -0.095617170645365 + }, { + "time" : 0.9118975599042641, + "velocity" : 2.94390959765307, + "acceleration" : -2.262555771483939, + "pose" : { + "translation" : { + "x" : 2.73558029233153, + "y" : 0.13979395880639345 + }, + "rotation" : { + "radians" : 2.5512874700147776 + } + }, + "curvature" : -0.12396891770742956 + }, { + "time" : 0.9496761220339536, + "velocity" : 2.8584334938681764, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 2.6449788691835465, + "y" : 0.20147053575601603 + }, + "rotation" : { + "radians" : 2.5359039299223154 + } + }, + "curvature" : -0.15796560005178947 + }, { + "time" : 0.9877492069346551, + "velocity" : 2.7061411542653704, + "acceleration" : -3.9999999999999956, + "pose" : { + "translation" : { + "x" : 2.558441838945047, + "y" : 0.26256523847658286 + }, + "rotation" : { + "radians" : 2.517083600091246 + } + }, + "curvature" : -0.19888275461263835 + }, { + "time" : 1.0265482053771444, + "velocity" : 2.550945160495413, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 2.4763635136187077, + "y" : 0.323096688836813 + }, + "rotation" : { + "radians" : 2.494378068434297 + } + }, + "curvature" : -0.2482604413289035 + }, { + "time" : 1.0661195848558005, + "velocity" : 2.3926596425807887, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.3991086522800593, + "y" : 0.383088331162611 + }, + "rotation" : { + "radians" : 2.4672909353168477 + } + }, + "curvature" : -0.3078783541033658 + }, { + "time" : 1.1065518464830886, + "velocity" : 2.230930596071637, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.3270043061428396, + "y" : 0.44256782977754483 + }, + "rotation" : { + "radians" : 2.4352859073052295 + } + }, + "curvature" : -0.37965241689117946 + }, { + "time" : 1.1479982964991189, + "velocity" : 2.065144796007515, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 2.2603316636243456, + "y" : 0.5015664665433235 + }, + "rotation" : { + "radians" : 2.397808651299899 + } + }, + "curvature" : -0.46538626343634765 + }, { + "time" : 1.1907134105418258, + "velocity" : 1.8942843398366878, + "acceleration" : -3.8004076554878155, + "pose" : { + "translation" : { + "x" : 2.199317895410786, + "y" : 0.5601185384002747 + }, + "rotation" : { + "radians" : 2.35433030289942 + } + }, + "curvature" : -0.5662827934211753 + }, { + "time" : 1.2350003317172529, + "velocity" : 1.725975985563609, + "acceleration" : -3.096610871868397, + "pose" : { + "translation" : { + "x" : 2.1441279995226346, + "y" : 0.6182607549078227 + }, + "rotation" : { + "radians" : 2.304422426440884 + } + }, + "curvature" : -0.6821096692467903 + }, { + "time" : 1.2808802623624265, + "velocity" : 1.5839036935271966, + "acceleration" : -2.455893226230777, + "pose" : { + "translation" : { + "x" : 2.0948566463799807, + "y" : 0.6760316357849661 + }, + "rotation" : { + "radians" : 2.24787278990788 + } + }, + "curvature" : -0.8099648281546794 + }, { + "time" : 1.3280319668435383, + "velocity" : 1.4681041418867986, + "acceleration" : -1.8499614404769908, + "pose" : { + "translation" : { + "x" : 2.0515200238678837, + "y" : 0.7334709084507551 + }, + "rotation" : { + "radians" : 2.184845091649205 + } + }, + "curvature" : -0.9427791639774807 + }, { + "time" : 1.376032015282584, + "velocity" : 1.3793059031335362, + "acceleration" : -1.248435182006515, + "pose" : { + "translation" : { + "x" : 2.014047682401724, + "y" : 0.7906189055647701 + }, + "rotation" : { + "radians" : 2.1160696333836517 + } + }, + "curvature" : -1.0680768788914654 + }, { + "time" : 1.424334569704738, + "velocity" : 1.3190032948121349, + "acceleration" : -0.6091506293120736, + "pose" : { + "translation" : { + "x" : 1.9822743799925568, + "y" : 0.8475159625675985 + }, + "rotation" : { + "radians" : 2.0430250430635404 + } + }, + "curvature" : -1.1679706858124066 + }, { + "time" : 1.4722549198410007, + "velocity" : 1.2898125833697756, + "acceleration" : 0.14219135021541737, + "pose" : { + "translation" : { + "x" : 1.955931927312463, + "y" : 0.9042018152213132 + }, + "rotation" : { + "radians" : 1.9680433024899477 + } + }, + "curvature" : -1.221435348671096 + }, { + "time" : 1.5189560337976418, + "velocity" : 1.2964530778198344, + "acceleration" : 1.1656913546554033, + "pose" : { + "translation" : { + "x" : 1.9346410327599024, + "y" : 0.9607149971499496 + }, + "rotation" : { + "radians" : 1.894265907226921 + } + }, + "curvature" : -1.2089548933496592 + }, { + "time" : 1.563428663287173, + "velocity" : 1.348294437534574, + "acceleration" : 2.8841346574602866, + "pose" : { + "translation" : { + "x" : 1.9179031475250667, + "y" : 1.017092237379984 + }, + "rotation" : { + "radians" : 1.8254232181396424 + } + }, + "curvature" : -1.1177745520566962 + }, { + "time" : 1.604436283065203, + "velocity" : 1.4665659349563442, + "acceleration" : 3.999999999999992, + "pose" : { + "translation" : { + "x" : 1.905092310655231, + "y" : 1.073367857880811 + }, + "rotation" : { + "radians" : 1.7654947807008987 + } + }, + "curvature" : -0.9447578680023896 + }, { + "time" : 1.6414523727107633, + "velocity" : 1.6146302935385846, + "acceleration" : 3.7005826612074797, + "pose" : { + "translation" : { + "x" : 1.8954469941201069, + "y" : 1.1295731711052213 + }, + "rotation" : { + "radians" : 1.7183797533566942 + } + }, + "curvature" : -0.6949978934363836 + }, { + "time" : 1.706643337288398, + "velocity" : 1.855874846721971, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.8818800449371338, + "y" : 1.2418794631958008 + }, + "rotation" : { + "radians" : 1.676778825261928 + } + }, + "curvature" : -0.0 + }, { + "time" : 1.7723136975830924, + "velocity" : 1.5931934055431933, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.8715964211490927, + "y" : 1.3546623770804445 + }, + "rotation" : { + "radians" : 1.632957005658238 + } + }, + "curvature" : -0.7254972162927384 + }, { + "time" : 1.810034487394077, + "velocity" : 1.4423102462992545, + "acceleration" : -2.710726662081756, + "pose" : { + "translation" : { + "x" : 1.8693755361295565, + "y" : 1.4118700819531647 + }, + "rotation" : { + "radians" : 1.5838413049635738 + } + }, + "curvature" : -0.9768015183979825 + }, { + "time" : 1.8519046258057839, + "velocity" : 1.3288117457615873, + "acceleration" : -1.1894816130443073, + "pose" : { + "translation" : { + "x" : 1.870371506531228, + "y" : 1.4698751626601734 + }, + "rotation" : { + "radians" : 1.521732134311164 + } + }, + "curvature" : -1.1507918750528034 + }, { + "time" : 1.8972897575518486, + "velocity" : 1.2748269660440499, + "acceleration" : -0.3477506556011637, + "pose" : { + "translation" : { + "x" : 1.8753421949347988, + "y" : 1.528748941692104 + }, + "rotation" : { + "radians" : 1.4504225267826032 + } + }, + "curvature" : -1.2503201227521346 + }, { + "time" : 1.9450499018565859, + "velocity" : 1.2582183445504713, + "acceleration" : 0.2016282832037308, + "pose" : { + "translation" : { + "x" : 1.8849010019640673, + "y" : 1.5884782075607973 + }, + "rotation" : { + "radians" : 1.3734581625763413 + } + }, + "curvature" : -1.28354671036822 + }, { + "time" : 1.9943208439648752, + "velocity" : 1.268152760019596, + "acceleration" : 0.5969274636685241, + "pose" : { + "translation" : { + "x" : 1.89952602502051, + "y" : 1.6489738267635623 + }, + "rotation" : { + "radians" : 1.293941372408038 + } + }, + "curvature" : -1.2635154627438518 + }, { + "time" : 2.0444402181362453, + "velocity" : 1.2980703909243658, + "acceleration" : 0.8935195397370149, + "pose" : { + "translation" : { + "x" : 1.9195692170178518, + "y" : 1.710079355747439 + }, + "rotation" : { + "radians" : 1.2143690380279577 + } + }, + "curvature" : -1.2059442083489496 + }, { + "time" : 2.094911061001386, + "velocity" : 1.3431670752113656, + "acceleration" : 1.1141266498975333, + "pose" : { + "translation" : { + "x" : 1.9452655451166372, + "y" : 1.7715796528734593 + }, + "rotation" : { + "radians" : 1.1365566560794031 + } + }, + "curvature" : -1.1263247530634717 + }, { + "time" : 2.1453768398619966, + "velocity" : 1.3993923443478076, + "acceleration" : 1.2676595640303998, + "pose" : { + "translation" : { + "x" : 1.9767421494588007, + "y" : 1.8332094903809093 + }, + "rotation" : { + "radians" : 1.061661442713639 + } + }, + "curvature" : -1.0376352482876874 + }, { + "time" : 2.1955990497229334, + "velocity" : 1.4630570090047659, + "acceleration" : 1.3576641534621845, + "pose" : { + "translation" : { + "x" : 2.0140275019022376, + "y" : 1.8946621663515906 + }, + "rotation" : { + "radians" : 0.9902801108323195 + } + }, + "curvature" : -0.949295026502291 + }, { + "time" : 2.2454352355231433, + "velocity" : 1.5307178120109917, + "acceleration" : 1.3867285924070238, + "pose" : { + "translation" : { + "x" : 2.057060564755375, + "y" : 1.9555981166740821 + }, + "rotation" : { + "radians" : 0.9225815953395504 + } + }, + "curvature" : -0.8672282806254888 + }, { + "time" : 2.2948177631789672, + "velocity" : 1.5991979750766532, + "acceleration" : 1.3589820685583802, + "pose" : { + "translation" : { + "x" : 2.1056999495117417, + "y" : 2.015653527008002 + }, + "rotation" : { + "radians" : 0.858440269383883 + } + }, + "curvature" : -0.7945463578337496 + }, { + "time" : 2.3437341392878004, + "velocity" : 1.6656744530674152, + "acceleration" : 1.2814168669463941, + "pose" : { + "translation" : { + "x" : 2.1597330755845405, + "y" : 2.0744489447482692 + }, + "rotation" : { + "radians" : 0.7975494729913294 + } + }, + "curvature" : -0.7323917673147309 + }, { + "time" : 2.3922093962778366, + "velocity" : 1.7277914650040085, + "acceleration" : 1.1642994999905252, + "pose" : { + "translation" : { + "x" : 2.2188853290412176, + "y" : 2.131597890989365 + }, + "rotation" : { + "radians" : 0.7395085520599514 + } + }, + "curvature" : -0.6806769668594456 + }, { + "time" : 2.4402906479447277, + "velocity" : 1.7837724422786883, + "acceleration" : 1.0208386073494335, + "pose" : { + "translation" : { + "x" : 2.2828292213380337, + "y" : 2.1867154724895954 + }, + "rotation" : { + "radians" : 0.6838850476440103 + } + }, + "curvature" : -0.6386233642001887 + }, { + "time" : 2.4880336196518704, + "velocity" : 1.8325103110269312, + "acceleration" : 0.8663296664660994, + "pose" : { + "translation" : { + "x" : 2.351193548054635, + "y" : 2.2394269936353517 + }, + "rotation" : { + "radians" : 0.6302573876108508 + } + }, + "curvature" : -0.6051051506550083 + }, { + "time" : 2.5354908013854165, + "velocity" : 1.873623875449575, + "acceleration" : 0.7170825323129972, + "pose" : { + "translation" : { + "x" : 2.4235725476286234, + "y" : 2.2893765684053733 + }, + "rotation" : { + "radians" : 0.5782441181597022 + } + }, + "curvature" : -0.5788404596526515 + }, { + "time" : 2.582700817287668, + "velocity" : 1.9074773532032985, + "acceleration" : 0.5894907207875506, + "pose" : { + "translation" : { + "x" : 2.4995350600901283, + "y" : 2.336235732335009 + }, + "rotation" : { + "radians" : 0.5275249535906203 + } + }, + "curvature" : -0.5584765250518172 + }, { + "time" : 2.629678599279987, + "velocity" : 1.935170319770951, + "acceleration" : 0.49960482001977663, + "pose" : { + "translation" : { + "x" : 2.578633685796376, + "y" : 2.3797120544804784 + }, + "rotation" : { + "radians" : 0.47785770284077494 + } + }, + "curvature" : -0.5426069030826571 + }, { + "time" : 2.6764059539315443, + "velocity" : 1.9585155313816427, + "acceleration" : 0.46356554662244476, + "pose" : { + "translation" : { + "x" : 2.6604139441662618, + "y" : 2.4195577493831344 + }, + "rotation" : { + "radians" : 0.42909391062732893 + } + }, + "curvature" : -0.5297484122773238 + }, { + "time" : 2.7228220905005966, + "velocity" : 1.9800324531023774, + "acceleration" : 0.4993320852380286, + "pose" : { + "translation" : { + "x" : 2.7444234324149193, + "y" : 2.455578289033724 + }, + "rotation" : { + "radians" : 0.3811949720767674 + } + }, + "curvature" : -0.5182974673057555 + }, { + "time" : 2.7688136027162047, + "velocity" : 2.0029974908002472, + "acceleration" : 0.6304938957131165, + "pose" : { + "translation" : { + "x" : 2.8302209842882924, + "y" : 2.487641014836651 + }, + "rotation" : { + "radians" : 0.33424953905468247 + } + }, + "curvature" : -0.5064806911118291 + }, { + "time" : 2.8142032161774035, + "velocity" : 2.031615365016311, + "acceleration" : 0.8939968521835171, + "pose" : { + "translation" : { + "x" : 2.9173858287977055, + "y" : 2.5156837495742366 + }, + "rotation" : { + "radians" : 0.28849219299419626 + } + }, + "curvature" : -0.4923123451071928 + }, { + "time" : 2.85873622335247, + "velocity" : 2.0714277332490862, + "acceleration" : 1.3565333753864743, + "pose" : { + "translation" : { + "x" : 3.005526748954434, + "y" : 2.539723409370982 + }, + "rotation" : { + "radians" : 0.24432259326770525 + } + }, + "curvature" : -0.4735699432966268 + }, { + "time" : 2.902062698889524, + "velocity" : 2.1302015433529657, + "acceleration" : 2.153155459162207, + "pose" : { + "translation" : { + "x" : 3.094291240504276, + "y" : 2.5598646156578297 + }, + "rotation" : { + "radians" : 0.20232364570316902 + } + }, + "curvature" : -0.4477981691549623 + }, { + "time" : 2.9437117576976832, + "velocity" : 2.2198784416947213, + "acceleration" : 3.592461951491249, + "pose" : { + "translation" : { + "x" : 3.183374670662122, + "y" : 2.5763083071364257 + }, + "rotation" : { + "radians" : 0.1632767221779342 + } + }, + "curvature" : -0.4123493520484391 + }, { + "time" : 2.9830497106114113, + "velocity" : 2.3611985407868445, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 3.2725294368465256, + "y" : 2.5893603517433803 + }, + "rotation" : { + "radians" : 0.12817160720952014 + } + }, + "curvature" : -0.36446740220852936 + }, { + "time" : 3.0198548135911913, + "velocity" : 2.508418952705964, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 3.3615741254142755, + "y" : 2.599440158614531 + }, + "rotation" : { + "radians" : 0.0982084966435118 + } + }, + "curvature" : -0.30142499142103213 + }, { + "time" : 3.0544441058745386, + "velocity" : 2.646776121839353, + "acceleration" : 3.99999999999999, + "pose" : { + "translation" : { + "x" : 3.450402670394965, + "y" : 2.6070892900492026 + }, + "rotation" : { + "radians" : 0.07478856387691135 + } + }, + "curvature" : -0.22073732623662828 + }, { + "time" : 3.087179502133278, + "velocity" : 2.7777177068743097, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.538993512225563, + "y" : 2.6129800734744713 + }, + "rotation" : { + "radians" : 0.05948748242671234 + } + }, + "curvature" : -0.1205067561761745 + }, { + "time" : 3.118362856162708, + "velocity" : 2.9024511229920305, + "acceleration" : 3.2635486192884477, + "pose" : { + "translation" : { + "x" : 3.6274187564849854, + "y" : 2.617924213409424 + }, + "rotation" : { + "radians" : 0.0540017456962338 + } + }, + "curvature" : 0.0 + }, { + "time" : 3.1482532878331386, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.7155035994555945, + "y" : 2.6226850363620713 + }, + "rotation" : { + "radians" : 0.053981642702760804 + } + }, + "curvature" : -5.351308120085834E-4 + }, { + "time" : 3.1770862157675355, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.801876553490956, + "y" : 2.627348984328819 + }, + "rotation" : { + "radians" : 0.05389383309434626 + } + }, + "curvature" : -0.001601464316657459 + }, { + "time" : 3.2048908409093957, + "velocity" : 3.0, + "acceleration" : -1.1198792336370368E-14, + "pose" : { + "translation" : { + "x" : 3.885169715891969, + "y" : 2.631834930243855 + }, + "rotation" : { + "radians" : 0.05369200401278312 + } + }, + "curvature" : -0.0033948580681829725 + }, { + "time" : 3.231327573044508, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.964366333158978, + "y" : 2.6360779324022587 + }, + "rotation" : { + "radians" : 0.053319276839782104 + } + }, + "curvature" : -0.006242169384276816 + }, { + "time" : 3.256165149733738, + "velocity" : 2.9999999999999996, + "acceleration" : 1.2812741871263266E-14, + "pose" : { + "translation" : { + "x" : 4.038774281627788, + "y" : 2.640028140852877 + }, + "rotation" : { + "radians" : 0.0527039245624449 + } + }, + "curvature" : -0.01063132499803715 + }, { + "time" : 3.2792717943698393, + "velocity" : 3.0, + "acceleration" : -7.231938957008415E-15, + "pose" : { + "translation" : { + "x" : 4.107999548105681, + "y" : 2.6436497037911977 + }, + "rotation" : { + "radians" : 0.05175599940745225 + } + }, + "curvature" : -0.017237888913125436 + }, { + "time" : 3.320209567074138, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.230657418491319, + "y" : 2.6498269150033593 + }, + "rotation" : { + "radians" : 0.04841053128135275 + } + }, + "curvature" : -0.04051841469935257 + }, { + "time" : 3.3547406099731343, + "velocity" : 2.9999999999999996, + "acceleration" : -0.6325094018133772, + "pose" : { + "translation" : { + "x" : 4.334142312372251, + "y" : 2.654561157464741 + }, + "rotation" : { + "radians" : 0.0423299273622696 + } + }, + "curvature" : -0.08057010884102427 + }, { + "time" : 3.384584997268422, + "velocity" : 2.9811231444443704, + "acceleration" : -0.08050231493317403, + "pose" : { + "translation" : { + "x" : 4.4233291260534315, + "y" : 2.6579580020916183 + }, + "rotation" : { + "radians" : 0.03315653358683334 + } + }, + "curvature" : -0.1233537474620638 + }, { + "time" : 3.4121081263600206, + "velocity" : 2.978907468838292, + "acceleration" : 0.757187202067055, + "pose" : { + "translation" : { + "x" : 4.5053166644408975, + "y" : 2.6602420038625496 + }, + "rotation" : { + "radians" : 0.022490284597376595 + } + }, + "curvature" : -0.1268933627335242 + }, { + "time" : 3.4399645548682245, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.588579021394253, + "y" : 2.661721706390381 + }, + "rotation" : { + "radians" : 0.01374388263590382 + } + }, + "curvature" : -0.07804765164973368 + }, { + "time" : 3.471145768832688, + "velocity" : 3.0, + "acceleration" : -7.895171233701495E-15, + "pose" : { + "translation" : { + "x" : 4.682116960079156, + "y" : 2.6627546464942498 + }, + "rotation" : { + "radians" : 0.009189096860167837 + } + }, + "curvature" : -0.024057614430750256 + }, { + "time" : 3.508644572148012, + "velocity" : 2.9999999999999996, + "acceleration" : 1.3503955874947296E-14, + "pose" : { + "translation" : { + "x" : 4.794609293319809, + "y" : 2.6637123587715905 + }, + "rotation" : { + "radians" : 0.008349254243389597 + } + }, + "curvature" : 0.0037103968236503098 + }, { + "time" : 3.5305684791849585, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.860378613618231, + "y" : 2.6642743243984057 + }, + "rotation" : { + "radians" : 0.00879853748094375 + } + }, + "curvature" : 0.009166818258418976 + }, { + "time" : 3.554964721451485, + "velocity" : 3.0, + "acceleration" : -1.992727596873581, + "pose" : { + "translation" : { + "x" : 4.933564263951439, + "y" : 2.6649453801701384 + }, + "rotation" : { + "radians" : 0.009565324282873914 + } + }, + "curvature" : 0.011329196406191677 + }, { + "time" : 3.5822868057216533, + "velocity" : 2.9455545286707294, + "acceleration" : -3.9999999999999902, + "pose" : { + "translation" : { + "x" : 5.01478264645214, + "y" : 2.6657603208850844 + }, + "rotation" : { + "radians" : 0.010504868424232214 + } + }, + "curvature" : 0.011571381180452858 + }, { + "time" : 3.613394389130737, + "velocity" : 2.821124195034394, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.104470925172791, + "y" : 2.666748254559934 + }, + "rotation" : { + "radians" : 0.011513061014706312 + } + }, + "curvature" : 0.01080959208114291 + }, { + "time" : 3.649180845248966, + "velocity" : 2.6779783705614784, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.202860506721613, + "y" : 2.6679315088226474 + }, + "rotation" : { + "radians" : 0.012518180765925502 + } + }, + "curvature" : 0.009593832950905505 + }, { + "time" : 3.690445008393871, + "velocity" : 2.5129217179818584, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.30995052089861, + "y" : 2.6693245373053287 + }, + "rotation" : { + "radians" : 0.01347191339238352 + } + }, + "curvature" : 0.008225115612607133 + }, { + "time" : 3.738242422456052, + "velocity" : 2.3217320617331336, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.42548130133158, + "y" : 2.6709328260371024 + }, + "rotation" : { + "radians" : 0.014341527416621765 + } + }, + "curvature" : 0.006849417564785735 + }, { + "time" : 3.7940970341308637, + "velocity" : 2.098313615033887, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.548907866112131, + "y" : 2.6727517998369876 + }, + "rotation" : { + "radians" : 0.015103596532319463 + } + }, + "curvature" : 0.005520155477322901 + }, { + "time" : 3.825756511779807, + "velocity" : 1.9716757044381135, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 5.6133272185697205, + "y" : 2.673735843651399 + }, + "rotation" : { + "radians" : 0.015438273175695332 + } + }, + "curvature" : 0.004873668640140536 + }, { + "time" : 3.860481196136515, + "velocity" : 1.8327769670112823, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.6793733984317, + "y" : 2.674765728706774 + }, + "rotation" : { + "radians" : 0.015738980768460056 + } + }, + "curvature" : 0.004234449415552546 + }, { + "time" : 3.89893588264187, + "velocity" : 1.678958220989862, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.746886228560693, + "y" : 2.6758375663932243 + }, + "rotation" : { + "radians" : 0.016003288340590872 + } + }, + "curvature" : 0.003596071353112922 + }, { + "time" : 3.9421404788446717, + "velocity" : 1.5061398361786555, + "acceleration" : -3.999999999999997, + "pose" : { + "translation" : { + "x" : 5.815682727217563, + "y" : 2.6769466342238957 + }, + "rotation" : { + "radians" : 0.016228495089680457 + } + }, + "curvature" : 0.0029497967820049952 + }, { + "time" : 3.991815920611223, + "velocity" : 1.3074380691124496, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.885556279331293, + "y" : 2.678087341659743 + }, + "rotation" : { + "radians" : 0.016411476850763638 + } + }, + "curvature" : 0.0022842194098207026 + }, { + "time" : 4.0513318851535365, + "velocity" : 1.0693742109431945, + "acceleration" : -3.9999999999999982, + "pose" : { + "translation" : { + "x" : 5.956275807768854, + "y" : 2.679253195934308 + }, + "rotation" : { + "radians" : 0.0165485050781744 + } + }, + "curvature" : 0.0015845831447143007 + }, { + "time" : 4.129431954649663, + "velocity" : 0.75697393295869, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.027584944605088, + "y" : 2.680436767878498 + }, + "rotation" : { + "radians" : 0.016635023032639307 + } + }, + "curvature" : 8.316618868490522E-4 + }, { + "time" : 4.318675437889335, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.099201202392578, + "y" : 2.6816296577453613 + }, + "rotation" : { + "radians" : 0.016665357555075345 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.100522994995117, -3.486077070236206, 0.0 ], + "y" : [ -0.7109201550483704, 2.165250778198242, 0.0 ] + }, { + "x" : [ 1.8818800449371338, -0.19111382961273193, 0.0 ], + "y" : [ 1.2418794631958008, 1.7965017557144165, 0.0 ] + }, { + "x" : [ 3.6274187564849854, 2.828535318374634, 0.0 ], + "y" : [ 2.617924213409424, 0.1528944969177246, 0.0 ] + }, { + "x" : [ 6.099201202392578, 4.586813449859619, 0.0 ], + "y" : [ 2.6816296577453613, 0.07644796371459961, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.584307420515759 + } + }, { + "time" : 1.699999962002039, + "rotation" : { + "radians" : 2.7676508674151563 + } + }, { + "time" : 3.1199999302625656, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 4.229999905452132, + "rotation" : { + "radians" : 0.00740696187012303 + } + } ], + "reversed" : false, + "color" : 286.3529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + } ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/red/buddyautoleftsteal.json b/src/main/deploy/autos/red/buddyautoleftsteal.json new file mode 100644 index 00000000..5264679f --- /dev/null +++ b/src/main/deploy/autos/red/buddyautoleftsteal.json @@ -0,0 +1,3806 @@ +{ + "autonomousSteps" : [ { + "type" : "script", + "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON\nsleep 1500", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { + "methodName" : "sleep", + "args" : [ "1500" ], + "argTypes" : [ "long" ], + "reflection" : false + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.979461669921875, + "y" : 0.4386838674545288 + }, + "rotation" : { + "radians" : 2.4670512351406453 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.2218186293424793, + "velocity" : 0.8872745173699175, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 6.902607691762569, + "y" : 0.5001444677761242 + }, + "rotation" : { + "radians" : 2.4669815013720786 + } + }, + "curvature" : -0.001770057444385252 + }, { + "time" : 0.3128823124020749, + "velocity" : 1.2515292496083, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 6.826564423769014, + "y" : 0.5609803238967288 + }, + "rotation" : { + "radians" : 2.46664012448608 + } + }, + "curvature" : -0.005584153832447457 + }, { + "time" : 0.38171187830481973, + "velocity" : 1.526847513219279, + "acceleration" : 3.999999999999998, + "pose" : { + "translation" : { + "x" : 6.751933529499183, + "y" : 0.6207539377647322 + }, + "rotation" : { + "radians" : 2.465845563577437 + } + }, + "curvature" : -0.011384177747344891 + }, { + "time" : 0.4386276460816924, + "velocity" : 1.7545105843267694, + "acceleration" : 3.999999999999998, + "pose" : { + "translation" : { + "x" : 6.679111945442855, + "y" : 0.6792085023480467 + }, + "rotation" : { + "radians" : 2.4644380166116457 + } + }, + "curvature" : -0.019103896941460693 + }, { + "time" : 0.4876979714419739, + "velocity" : 1.9507918857678952, + "acceleration" : 3.9999999999999925, + "pose" : { + "translation" : { + "x" : 6.608317537175935, + "y" : 0.7362419663763262 + }, + "rotation" : { + "radians" : 2.4622863748597443 + } + }, + "curvature" : -0.028529142584546568 + }, { + "time" : 0.5310864114411233, + "velocity" : 2.1243456457644925, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.539614755514776, + "y" : 0.7918810990831844 + }, + "rotation" : { + "radians" : 2.459302059694302 + } + }, + "curvature" : -0.03918060333359783 + }, { + "time" : 0.5701494584010045, + "velocity" : 2.2805978336040176, + "acceleration" : -0.47832492111012465, + "pose" : { + "translation" : { + "x" : 6.472940292670501, + "y" : 0.8462555549484136 + }, + "rotation" : { + "radians" : 2.455456663103906 + } + }, + "curvature" : -0.05024047921606022 + }, { + "time" : 0.6070915310824326, + "velocity" : 2.262927519603029, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.40812873840332, + "y" : 0.8995719384402037 + }, + "rotation" : { + "radians" : 2.4507991475637327 + } + }, + "curvature" : -0.06056066057312981 + }, { + "time" : 0.6446468880225168, + "velocity" : 2.1127060918426923, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 6.3449382361768585, + "y" : 0.95208786875736 + }, + "rotation" : { + "radians" : 2.445467378655361 + } + }, + "curvature" : -0.06878202666638442 + }, { + "time" : 0.6843932978096723, + "velocity" : 1.9537204526940704, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.28307613931247, + "y" : 1.0040860445715225 + }, + "rotation" : { + "radians" : 2.4396891056204377 + } + }, + "curvature" : -0.07355656872184071 + }, { + "time" : 0.7271558960672502, + "velocity" : 1.782670059663759, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.222224667143564, + "y" : 1.0558483087693844 + }, + "rotation" : { + "radians" : 2.4337699158796475 + } + }, + "curvature" : -0.07380896893317555 + }, { + "time" : 0.7741602914447601, + "velocity" : 1.5946524781537195, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.162066561169922, + "y" : 1.1076297131949104 + }, + "rotation" : { + "radians" : 2.428069867169106 + } + }, + "curvature" : -0.06893533742465202 + }, { + "time" : 0.8273894003721558, + "velocity" : 1.3817360424441365, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 6.1023107412120225, + "y" : 1.1596325833915557 + }, + "rotation" : { + "radians" : 2.422974921973857 + } + }, + "curvature" : -0.058846523386872546 + }, { + "time" : 0.8905738529525908, + "velocity" : 1.1289982321223961, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 6.042717961565359, + "y" : 1.2119805833444843 + }, + "rotation" : { + "radians" : 2.418872019409643 + } + }, + "curvature" : -0.043821739722789785 + }, { + "time" : 0.9731110220523553, + "velocity" : 0.7988495557233383, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.9831264671547615, + "y" : 1.2646927802227879 + }, + "rotation" : { + "radians" : 2.4161366104589406 + } + }, + "curvature" : -0.02420493001126614 + }, { + "time" : 1.1728234109831899, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.923477649688721, + "y" : 1.317657709121704 + }, + "rotation" : { + "radians" : 2.4151395686141304 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 6.979461669921875, -1.232039451599121, 0.0 ], + "y" : [ 0.4386838674545288, 0.9852284789085388, 0.0 ] + }, { + "x" : [ 5.923477649688721, -0.9546575546264648, 0.0 ], + "y" : [ 1.317657709121704, 0.8482611179351807, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.7759904509715376 + } + }, { + "time" : 1.0799999758601189, + "rotation" : { + "radians" : 2.3681701999637936 + } + } ], + "reversed" : false, + "color" : 209.88235, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "VisionManager.shootBalls 13", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "13" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.923477649688721, + "y" : 1.317657709121704 + }, + "rotation" : { + "radians" : 2.4280629259945457 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.1916862579332135, + "velocity" : 0.766745031732854, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.8679256060540865, + "y" : 1.3657653522359396 + }, + "rotation" : { + "radians" : 2.427546282399253 + } + }, + "curvature" : -0.013552982296289647 + }, { + "time" : 0.26883282720714063, + "velocity" : 1.0753313088285628, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.814256962185027, + "y" : 1.4123322981758974 + }, + "rotation" : { + "radians" : 2.426170125310746 + } + }, + "curvature" : -0.025078420554477936 + }, { + "time" : 0.3253598004362359, + "velocity" : 1.3014392017449439, + "acceleration" : 3.999999999999993, + "pose" : { + "translation" : { + "x" : 5.763593902279354, + "y" : 1.4564440353524333 + }, + "rotation" : { + "radians" : 2.424133609238276 + } + }, + "curvature" : -0.035413495196781865 + }, { + "time" : 0.37047014804781875, + "velocity" : 1.4818805921912748, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.716341427527368, + "y" : 1.4977757879532874 + }, + "rotation" : { + "radians" : 2.4216494598565745 + } + }, + "curvature" : -0.043163158740255655 + }, { + "time" : 0.44071473447231035, + "velocity" : 1.7628589378892412, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.630849667446455, + "y" : 1.57313263497781 + }, + "rotation" : { + "radians" : 2.4166846345884783 + } + }, + "curvature" : -0.03766210226111181 + }, { + "time" : 0.4973489273055233, + "velocity" : 1.989395709222093, + "acceleration" : -1.536593152344523, + "pose" : { + "translation" : { + "x" : 5.551411211490631, + "y" : 1.643696241080761 + }, + "rotation" : { + "radians" : 2.4146239525620357 + } + }, + "curvature" : 0.002386794122151864 + }, { + "time" : 0.5545817819308377, + "velocity" : 1.9014520967157054, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.468152332963655, + "y" : 1.7176221262780018 + }, + "rotation" : { + "radians" : 2.416957846275337 + } + }, + "curvature" : 0.03338570540861069 + }, { + "time" : 0.5881329413084252, + "velocity" : 1.7672474592053555, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.422027663326844, + "y" : 1.7583682663707805 + }, + "rotation" : { + "radians" : 2.419124204295016 + } + }, + "curvature" : 0.03568476607637792 + }, { + "time" : 0.6278550666121283, + "velocity" : 1.6083589579905428, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.3716818848624825, + "y" : 1.8026411016471684 + }, + "rotation" : { + "radians" : 2.42141292546337 + } + }, + "curvature" : 0.03192840803852446 + }, { + "time" : 0.67622301229786, + "velocity" : 1.4148871752476162, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.316670433480795, + "y" : 1.850801377645439 + }, + "rotation" : { + "radians" : 2.4235110036015075 + } + }, + "curvature" : 0.025280758123603746 + }, { + "time" : 0.7373987455586821, + "velocity" : 1.1701842422043276, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.257077135815052, + "y" : 1.9027724422048777 + }, + "rotation" : { + "radians" : 2.4252082067772336 + } + }, + "curvature" : 0.017656926445273908 + }, { + "time" : 0.8212616808414924, + "velocity" : 0.8347325010730865, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.193638766594631, + "y" : 1.9579375321181942 + }, + "rotation" : { + "radians" : 2.4263532239110557 + } + }, + "curvature" : 0.009537673600406624 + }, { + "time" : 1.029944806109764, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.127869606018066, + "y" : 2.0150370597839355 + }, + "rotation" : { + "radians" : 2.4267848465547592 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 5.923477649688721, -0.8946475982666016, 0.0 ], + "y" : [ 1.317657709121704, 0.7744828462600708, 0.0 ] + }, { + "x" : [ 5.127869606018066, -1.0593681335449219, 0.0 ], + "y" : [ 2.0150370597839355, 0.9194501638412476, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.356194490192345 + } + }, { + "time" : 0.9399999789893627, + "rotation" : { + "radians" : 2.4532338595176353 + } + } ], + "reversed" : false, + "color" : 154.35294, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "VisionManager.shootBalls 8\nDrive.turnToAngle 40", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "8" ], + "argTypes" : [ "double" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Drive.turnToAngle", + "args" : [ "40" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.127869606018066, + "y" : 2.0150370597839355 + }, + "rotation" : { + "radians" : 0.9676638940223135 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.23907872328411964, + "velocity" : 0.9563148931364785, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.192705269680573, + "y" : 2.109189995806446 + }, + "rotation" : { + "radians" : 0.9679091836027879 + } + }, + "curvature" : 0.004044048851551997 + }, { + "time" : 0.3327971604973431, + "velocity" : 1.3311886419893724, + "acceleration" : 3.9999999999999907, + "pose" : { + "translation" : { + "x" : 5.253460748921498, + "y" : 2.197499692440033 + }, + "rotation" : { + "radians" : 0.9685084969782757 + } + }, + "curvature" : 0.007076805090390186 + }, { + "time" : 0.39816082847441153, + "velocity" : 1.5926433138976455, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.307566488200337, + "y" : 2.2762623392827663 + }, + "rotation" : { + "radians" : 0.9692958679676649 + } + }, + "curvature" : 0.009223838838357203 + }, { + "time" : 0.44663324990958686, + "velocity" : 1.7865329996383468, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.353885251563042, + "y" : 2.343804362230003 + }, + "rotation" : { + "radians" : 0.9700562639818262 + } + }, + "curvature" : 0.008519844053710474 + }, { + "time" : 0.5116917209128728, + "velocity" : 2.0467668836514905, + "acceleration" : 1.992648464590459, + "pose" : { + "translation" : { + "x" : 5.424351285982993, + "y" : 2.4466790923615918 + }, + "rotation" : { + "radians" : 0.9696596521756633 + } + }, + "curvature" : -0.030593782175814987 + }, { + "time" : 0.5546990819250267, + "velocity" : 2.1324654355384465, + "acceleration" : -3.9999999999999925, + "pose" : { + "translation" : { + "x" : 5.475393638014793, + "y" : 2.5206459313631058 + }, + "rotation" : { + "radians" : 0.9621486944770179 + } + }, + "curvature" : -0.13823598332150094 + }, { + "time" : 0.5969447671310003, + "velocity" : 1.9634826947145523, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.5252832076948835, + "y" : 2.591331197647378 + }, + "rotation" : { + "radians" : 0.9510157851137995 + } + }, + "curvature" : -0.09101992339026437 + }, { + "time" : 0.6595418386244202, + "velocity" : 1.7130944087408728, + "acceleration" : -3.999999999999997, + "pose" : { + "translation" : { + "x" : 5.592436509672552, + "y" : 2.6847757240757346 + }, + "rotation" : { + "radians" : 0.9456880198624937 + } + }, + "curvature" : -0.018285197310074868 + }, { + "time" : 0.7058887043411002, + "velocity" : 1.527706945874153, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.636414063088068, + "y" : 2.745653131905783 + }, + "rotation" : { + "radians" : 0.9448592549816716 + } + }, + "curvature" : -0.005896965233362419 + }, { + "time" : 0.768439061104125, + "velocity" : 1.2775055188220539, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 5.687825173619785, + "y" : 2.8167451025219634 + }, + "rotation" : { + "radians" : 0.9445964731587643 + } + }, + "curvature" : -0.001026058794484897 + }, { + "time" : 0.8582844674175891, + "velocity" : 0.9181238935681972, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.745632715359989, + "y" : 2.896663040502972 + }, + "rotation" : { + "radians" : 0.9445853727793603 + } + }, + "curvature" : 3.9777751022351305E-4 + }, { + "time" : 1.0878154408096383, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.807385444641113, + "y" : 2.9820399284362793 + }, + "rotation" : { + "radians" : 0.9446200461231519 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.127869606018066, 1.0498194694519043, 0.0 ], + "y" : [ 2.0150370597839355, 1.5242493152618408, 0.0 ] + }, { + "x" : [ 5.807385444641113, 1.0001163482666016, 0.0 ], + "y" : [ 2.9820399284362793, 1.3827617168426514, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.9380478872771981 + } + }, { + "time" : 0.9999999776482582, + "rotation" : { + "radians" : 0.9272952180016122 + } + } ], + "reversed" : false, + "color" : 59.52942, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@time 1.4\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.4, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.807385444641113, + "y" : 2.9820399284362793 + }, + "rotation" : { + "radians" : -3.1077984471976654 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2180776573035808, + "velocity" : 0.8723106292143229, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.712328077295783, + "y" : 2.978708433225936 + }, + "rotation" : { + "radians" : -3.1041624869647366 + } + }, + "curvature" : 0.07273634738038368 + }, { + "time" : 0.3111557355200276, + "velocity" : 1.2446229420801103, + "acceleration" : 3.9999999999999956, + "pose" : { + "translation" : { + "x" : 5.613894489667587, + "y" : 2.974581701005377 + }, + "rotation" : { + "radians" : -3.094472230094453 + } + }, + "curvature" : 0.11857740286674891 + }, { + "time" : 0.38620859133114593, + "velocity" : 1.5448343653245833, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.509367328174033, + "y" : 2.9689573476464197 + }, + "rotation" : { + "radians" : -3.0808888420166256 + } + }, + "curvature" : 0.1367542913886183 + }, { + "time" : 0.4534555009735825, + "velocity" : 1.8138220038943296, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 5.39670240160558, + "y" : 2.961229747164907 + }, + "rotation" : { + "radians" : -3.065345218180051 + } + }, + "curvature" : 0.13629877278960334 + }, { + "time" : 0.5166742966413302, + "velocity" : 2.0666971865653205, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 5.274478519695425, + "y" : 2.950885042220694 + }, + "rotation" : { + "radians" : -3.0491647636938963 + } + }, + "curvature" : 0.1267933963841827 + }, { + "time" : 0.5473532125193565, + "velocity" : 2.189412850077426, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 5.209495855934646, + "y" : 2.9445945602601076 + }, + "rotation" : { + "radians" : -3.041082387571697 + } + }, + "curvature" : 0.12079034959194605 + }, { + "time" : 0.5775859258575893, + "velocity" : 2.310343703430357, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 5.141847331689291, + "y" : 2.9374961546176337 + }, + "rotation" : { + "radians" : -3.0330761640829413 + } + }, + "curvature" : 0.11467075614396673 + }, { + "time" : 0.607452738767687, + "velocity" : 2.429810955070748, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 5.071508179679264, + "y" : 2.929549181170158 + }, + "rotation" : { + "radians" : -3.025171418955131 + } + }, + "curvature" : 0.10876301194249897 + }, { + "time" : 0.6370065753804001, + "velocity" : 2.5480263015216003, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.998483164915225, + "y" : 2.920717795803565 + }, + "rotation" : { + "radians" : -3.017377231431235 + } + }, + "curvature" : 0.10327528635802412 + }, { + "time" : 0.6662799728523379, + "velocity" : 2.6651198914093515, + "acceleration" : 3.9999999999999942, + "pose" : { + "translation" : { + "x" : 4.922805017153899, + "y" : 2.910970798490861 + }, + "rotation" : { + "radians" : -3.0096904819137515 + } + }, + "curvature" : 0.09832969802645626 + }, { + "time" : 0.6952901141770801, + "velocity" : 2.7811604567083203, + "acceleration" : 3.9999999999999836, + "pose" : { + "translation" : { + "x" : 4.844532863353379, + "y" : 2.9002814773702994 + }, + "rotation" : { + "radians" : -3.0020990455222543 + } + }, + "curvature" : 0.09398961737092937 + }, { + "time" : 0.724042537599304, + "velocity" : 2.8961701503972157, + "acceleration" : 3.6380002628705683, + "pose" : { + "translation" : { + "x" : 4.763750660128433, + "y" : 2.888627452823507 + }, + "rotation" : { + "radians" : -2.994584224283115 + } + }, + "curvature" : 0.09028031417064307 + }, { + "time" : 0.7525829010133674, + "velocity" : 3.0, + "acceleration" : -1.0263248734009206E-14, + "pose" : { + "translation" : { + "x" : 4.680565626205809, + "y" : 2.8759905215536072 + }, + "rotation" : { + "radians" : -2.9871225391208203 + } + }, + "curvature" : 0.08720396777763022 + }, { + "time" : 0.7814294661611385, + "velocity" : 2.9999999999999996, + "acceleration" : 1.000209901006119E-14, + "pose" : { + "translation" : { + "x" : 4.595106674879542, + "y" : 2.8623565006633434 + }, + "rotation" : { + "radians" : -2.979687003342993 + } + }, + "curvature" : 0.08475025940246926 + }, { + "time" : 0.8110292004704637, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.507522846466259, + "y" : 2.847715071733205 + }, + "rotation" : { + "radians" : -2.9722479862521185 + } + }, + "curvature" : 0.08290368133012417 + }, { + "time" : 0.8413290035988551, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.417981740760487, + "y" : 2.832059624899553 + }, + "rotation" : { + "radians" : -2.9647737573971904 + } + }, + "curvature" : 0.08164850595350744 + }, { + "time" : 0.872270133120958, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.326667949489956, + "y" : 2.8153871029327426 + }, + "rotation" : { + "radians" : -2.9572307833972085 + } + }, + "curvature" : 0.08097214961252051 + }, { + "time" : 0.9037887421215142, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.233781488770907, + "y" : 2.7976978453152483 + }, + "rotation" : { + "radians" : -2.9495838324025407 + } + }, + "curvature" : 0.08086748247871706 + }, { + "time" : 0.9358164189685482, + "velocity" : 3.0, + "acceleration" : -9.119536798471233E-15, + "pose" : { + "translation" : { + "x" : 4.1395362315633975, + "y" : 2.778995432319789 + }, + "rotation" : { + "radians" : -2.941795926901371 + } + }, + "curvature" : 0.08133448893648988 + }, { + "time" : 0.9682807293579756, + "velocity" : 2.9999999999999996, + "acceleration" : 9.019320323200669E-15, + "pose" : { + "translation" : { + "x" : 4.044158340126607, + "y" : 2.7592865290874524 + }, + "rotation" : { + "radians" : -2.9338281737918814 + } + }, + "curvature" : 0.08238157300892571 + }, { + "time" : 1.0011057608042824, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.9478846984741445, + "y" : 2.73858072970582 + }, + "rotation" : { + "radians" : -2.925639491155511 + } + }, + "curvature" : 0.08402672524453093 + }, { + "time" : 1.0342126698325704, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.850961344829351, + "y" : 2.7168904012870905 + }, + "rotation" : { + "radians" : -2.91718624359335 + } + }, + "curvature" : 0.08629871484426285 + }, { + "time" : 1.0675202322086716, + "velocity" : 3.0, + "acceleration" : -0.1315579405706849, + "pose" : { + "translation" : { + "x" : 3.7536419040806095, + "y" : 2.694230528046205 + }, + "rotation" : { + "radians" : -2.9084217919318283 + } + }, + "curvature" : 0.08923843792521573 + }, { + "time" : 1.1009699296037385, + "velocity" : 2.9955994266979924, + "acceleration" : -0.16210586852590414, + "pose" : { + "translation" : { + "x" : 3.6561860202366483, + "y" : 2.6706185553789727 + }, + "rotation" : { + "radians" : -2.899295958219869 + } + }, + "curvature" : 0.09290053504803716 + }, { + "time" : 1.1345079603171326, + "velocity" : 2.990162715100549, + "acceleration" : -0.19242213116635215, + "pose" : { + "translation" : { + "x" : 3.5588577888818484, + "y" : 2.646074233940194 + }, + "rotation" : { + "radians" : -2.889754402955238 + } + }, + "curvature" : 0.0973553849716613 + }, { + "time" : 1.1680607870231765, + "velocity" : 2.983706408679117, + "acceleration" : -0.22706531667766747, + "pose" : { + "translation" : { + "x" : 3.4619241896315485, + "y" : 2.620619463721785 + }, + "rotation" : { + "radians" : -2.8797379082054317 + } + }, + "curvature" : 0.10269158445623167 + }, { + "time" : 1.2015549415534306, + "velocity" : 2.976101047873854, + "acceleration" : -0.2670814149888477, + "pose" : { + "translation" : { + "x" : 3.365653518587351, + "y" : 2.5942781381309032 + }, + "rotation" : { + "radians" : -2.869181557635527 + } + }, + "curvature" : 0.10901903383045562 + }, { + "time" : 1.2349184127471249, + "velocity" : 2.9671902847785026, + "acceleration" : -0.31371465342134314, + "pose" : { + "translation" : { + "x" : 3.2703138207924285, + "y" : 2.567075988068072 + }, + "rotation" : { + "radians" : -2.858013802441017 + } + }, + "curvature" : 0.11647276318602535 + }, { + "time" : 1.2680813779246811, + "velocity" : 2.9567865766514014, + "acceleration" : -0.3684468103258239, + "pose" : { + "translation" : { + "x" : 3.1761713226868302, + "y" : 2.539040426005304 + }, + "rotation" : { + "radians" : -2.8461554009678793 + } + }, + "curvature" : 0.12521765233260732 + }, { + "time" : 1.3009769666485926, + "velocity" : 2.944666301912286, + "acceleration" : -0.43303740193218687, + "pose" : { + "translation" : { + "x" : 3.0834888645627863, + "y" : 2.510200390064227 + }, + "rotation" : { + "radians" : -2.833518219736519 + } + }, + "curvature" : 0.1354542158098308 + }, { + "time" : 1.3335420599902073, + "velocity" : 2.930564398497954, + "acceleration" : -0.5095598286786093, + "pose" : { + "translation" : { + "x" : 2.9925243330200146, + "y" : 2.4805861880942075 + }, + "rotation" : { + "radians" : -2.820003885293717 + } + }, + "curvature" : 0.14742563679773568 + }, { + "time" : 1.365718127787587, + "velocity" : 2.91416876690357, + "acceleration" : -0.6004245148597813, + "pose" : { + "translation" : { + "x" : 2.9035290934210276, + "y" : 2.4502293417504766 + }, + "rotation" : { + "radians" : -2.8055022808143084 + } + }, + "curvature" : 0.16142623105857898 + }, { + "time" : 1.3974521047697568, + "velocity" : 2.8951149091694792, + "acceleration" : -0.7083738286934076, + "pose" : { + "translation" : { + "x" : 2.816746422346437, + "y" : 2.4191624305722534 + }, + "rotation" : { + "radians" : -2.7898898902549565 + } + }, + "curvature" : 0.17781148738953634 + }, { + "time" : 1.4286973034286823, + "velocity" : 2.87298162816717, + "acceleration" : -0.8769242761272262, + "pose" : { + "translation" : { + "x" : 2.7324099400502604, + "y" : 2.38741893606087 + }, + "rotation" : { + "radians" : -2.7730280085554613 + } + }, + "curvature" : 0.1970097361358105 + }, { + "time" : 1.4594210697670953, + "velocity" : 2.846039211610955, + "acceleration" : -3.9999999999999947, + "pose" : { + "translation" : { + "x" : 2.650742042915226, + "y" : 2.3550330857578956 + }, + "rotation" : { + "radians" : -2.7547608625141673 + } + }, + "curvature" : 0.2195352951517829 + }, { + "time" : 1.4900955256707504, + "velocity" : 2.723341387996335, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.5719523359080814, + "y" : 2.3220396973232624 + }, + "rotation" : { + "radians" : -2.7349137288234022 + } + }, + "curvature" : 0.24600255746489014 + }, { + "time" : 1.5212190951177167, + "velocity" : 2.5988471102084696, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.4962360650348963, + "y" : 2.2884740226133893 + }, + "rotation" : { + "radians" : -2.7132912008461836 + } + }, + "curvature" : 0.2771397985751853 + }, { + "time" : 1.5528031633912385, + "velocity" : 2.4725108371143825, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.42377254979637, + "y" : 2.254371591759306 + }, + "rotation" : { + "radians" : -2.689675854298644 + } + }, + "curvature" : 0.3138003172075707 + }, { + "time" : 1.5848723080728448, + "velocity" : 2.3442342583879574, + "acceleration" : -3.9999999999999942, + "pose" : { + "translation" : { + "x" : 2.354723615643138, + "y" : 2.219768057244778 + }, + "rotation" : { + "radians" : -2.6638277072718006 + } + }, + "curvature" : 0.3569666341755263 + }, { + "time" : 1.6174693044153108, + "velocity" : 2.213846273018094, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.289232026431076, + "y" : 2.184699037984432 + }, + "rotation" : { + "radians" : -2.6354850773300993 + } + }, + "curvature" : 0.4077405496577045 + }, { + "time" : 1.6506622716676704, + "velocity" : 2.0810744040086555, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 2.2274199168766096, + "y" : 2.1491999634018804 + }, + "rotation" : { + "radians" : -2.6043677221311743 + } + }, + "curvature" : 0.4673075916967568 + }, { + "time" : 1.6845551653471331, + "velocity" : 1.9455028292908045, + "acceleration" : -3.7890145173659073, + "pose" : { + "translation" : { + "x" : 2.169387225012014, + "y" : 2.1133059175078435 + }, + "rotation" : { + "radians" : -2.5701835156594286 + } + }, + "curvature" : 0.5368586687945079 + }, { + "time" : 1.7192335134624979, + "velocity" : 1.8141060648434189, + "acceleration" : -3.2696015880426708, + "pose" : { + "translation" : { + "x" : 2.1152101246407256, + "y" : 2.077051482978277 + }, + "rotation" : { + "radians" : -2.532640340224557 + } + }, + "curvature" : 0.6174451066520994 + }, { + "time" : 1.7907419934854965, + "velocity" : 1.5803018250017051, + "acceleration" : -2.726779669291407, + "pose" : { + "translation" : { + "x" : 2.018599167179449, + "y" : 2.003596336511297 + }, + "rotation" : { + "radians" : -2.446433526852606 + } + }, + "curvature" : 0.8136612252229765 + }, { + "time" : 1.8275860774684607, + "velocity" : 1.479836125863293, + "acceleration" : -2.359563803315625, + "pose" : { + "translation" : { + "x" : 1.9761847286498835, + "y" : 1.9664608799550876 + }, + "rotation" : { + "radians" : -2.397408828393692 + } + }, + "curvature" : 0.9278899124682813 + }, { + "time" : 1.8649660152071963, + "velocity" : 1.391635777804781, + "acceleration" : -1.9838468623386898, + "pose" : { + "translation" : { + "x" : 1.937661583645081, + "y" : 1.9290952336820055 + }, + "rotation" : { + "radians" : -2.3443966957035403 + } + }, + "curvature" : 1.0492344280773838 + }, { + "time" : 1.902729666191217, + "velocity" : 1.3167184772896783, + "acceleration" : -1.5953761316272526, + "pose" : { + "translation" : { + "x" : 1.9029635716538653, + "y" : 1.891529134866047 + }, + "rotation" : { + "radians" : -2.2876080290002703 + } + }, + "curvature" : 1.1720276125742721 + }, { + "time" : 1.940679658288589, + "velocity" : 1.256173965702088, + "acceleration" : -1.186040735740344, + "pose" : { + "translation" : { + "x" : 1.8719913626680516, + "y" : 1.85379088381519 + }, + "rotation" : { + "radians" : -2.227526882083615 + } + }, + "curvature" : 1.2877279640741648 + }, { + "time" : 1.9785675742743187, + "velocity" : 1.2112373539507049, + "acceleration" : -0.7410587042667358, + "pose" : { + "translation" : { + "x" : 1.8446108896377584, + "y" : 1.8159071880495183 + }, + "rotation" : { + "radians" : -2.164970018044204 + } + }, + "curvature" : 1.3850491721536529 + }, { + "time" : 2.0160893592543716, + "velocity" : 1.1834315085916118, + "acceleration" : -0.23332707042705494, + "pose" : { + "translation" : { + "x" : 1.8206517809267098, + "y" : 1.777903006379347 + }, + "rotation" : { + "radians" : -2.1011212361539733 + } + }, + "curvature" : 1.4508998888713995 + }, { + "time" : 2.052881886984702, + "velocity" : 1.1748468158826877, + "acceleration" : 0.3888458638588137, + "pose" : { + "translation" : { + "x" : 1.799905792767543, + "y" : 1.7398013929833471 + }, + "rotation" : { + "radians" : -2.037523308037849 + } + }, + "curvature" : 1.4721810231732262 + }, { + "time" : 2.088519327343306, + "velocity" : 1.188704287164646, + "acceleration" : 1.231210997720917, + "pose" : { + "translation" : { + "x" : 1.7821252417171145, + "y" : 1.7016233414866697 + }, + "rotation" : { + "radians" : -1.976018179939325 + } + }, + "curvature" : 1.4380568182608686 + }, { + "time" : 2.1225056544060728, + "velocity" : 1.2305486268164643, + "acceleration" : 2.534651579871874, + "pose" : { + "translation" : { + "x" : 1.7670214371118043, + "y" : 1.6633876290390703 + }, + "rotation" : { + "radians" : -1.918641555195968 + } + }, + "curvature" : 1.341918509745125 + }, { + "time" : 2.1542554976740123, + "velocity" : 1.311023417216232, + "acceleration" : 3.9999999999999845, + "pose" : { + "translation" : { + "x" : 1.7542631135228248, + "y" : 1.6251106603930339 + }, + "rotation" : { + "radians" : -1.8674949603224105 + } + }, + "curvature" : 1.182232258965127 + }, { + "time" : 2.2099359231623428, + "velocity" : 1.5337451191695526, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 1.7342355685846904, + "y" : 1.5484857759979853 + }, + "rotation" : { + "radians" : -1.7919535583318533 + } + }, + "curvature" : 0.6868485079079413 + }, { + "time" : 2.2579553489086845, + "velocity" : 1.7258228221549206, + "acceleration" : -1.2499750022631253, + "pose" : { + "translation" : { + "x" : 1.718481421470642, + "y" : 1.4718265533447266 + }, + "rotation" : { + "radians" : -1.7639859651971548 + } + }, + "curvature" : 0.0 + }, { + "time" : 2.3039131087144136, + "velocity" : 1.6683767712377462, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.7040171629614953, + "y" : 1.3951845909212786 + }, + "rotation" : { + "radians" : -1.744243059159777 + } + }, + "curvature" : 0.4949885871870906 + }, { + "time" : 2.353201866781118, + "velocity" : 1.4712217389709286, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 1.692568655125001, + "y" : 1.3186628076501847 + }, + "rotation" : { + "radians" : -1.688734274061912 + } + }, + "curvature" : 0.9318135483719395 + }, { + "time" : 2.409519830422982, + "velocity" : 1.2459498844034724, + "acceleration" : -2.1690899330402935, + "pose" : { + "translation" : { + "x" : 1.6866636401594342, + "y" : 1.2423782268174186 + }, + "rotation" : { + "radians" : -1.6025452180114088 + } + }, + "curvature" : 1.3089484866875387 + }, { + "time" : 2.4408900706331065, + "velocity" : 1.1779050121666355, + "acceleration" : -1.4371594032530797, + "pose" : { + "translation" : { + "x" : 1.6864417690519602, + "y" : 1.204360419059464 + }, + "rotation" : { + "radians" : -1.5497323811772583 + } + }, + "curvature" : 1.4645464958895065 + }, { + "time" : 2.4737794652080054, + "velocity" : 1.130637709486019, + "acceleration" : -0.882454393290906, + "pose" : { + "translation" : { + "x" : 1.6883282148664875, + "y" : 1.1664440317872504 + }, + "rotation" : { + "radians" : -1.4916448185226532 + } + }, + "curvature" : 1.589559435549557 + }, { + "time" : 2.5078700922418036, + "velocity" : 1.100554285890002, + "acceleration" : -0.4285512346859183, + "pose" : { + "translation" : { + "x" : 1.6925034029607406, + "y" : 1.1286425412588699 + }, + "rotation" : { + "radians" : -1.4293799219170706 + } + }, + "curvature" : 1.677647696189635 + }, { + "time" : 2.542863859363714, + "velocity" : 1.0855576637835957, + "acceleration" : -0.03409957451987376, + "pose" : { + "translation" : { + "x" : 1.6991206308842841, + "y" : 1.0909691291280303 + }, + "rotation" : { + "radians" : -1.3641679929709554 + } + }, + "curvature" : 1.7243201694504038 + }, { + "time" : 2.578478719165117, + "velocity" : 1.084343212217783, + "acceleration" : 0.32480160779729494, + "pose" : { + "translation" : { + "x" : 1.708307124635804, + "y" : 1.0534366687917367 + }, + "rotation" : { + "radians" : -1.2973160315953416 + } + }, + "curvature" : 1.72818476876106 + }, { + "time" : 2.6144494673900835, + "velocity" : 1.0960265690749238, + "acceleration" : 0.662753960787673, + "pose" : { + "translation" : { + "x" : 1.7201650949203895, + "y" : 1.0160577117379717 + }, + "rotation" : { + "radians" : -1.2301287631472986 + } + }, + "curvature" : 1.691537146255751 + }, { + "time" : 2.6505308994153998, + "velocity" : 1.1199396810605933, + "acceleration" : 0.9891536088281452, + "pose" : { + "translation" : { + "x" : 1.7347727934068158, + "y" : 0.9788444738933771 + }, + "rotation" : { + "radians" : -1.163822987935704 + } + }, + "curvature" : 1.6200724563638298 + }, { + "time" : 2.686501528745972, + "velocity" : 1.1555201588747483, + "acceleration" : 1.310397118703277, + "pose" : { + "translation" : { + "x" : 1.7521855689848245, + "y" : 0.941808821970934 + }, + "rotation" : { + "radians" : -1.0994531238813727 + } + }, + "curvature" : 1.52183879385189 + }, { + "time" : 2.7221665450113437, + "velocity" : 1.202255493427397, + "acceleration" : 1.6311499809238765, + "pose" : { + "translation" : { + "x" : 1.7724369240224052, + "y" : 0.9049622598176431 + }, + "rotation" : { + "radians" : -1.0378615567095117 + } + }, + "curvature" : 1.405821442899369 + }, { + "time" : 2.757359258068479, + "velocity" : 1.2596600866592031, + "acceleration" : 1.9551274988150928, + "pose" : { + "translation" : { + "x" : 1.7955395706230775, + "y" : 0.8683159147622064 + }, + "rotation" : { + "radians" : -0.9796592961829077 + } + }, + "curvature" : 1.2806102287854289 + }, { + "time" : 2.7919408310448324, + "velocity" : 1.3272714709375524, + "acceleration" : 2.2856166223411245, + "pose" : { + "translation" : { + "x" : 1.821486486883173, + "y" : 0.8318805239627071 + }, + "rotation" : { + "radians" : -0.9252342480030827 + } + }, + "curvature" : 1.1534643716591353 + }, { + "time" : 2.825798523286439, + "velocity" : 1.4046571751190786, + "acceleration" : 2.6258637992019844, + "pose" : { + "translation" : { + "x" : 1.850251973149117, + "y" : 0.7956664207542907 + }, + "rotation" : { + "radians" : -0.8747791350162617 + } + }, + "curvature" : 1.0298714520728465 + }, { + "time" : 2.8588428851625496, + "velocity" : 1.4914271687372875, + "acceleration" : 2.9794041928936728, + "pose" : { + "translation" : { + "x" : 1.8817927082747086, + "y" : 0.7596835209968459 + }, + "rotation" : { + "radians" : -0.8283296056482383 + } + }, + "curvature" : 0.9135232462092671 + }, { + "time" : 2.891004389345329, + "velocity" : 1.5872492891492276, + "acceleration" : 3.5509647371659865, + "pose" : { + "translation" : { + "x" : 1.916048805878404, + "y" : 0.7239413094226848 + }, + "rotation" : { + "radians" : -0.7858044305587907 + } + }, + "curvature" : 0.8065539475387932 + }, { + "time" : 2.9523592966906644, + "velocity" : 1.8051184015845994, + "acceleration" : 3.999999999999984, + "pose" : { + "translation" : { + "x" : 1.9923910543609047, + "y" : 0.6532146522016653 + }, + "rotation" : { + "radians" : -0.7118327324730775 + } + }, + "curvature" : 0.6236089171423088 + }, { + "time" : 3.0100353656283176, + "velocity" : 2.035822677335212, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 2.0785084606141027, + "y" : 0.5835531856100715 + }, + "rotation" : { + "radians" : -0.6511181043270622 + } + }, + "curvature" : 0.4800010235812804 + }, { + "time" : 3.0646190962309157, + "velocity" : 2.2541575997456036, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 2.1734333233560115, + "y" : 0.5150158743770881 + }, + "rotation" : { + "radians" : -0.6017399678104041 + } + }, + "curvature" : 0.3691740752256128 + }, { + "time" : 3.1166653761479624, + "velocity" : 2.46234271941379, + "acceleration" : 4.000000000000028, + "pose" : { + "translation" : { + "x" : 2.276034298238187, + "y" : 0.4476534745680283 + }, + "rotation" : { + "radians" : -0.5619346801385068 + } + }, + "curvature" : 0.2836399510576229 + }, { + "time" : 3.1418275733370513, + "velocity" : 2.562991508170146, + "acceleration" : 4.0000000000000275, + "pose" : { + "translation" : { + "x" : 2.3298233544980946, + "y" : 0.4144263756493385 + }, + "rotation" : { + "radians" : -0.5451464563946322 + } + }, + "curvature" : 0.24824311811855138 + }, { + "time" : 3.1664400864835103, + "velocity" : 2.6614415607559834, + "acceleration" : 3.999999999999966, + "pose" : { + "translation" : { + "x" : 2.3850501980787406, + "y" : 0.38150809671011743 + }, + "rotation" : { + "radians" : -0.5302189330250398 + } + }, + "curvature" : 0.21679631372622093 + }, { + "time" : 3.1905125871491076, + "velocity" : 2.757731563418372, + "acceleration" : 4.000000000000013, + "pose" : { + "translation" : { + "x" : 2.4415424379903916, + "y" : 0.3489024790262656 + }, + "rotation" : { + "radians" : -0.5170134828220253 + } + }, + "curvature" : 0.1886809876230122 + }, { + "time" : 3.214049672205038, + "velocity" : 2.851879903642094, + "acceleration" : 3.303657668196632, + "pose" : { + "translation" : { + "x" : 2.4991237930953503, + "y" : 0.31661276891827583 + }, + "rotation" : { + "radians" : -0.5054087566305369 + } + }, + "curvature" : 0.16335410873820064 + }, { + "time" : 3.2371151553370474, + "velocity" : 2.928080363861817, + "acceleration" : 1.0968430910701312, + "pose" : { + "translation" : { + "x" : 2.5576151483652376, + "y" : 0.2846416040989146 + }, + "rotation" : { + "radians" : -0.49529950641493536 + } + }, + "curvature" : 0.14033935814094875 + }, { + "time" : 3.259949848690555, + "velocity" : 2.953126439503317, + "acceleration" : 1.0318522973125155, + "pose" : { + "translation" : { + "x" : 2.6168356111382742, + "y" : 0.25299100002090213 + }, + "rotation" : { + "radians" : -0.48659537038691486 + } + }, + "curvature" : 0.11921786782590763 + }, { + "time" : 3.2827100656165435, + "velocity" : 2.9766116216257297, + "acceleration" : 0.9835246105005508, + "pose" : { + "translation" : { + "x" : 2.6766035673765614, + "y" : 0.22166233622459286 + }, + "rotation" : { + "radians" : -0.47921968759438144 + } + }, + "curvature" : 0.09961923590719055 + }, { + "time" : 3.305354934644453, + "velocity" : 2.99888340761624, + "acceleration" : 0.04948814588093301, + "pose" : { + "translation" : { + "x" : 2.736737737923363, + "y" : 0.19065634268565645 + }, + "rotation" : { + "radians" : -0.4731083784610581 + } + }, + "curvature" : 0.08121320610983931 + }, { + "time" : 3.3279177598197176, + "velocity" : 2.9999999999999996, + "acceleration" : 1.3150721678562816E-14, + "pose" : { + "translation" : { + "x" : 2.7970582347603883, + "y" : 0.15997308616275907 + }, + "rotation" : { + "radians" : -0.4682089061577221 + } + }, + "curvature" : 0.063702197976732 + }, { + "time" : 3.3504305526891756, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 2.857387617265072, + "y" : 0.1296119565452436 + }, + "rotation" : { + "radians" : -0.4644793182582315 + } + }, + "curvature" : 0.0468147679013892 + }, { + "time" : 3.3728462381053794, + "velocity" : 3.0, + "acceleration" : -1.3294976180711803E-14, + "pose" : { + "translation" : { + "x" : 2.9175519484678567, + "y" : 0.09957165320080996 + }, + "rotation" : { + "radians" : -0.461887356526069 + } + }, + "curvature" : 0.03030003960646033 + }, { + "time" : 3.395114760380951, + "velocity" : 2.9999999999999996, + "acceleration" : 1.3412784420541495E-14, + "pose" : { + "translation" : { + "x" : 2.977381851309474, + "y" : 0.06985017132319626 + }, + "rotation" : { + "radians" : -0.4604096131005984 + } + }, + "curvature" : 0.013923147393524328 + }, { + "time" : 3.4171876919923743, + "velocity" : 3.0, + "acceleration" : -0.04926038986669166, + "pose" : { + "translation" : { + "x" : 3.0367135648982266, + "y" : 0.04044478827985998 + }, + "rotation" : { + "radians" : -0.46003070242576016 + } + }, + "curvature" : -0.0025382237303508883 + }, { + "time" : 3.43902255151928, + "velocity" : 2.99892440630702, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.0953900007672708, + "y" : 0.011352049959658217 + }, + "rotation" : { + "radians" : -0.4607424088911207 + } + }, + "curvature" : -0.019296044170462233 + }, { + "time" : 3.460894301999556, + "velocity" : 2.911437404385916, + "acceleration" : -4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.153261799131896, + "y" : -0.017432242879471982 + }, + "rotation" : { + "radians" : -0.46254275944377354 + } + }, + "curvature" : -0.03655263042494752 + }, { + "time" : 3.483096236841796, + "velocity" : 2.8226296650169553, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.2101883851468074, + "y" : -0.04591304826283249 + }, + "rotation" : { + "radians" : -0.4654349576602489 + } + }, + "curvature" : -0.05449941944261404 + }, { + "time" : 3.5284559791470897, + "velocity" : 2.6411906957957814, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.320693882987083, + "y" : -0.10198790459356033 + }, + "rotation" : { + "radians" : -0.4745255802104961 + } + }, + "curvature" : -0.09314935868314721 + }, { + "time" : 3.5750713755207792, + "velocity" : 2.454729110301023, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 3.4259977320907637, + "y" : -0.1569278776878491 + }, + "rotation" : { + "radians" : -0.488085849209704 + } + }, + "curvature" : -0.13632788196047352 + }, { + "time" : 3.6230020187291068, + "velocity" : 2.2630065374677133, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 3.525399329757196, + "y" : -0.21080134048127874 + }, + "rotation" : { + "radians" : -0.5061441065978821 + } + }, + "curvature" : -0.18426344503119224 + }, { + "time" : 3.6724555712310893, + "velocity" : 2.065192327459783, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.6184400330154176, + "y" : -0.2636901170639021 + }, + "rotation" : { + "radians" : -0.528561557547075 + } + }, + "curvature" : -0.2354062697850754 + }, { + "time" : 3.723886359469485, + "velocity" : 1.8594691745062002, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.7049369588571714, + "y" : -0.3156899195544618 + }, + "rotation" : { + "radians" : -0.5548405336431187 + } + }, + "curvature" : -0.28492058176951146 + }, { + "time" : 3.7781787791469426, + "velocity" : 1.6422994957963697, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 3.785016784469917, + "y" : -0.36691078497460694 + }, + "rotation" : { + "radians" : -0.5838549927785415 + } + }, + "curvature" : -0.32275632767074536 + }, { + "time" : 3.837038685288918, + "velocity" : 1.4068598712284692, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.859149547469844, + "y" : -0.41747751212311 + }, + "rotation" : { + "radians" : -0.6135313524400181 + } + }, + "curvature" : -0.3322823649480369 + }, { + "time" : 3.9040276125177047, + "velocity" : 1.1389041623133211, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 3.9281824461348833, + "y" : -0.46753009845008364 + }, + "rotation" : { + "radians" : -0.6405875389886441 + } + }, + "curvature" : -0.29180251710931904 + }, { + "time" : 3.988546345848521, + "velocity" : 0.800829228990057, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 3.9933736396377206, + "y" : -0.5172241769311974 + }, + "rotation" : { + "radians" : -0.6605456517130821 + } + }, + "curvature" : -0.1820147647738938 + }, { + "time" : 4.188753653096035, + "velocity" : 0.0, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 4.056426048278809, + "y" : -0.5667314529418945 + }, + "rotation" : { + "radians" : -0.6682894149758353 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.807385444641113, -3.022507667541504, 0.0 ], + "y" : [ 2.9820399284362793, -0.10218214988708496, 0.0 ] + }, { + "x" : [ 1.718481421470642, -0.47991621494293213, 0.0 ], + "y" : [ 1.4718265533447266, -2.453189492225647, 0.0 ] + }, { + "x" : [ 4.056426048278809, 2.0053954124450684, 0.0 ], + "y" : [ -0.5667314529418945, -1.5832068920135498, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : -3.127725471555593 + } + }, { + "time" : 2.249999949708581, + "rotation" : { + "radians" : -0.798910986801039 + } + }, { + "time" : 4.099999908357859, + "rotation" : { + "radians" : -0.6659689899224528 + } + } ], + "reversed" : false, + "color" : 192.47057, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 2\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 2.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "@t 2.7\nDrive.setAutoRotation 35", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 2.7, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.setAutoRotation", + "args" : [ "35" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.056426048278809, + "y" : -0.5667314529418945 + }, + "rotation" : { + "radians" : 2.4520537478581654 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.18493870993194386, + "velocity" : 0.7397548397277754, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 4.003633991609843, + "y" : -0.5232320818440126 + }, + "rotation" : { + "radians" : 2.4530901867155106 + } + }, + "curvature" : 0.029752937013657187 + }, { + "time" : 0.26148282578134485, + "velocity" : 1.0459313031253796, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.9508028739061523, + "y" : -0.47987904225311695 + }, + "rotation" : { + "radians" : 2.4560494170073883 + } + }, + "curvature" : 0.05635458173870567 + }, { + "time" : 0.32012947576533146, + "velocity" : 1.2805179030613263, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 3.897901180103951, + "y" : -0.4368061550801574 + }, + "rotation" : { + "radians" : 2.4607189045800832 + } + }, + "curvature" : 0.08009487969635327 + }, { + "time" : 0.36946312936714043, + "velocity" : 1.477852517468562, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.8449053191515077, + "y" : -0.39413398946635425 + }, + "rotation" : { + "radians" : 2.466900851787993 + } + }, + "curvature" : 0.10120927083782304 + }, { + "time" : 0.4128042913826585, + "velocity" : 1.6512171655306345, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.791799285257279, + "y" : -0.35197029855173945 + }, + "rotation" : { + "radians" : 2.474409887161352 + } + }, + "curvature" : 0.1198860351486836 + }, { + "time" : 0.451853631575856, + "velocity" : 1.8074145263034247, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.738574319138042, + "y" : -0.3104104552436979 + }, + "rotation" : { + "radians" : 2.483071079673296 + } + }, + "curvature" : 0.13627342891926963 + }, { + "time" : 0.4876202915697968, + "velocity" : 1.9504811662791879, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.6852285692670304, + "y" : -0.2695378879855088 + }, + "rotation" : { + "radians" : 2.4927182713379636 + } + }, + "curvature" : 0.15048631581735405 + }, { + "time" : 0.5207612575253416, + "velocity" : 2.0830450301013674, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.631766753122065, + "y" : -0.2294245165248867 + }, + "rotation" : { + "radians" : 2.5031927075423317 + } + }, + "curvature" : 0.16261213661992346 + }, { + "time" : 0.5517326643887606, + "velocity" : 2.206930657555043, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 3.5781998184336885, + "y" : -0.1901311876825229 + }, + "rotation" : { + "radians" : 2.514341935981025 + } + }, + "curvature" : 0.1727161388917002 + }, { + "time" : 0.5808665583284028, + "velocity" : 2.3234662333136122, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.5245446044332986, + "y" : -0.15170811112062665 + }, + "rotation" : { + "radians" : 2.526018940159826 + } + }, + "curvature" : 0.18084582936508686 + }, { + "time" : 0.6084135883193114, + "velocity" : 2.4336543532772463, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.470823503101282, + "y" : -0.11419529511146642 + }, + "rotation" : { + "radians" : 2.538081470866874 + } + }, + "curvature" : 0.18703463399877743 + }, { + "time" : 0.6345684464707756, + "velocity" : 2.5382737858831033, + "acceleration" : 3.9999999999999862, + "pose" : { + "translation" : { + "x" : 3.417064120415148, + "y" : -0.07762298230591114 + }, + "rotation" : { + "radians" : 2.5503915380457647 + } + }, + "curvature" : 0.19130476386228212 + }, { + "time" : 0.6594858696162618, + "velocity" : 2.6379434784650475, + "acceleration" : 3.999999999999993, + "pose" : { + "translation" : { + "x" : 3.36329893759766, + "y" : -0.04201208550197144 + }, + "rotation" : { + "radians" : 2.5628150257457696 + } + }, + "curvature" : 0.19366929489891171 + }, { + "time" : 0.6832911528298411, + "velocity" : 2.733164611319365, + "acceleration" : 3.0356954614896483, + "pose" : { + "translation" : { + "x" : 3.3095649723649707, + "y" : -0.007374623413340942 + }, + "rotation" : { + "radians" : 2.575221394062975 + } + }, + "curvature" : 0.1941334791085683 + }, { + "time" : 0.7282945981321051, + "velocity" : 2.8697813659748452, + "acceleration" : 0.35602362885687006, + "pose" : { + "translation" : { + "x" : 3.2023594154743478, + "y" : 0.05897377757355571 + }, + "rotation" : { + "radians" : 2.5994770448888582 + } + }, + "curvature" : 0.18934541322883708 + }, { + "time" : 0.7712234425204774, + "velocity" : 2.8850650489366254, + "acceleration" : 0.5853616532790817, + "pose" : { + "translation" : { + "x" : 3.095821448769353, + "y" : 0.1214873934270031 + }, + "rotation" : { + "radians" : 2.622176713767235 + } + }, + "curvature" : 0.17683058489309955 + }, { + "time" : 0.8129042620605731, + "velocity" : 2.909463402372643, + "acceleration" : 0.8532903307335525, + "pose" : { + "translation" : { + "x" : 2.9903759750508243, + "y" : 0.18034581308165798 + }, + "rotation" : { + "radians" : 2.6423801962411266 + } + }, + "curvature" : 0.156321766875571 + }, { + "time" : 0.8531469929208557, + "velocity" : 2.943802135498035, + "acceleration" : 0.5959644267737211, + "pose" : { + "translation" : { + "x" : 2.886487960834632, + "y" : 0.23582909704128951 + }, + "rotation" : { + "radians" : 2.659177929985617 + } + }, + "curvature" : 0.12732676169969878 + }, { + "time" : 0.8919109189015986, + "velocity" : 2.966904056424647, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 2.784651596291951, + "y" : 0.28830383278545924 + }, + "rotation" : { + "radians" : 2.6716758173179893 + } + }, + "curvature" : 0.0890361198334152 + }, { + "time" : 0.9303572438648844, + "velocity" : 2.813118756571504, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.68537945518953, + "y" : 0.338209190176201 + }, + "rotation" : { + "radians" : 2.6789757213651453 + } + }, + "curvature" : 0.04021801225466915 + }, { + "time" : 0.9696416444039297, + "velocity" : 2.6559811544153225, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 2.5891916548299605, + "y" : 0.3860429768647009 + }, + "rotation" : { + "radians" : 2.680153087048729 + } + }, + "curvature" : -0.02088202984869535 + }, { + "time" : 1.0098343280634516, + "velocity" : 2.4952104197772345, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.4966050159919497, + "y" : 0.4323476936979773 + }, + "rotation" : { + "radians" : 2.6742351029390625 + } + }, + "curvature" : -0.09660379275555367 + }, { + "time" : 1.0510425689247183, + "velocity" : 2.3303774563321675, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.4081222228705883, + "y" : 0.47769659012556076 + }, + "rotation" : { + "radians" : 2.660185719014542 + } + }, + "curvature" : -0.18983275500917693 + }, { + "time" : 1.0934364510155294, + "velocity" : 2.160801927968923, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 2.3242209830176215, + "y" : 0.5226797196061739 + }, + "rotation" : { + "radians" : 2.636908345303076 + } + }, + "curvature" : -0.30373535138801905 + }, { + "time" : 1.1372916096013064, + "velocity" : 1.9853812936258146, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 2.245343187281719, + "y" : 0.5678899950144114 + }, + "rotation" : { + "radians" : 2.6032836790059384 + } + }, + "curvature" : -0.4410606681722093 + }, { + "time" : 1.1830628813303603, + "velocity" : 1.8022962067095987, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.171884069748746, + "y" : 0.6139092440474201 + }, + "rotation" : { + "radians" : 2.5582681034202466 + } + }, + "curvature" : -0.6027205654193771 + }, { + "time" : 1.2315199026124195, + "velocity" : 1.6084681215813614, + "acceleration" : -3.2340381044624498, + "pose" : { + "translation" : { + "x" : 2.104181367682031, + "y" : 0.6612942646315787 + }, + "rotation" : { + "radians" : 2.501083096666535 + } + }, + "curvature" : -0.7854142700643647 + }, { + "time" : 1.2832918933759534, + "velocity" : 1.441035530708215, + "acceleration" : -2.161144532997304, + "pose" : { + "translation" : { + "x" : 2.0425044814626396, + "y" : 0.7105628803291779 + }, + "rotation" : { + "radians" : 2.431516535185916 + } + }, + "curvature" : -0.978530406846595 + }, { + "time" : 1.3381225717901073, + "velocity" : 1.322538509812933, + "acceleration" : -1.509248244590621, + "pose" : { + "translation" : { + "x" : 1.9870436345296412, + "y" : 0.7621799957451003 + }, + "rotation" : { + "radians" : 2.3503134945481206 + } + }, + "curvature" : -1.161734937054165 + }, { + "time" : 1.3664915633080867, + "velocity" : 1.2797226591636164, + "acceleration" : -1.1263537161568526, + "pose" : { + "translation" : { + "x" : 1.961679353004223, + "y" : 0.7889966218317799 + }, + "rotation" : { + "radians" : 2.3059373339142457 + } + }, + "curvature" : -1.24077199916201 + }, { + "time" : 1.3952936681311403, + "velocity" : 1.2472813013630308, + "acceleration" : -0.7667596602916469, + "pose" : { + "translation" : { + "x" : 1.937899033320381, + "y" : 0.8165436519335003 + }, + "rotation" : { + "radians" : 2.2595451461663565 + } + }, + "curvature" : -1.306155490343675 + }, { + "time" : 1.424400522054752, + "velocity" : 1.2249633399364037, + "acceleration" : -0.42132507119357915, + "pose" : { + "translation" : { + "x" : 1.9156993576789647, + "y" : 0.8448581059673881 + }, + "rotation" : { + "radians" : 2.2116259567064747 + } + }, + "curvature" : -1.3541835082656821 + }, { + "time" : 1.4536761435963022, + "velocity" : 1.2126287866061738, + "acceleration" : -0.08170141033781976, + "pose" : { + "translation" : { + "x" : 1.8950700272107497, + "y" : 0.8739710818044841 + }, + "rotation" : { + "radians" : 2.162745065781283 + } + }, + "curvature" : -1.3818724424188715 + }, { + "time" : 1.4829785112196618, + "velocity" : 1.210234741845108, + "acceleration" : 0.26045790812859687, + "pose" : { + "translation" : { + "x" : 1.8759934232245703, + "y" : 0.9039073195012026 + }, + "rotation" : { + "radians" : 2.113522302547328 + } + }, + "curvature" : -1.387344994997751 + }, { + "time" : 1.5121614711143974, + "velocity" : 1.2178356745322916, + "acceleration" : 0.6143648008907316, + "pose" : { + "translation" : { + "x" : 1.8584442684554539, + "y" : 0.9346847655307897 + }, + "rotation" : { + "radians" : 2.064604229359281 + } + }, + "curvature" : -1.3700812404747664 + }, { + "time" : 1.5410766974782304, + "velocity" : 1.2356001718200182, + "acceleration" : 0.9912142506014653, + "pose" : { + "translation" : { + "x" : 1.8423892883127544, + "y" : 0.9663141370147823 + }, + "rotation" : { + "radians" : 2.0166336726291316 + } + }, + "curvature" : -1.3309685196438428 + }, { + "time" : 1.5695753786663202, + "velocity" : 1.2638484707370008, + "acceleration" : 1.6439611484263257, + "pose" : { + "translation" : { + "x" : 1.8277868721282857, + "y" : 0.9987984859544667 + }, + "rotation" : { + "radians" : 1.9702206136785245 + } + }, + "curvature" : -1.2721364283154866 + }, { + "time" : 1.6245807356379516, + "velocity" : 1.3542751405536837, + "acceleration" : 2.798992607878158, + "pose" : { + "translation" : { + "x" : 1.8027295760623936, + "y" : 1.0663033839935565 + }, + "rotation" : { + "radians" : 1.8842065669188885 + } + }, + "curvature" : -1.1079237968679712 + }, { + "time" : 1.676119381405686, + "velocity" : 1.4985314290776237, + "acceleration" : 3.999999999999997, + "pose" : { + "translation" : { + "x" : 1.782759900790552, + "y" : 1.137054014048772 + }, + "rotation" : { + "radians" : 1.8100759100562795 + } + }, + "curvature" : -0.904882088448944 + }, { + "time" : 1.7234002817652252, + "velocity" : 1.6876550305157805, + "acceleration" : 3.9999999999999885, + "pose" : { + "translation" : { + "x" : 1.7672103094854315, + "y" : 1.2107543994101775 + }, + "rotation" : { + "radians" : 1.7501344274699886 + } + }, + "curvature" : -0.6861541138888728 + }, { + "time" : 1.7668603468362696, + "velocity" : 1.8614952907999578, + "acceleration" : -2.6118583184812723, + "pose" : { + "translation" : { + "x" : 1.7552473678998695, + "y" : 1.2869440876638691 + }, + "rotation" : { + "radians" : 1.7057998025006624 + } + }, + "curvature" : -0.4641078115451259 + }, { + "time" : 1.8104168065342312, + "velocity" : 1.7477319892142427, + "acceleration" : -4.000000000000003, + "pose" : { + "translation" : { + "x" : 1.7458609043071398, + "y" : 1.3649842060986543 + }, + "rotation" : { + "radians" : 1.6781427516970726 + } + }, + "curvature" : -0.23909570910678915 + }, { + "time" : 1.858532964177138, + "velocity" : 1.5552673586426153, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.7378531694412231, + "y" : 1.444043517112732 + }, + "rotation" : { + "radians" : 1.668497763241333 + } + }, + "curvature" : -0.0 + }, { + "time" : 1.9135617600953347, + "velocity" : 1.3351521749698287, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 1.731150510518038, + "y" : 1.5232887159155766 + }, + "rotation" : { + "radians" : 1.6290693879713087 + } + }, + "curvature" : -0.9651764780400796 + }, { + "time" : 1.9447141490181088, + "velocity" : 1.2105426192787325, + "acceleration" : -3.8758391033227886, + "pose" : { + "translation" : { + "x" : 1.7297123504678038, + "y" : 1.5629148639371933 + }, + "rotation" : { + "radians" : 1.582290663183503 + } + }, + "curvature" : -1.3866393975350608 + }, { + "time" : 1.9793752955290544, + "velocity" : 1.0762015922656092, + "acceleration" : -2.0744901253079546, + "pose" : { + "translation" : { + "x" : 1.7304462942776127, + "y" : 1.6025386552453256 + }, + "rotation" : { + "radians" : 1.5198280388303091 + } + }, + "curvature" : -1.7544316082551803 + }, { + "time" : 2.0177456093542387, + "velocity" : 0.9966027551302967, + "acceleration" : -1.0463551078296154, + "pose" : { + "translation" : { + "x" : 1.7339390283538525, + "y" : 1.6421520518968291 + }, + "rotation" : { + "radians" : 1.4439430669964353 + } + }, + "curvature" : -2.0458770787946707 + }, { + "time" : 2.0589400093906947, + "velocity" : 0.9534987842381746, + "acceleration" : -0.4880905611008773, + "pose" : { + "translation" : { + "x" : 1.7407113486278938, + "y" : 1.6817436414219813 + }, + "rotation" : { + "radians" : 1.3575512036532698 + } + }, + "curvature" : -2.2350303059988965 + }, { + "time" : 2.0803982879163243, + "velocity" : 0.9430252010323411, + "acceleration" : -0.17603090665285, + "pose" : { + "translation" : { + "x" : 1.7454726503711389, + "y" : 1.70152681499276 + }, + "rotation" : { + "radians" : 1.3115071881305223 + } + }, + "curvature" : -2.2849521300818747 + }, { + "time" : 2.1022775055735607, + "velocity" : 0.9391737825112827, + "acceleration" : 0.10804204147343682, + "pose" : { + "translation" : { + "x" : 1.751220456348885, + "y" : 1.7212988221176222 + }, + "rotation" : { + "radians" : 1.2642073316351676 + } + }, + "curvature" : -2.3037310856346584 + }, { + "time" : 2.124492132931374, + "velocity" : 0.9415738962015925, + "acceleration" : 0.3726592944849579, + "pose" : { + "translation" : { + "x" : 1.7580023567016838, + "y" : 1.7410573696656044 + }, + "rotation" : { + "radians" : 1.2161486950907356 + } + }, + "curvature" : -2.2920014286625774 + }, { + "time" : 2.146960488972026, + "velocity" : 0.9499469379119386, + "acceleration" : 0.6237338591278402, + "pose" : { + "translation" : { + "x" : 1.7658622538765485, + "y" : 1.7607999883402954 + }, + "rotation" : { + "radians" : 1.1678204422268519 + } + }, + "curvature" : -2.2517750837082873 + }, { + "time" : 2.1696053252037935, + "velocity" : 0.9640712890040968, + "acceleration" : 0.8654335988347971, + "pose" : { + "translation" : { + "x" : 1.7748404343704771, + "y" : 1.780524038470248 + }, + "rotation" : { + "radians" : 1.119686904181982 + } + }, + "curvature" : -2.1862781024111237 + }, { + "time" : 2.192354484026944, + "velocity" : 0.9837591753948806, + "acceleration" : 1.1007322784603935, + "pose" : { + "translation" : { + "x" : 1.7849736404739778, + "y" : 1.8002267157993888 + }, + "rotation" : { + "radians" : 1.0721723951266355 + } + }, + "curvature" : -2.0996461580977543 + }, { + "time" : 2.215141507868605, + "velocity" : 1.008841588067443, + "acceleration" : 1.3317674781680595, + "pose" : { + "translation" : { + "x" : 1.796295142014595, + "y" : 1.819905057277429 + }, + "rotation" : { + "radians" : 1.025649276755208 + } + }, + "curvature" : -1.996538776374713 + }, { + "time" : 2.2379061099913464, + "velocity" : 1.0391587448279456, + "acceleration" : 1.6754933878837617, + "pose" : { + "translation" : { + "x" : 1.808834808100432, + "y" : 1.8395559468502753 + }, + "rotation" : { + "radians" : 0.9804301798041166 + } + }, + "curvature" : -1.8817412716281585 + }, { + "time" : 2.2830950022778635, + "velocity" : 1.1148724350597967, + "acceleration" : 2.127602917951782, + "pose" : { + "translation" : { + "x" : 1.8376715372041317, + "y" : 1.8787621757874542 + }, + "rotation" : { + "radians" : 0.8948396405471469 + } + }, + "curvature" : -1.6348328210818963 + }, { + "time" : 2.3276395725958383, + "velocity" : 1.2096455928472285, + "acceleration" : 2.5789601207010184, + "pose" : { + "translation" : { + "x" : 1.8716574925150553, + "y" : 1.9178176341376911 + }, + "rotation" : { + "radians" : 0.8166729724145454 + } + }, + "curvature" : -1.3886967164626418 + }, { + "time" : 2.371283333319735, + "velocity" : 1.3222011112715757, + "acceleration" : 3.0313394226052046, + "pose" : { + "translation" : { + "x" : 1.9109165188377353, + "y" : 1.9566924759531596 + }, + "rotation" : { + "radians" : 0.7463747869451778 + } + }, + "curvature" : -1.1623279144475585 + }, { + "time" : 2.4138523222192685, + "velocity" : 1.4512421655031749, + "acceleration" : 3.4847988479105942, + "pose" : { + "translation" : { + "x" : 1.9555249368440535, + "y" : 1.9953549631045817 + }, + "rotation" : { + "radians" : 0.683796860013437 + } + }, + "curvature" : -0.9648147341420715 + }, { + "time" : 2.455238313142531, + "velocity" : 1.5954640189921991, + "acceleration" : 3.93825151072416, + "pose" : { + "translation" : { + "x" : 2.0055138388660367, + "y" : 2.0337716505743684 + }, + "rotation" : { + "radians" : 0.6284298787308873 + } + }, + "curvature" : -0.7982697547383403 + }, { + "time" : 2.49538256040239, + "velocity" : 1.7535621614102237, + "acceleration" : 4.000000000000021, + "pose" : { + "translation" : { + "x" : 2.060871384688653, + "y" : 2.0719075717497617 + }, + "rotation" : { + "radians" : 0.5795857801198268 + } + }, + "curvature" : -0.6608172515589942 + }, { + "time" : 2.53441622407071, + "velocity" : 1.9096968160835055, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.121545097342608, + "y" : 2.1097264237159736 + }, + "rotation" : { + "radians" : 0.5365211642557431 + } + }, + "curvature" : -0.5487920239178083 + }, { + "time" : 2.5725848078202302, + "velocity" : 2.062371151081586, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.18744415889714, + "y" : 2.147190752549328 + }, + "rotation" : { + "radians" : 0.4985109954494367 + } + }, + "curvature" : -0.45807197306180775 + }, { + "time" : 2.6100585451602245, + "velocity" : 2.212266100441563, + "acceleration" : 3.9999999999999893, + "pose" : { + "translation" : { + "x" : 2.258441706252816, + "y" : 2.1842621386104017 + }, + "rotation" : { + "radians" : 0.4648869468393921 + } + }, + "curvature" : -0.3847621795776758 + }, { + "time" : 2.646940241630883, + "velocity" : 2.359792886324197, + "acceleration" : 4.00000000000001, + "pose" : { + "translation" : { + "x" : 2.334377126934328, + "y" : 2.220901381837166 + }, + "rotation" : { + "radians" : 0.43505326862724697 + } + }, + "curvature" : -0.32547741802200536 + }, { + "time" : 2.683288527639372, + "velocity" : 2.505186030358152, + "acceleration" : 4.000000000000014, + "pose" : { + "translation" : { + "x" : 2.415058354883288, + "y" : 2.2570686870381245 + }, + "rotation" : { + "radians" : 0.4084896240286113 + } + }, + "curvature" : -0.2774076507783822 + }, { + "time" : 2.719132405668665, + "velocity" : 2.6485615424753246, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 2.500264166251025, + "y" : 2.2927238491854567 + }, + "rotation" : { + "radians" : 0.384747078396271 + } + }, + "curvature" : -0.23828130083939184 + }, { + "time" : 2.754480704601807, + "velocity" : 2.789954738207894, + "acceleration" : 2.9488324318429173, + "pose" : { + "translation" : { + "x" : 2.5897464751913803, + "y" : 2.3278264387081586 + }, + "rotation" : { + "radians" : 0.3634409880883115 + } + }, + "curvature" : -0.20628975468482194 + }, { + "time" : 2.7895490251950776, + "velocity" : 2.8933653393035956, + "acceleration" : 0.7037966720327286, + "pose" : { + "translation" : { + "x" : 2.683232629653503, + "y" : 2.362335986785183 + }, + "rotation" : { + "radians" : 0.34424291191521855 + } + }, + "curvature" : -0.18000474642406875 + }, { + "time" : 2.8249707390558063, + "velocity" : 2.918295023636472, + "acceleration" : 0.5763378030245723, + "pose" : { + "translation" : { + "x" : 2.7804277071746464, + "y" : 2.39621217063858 + }, + "rotation" : { + "radians" : 0.32687266278130106 + } + }, + "curvature" : -0.15830253678418688 + }, { + "time" : 2.861139234917162, + "velocity" : 2.939140295079909, + "acceleration" : 0.4740600472954994, + "pose" : { + "translation" : { + "x" : 2.881016810672964, + "y" : 2.429414998826637 + }, + "rotation" : { + "radians" : 0.31109102748658707 + } + }, + "curvature" : -0.1402996898246936 + }, { + "time" : 2.8979872681123133, + "velocity" : 2.9566084754391486, + "acceleration" : 0.3917399621578322, + "pose" : { + "translation" : { + "x" : 2.984667364240305, + "y" : 2.4619049965370223 + }, + "rotation" : { + "radians" : 0.2966933532809512 + } + }, + "curvature" : -0.1253009626312453 + }, { + "time" : 2.935436818258982, + "velocity" : 2.9712789607964325, + "acceleration" : 0.3251659903474371, + "pose" : { + "translation" : { + "x" : 3.0910314089350095, + "y" : 2.4936433908799245 + }, + "rotation" : { + "radians" : 0.28350402582744755 + } + }, + "curvature" : -0.11275805306566417 + }, { + "time" : 2.9734007910904325, + "velocity" : 2.9836235536196942, + "acceleration" : 0.2709852112638229, + "pose" : { + "translation" : { + "x" : 3.199747898574706, + "y" : 2.5245922961811917 + }, + "rotation" : { + "radians" : 0.2713717786654788 + } + }, + "curvature" : -0.10223741084606355 + }, { + "time" : 3.0117845646161134, + "velocity" : 2.9940249885976535, + "acceleration" : 0.1543098408303897, + "pose" : { + "translation" : { + "x" : 3.3104449955291066, + "y" : 2.5547148992754742 + }, + "rotation" : { + "radians" : 0.2601657392085518 + } + }, + "curvature" : -0.09339533006984713 + }, { + "time" : 3.0505054353667576, + "velocity" : 2.9999999999999996, + "acceleration" : 7.591176208555741E-15, + "pose" : { + "translation" : { + "x" : 3.422742366512802, + "y" : 2.5839756447993656 + }, + "rotation" : { + "radians" : 0.24977210747664394 + } + }, + "curvature" : -0.08595876803031433 + }, { + "time" : 3.0895059097464324, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.5362534783780575, + "y" : 2.612340420484543 + }, + "rotation" : { + "radians" : 0.24009136790995844 + } + }, + "curvature" : -0.07971061642849785 + }, { + "time" : 3.1286993195347663, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.650587893907611, + "y" : 2.6397767424509064 + }, + "rotation" : { + "radians" : 0.23103594442528902 + } + }, + "curvature" : -0.07447841843016513 + }, { + "time" : 3.16795942350856, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.765353567607466, + "y" : 2.666253940499722 + }, + "rotation" : { + "radians" : 0.22252822046257315 + } + }, + "curvature" : -0.07012575415648367 + }, { + "time" : 3.2071598053906003, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.8801591414996897, + "y" : 2.6917433434067615 + }, + "rotation" : { + "radians" : 0.21449885716390527 + } + }, + "curvature" : -0.06654570394142899 + }, { + "time" : 3.2461747016130165, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.9946162409152066, + "y" : 2.7162184642154443 + }, + "rotation" : { + "radians" : 0.20688535313560402 + } + }, + "curvature" : -0.06365594712905223 + }, { + "time" : 3.2848798203338982, + "velocity" : 3.0, + "acceleration" : -7.735398279518994E-15, + "pose" : { + "translation" : { + "x" : 4.1083417702865965, + "y" : 2.7396551855299762 + }, + "rotation" : { + "radians" : 0.19963079814758483 + } + }, + "curvature" : -0.06139517108104972 + }, { + "time" : 3.32315315320381, + "velocity" : 2.9999999999999996, + "acceleration" : 7.84832578379943E-15, + "pose" : { + "translation" : { + "x" : 4.220960208940891, + "y" : 2.7620319448084913 + }, + "rotation" : { + "radians" : 0.19268278060685093 + } + }, + "curvature" : -0.05972055737308964 + }, { + "time" : 3.3608757811105803, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.332105906892365, + "y" : 2.783329919656194 + }, + "rotation" : { + "radians" : 0.18599241484195372 + } + }, + "curvature" : -0.0586061860352566 + }, { + "time" : 3.3979326749154137, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.441425380635337, + "y" : 2.8035332131184987 + }, + "rotation" : { + "radians" : 0.17951345937534083 + } + }, + "curvature" : -0.05804225916920248 + }, { + "time" : 3.43421349202006, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.548579608936964, + "y" : 2.8226290389741697 + }, + "rotation" : { + "radians" : 0.17320150172377563 + } + }, + "curvature" : -0.058035096135246704 + }, { + "time" : 3.4696133694646405, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.653246328630036, + "y" : 2.8406079070284633 + }, + "rotation" : { + "radians" : 0.16701318920823915 + } + }, + "curvature" : -0.05860789609869524 + }, { + "time" : 3.5040337141415066, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.755122330405774, + "y" : 2.857463808406269 + }, + "rotation" : { + "radians" : 0.16090548924261228 + } + }, + "curvature" : -0.05980230049917739 + }, { + "time" : 3.5373829906159067, + "velocity" : 3.0, + "acceleration" : -1.3000955620508416, + "pose" : { + "translation" : { + "x" : 4.853925754606621, + "y" : 2.87319440084525 + }, + "rotation" : { + "radians" : 0.15483496726687765 + } + }, + "curvature" : -0.06168081541885887 + }, { + "time" : 3.5698052848244366, + "velocity" : 2.9578479191879836, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 4.9493983870190466, + "y" : 2.8878011939889827 + }, + "rotation" : { + "radians" : 0.14875707688110845 + } + }, + "curvature" : -0.0643301641863426 + }, { + "time" : 3.601908104387069, + "velocity" : 2.8294366409374536, + "acceleration" : -3.999999999999995, + "pose" : { + "translation" : { + "x" : 5.041307954666333, + "y" : 2.9012897346800983 + }, + "rotation" : { + "radians" : 0.1426254663359473 + } + }, + "curvature" : -0.06786561693146706 + }, { + "time" : 3.634098277881151, + "velocity" : 2.700675946961127, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.12945042160138, + "y" : 2.913669792253425 + }, + "rotation" : { + "radians" : 0.13639132070467594 + } + }, + "curvature" : -0.07243625256641337 + }, { + "time" : 3.6663242273354184, + "velocity" : 2.5717721491440564, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.213652284699492, + "y" : 2.924955543829128 + }, + "rotation" : { + "radians" : 0.13000278348350505 + } + }, + "curvature" : -0.07823088630580577 + }, { + "time" : 3.6985369799811316, + "velocity" : 2.4429211385612026, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 5.293772869451182, + "y" : 2.935165759605849 + }, + "rotation" : { + "radians" : 0.12340454072534943 + } + }, + "curvature" : -0.08548392661118162 + }, { + "time" : 3.730691893404645, + "velocity" : 2.314301484867149, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.3697066257549615, + "y" : 2.9443239881538474 + }, + "rotation" : { + "radians" : 0.11653771369919076 + } + }, + "curvature" : -0.09447950544669352 + }, { + "time" : 3.7627510266183024, + "velocity" : 2.1860649520125195, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.441385423710139, + "y" : 2.952458741708144 + }, + "rotation" : { + "radians" : 0.10934030498899684 + } + }, + "curvature" : -0.10555051064633172 + }, { + "time" : 3.7946864190689498, + "velocity" : 2.058323382209931, + "acceleration" : -3.9999999999999982, + "pose" : { + "translation" : { + "x" : 5.508780849409618, + "y" : 2.959603681461658 + }, + "rotation" : { + "radians" : 0.10174859495289124 + } + }, + "curvature" : -0.1190660934873409 + }, { + "time" : 3.8581528018765208, + "velocity" : 1.8044578509796478, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.630820283137819, + "y" : 2.971085620886363 + }, + "rotation" : { + "radians" : 0.08513910857652777 + } + }, + "curvature" : -0.15483239704424728 + }, { + "time" : 3.9212966580627038, + "velocity" : 1.5518824262349158, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.73647917568087, + "y" : 2.9791491162686725 + }, + "rotation" : { + "radians" : 0.0663518340621249 + } + }, + "curvature" : -0.20277252875109172 + }, { + "time" : 3.985088994027884, + "velocity" : 1.296713082374195, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.827194162416493, + "y" : 2.98426771953671 + }, + "rotation" : { + "radians" : 0.045585623619075005 + } + }, + "curvature" : -0.2546275827711898 + }, { + "time" : 4.0522933348683985, + "velocity" : 1.027895719012139, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.9052577307282945, + "y" : 2.987015154086521 + }, + "rotation" : { + "radians" : 0.02445962652264226 + } + }, + "curvature" : -0.27725843604659994 + }, { + "time" : 4.131181440085891, + "velocity" : 0.7123432981421679, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.973891685375232, + "y" : 2.988071244162576 + }, + "rotation" : { + "radians" : 0.007123366020833881 + } + }, + "curvature" : -0.20476533171945074 + }, { + "time" : 4.309267264621433, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.037320613861084, + "y" : 2.9882278442382812 + }, + "rotation" : { + "radians" : 0.0 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.056426048278809, -3.3782451152801514, 0.0 ], + "y" : [ -0.5667314529418945, 2.785569906234741, 0.0 ] + }, { + "x" : [ 1.7378531694412231, -0.24853384494781494, 0.0 ], + "y" : [ 1.444043517112732, 2.535710334777832, 0.0 ] + }, { + "x" : [ 6.037320613861084, 2.0, 0.0 ], + "y" : [ 2.9882278442382812, 0.0, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.2873733127254927 + } + }, { + "time" : 1.8499999586492777, + "rotation" : { + "radians" : 2.3228735180023055 + } + }, { + "time" : 4.21999990567565, + "rotation" : { + "radians" : 0.6128208180837543 + } + } ], + "reversed" : false, + "color" : 124.23529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + } ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/red/hideballs.json b/src/main/deploy/autos/red/hidecargo.json similarity index 77% rename from src/main/deploy/autos/red/hideballs.json rename to src/main/deploy/autos/red/hidecargo.json index 7d69ec29..9932a2f5 100644 --- a/src/main/deploy/autos/red/hideballs.json +++ b/src/main/deploy/autos/red/hidecargo.json @@ -2109,1057 +2109,982 @@ }, "curvature" : 0.0 }, { - "time" : 0.1839004373900963, - "velocity" : 0.7356017495603853, - "acceleration" : 3.999999999999999, - "pose" : { - "translation" : { - "x" : 4.079839881543424, - "y" : -0.9175061170373492 - }, - "rotation" : { - "radians" : 0.032597811527258144 - } - }, - "curvature" : 0.05820539136138801 - }, { - "time" : 0.2598794478276359, - "velocity" : 1.0395177913105436, - "acceleration" : 4.000000000000002, + "time" : 0.18393581920253443, + "velocity" : 0.7357432768101378, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 4.147234073981139, - "y" : -0.9151343674395811 + "x" : 4.079866022458708, + "y" : -0.9175092776647826 }, "rotation" : { - "radians" : 0.03836566081266323 + "radians" : 0.032422495075255596 } }, - "curvature" : 0.11233457015103718 + "curvature" : 0.05303783852412645 }, { - "time" : 0.31790134200340625, - "velocity" : 1.2716053680136252, - "acceleration" : 3.9999999999999982, + "time" : 0.2600740573066539, + "velocity" : 1.0402962292266158, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 4.2142208005905895, - "y" : -0.912271579263629 + "x" : 4.147437401812738, + "y" : -0.9151584665515866 }, "rotation" : { - "radians" : 0.047629178342962146 + "radians" : 0.03767860106119578 } }, - "curvature" : 0.1636754126870864 + "curvature" : 0.10177004653056979 }, { - "time" : 0.3664818052070989, - "velocity" : 1.4659272208283958, - "acceleration" : 4.0000000000000036, + "time" : 0.31842255095187355, + "velocity" : 1.2736902038074942, + "acceleration" : 3.9999999999999902, "pose" : { "translation" : { - "x" : 4.280620524327787, - "y" : -0.9087076362808375 + "x" : 4.214887726988865, + "y" : -0.9123489750712632 }, "rotation" : { - "radians" : 0.060164770303775844 + "radians" : 0.046083444431288076 } }, - "curvature" : 0.2132011191407471 + "curvature" : 0.14663006175024168 }, { - "time" : 0.40891181812753247, - "velocity" : 1.6356472725101303, - "acceleration" : 4.0, + "time" : 0.36752120065207927, + "velocity" : 1.4700848026083166, + "acceleration" : 3.9999999999999933, "pose" : { "translation" : { - "x" : 4.346269665970026, - "y" : -0.9042556640216365 + "x" : 4.28215626266865, + "y" : -0.9088819079360633 }, "rotation" : { - "radians" : 0.07578809002213045 + "radians" : 0.05737092549763 } }, - "curvature" : 0.261623846845801 + "curvature" : 0.18797575110518114 }, { - "time" : 0.44687903311679705, - "velocity" : 1.7875161324671887, - "acceleration" : 4.0, + "time" : 0.41067486026855743, + "velocity" : 1.642699441074229, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.411020111290583, - "y" : -0.8987512250670413 + "x" : 4.349182263537044, + "y" : -0.904578393853863 }, "rotation" : { - "radians" : 0.09434193350703354 + "radians" : 0.0712929378915065 } }, - "curvature" : 0.30942902122153493 + "curvature" : 0.22608984719455819 }, { - "time" : 0.4813894477163118, - "velocity" : 1.9255577908652477, - "acceleration" : 3.9999999999999964, + "time" : 0.4495779246156969, + "velocity" : 1.7983116984627867, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.474738718233415, - "y" : -0.892051514340153 + "x" : 4.415905077562968, + "y" : -0.8992789224692164 }, "rotation" : { - "radians" : 0.11568561342262434 + "radians" : 0.08761614166764932 } }, - "curvature" : 0.3568953774098475 + "curvature" : 0.2611858070311432 }, { - "time" : 0.5131039023083906, - "velocity" : 2.052415609233563, - "acceleration" : -0.5080487315100024, + "time" : 0.48523766875483854, + "velocity" : 1.940950675019353, + "acceleration" : 3.9999999999999867, "pose" : { "translation" : { - "x" : 4.537306824087864, - "y" : -0.8840345543976582 + "x" : 4.482264249279902, + "y" : -0.8928426813044079 }, "rotation" : { - "radians" : 0.13968544333222133 + "radians" : 0.10611908836229655 } }, - "curvature" : 0.404105267004568 + "curvature" : 0.29341692608996234 }, { - "time" : 0.5734741207486527, - "velocity" : 2.021744596334006, - "acceleration" : -2.5986497425654487, + "time" : 0.5183117595380216, + "velocity" : 2.0732470381520844, + "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.658586321464071, - "y" : -0.8636602870095267 + "x" : 4.548199623066466, + "y" : -0.8851468927005044 }, "rotation" : { - "radians" : 0.19510294768348427 + "radians" : 0.12658982254096487 } }, - "curvature" : 0.4971313158757367 + "curvature" : 0.32288731915441804 }, { - "time" : 0.6345428990288139, - "velocity" : 1.8630482313774788, - "acceleration" : -1.782825751736676, + "time" : 0.5492586656367637, + "velocity" : 2.197034662547053, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.774180161280128, - "y" : -0.8370385771048632 + "x" : 4.613651446427005, + "y" : -0.876086150758407 }, "rotation" : { - "radians" : 0.2593591749925202 + "radians" : 0.14882402397746977 } }, - "curvature" : 0.5854307189034433 + "curvature" : 0.3496638342427625 }, { - "time" : 0.6978369262592252, - "velocity" : 1.750206009699979, - "acceleration" : -1.1573812389052407, + "time" : 0.5784138965550384, + "velocity" : 2.3136555862201518, + "acceleration" : -1.6236735964798095, "pose" : { "translation" : { - "x" : 4.883610030103824, - "y" : -0.8038613196792674 + "x" : 4.678560473272171, + "y" : -0.8655717582799038 }, "rotation" : { - "radians" : 0.33087764946357995 + "radians" : 0.17262371101994242 } }, - "curvature" : 0.6633540150470494 + "curvature" : 0.3737882085974883 }, { - "time" : 0.7622830361592594, - "velocity" : 1.6756172911812541, - "acceleration" : -0.6206912248410478, + "time" : 0.6069779983301187, + "velocity" : 2.267276808360792, + "acceleration" : -1.8021731001922658, "pose" : { "translation" : { - "x" : 4.986581972800195, - "y" : -0.7640763998497277 + "x" : 4.742868067199511, + "y" : -0.8535310637087223 }, "rotation" : { - "radians" : 0.40763331940017733 + "radians" : 0.19779649220341822 } }, - "curvature" : 0.7237257699378088 + "curvature" : 0.3952888936073791 }, { - "time" : 0.8268495770863082, - "velocity" : 1.6355414058094946, - "acceleration" : -0.1141447568431195, + "time" : 0.6360217316741164, + "velocity" : 2.214934973399082, + "acceleration" : -1.4376529363116377, "pose" : { "translation" : { - "x" : 5.0829706221219055, - "y" : -0.7178619421826262 + "x" : 4.80651630477405, + "y" : -0.8399067980715813 }, "rotation" : { - "radians" : 0.48716997806870804 + "radians" : 0.22415532600922183 } }, - "curvature" : 0.7596273921140029 + "curvature" : 0.41419203433477625 }, { - "time" : 0.890535049243441, - "velocity" : 1.6282720430756794, - "acceleration" : 0.4095450005884974, + "time" : 0.6655393241600903, + "velocity" : 2.1724989198887714, + "acceleration" : -1.1396966079827222, "pose" : { "translation" : { - "x" : 5.1728034282996305, - "y" : -0.6656005600217441 + "x" : 4.869448078808871, + "y" : -0.824656411919243 }, "rotation" : { - "radians" : 0.5667524438791094 + "radians" : 0.25151872624934885 } }, - "curvature" : 0.7664251916360058 + "curvature" : 0.4305311361191539 }, { - "time" : 0.9523748257700176, - "velocity" : 1.6535982143896488, - "acceleration" : 1.0079964321999564, + "time" : 0.6954246504047454, + "velocity" : 2.138438714939281, + "acceleration" : -0.8908554566969, "pose" : { "translation" : { - "x" : 5.2562448886324376, - "y" : -0.6078536048162668 + "x" : 4.931607201645704, + "y" : -0.8077514122675655 }, "rotation" : { - "radians" : 0.6436149831528346 + "radians" : 0.2797113326941941 } }, - "curvature" : 0.7431281524527987 + "curvature" : 0.4443550158057545 }, { - "time" : 1.011449874872387, - "velocity" : 1.7131456531168745, - "acceleration" : 1.7790042074143708, + "time" : 0.725580967378834, + "velocity" : 2.111573795409033, + "acceleration" : -0.6793465022751951, "pose" : { "translation" : { - "x" : 5.3335807770781685, - "y" : -0.5453354154487897 + "x" : 4.992938508435509, + "y" : -0.7891766995385554 }, "rotation" : { - "radians" : 0.7152240168950873 + "radians" : 0.3085647538618645 } }, - "curvature" : 0.6923650331342213 + "curvature" : 0.45573373321158034 }, { - "time" : 1.066883029265564, - "velocity" : 1.8117614680125869, - "acceleration" : 2.9290544621119077, + "time" : 0.7559198081982444, + "velocity" : 2.0909632100152824, + "acceleration" : -0.4179681793139011, "pose" : { "translation" : { - "x" : 5.405202373843821, - "y" : -0.47888756756332285 + "x" : 5.053387960419059, + "y" : -0.7689299045014195 }, "rotation" : { - "radians" : 0.7794678142970032 + "radians" : 0.33791858215085874 } }, - "curvature" : 0.6190442095943712 + "curvature" : 0.4647623301686379 }, { - "time" : 1.1178097408882126, - "velocity" : 1.960928579931592, - "acceleration" : 4.000000000000004, + "time" : 0.8167858290266697, + "velocity" : 2.0655231501075435, + "acceleration" : -0.1384063526866644, "pose" : { "translation" : { - "x" : 5.47159069497593, - "y" : -0.4094531228932965 + "x" : 5.171431395063053, + "y" : -0.7234702639619126 }, "rotation" : { - "radians" : 0.8347237020762335 + "radians" : 0.39753225232864264 } }, - "curvature" : 0.5284454355848308 + "curvature" : 0.47628134337229905 }, { - "time" : 1.163781325410918, - "velocity" : 2.144814918022414, - "acceleration" : 4.0, + "time" : 0.8775052043910777, + "velocity" : 2.0571192028259433, + "acceleration" : 0.07651708161584311, "pose" : { "translation" : { - "x" : 5.533300721950951, - "y" : -0.3380508785895664 + "x" : 5.28533164196233, + "y" : -0.6715829475066926 }, "rotation" : { - "radians" : 0.879806448814223 + "radians" : 0.457468911728272 } }, - "curvature" : 0.4245631719643485 + "curvature" : 0.4801807958619656 }, { - "time" : 1.2052874260275384, - "velocity" : 2.3108393204888955, - "acceleration" : 0.9499793055242893, + "time" : 0.9376075969896146, + "velocity" : 2.061718062505713, + "acceleration" : 0.23719713417019256, "pose" : { "translation" : { - "x" : 5.59094563126564, - "y" : -0.265749616548419 + "x" : 5.39470746489593, + "y" : -0.6136396617759576 }, "rotation" : { - "radians" : 0.9138373619233752 + "radians" : 0.51683377374029 } }, - "curvature" : 0.3090988446696365 + "curvature" : 0.47804100648283726 }, { - "time" : 1.2440241708410498, - "velocity" : 2.3476384264251067, - "acceleration" : -4.000000000000005, + "time" : 0.9967126677163254, + "velocity" : 2.0757376158970153, + "acceleration" : 0.3466981374198625, "pose" : { "translation" : { - "x" : 5.645181024027437, - "y" : -0.19364235273957675 + "x" : 5.49920556001598, + "y" : -0.5501521453061287 }, "rotation" : { - "radians" : 0.93608126262559 + "radians" : 0.5749352590162321 } }, - "curvature" : 0.18105135742527462 + "curvature" : 0.471605425281159 }, { - "time" : 1.2825935410962108, - "velocity" : 2.1933609454044625, - "acceleration" : -4.000000000000005, + "time" : 1.0545224472486359, + "velocity" : 2.0957801587855203, + "acceleration" : 0.40356633939226144, "pose" : { "translation" : { - "x" : 5.696689155544846, - "y" : -0.12282058653420336 + "x" : 5.5985038608263835, + "y" : -0.48175095064352114 }, "rotation" : { - "radians" : 0.9457881291979806 + "radians" : 0.6312847326123074 } }, - "curvature" : 0.036672195665488225 + "curvature" : 0.46262836247598543 }, { - "time" : 1.3225645194975204, - "velocity" : 2.033477031799224, - "acceleration" : -4.000000000000003, + "time" : 1.1108140478616002, + "velocity" : 2.1184975539834254, + "acceleration" : 0.40345921603853396, "pose" : { "translation" : { - "x" : 5.746163164917817, - "y" : -0.05434855003290906 + "x" : 5.692314843161512, + "y" : -0.4091642264580173 }, "rotation" : { - "radians" : 0.9420639655569212 + "radians" : 0.6855833217165795 } }, - "curvature" : -0.13035733884148892 + "curvature" : 0.4527597065949554 }, { - "time" : 1.3640767963731968, - "velocity" : 1.8674279242965188, - "acceleration" : -4.0, + "time" : 1.1654323160372946, + "velocity" : 2.1405337976429735, + "acceleration" : 0.34034527303420153, "pose" : { "translation" : { - "x" : 5.794291304628132, - "y" : 0.010762542606244097 + "x" : 5.780388830164895, + "y" : -0.33319649965673825 }, "rotation" : { - "radians" : 0.9237929192972799 + "radians" : 0.7377000747034669 } }, - "curvature" : -0.3283818138757726 + "curvature" : 0.443485601912384 }, { - "time" : 1.4073934572912177, - "velocity" : 1.6941612806244348, - "acceleration" : -4.000000000000003, + "time" : 1.2182824818871505, + "velocity" : 2.1585211017690455, + "acceleration" : 0.20771430856564294, "pose" : { "translation" : { - "x" : 5.8417411701297794, - "y" : 0.07158024583973699 + "x" : 5.862517297267914, + "y" : -0.25470745749771595 }, "rotation" : { - "radians" : 0.8896456491310966 + "radians" : 0.7876462908989643 } }, - "curvature" : -0.5662511162346159 + "curvature" : 0.4361251242987918 }, { - "time" : 1.4530216175559991, - "velocity" : 1.5116486395653088, - "acceleration" : -4.000000000000002, + "time" : 1.2693229851547776, + "velocity" : 2.169122944614123, + "acceleration" : 1.0817549407792827E-4, "pose" : { "translation" : { - "x" : 5.889143929439342, - "y" : 0.12727663341058815 + "x" : 5.938536177168487, + "y" : -0.17459072970356537 }, "rotation" : { - "radians" : 0.8382382104223547 + "radians" : 0.8355501360445428 } }, - "curvature" : -0.8496047780846073 + "curvature" : 0.431872317394874 }, { - "time" : 1.5019520124723162, - "velocity" : 1.31592705990004, - "acceleration" : -3.2992132129798613, + "time" : 1.3185585791987948, + "velocity" : 2.169128270698835, + "acceleration" : -0.2848041321796146, "pose" : { "translation" : { - "x" : 5.937078552726376, - "y" : 0.17715404902034848 + "x" : 6.008329164809766, + "y" : -0.09375267057515657 }, "rotation" : { - "radians" : 0.7685396527813694 + "radians" : 0.8816343285891601 } }, - "curvature" : -1.173437784895902 + "curvature" : 0.431870196556052 }, { - "time" : 1.5280288447019, - "velocity" : 1.2298940304555388, - "acceleration" : -2.595879024435883, + "time" : 1.3660336232443555, + "velocity" : 2.15560718197925, + "acceleration" : -0.6435155746795979, "pose" : { "translation" : { - "x" : 5.961408087691393, - "y" : 0.19973458251777654 + "x" : 6.071831022358822, + "y" : -0.013091141105286752 }, "rotation" : { - "radians" : 0.7267822918880255 + "radians" : 0.9261982414140092 } }, - "curvature" : -1.343347329975652 + "curvature" : 0.43730501698866603 }, { - "time" : 1.5550966642268162, - "velocity" : 1.1596292455135926, - "acceleration" : -2.0093413111156626, + "time" : 1.4118252194909213, + "velocity" : 2.126139576605145, + "acceleration" : -1.063489692084961, "pose" : { "translation" : { - "x" : 5.986056041903794, - "y" : 0.220670857001096 + "x" : 6.12903088418534, + "y" : 0.06652570890764764 }, "rotation" : { - "radians" : 0.6806182852772713 + "radians" : 0.9696044259991614 } }, - "curvature" : -1.5110727869831724 + "curvature" : 0.4495108307569809 }, { - "time" : 1.582993226711798, - "velocity" : 1.103575530074399, - "acceleration" : -1.499480490388413, + "time" : 1.4560355521525066, + "velocity" : 2.0791223435359023, + "acceleration" : -1.52002157710405, "pose" : { "translation" : { - "x" : 6.011073083229763, - "y" : 0.23992377095496042 + "x" : 6.179975561840308, + "y" : 0.14428065874597884 }, "rotation" : { - "radians" : 0.6303974993218476 + "radians" : 1.0122682566684933 } }, - "curvature" : -1.6684745220647488 + "curvature" : 0.4700711657871874 }, { - "time" : 1.6115420851073832, - "velocity" : 1.0607670738873578, - "acceleration" : -1.0383464958403599, + "time" : 1.4987824321910335, + "velocity" : 2.014146163523463, + "acceleration" : -1.974071718041697, "pose" : { "translation" : { - "x" : 6.036503660218244, - "y" : 0.2574671929874306 + "x" : 6.224772849034707, + "y" : 0.21942863465909568 }, "rotation" : { - "radians" : 0.576664005958531 + "radians" : 1.0546477790856124 } }, - "curvature" : -1.8058581852202884 + "curvature" : 0.500889279611352 }, { - "time" : 1.6405508670810625, - "velocity" : 1.030645906776391, - "acceleration" : -0.6046251421798862, + "time" : 1.540186616781137, + "velocity" : 1.9324113337155617, + "acceleration" : -2.3720307789417294, "pose" : { "translation" : { - "x" : 6.062385509275637, - "y" : 0.27328876653847445 + "x" : 6.263594826618203, + "y" : 0.2913179277497715 }, "rotation" : { - "radians" : 0.5201672456863837 + "radians" : 1.0972283316685527 } }, - "curvature" : -1.9129549301403845 + "curvature" : 0.5441574151816366 }, { - "time" : 1.6698112663305722, - "velocity" : 1.0129543337199158, - "acceleration" : -0.17989538152775067, + "time" : 1.580353908329549, + "velocity" : 1.8371332818560024, + "acceleration" : -2.6474197214125605, "pose" : { "translation" : { - "x" : 6.0887491618404965, - "y" : 0.28739071458846865 + "x" : 6.296681167557836, + "y" : 0.3594114118604921 }, "rotation" : { - "radians" : 0.46185036100408955 + "radians" : 1.140492186777639 } }, - "curvature" : -1.9803591986041147 + "curvature" : 0.6020636034649972 }, { - "time" : 1.6991006131857587, - "velocity" : 1.0076853154927035, - "acceleration" : 0.25454795207597797, + "time" : 1.6193492670845173, + "velocity" : 1.7338962000445413, + "acceleration" : -2.721672977384236, "pose" : { "translation" : { - "x" : 6.1156174515582356, - "y" : 0.2997906443666931 + "x" : 6.324342441916713, + "y" : 0.42330776145978355 }, "rotation" : { - "radians" : 0.40281210939179324 + "radians" : 1.1848561982155952 } }, - "curvature" : -2.00112327804307 + "curvature" : 0.6758923235708024 }, { - "time" : 1.728184543388813, - "velocity" : 1.0150885703642116, - "acceleration" : 0.7213256391379216, + "time" : 1.6571590723066265, + "velocity" : 1.6309902748913652, + "acceleration" : -2.4945738768301835, "pose" : { "translation" : { - "x" : 6.143005021455821, - "y" : 0.3105223520598379 + "x" : 6.346963421832697, + "y" : 0.4827626695285403 }, "rotation" : { - "radians" : 0.3442451501516796 + "radians" : 1.2305496048186084 } }, - "curvature" : -1.9720404920384738 + "curvature" : 0.7638726499983753 }, { - "time" : 1.756819895841348, - "velocity" : 1.035743984273976, - "acceleration" : 1.2524391885977852, + "time" : 1.6936360168640041, + "velocity" : 1.539995841891948, + "acceleration" : -1.7970788234851456, "pose" : { "translation" : { - "x" : 6.170917831116474, - "y" : 0.31963662752049693 + "x" : 6.3650063864971, + "y" : 0.5377100654463529 }, "rotation" : { - "radians" : 0.28736056159514967 + "radians" : 1.2773923275414163 } }, - "curvature" : -1.8941696100081031 + "curvature" : 0.8568101590190533 }, { - "time" : 1.784756647104708, - "velocity" : 1.0707330663583168, - "acceleration" : 1.8992599815033173, + "time" : 1.7284182440412232, + "velocity" : 1.477489437998118, + "acceleration" : 1.705632879833736, "pose" : { "translation" : { - "x" : 6.19935266385437, - "y" : 0.3272020589756721 + "x" : 6.37901442713337, + "y" : 0.5882833328778361 }, "rotation" : { - "radians" : 0.23331316860807427 + "radians" : 1.3244344674414545 } }, - "curvature" : -1.7723980930633711 + "curvature" : 0.9308397775185885 }, { - "time" : 1.8117374729698128, - "velocity" : 1.1219766691918198, - "acceleration" : 2.7534793106504156, + "time" : 1.7883248954688422, + "velocity" : 1.5796681923938034, + "acceleration" : 3.9999999999999862, "pose" : { "translation" : { - "x" : 6.228296633889339, - "y" : 0.33330583773527067 + "x" : 6.397521991248141, + "y" : 0.6779655956833608 }, "rotation" : { - "radians" : 0.183142361520775 + "radians" : 1.4085741509062624 } }, - "curvature" : -1.614195199267953 + "curvature" : 0.8143141037035398 }, { - "time" : 1.8374933012978958, - "velocity" : 1.1928948096218601, - "acceleration" : -3.5661641751301776, + "time" : 1.8363455674677351, + "velocity" : 1.7717508803893742, + "acceleration" : 4.000000000000008, "pose" : { "translation" : { - "x" : 6.257726693521562, - "y" : 0.33805456290060576 + "x" : 6.408572673797607, + "y" : 0.7576718926429749 }, "rotation" : { - "radians" : 0.1377385534824891 + "radians" : 1.4464418628084068 } }, - "curvature" : -1.4279710594799537 - }, { - "time" : 1.8927457727102306, - "velocity" : 0.9958554254837872, - "acceleration" : -3.999999999999999, - "pose" : { - "translation" : { - "x" : 6.317899124228461, - "y" : 0.3440151160617688 - }, - "rotation" : { - "radians" : 0.06404669180836077 - } - }, - "curvature" : -1.0007588885962067 + "curvature" : 0.0 }, { - "time" : 1.9651351974193783, - "velocity" : 0.7062977266471963, - "acceleration" : -4.0, + "time" : 1.879584211551695, + "velocity" : 1.9447054567252142, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 6.37946360862216, - "y" : 0.3463552460207815 + "x" : 6.417938666279497, + "y" : 0.8374714001847678 }, "rotation" : { - "radians" : 0.01679972270125931 + "radians" : 1.4673635124320494 } }, - "curvature" : -0.5272785953582695 + "curvature" : 0.4361537758239235 }, { - "time" : 2.1417096290811775, - "velocity" : 0.0, - "acceleration" : -4.0, + "time" : 1.9241999105096688, + "velocity" : 2.123168252557109, + "acceleration" : 4.000000000000008, "pose" : { "translation" : { - "x" : 6.441819667816162, - "y" : 0.3467085063457489 + "x" : 6.425398940205923, + "y" : 0.9279097366234055 }, "rotation" : { - "radians" : 0.0 + "radians" : 1.509659326369691 } }, - "curvature" : 0.0 - } ], - "pointList" : [ { - "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], - "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + "curvature" : 0.44722076145528444 }, { - "x" : [ 6.441819667816162, 2.0, 0.0 ], - "y" : [ 0.3467085063457489, 0.0, 0.0 ] - } ], - "rotations" : [ { - "time" : 0.0, - "rotation" : { - "radians" : 0.0 - } - }, { - "time" : 2.0499999541789293, - "rotation" : { - "radians" : 0.0 - } - } ], - "reversed" : false, - "color" : 286.3529, - "closed" : false, - "velocityStart" : 0.0, - "velocityEnd" : 0.0, - "constraints" : [ ] - }, { - "type" : "script", - "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "sleep", - "args" : [ "1000" ], - "argTypes" : [ "long" ], - "reflection" : false - }, { - "methodName" : "frc.subsystem.Intake.setWantedIntakeState", - "args" : [ "EJECT" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], - "reflection" : true - } ] - } - }, { - "type" : "script", - "script" : "Intake.setIntakeSolState CLOSE", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "frc.subsystem.Intake.setIntakeSolState", - "args" : [ "CLOSE" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], - "reflection" : true - } ] - } - }, { - "type" : "trajectory", - "states" : [ { - "time" : 0.0, - "velocity" : 0.0, - "acceleration" : 4.0, + "time" : 1.9728110085852033, + "velocity" : 2.317612644859248, + "acceleration" : 3.999999999999993, "pose" : { "translation" : { - "x" : 6.441819667816162, - "y" : 0.3467085063457489 + "x" : 6.429642048575715, + "y" : 1.035761920819823 }, "rotation" : { - "radians" : 1.6078163234494292 + "radians" : 1.550908753938559 } }, - "curvature" : -0.0 + "curvature" : 0.31730031892680094 }, { - "time" : 0.1953077076111654, - "velocity" : 0.7812308304446616, - "acceleration" : 3.999999999999996, + "time" : 1.9987893246112054, + "velocity" : 2.421525908963256, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 6.4389816843095105, - "y" : 0.42294590310561375 + "x" : 6.430305899986379, + "y" : 1.0973157606626955 }, "rotation" : { - "radians" : 1.6083198170208068 + "radians" : 1.5684899825119734 } }, - "curvature" : 0.010007683937884466 + "curvature" : 0.25637867502172557 }, { - "time" : 0.28418183429804866, - "velocity" : 1.1367273371921942, - "acceleration" : 3.9999999999999987, + "time" : 2.025781495555629, + "velocity" : 2.5294945927409493, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 6.435750551492674, - "y" : 0.5081130612115885 + "x" : 6.429927970748395, + "y" : 1.1641340877395123 }, "rotation" : { - "radians" : 1.609052997763179 + "radians" : 1.583853790503598 } }, - "curvature" : 0.006001548474080814 + "curvature" : 0.20594813300808948 }, { - "time" : 0.3615636139485796, - "velocity" : 1.446254455794318, - "acceleration" : 4.0, + "time" : 2.0536364380792893, + "velocity" : 2.6409143628355918, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.431910399555818, - "y" : 0.6079771182521938 + "x" : 6.42849117103934, + "y" : 1.2361304745226107 }, "rotation" : { - "radians" : 1.6092943557828885 + "radians" : 1.597166880767735 } }, - "curvature" : -8.5488367035683E-4 + "curvature" : 0.16589805557058687 }, { - "time" : 0.43523665263811134, - "velocity" : 1.740946610552445, - "acceleration" : 4.0, + "time" : 2.082166732992299, + "velocity" : 2.7550355424876307, + "acceleration" : 3.999999999999967, "pose" : { "translation" : { - "x" : 6.427411179058254, - "y" : 0.72529627053882 + "x" : 6.4260035555757895, + "y" : 1.3130642882106258 }, "rotation" : { - "radians" : 1.6088589590984765 + "radians" : 1.6086729537639617 } }, - "curvature" : -0.006111352293260847 + "curvature" : 0.13471207633826426 }, { - "time" : 0.47131335599478547, - "velocity" : 1.8852534239791414, - "acceleration" : 3.999999999999987, + "time" : 2.1111641583855323, + "velocity" : 2.8710252440605633, + "acceleration" : 4.000000000000011, "pose" : { "translation" : { - "x" : 6.42493665439585, - "y" : 0.7906601188879874 + "x" : 6.422495681211174, + "y" : 1.3945594657209313 }, "rotation" : { - "radians" : 1.6083911287059784 + "radians" : 1.6186275742658227 } }, - "curvature" : -0.008116127399784383 + "curvature" : 0.11061622885252656 }, { - "time" : 0.5069348966838761, - "velocity" : 2.0277395867355037, - "acceleration" : 4.0, + "time" : 2.1404115791378824, + "velocity" : 2.988014927069964, + "acceleration" : 0.40226107984247866, "pose" : { "translation" : { - "x" : 6.422338327934085, - "y" : 0.8603050862099906 + "x" : 6.418017964533647, + "y" : 1.4801232886820799 }, "rotation" : { - "radians" : 1.607763133691658 + "radians" : 1.6272684320752597 } }, - "curvature" : -0.009853447575726546 + "curvature" : 0.09202002758878106 }, { - "time" : 0.5420590363593313, - "velocity" : 2.1682361454373242, - "acceleration" : 4.000000000000005, + "time" : 2.170205843436901, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.419643072810146, - "y" : 0.9339457983013029 + "x" : 6.412638039463943, + "y" : 1.5691651584262445 }, "rotation" : { - "radians" : 1.6069775892569418 + "radians" : 1.6348043576150604 } }, - "curvature" : -0.011436025231544151 + "curvature" : 0.07763517137648186 }, { - "time" : 0.5766104955030272, - "velocity" : 2.306441982012108, - "acceleration" : 3.9999999999999947, + "time" : 2.200892251234442, + "velocity" : 2.9999999999999996, + "acceleration" : 9.430296455303456E-15, "pose" : { "translation" : { - "x" : 6.416882438497851, - "y" : 1.0111998183356263 + "x" : 6.406438114853245, + "y" : 1.6610153709816586 }, "rotation" : { - "radians" : 1.6060340313214787 + "radians" : 1.6414135047463148 } }, - "curvature" : -0.012962877357532548 + "curvature" : 0.06646668401976608 }, { - "time" : 0.6104960029929839, - "velocity" : 2.4419840119719347, - "acceleration" : 4.0, + "time" : 2.232286754539466, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.4140917029015725, - "y" : 1.0916028128984 + "x" : 6.399512332081045, + "y" : 1.7549438920650573 }, "rotation" : { - "radians" : 1.604928600003804 + "radians" : 1.6472455716072028 } }, - "curvature" : -0.014520270296573292 + "curvature" : 0.05776403436142682 }, { - "time" : 0.6436141193910724, - "velocity" : 2.5744564775642886, - "acceleration" : 3.9999999999999947, + "time" : 2.2641313884937704, + "velocity" : 3.0, + "acceleration" : -0.5220281507380499, "pose" : { "translation" : { - "x" : 6.411308924450168, - "y" : 1.1746237180213086 + "x" : 6.3919641226530075, + "y" : 1.8501791320741177 }, "rotation" : { - "radians" : 1.603653938746236 + "radians" : 1.6524254678653565 } }, - "curvature" : -0.01618596901731972 + "curvature" : 0.050967461408882746 }, { - "time" : 0.6758618258229525, - "velocity" : 2.703447303291809, - "acceleration" : 4.0, + "time" : 2.2962499027431713, + "velocity" : 2.9832332314019316, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.408573994190903, - "y" : 1.2596799052167897 + "x" : 6.383903565798832, + "y" : 1.9459267210798998 }, "rotation" : { - "radians" : 1.6021991606605406 + "radians" : 1.6570571945158592 } }, - "curvature" : -0.01803416029898833 + "curvature" : 0.04566147685699515 }, { - "time" : 0.7071390741340647, - "velocity" : 2.828556296536258, - "acceleration" : 3.5711423686938364, + "time" : 2.329098019757857, + "velocity" : 2.851840763343189, + "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.405927687883377, - "y" : 1.3461523475125432 + "x" : 6.375444746070116, + "y" : 2.0413882838192876 }, "rotation" : { - "radians" : 1.6005498243279082 + "radians" : 1.6612273952864063 } }, - "curvature" : -0.020140089228601762 + "curvature" : 0.04153801107443314 }, { - "time" : 0.7374186993151032, - "velocity" : 2.936689148928433, - "acceleration" : -3.99999999999999, + "time" : 2.3631515088169963, + "velocity" : 2.7156268071066307, + "acceleration" : -3.999999999999995, "pose" : { "translation" : { - "x" : 6.403410718093454, - "y" : 1.4334007854860387 + "x" : 6.366703110938218, + "y" : 2.135780214687429 }, "rotation" : { - "radians" : 1.5986879179972218 + "radians" : 1.6650083738352117 } }, - "curvature" : -0.02258406827188887 + "curvature" : 0.038368183750837316 }, { - "time" : 0.7678125246032664, - "velocity" : 2.8151138477757804, + "time" : 2.3983080444520524, + "velocity" : 2.5750006645664074, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.401062786287184, - "y" : 1.520778893299024 + "x" : 6.357792828392121, + "y" : 2.228352452730178 }, "rotation" : { - "radians" : 1.5965918957171965 + "radians" : 1.6684605177848264 } }, - "curvature" : -0.02545459002718879 + "curvature" : 0.03598061733215407 }, { - "time" : 0.7993888917739188, - "velocity" : 2.688808379093171, - "acceleration" : -4.0, + "time" : 2.4344694314979898, + "velocity" : 2.4303551163826573, + "acceleration" : -3.999999999999995, "pose" : { "translation" : { - "x" : 6.398921634924733, - "y" : 1.6076494447320337 + "x" : 6.3488241445362945, + "y" : 2.318407256636533 }, "rotation" : { - "radians" : 1.59423685765634 + "radians" : 1.6716341182037762 } }, - "curvature" : -0.028849988627757065 + "curvature" : 0.03424396145033817 }, { - "time" : 0.8320832691315886, - "velocity" : 2.558030869662492, - "acceleration" : -4.0, + "time" : 2.4715493896338248, + "velocity" : 2.2820352838393174, + "acceleration" : -4.000000000000005, "pose" : { "translation" : { - "x" : 6.397022099554306, - "y" : 1.6933994792188969 + "x" : 6.339900741188558, + "y" : 2.4053179797310804 }, "rotation" : { - "radians" : 1.59159503414185 + "radians" : 1.6745705658893675 } }, - "curvature" : -0.03287744142106298 + "curvature" : 0.03305114421605748 }, { - "time" : 0.8658400026260393, - "velocity" : 2.423003935684689, + "time" : 2.509484949213048, + "velocity" : 2.1302930455224227, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.395395160906077, - "y" : 1.7774554678812455 + "x" : 6.331117093477943, + "y" : 2.4885478449664333 }, "rotation" : { - "radians" : 1.588636834345054 + "radians" : 1.6773028650946311 } }, - "curvature" : -0.03764692669916069 + "curvature" : 0.03230248453046758 }, { - "time" : 0.9006204438645928, - "velocity" : 2.283882170730475, + "time" : 2.5482536718462243, + "velocity" : 1.9752181549897188, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.394066996986112, - "y" : 1.859298479563023 + "x" : 6.322555827442557, + "y" : 2.567668719915673 }, "rotation" : { - "radians" : 1.5853328740324872 + "radians" : 1.6798553438041104 } }, - "curvature" : -0.04325577926527458 + "curvature" : 0.03188409115100577 }, { - "time" : 0.9364146483950042, - "velocity" : 2.1407053526088293, + "time" : 2.587900600923266, + "velocity" : 1.8166304386815513, "acceleration" : -3.999999999999997, "pose" : { "translation" : { - "x" : 6.393058035170299, - "y" : 1.938479346864992 - }, - "rotation" : { - "radians" : 1.5816576204489718 - } - }, - "curvature" : -0.04975638894367889 - }, { - "time" : 0.9732587981504898, - "velocity" : 1.993328753586887, - "acceleration" : -4.000000000000003, - "pose" : { - "translation" : { - "x" : 6.39238200429827, - "y" : 2.0146338321792427 - }, - "rotation" : { - "radians" : 1.5775955933008623 - } - }, - "curvature" : -0.05709527513223549 - }, { - "time" : 1.0112621915316293, - "velocity" : 1.841315180062329, - "acceleration" : -4.0, - "pose" : { - "translation" : { - "x" : 6.392044986767331, - "y" : 2.087497793723702 + "x" : 6.314285077627446, + "y" : 2.64237989176479 }, "rotation" : { - "radians" : 1.57315140430575 + "radians" : 1.682242375809382 } }, - "curvature" : -0.06500723564977115 + "curvature" : 0.031637223157170435 }, { - "time" : 1.0506511801697798, - "velocity" : 1.683759225509727, + "time" : 2.6285823403067847, + "velocity" : 1.6539034811474767, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.392044470626388, - "y" : 2.1569223515766396 + "x" : 6.306355844682457, + "y" : 2.712526842305124 }, "rotation" : { - "radians" : 1.5683651575191264 + "radians" : 1.6844659116451925 } }, - "curvature" : -0.07284766972073614 + "curvature" : 0.03131479782522635 }, { - "time" : 1.0918455825454725, - "velocity" : 1.5189816160069558, - "acceleration" : -4.0, + "time" : 2.6706436297897875, + "velocity" : 1.4856583232154645, + "acceleration" : -4.000000000000005, "pose" : { "translation" : { - "x" : 6.39236840166987, - "y" : 2.222889053711179 + "x" : 6.298799352960103, + "y" : 2.7781200229258047 }, "rotation" : { - "radians" : 1.5633344792916735 + "radians" : 1.6865117620775738 } }, - "curvature" : -0.07936077553696355 + "curvature" : 0.03052674983936547 }, { - "time" : 1.1356043581385193, - "velocity" : 1.3439465136347681, + "time" : 2.71476243308551, + "velocity" : 1.3091831100325741, "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.39299423553166, - "y" : 2.2855250420298034 + "x" : 6.291624408113421, + "y" : 2.839353629606194 }, "rotation" : { - "radians" : 1.5582428882782466 + "radians" : 1.6883451359021284 } }, - "curvature" : -0.0824337723791933 + "curvature" : 0.028694423143857225 }, { - "time" : 1.2379787498796464, - "velocity" : 0.9344489466702602, - "acceleration" : -4.000000000000001, + "time" : 2.815943111560705, + "velocity" : 0.904460396131795, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.395003296006507, - "y" : 2.4021324106830946 + "x" : 6.278326433749044, + "y" : 2.9505502779693416 }, "rotation" : { - "radians" : 1.549211634448327 + "radians" : 1.6911174188705012 } }, - "curvature" : -0.06547973929352308 + "curvature" : 0.018977039717131546 }, { - "time" : 1.4715909865472114, + "time" : 3.0420582105936536, "velocity" : 0.0, - "acceleration" : -4.000000000000001, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.397645473480225, - "y" : 2.5112497806549072 + "x" : 6.265983581542969, + "y" : 3.052058696746826 }, "rotation" : { - "radians" : 1.5451593331499225 + "radians" : 1.6921482303347304 } }, - "curvature" : 0.0 + "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 6.441819667816162, -0.0441741943359375, 0.0 ], - "y" : [ 0.3467085063457489, 1.1927070915699005, 0.0 ] + "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], + "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + }, { + "x" : [ 6.408572673797607, 0.15555095672607422, 0.0 ], + "y" : [ 0.7576718926429749, 1.2444130182266235, 0.0 ] }, { - "x" : [ 6.397645473480225, 0.04417705535888672, 0.0 ], - "y" : [ 2.5112497806549072, 1.7227985858917236, 0.0 ] + "x" : [ 6.265983581542969, -0.19443941116333008, 0.0 ], + "y" : [ 3.052058696746826, 1.5944044589996338, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : 0.0 + "radians" : -0.13962634015954636 } }, { - "time" : 1.3799999691545963, + "time" : 1.8299999590963125, "rotation" : { - "radians" : 0.6716053016572916 + "radians" : 0.4537856055185257 + } + }, { + "time" : 2.9499999340623617, + "rotation" : { + "radians" : 0.4537856055185257 } } ], "reversed" : false, - "color" : 92.0, + "color" : 286.3529, "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] + }, { + "type" : "script", + "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "sleep", + "args" : [ "1000" ], + "argTypes" : [ "long" ], + "reflection" : false + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Intake.setIntakeSolState CLOSE", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + } ] + } } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/red/movecargotohanger.json b/src/main/deploy/autos/red/movecargotohanger.json new file mode 100644 index 00000000..8eb15ca5 --- /dev/null +++ b/src/main/deploy/autos/red/movecargotohanger.json @@ -0,0 +1,3458 @@ +{ + "autonomousSteps" : [ { + "type" : "script", + "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 2.351956220256751, + "pose" : { + "translation" : { + "x" : 6.1293768882751465, + "y" : 1.0937262773513794 + }, + "rotation" : { + "radians" : 2.4178655244299407 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.3188834866654645, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.039938468317359, + "y" : 1.173101702633744 + }, + "rotation" : { + "radians" : 2.411607121974174 + } + }, + "curvature" : -0.10063347960741297 + }, { + "time" : 0.47317666597794406, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.954285601692391, + "y" : 1.2509135147447523 + }, + "rotation" : { + "radians" : 2.395009715228919 + } + }, + "curvature" : -0.1855623376134662 + }, { + "time" : 0.6188308342429313, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.87496604004582, + "y" : 1.3260262403808838 + }, + "rotation" : { + "radians" : 2.3703948601089895 + } + }, + "curvature" : -0.26531768438500725 + }, { + "time" : 0.7539530640358325, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.803342186380178, + "y" : 1.3977211365709081 + }, + "rotation" : { + "radians" : 2.339767645326564 + } + }, + "curvature" : -0.3381985496587611 + }, { + "time" : 0.8779901904625292, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.7397725735722815, + "y" : 1.4656408262000014 + }, + "rotation" : { + "radians" : 2.3056558386592805 + } + }, + "curvature" : -0.3907488038191573 + }, { + "time" : 0.9914536504675615, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.683793342890567, + "y" : 1.529733933533862 + }, + "rotation" : { + "radians" : 2.271678768601324 + } + }, + "curvature" : -0.39766870774643054 + }, { + "time" : 1.095639209373063, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.634299722512424, + "y" : 1.5901997197428273 + }, + "rotation" : { + "radians" : 2.242697343821613 + } + }, + "curvature" : -0.32812227024366547 + }, { + "time" : 1.1923614795366175, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.589727506041527, + "y" : 1.6474327184259892 + }, + "rotation" : { + "radians" : 2.224227521884603 + } + }, + "curvature" : -0.16341344710143665 + }, { + "time" : 1.2837283378298283, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.548234531025173, + "y" : 1.7019673711353107 + }, + "rotation" : { + "radians" : 2.2209763317101947 + } + }, + "curvature" : 0.07956164338822755 + }, { + "time" : 1.3719691365115938, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.507882157471613, + "y" : 1.7544226628997421 + }, + "rotation" : { + "radians" : 2.234944043969041 + } + }, + "curvature" : 0.33953937734428047 + }, { + "time" : 1.4592981567041168, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.466816746367385, + "y" : 1.805446757749337 + }, + "rotation" : { + "radians" : 2.264150133653273 + } + }, + "curvature" : 0.5355105351904276 + }, { + "time" : 1.5477626861241707, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.42345113819465, + "y" : 1.8556616342393681 + }, + "rotation" : { + "radians" : 2.30296870864004 + } + }, + "curvature" : 0.6126486386243994 + }, { + "time" : 1.6390285515859833, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.376646131448524, + "y" : 1.9056077209744444 + }, + "rotation" : { + "radians" : 2.343996330815072 + } + }, + "curvature" : 0.568293999561127 + }, { + "time" : 1.7340988034913118, + "velocity" : 0.75, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.3258919611544115, + "y" : 1.9556885321326263 + }, + "rotation" : { + "radians" : 2.3801609988469874 + } + }, + "curvature" : 0.4354667748411578 + }, { + "time" : 1.8330035326706784, + "velocity" : 0.75, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.271489777385341, + "y" : 2.006115302989542 + }, + "rotation" : { + "radians" : 2.405692727133013 + } + }, + "curvature" : 0.24590943352090008 + }, { + "time" : 2.0360118787648114, + "velocity" : 0.0, + "acceleration" : -3.6944293888894246, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 2.4154850346736763 + } + }, + "curvature" : -0.0 + } ], + "pointList" : [ { + "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ], + "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ] + }, { + "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ], + "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.37094272169677 + } + }, { + "time" : 1.939999956637621, + "rotation" : { + "radians" : 2.370942522371697 + } + } ], + "reversed" : false, + "color" : 209.88235, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ { + "type" : "MaxVelocityConstraint", + "maxVelocity" : 0.75 + } ] + }, { + "type" : "script", + "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.VisionManager.shootBalls", + "args" : [ "10" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "Drive.turnToAngle 40", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.turnToAngle", + "args" : [ "40" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.214733123779297, + "y" : 2.056851625442505 + }, + "rotation" : { + "radians" : 0.9530775920796433 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.22164892589636578, + "velocity" : 0.8865957035854634, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.272283410420641, + "y" : 2.136490201934066 + }, + "rotation" : { + "radians" : 0.93154333853559 + } + }, + "curvature" : -0.3354703724748357 + }, { + "time" : 0.32212126730993046, + "velocity" : 1.2884850692397223, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.339010516181588, + "y" : 2.223017249489203 + }, + "rotation" : { + "radians" : 0.8980578131179813 + } + }, + "curvature" : -0.24297628723974596 + }, { + "time" : 0.40798632695507736, + "velocity" : 1.6319453078203097, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.418256742414087, + "y" : 2.320179727881623 + }, + "rotation" : { + "radians" : 0.8787668506583872 + } + }, + "curvature" : -0.07195280186555318 + }, { + "time" : 0.4482103474158235, + "velocity" : 1.7928413896632942, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 5.462294308261335, + "y" : 2.373142516471262 + }, + "rotation" : { + "radians" : 0.8763509051285007 + } + }, + "curvature" : -0.0010310805010032027 + }, { + "time" : 0.48701783511066765, + "velocity" : 1.9480713404426708, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.508707731962204, + "y" : 2.428952641785145 + }, + "rotation" : { + "radians" : 0.8784798678297706 + } + }, + "curvature" : 0.057187573384384974 + }, { + "time" : 0.5244749433253648, + "velocity" : 2.0978997733014593, + "acceleration" : -3.006834100609329, + "pose" : { + "translation" : { + "x" : 5.5569134053257585, + "y" : 2.4874170951864016 + }, + "rotation" : { + "radians" : 0.8846611837531144 + } + }, + "curvature" : 0.10384088533571874 + }, { + "time" : 0.5628765788352517, + "velocity" : 1.9824324261311614, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 5.606271566124633, + "y" : 2.5482597037116648 + }, + "rotation" : { + "radians" : 0.8942836471940969 + } + }, + "curvature" : 0.13988861038856043 + }, { + "time" : 0.6051667706790917, + "velocity" : 1.813271658755801, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.656145019875112, + "y" : 2.6111436507878807 + }, + "rotation" : { + "radians" : 0.9066044762714179 + } + }, + "curvature" : 0.16515378657020438 + }, { + "time" : 0.6526161179857626, + "velocity" : 1.6234742695291178, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.7059578616172075, + "y" : 2.6756939969491214 + }, + "rotation" : { + "radians" : 0.9206836126942823 + } + }, + "curvature" : 0.17781422207911715 + }, { + "time" : 0.7069026874415512, + "velocity" : 1.4063279917059635, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.75525419769474, + "y" : 2.7415202005533956 + }, + "rotation" : { + "radians" : 0.9352834691742239 + } + }, + "curvature" : 0.1740904256082478 + }, { + "time" : 0.7714879126896856, + "velocity" : 1.1479870907134255, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.803756867535412, + "y" : 2.8082386384994606 + }, + "rotation" : { + "radians" : 0.9487507880978892 + } + }, + "curvature" : 0.14816445891139096 + }, { + "time" : 0.8556329295191124, + "velocity" : 0.8114070233957184, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.851426165430894, + "y" : 2.8754951269436333 + }, + "rotation" : { + "radians" : 0.9589069733276255 + } + }, + "curvature" : 0.09262519823750537 + }, { + "time" : 1.058484685368042, + "velocity" : 0.0, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : 0.9629949901604784 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ], + "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ] + }, { + "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ], + "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.9599310885968813 + } + }, { + "time" : 0.9699999783188105, + "rotation" : { + "radians" : 0.9599310885968813 + } + } ], + "reversed" : false, + "color" : 148.47061, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 1.3\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.3, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "@t 1.6\nIntake.setWantedIntakeState INTAKE", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.6, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.8985185623168945, + "y" : 2.9429874420166016 + }, + "rotation" : { + "radians" : -3.1024027203970066 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2301659590513677, + "velocity" : 0.9206638362054708, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.792647884092066, + "y" : 2.9388182625618144 + }, + "rotation" : { + "radians" : -3.1018966911012167 + } + }, + "curvature" : 0.009441605420194805 + }, { + "time" : 0.32549598090329745, + "velocity" : 1.3019839236131898, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.686791604118646, + "y" : 2.9345442011023373 + }, + "rotation" : { + "radians" : -3.100425032373633 + } + }, + "curvature" : 0.01823558838667193 + }, { + "time" : 0.3986288451113676, + "velocity" : 1.5945153804454704, + "acceleration" : 3.999999999999996, + "pose" : { + "translation" : { + "x" : 5.580971612904613, + "y" : 2.9300674260512527 + }, + "rotation" : { + "radians" : -3.098055153290077 + } + }, + "curvature" : 0.026416758928169817 + }, { + "time" : 0.4602561477805154, + "velocity" : 1.8410245911220613, + "acceleration" : 3.9999999999999916, + "pose" : { + "translation" : { + "x" : 5.475217593972275, + "y" : 2.9252976053729753 + }, + "rotation" : { + "radians" : -3.094851059223684 + } + }, + "curvature" : 0.0340260625248394 + }, { + "time" : 0.5145132056228292, + "velocity" : 2.0580528224913164, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.369566605466148, + "y" : 2.9201516955883897 + }, + "rotation" : { + "radians" : -3.090872945078929 + } + }, + "curvature" : 0.04110875647986678 + }, { + "time" : 0.563515762369635, + "velocity" : 2.2540630494785394, + "acceleration" : 4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.264062661760839, + "y" : 2.914553730779989 + }, + "rotation" : { + "radians" : -3.086176913297493 + } + }, + "curvature" : 0.047712969032265094 + }, { + "time" : 0.6085162323222143, + "velocity" : 2.4340649292888568, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 5.158756315068919, + "y" : 2.9084346115970146 + }, + "rotation" : { + "radians" : -3.080814792156677 + } + }, + "curvature" : 0.05388859471718488 + }, { + "time" : 0.6503267256223321, + "velocity" : 2.601306902489328, + "acceleration" : 3.9999999999999956, + "pose" : { + "translation" : { + "x" : 5.053704237048805, + "y" : 2.9017318942605925 + }, + "rotation" : { + "radians" : -3.0748340330236092 + } + }, + "curvature" : 0.059686480108153514 + }, { + "time" : 0.6895078614844453, + "velocity" : 2.758031445937781, + "acceleration" : 3.9999999999999956, + "pose" : { + "translation" : { + "x" : 4.948968800412642, + "y" : 2.894389579568873 + }, + "rotation" : { + "radians" : -3.0682776682148387 + } + }, + "curvature" : 0.06515785747811133 + }, { + "time" : 0.7264647120959763, + "velocity" : 2.9058588483839047, + "acceleration" : 2.666249486168943, + "pose" : { + "translation" : { + "x" : 4.844617660534176, + "y" : 2.8863579019021692 + }, + "rotation" : { + "radians" : -3.061184313833844 + } + }, + "curvature" : 0.07035398883055903 + }, { + "time" : 0.7617731677016681, + "velocity" : 2.9999999999999996, + "acceleration" : 8.556674489449135E-15, + "pose" : { + "translation" : { + "x" : 4.740723337056639, + "y" : 2.8775931182280945 + }, + "rotation" : { + "radians" : -3.0535882043682183 + } + }, + "curvature" : 0.07532598833602047 + }, { + "time" : 0.7963729965952918, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.637362795500621, + "y" : 2.868057297106702 + }, + "rotation" : { + "radians" : -3.045519247910405 + } + }, + "curvature" : 0.08012479693513168 + }, { + "time" : 0.8307945521766903, + "velocity" : 3.0, + "acceleration" : -8.652000136404901E-15, + "pose" : { + "translation" : { + "x" : 4.534617028871959, + "y" : 2.857718107695623 + }, + "rotation" : { + "radians" : -3.0370030926303286 + } + }, + "curvature" : 0.08480128843727726 + }, { + "time" : 0.8650131685157781, + "velocity" : 2.9999999999999996, + "acceleration" : 8.710165382862835E-15, + "pose" : { + "translation" : { + "x" : 4.432570639269606, + "y" : 2.846548608755203 + }, + "rotation" : { + "radians" : -3.028061196597019 + } + }, + "curvature" : 0.08940649170847131 + }, { + "time" : 0.899003277798418, + "velocity" : 3.0, + "acceleration" : -0.09574616288107782, + "pose" : { + "translation" : { + "x" : 4.331311419493517, + "y" : 2.834527037653644 + }, + "rotation" : { + "radians" : -3.018710894247568 + } + }, + "curvature" : 0.09399191846371814 + }, { + "time" : 0.9327567117085453, + "velocity" : 2.9967682382190453, + "acceleration" : -0.16603740413403223, + "pose" : { + "translation" : { + "x" : 4.230929934652522, + "y" : 2.821636599372141 + }, + "rotation" : { + "radians" : -3.0089654537653083 + } + }, + "curvature" : 0.0986099907832933 + }, { + "time" : 0.9662773051518138, + "velocity" : 2.9912025658986927, + "acceleration" : -0.17374271650320738, + "pose" : { + "translation" : { + "x" : 4.131519103772213, + "y" : 2.807865255510019 + }, + "rotation" : { + "radians" : -2.9988341203843856 + } + }, + "curvature" : 0.10331456682545719 + }, { + "time" : 0.9995509183178184, + "velocity" : 2.985421517959354, + "acceleration" : -0.18358691510613726, + "pose" : { + "translation" : { + "x" : 4.033173781402816, + "y" : 2.793205513289873 + }, + "rotation" : { + "radians" : -2.9883221412125427 + } + }, + "curvature" : 0.10816156739888892 + }, { + "time" : 1.0325512170508016, + "velocity" : 2.979363094917385, + "acceleration" : -0.1957881485667769, + "pose" : { + "translation" : { + "x" : 3.9359903392270725, + "y" : 2.7776542145627077 + }, + "rotation" : { + "radians" : -2.977430767583505 + } + }, + "curvature" : 0.11320971018612586 + }, { + "time" : 1.0652520605900002, + "velocity" : 2.9729606573042733, + "acceleration" : -0.2106132308438905, + "pose" : { + "translation" : { + "x" : 3.840066247668119, + "y" : 2.761212324813073 + }, + "rotation" : { + "radians" : -2.9661572312387867 + } + }, + "curvature" : 0.11852136257591563 + }, { + "time" : 1.0976276482842051, + "velocity" : 2.966141930179527, + "acceleration" : -0.22838513501532773, + "pose" : { + "translation" : { + "x" : 3.7454996574973665, + "y" : 2.743884722164204 + }, + "rotation" : { + "radians" : -2.9544946908190806 + } + }, + "curvature" : 0.1241635283632123 + }, { + "time" : 1.1296526738305466, + "velocity" : 2.9588278903962566, + "acceleration" : -0.2494917017630424, + "pose" : { + "translation" : { + "x" : 3.6523889814423782, + "y" : 2.7256799863831596 + }, + "rotation" : { + "radians" : -2.942432145241093 + } + }, + "curvature" : 0.1302089880914559 + }, { + "time" : 1.1613024877721105, + "velocity" : 2.950931524455492, + "acceleration" : -0.27439562802342504, + "pose" : { + "translation" : { + "x" : 3.560832475794749, + "y" : 2.7066101878859605 + }, + "rotation" : { + "radians" : -2.9299543105725916 + } + }, + "curvature" : 0.13673761759648564 + }, { + "time" : 1.1925532689948115, + "velocity" : 2.9423564467156664, + "acceleration" : -0.30364572614284985, + "pose" : { + "translation" : { + "x" : 3.470927822017984, + "y" : 2.686690676742728 + }, + "rotation" : { + "radians" : -2.9170414570270506 + } + }, + "curvature" : 0.1438379143723059 + }, { + "time" : 1.2233822059663813, + "velocity" : 2.932995371762722, + "acceleration" : -0.3378893144869457, + "pose" : { + "translation" : { + "x" : 3.3827717083553797, + "y" : 2.6659398716828218 + }, + "rotation" : { + "radians" : -2.9036692027182647 + } + }, + "curvature" : 0.15160876664672418 + }, { + "time" : 1.253767688448565, + "velocity" : 2.922728441916462, + "acceleration" : -0.377885389511768, + "pose" : { + "translation" : { + "x" : 3.296459411437901, + "y" : 2.644379049099978 + }, + "rotation" : { + "radians" : -2.8898082608960753 + } + }, + "curvature" : 0.1601615053363712 + }, { + "time" : 1.2836895103731156, + "velocity" : 2.9114214225836013, + "acceleration" : -0.4245178992880957, + "pose" : { + "translation" : { + "x" : 3.2120843778920607, + "y" : 2.62203213205745 + }, + "rotation" : { + "radians" : -2.875424137597093 + } + }, + "curvature" : 0.1696222839550847 + }, { + "time" : 1.313129084481963, + "velocity" : 2.898923796426977, + "acceleration" : -0.47880793939403443, + "pose" : { + "translation" : { + "x" : 3.129737805947798, + "y" : 2.5989254792931433 + }, + "rotation" : { + "radians" : -2.860476777086651 + } + }, + "curvature" : 0.180134835376471 + }, { + "time" : 1.3420696691632106, + "velocity" : 2.8850668147108904, + "acceleration" : -0.5419229606175602, + "pose" : { + "translation" : { + "x" : 3.049508227046359, + "y" : 2.5750876742247573 + }, + "rotation" : { + "radians" : -2.844920153276735 + } + }, + "curvature" : 0.19186365592042148 + }, { + "time" : 1.37049660762701, + "velocity" : 2.869661604057295, + "acceleration" : -0.6151800394568323, + "pose" : { + "translation" : { + "x" : 2.9714810874481756, + "y" : 2.550549313954921 + }, + "rotation" : { + "radians" : -2.828701806670222 + } + }, + "curvature" : 0.2049976646319762 + }, { + "time" : 1.3983975791037442, + "velocity" : 2.852497483323354, + "acceleration" : -0.7000388399543165, + "pose" : { + "translation" : { + "x" : 2.895738329840743, + "y" : 2.5253427982763332 + }, + "rotation" : { + "radians" : -2.8117623285706195 + } + }, + "curvature" : 0.21975437583412769 + }, { + "time" : 1.4257628610363704, + "velocity" : 2.8333407231042154, + "acceleration" : -0.8320723157548291, + "pose" : { + "translation" : { + "x" : 2.822357974946499, + "y" : 2.499502118676901 + }, + "rotation" : { + "radians" : -2.7940347976772673 + } + }, + "curvature" : 0.23638460141375914 + }, { + "time" : 1.4525899504690478, + "velocity" : 2.8110186446750056, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.751413703130705, + "y" : 2.473062647344876 + }, + "rotation" : { + "radians" : -2.7754441792642495 + } + }, + "curvature" : 0.25517765880922155 + }, { + "time" : 1.4792695579561794, + "velocity" : 2.7043002147264787, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.6829744360093244, + "y" : 2.4460609261739954 + }, + "rotation" : { + "radians" : -2.7559067046024177 + } + }, + "curvature" : 0.27646699088034715 + }, { + "time" : 1.506205081648366, + "velocity" : 2.5965581199577317, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.617103918056899, + "y" : 2.418534455768619 + }, + "rotation" : { + "radians" : -2.7353292590414324 + } + }, + "curvature" : 0.30063598962569904 + }, { + "time" : 1.533414427222619, + "velocity" : 2.4877207376607204, + "acceleration" : -3.9999999999999933, + "pose" : { + "translation" : { + "x" : 2.553860298214431, + "y" : 2.3905214844488683 + }, + "rotation" : { + "radians" : -2.713608822413696 + } + }, + "curvature" : 0.32812363568576425 + }, { + "time" : 1.5609222653091537, + "velocity" : 2.3776893853145813, + "acceleration" : -3.949364309865745, + "pose" : { + "translation" : { + "x" : 2.493295711497262, + "y" : 2.3620607972557637 + }, + "rotation" : { + "radians" : -2.690632026659688 + } + }, + "curvature" : 0.35942929054988465 + }, { + "time" : 1.5887534138112722, + "velocity" : 2.267774040717741, + "acceleration" : -3.7932782770182274, + "pose" : { + "translation" : { + "x" : 2.43545586060295, + "y" : 2.333191504956364 + }, + "rotation" : { + "radians" : -2.6662749246206054 + } + }, + "curvature" : 0.3951155704482967 + }, { + "time" : 1.6454161870589727, + "velocity" : 2.0528363738416293, + "acceleration" : -3.524370787870329, + "pose" : { + "translation" : { + "x" : 2.328098505131493, + "y" : 2.2743839107679378 + }, + "rotation" : { + "radians" : -2.612872321869423 + } + }, + "curvature" : 0.4821864866479269 + }, { + "time" : 1.7034881688217829, + "velocity" : 1.8481691777230431, + "acceleration" : -3.1886854801463445, + "pose" : { + "translation" : { + "x" : 2.232009308124283, + "y" : 2.214410084736084 + }, + "rotation" : { + "radians" : -2.552217385705653 + } + }, + "curvature" : 0.5948949158111395 + }, { + "time" : 1.7629801786134969, + "velocity" : 1.658467869915481, + "acceleration" : -2.793593360246646, + "pose" : { + "translation" : { + "x" : 2.1472796517252846, + "y" : 2.153573117658766 + }, + "rotation" : { + "radians" : -2.4830442955743517 + } + }, + "curvature" : 0.7387705578516988 + }, { + "time" : 1.823812432610913, + "velocity" : 1.4885272890594612, + "acceleration" : -2.4614261655202934, + "pose" : { + "translation" : { + "x" : 2.0738578353503314, + "y" : 2.092161056443846 + }, + "rotation" : { + "radians" : -2.404174055212181 + } + }, + "curvature" : 0.9170860803614409 + }, { + "time" : 1.8547141636761173, + "velocity" : 1.4124649596556962, + "acceleration" : -2.226093041455955, + "pose" : { + "translation" : { + "x" : 2.041328791717895, + "y" : 2.0613236050452572 + }, + "rotation" : { + "radians" : -2.3608351537787935 + } + }, + "curvature" : 1.0185171315348982 + }, { + "time" : 1.8858592963049856, + "velocity" : 1.3431329966353498, + "acceleration" : -1.9794032204108298, + "pose" : { + "translation" : { + "x" : 2.011535687139258, + "y" : 2.0304401522735134 + }, + "rotation" : { + "radians" : -2.314846284252715 + } + }, + "curvature" : 1.1263819090275555 + }, { + "time" : 1.9171879104709784, + "velocity" : 1.2811210368641754, + "acceleration" : -1.7210458088826468, + "pose" : { + "translation" : { + "x" : 1.98442585493639, + "y" : 1.9995394662212282 + }, + "rotation" : { + "radians" : -2.26626721249112 + } + }, + "curvature" : 1.2380648061508586 + }, { + "time" : 1.9486228942778938, + "velocity" : 1.22701998973099, + "acceleration" : -1.4497718123734797, + "pose" : { + "translation" : { + "x" : 1.9599351754080274, + "y" : 1.9686481087687149 + }, + "rotation" : { + "radians" : -2.2152625603384335 + } + }, + "curvature" : 1.3496477269860494 + }, { + "time" : 1.9800679118911164, + "velocity" : 1.1814318895557523, + "acceleration" : -1.1629063029917064, + "pose" : { + "translation" : { + "x" : 1.9379876574375503, + "y" : 1.9377902245891243 + }, + "rotation" : { + "radians" : -2.162125525962242 + } + }, + "curvature" : 1.4558154532790182 + }, { + "time" : 2.0114057081537027, + "velocity" : 1.144988968760121, + "acceleration" : -0.8555700128978933, + "pose" : { + "translation" : { + "x" : 1.9184950201008633, + "y" : 1.9069873301535836 + }, + "rotation" : { + "radians" : -2.1072976365024556 + } + }, + "curvature" : 1.5499621702421391 + }, { + "time" : 2.042496890239119, + "velocity" : 1.1183882857022904, + "acceleration" : -0.5193738693932407, + "pose" : { + "translation" : { + "x" : 1.9013562742742751, + "y" : 1.8762581027363336 + }, + "rotation" : { + "radians" : -2.051380200524095 + } + }, + "curvature" : 1.6245702075644999 + }, { + "time" : 2.0731792244008695, + "velocity" : 1.1024526830866856, + "acceleration" : -0.1401133909627551, + "pose" : { + "translation" : { + "x" : 1.8864573042423771, + "y" : 1.8456181694198683 + }, + "rotation" : { + "radians" : -1.9951332453181105 + } + }, + "curvature" : 1.6718749322032591 + }, { + "time" : 2.103267289985402, + "velocity" : 1.098236942190127, + "acceleration" : 0.3065421531438961, + "pose" : { + "translation" : { + "x" : 1.873670449305921, + "y" : 1.8150798961000731 + }, + "rotation" : { + "radians" : -1.939459195747904 + } + }, + "curvature" : 1.6847350337062899 + }, { + "time" : 2.132552032689131, + "velocity" : 1.1072139502727931, + "acceleration" : 0.8636627394205474, + "pose" : { + "translation" : { + "x" : 1.8628540853896993, + "y" : 1.7846521764913632 + }, + "rotation" : { + "radians" : -1.885371407123151 + } + }, + "curvature" : 1.6575269774282888 + }, { + "time" : 2.1607993444095768, + "velocity" : 1.1316101008945396, + "acceleration" : 1.6117144791019087, + "pose" : { + "translation" : { + "x" : 1.8538522066504246, + "y" : 1.7543402211318213 + }, + "rotation" : { + "radians" : -1.8339512599110672 + } + }, + "curvature" : 1.5868287956600426 + }, { + "time" : 2.1877462109747206, + "velocity" : 1.175040755904009, + "acceleration" : 3.6208105431392568, + "pose" : { + "translation" : { + "x" : 1.8464940070846083, + "y" : 1.7241453463883367 + }, + "rotation" : { + "radians" : -1.7863006275868372 + } + }, + "curvature" : 1.4716950974629326 + }, { + "time" : 2.236042382237465, + "velocity" : 1.349912042005413, + "acceleration" : 3.9999999999999813, + "pose" : { + "translation" : { + "x" : 1.8359489103056603, + "y" : 1.6640913673919613 + }, + "rotation" : { + "radians" : -1.7065641844274897 + } + }, + "curvature" : 1.11509729054641 + }, { + "time" : 2.277907500803531, + "velocity" : 1.517372516269675, + "acceleration" : 3.9833148393409696, + "pose" : { + "translation" : { + "x" : 1.829540444920319, + "y" : 1.6044148692087425 + }, + "rotation" : { + "radians" : -1.654002070934546 + } + }, + "curvature" : 0.613882276060416 + }, { + "time" : 2.315345763310788, + "velocity" : 1.6665009028739752, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.825326681137085, + "y" : 1.5449646711349487 + }, + "rotation" : { + "radians" : -1.6352154187320553 + } + }, + "curvature" : 0.0 + }, { + "time" : 2.3528636610794607, + "velocity" : 1.5164293117992842, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.8219009519815899, + "y" : 1.485354601351787 + }, + "rotation" : { + "radians" : -1.6145927519610666 + } + }, + "curvature" : 0.6613132762682022 + }, { + "time" : 2.3953009609475626, + "velocity" : 1.3466801123268766, + "acceleration" : -2.447765685928921, + "pose" : { + "translation" : { + "x" : 1.8207798104572248, + "y" : 1.4246136307510824 + }, + "rotation" : { + "radians" : -1.559258462845717 + } + }, + "curvature" : 1.12045601019587 + }, { + "time" : 2.4441469639679103, + "velocity" : 1.2271165422388892, + "acceleration" : -0.5720634881939421, + "pose" : { + "translation" : { + "x" : 1.8239080891303416, + "y" : 1.361831680281739 + }, + "rotation" : { + "radians" : -1.480370605655411 + } + }, + "curvature" : 1.349435348223791 + }, { + "time" : 2.4708676658243456, + "velocity" : 1.2118306043279066, + "acceleration" : 0.15979441326309798, + "pose" : { + "translation" : { + "x" : 1.8275716032411027, + "y" : 1.3294530877098598 + }, + "rotation" : { + "radians" : -1.43570297822473 + } + }, + "curvature" : 1.3836934054080776 + }, { + "time" : 2.498501305753145, + "velocity" : 1.2162463056066528, + "acceleration" : 0.7244111612153562, + "pose" : { + "translation" : { + "x" : 1.8328603306872537, + "y" : 1.2963242800469743 + }, + "rotation" : { + "radians" : -1.389335893805699 + } + }, + "curvature" : 1.3736643754703268 + }, { + "time" : 2.526759486377394, + "velocity" : 1.2367168470464982, + "acceleration" : 1.1926306446746133, + "pose" : { + "translation" : { + "x" : 1.839917232537418, + "y" : 1.2623921911031442 + }, + "rotation" : { + "radians" : -1.3424216946735859 + } + }, + "curvature" : 1.3285660480051897 + }, { + "time" : 2.5553998339251613, + "velocity" : 1.270874203206097, + "acceleration" : 1.601933018243381, + "pose" : { + "translation" : { + "x" : 1.8488651751439278, + "y" : 1.2276157532543266 + }, + "rotation" : { + "radians" : -1.2959209814340706 + } + }, + "curvature" : 1.258109886245163 + }, { + "time" : 2.5842198818567117, + "velocity" : 1.317041989575004, + "acceleration" : 1.9730587889589195, + "pose" : { + "translation" : { + "x" : 1.8598076922431273, + "y" : 1.191965371940812 + }, + "rotation" : { + "radians" : -1.2505887693826183 + } + }, + "curvature" : 1.171451900081923 + }, { + "time" : 2.613052469764473, + "velocity" : 1.3739303805548424, + "acceleration" : 2.4837574247785343, + "pose" : { + "translation" : { + "x" : 1.8728297470556754, + "y" : 1.1554224001656621 + }, + "rotation" : { + "radians" : -1.206979322113377 + } + }, + "curvature" : 1.0764509613846949 + }, { + "time" : 2.670135599245487, + "velocity" : 1.5157110272329066, + "acceleration" : 3.0990658562394877, + "pose" : { + "translation" : { + "x" : 1.9053640427268448, + "y" : 1.0796356820471829 + }, + "rotation" : { + "radians" : -1.1262654169222006 + } + }, + "curvature" : 0.8844858371105864 + }, { + "time" : 2.7259954044656856, + "velocity" : 1.6888242423270121, + "acceleration" : 3.649016814110955, + "pose" : { + "translation" : { + "x" : 1.9468053174205124, + "y" : 1.000305405119434 + }, + "rotation" : { + "radians" : -1.0550981178089267 + } + }, + "curvature" : 0.7124506625993454 + }, { + "time" : 2.7801731094475235, + "velocity" : 1.8865195987556824, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 1.9972424725681748, + "y" : 0.9176229045073114 + }, + "rotation" : { + "radians" : -0.9932815877369497 + } + }, + "curvature" : 0.5709539223900668 + }, { + "time" : 2.8325182827204705, + "velocity" : 2.0959002918474696, + "acceleration" : 3.999999999999984, + "pose" : { + "translation" : { + "x" : 2.05654044297944, + "y" : 0.8319042281900693 + }, + "rotation" : { + "radians" : -0.939869543584731 + } + }, + "curvature" : 0.45987023815540634 + }, { + "time" : 2.8832022831916913, + "velocity" : 2.298636293732353, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 2.124364584051719, + "y" : 0.7435733209513273 + }, + "rotation" : { + "radians" : -0.8936544133402136 + } + }, + "curvature" : 0.37479587351455695 + }, { + "time" : 2.9324371813867773, + "velocity" : 2.4955758865126962, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 2.2002050589799182, + "y" : 0.6531452083290787 + }, + "rotation" : { + "radians" : -0.853427174289811 + } + }, + "curvature" : 0.3104667355759126 + }, { + "time" : 2.980286705128464, + "velocity" : 2.686973981479444, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 2.2834012259661307, + "y" : 0.5612091805656974 + }, + "rotation" : { + "radians" : -0.8180911393767909 + } + }, + "curvature" : 0.26216926405981567 + }, { + "time" : 3.0036886028665157, + "velocity" : 2.7805815724316503, + "acceleration" : 1.7834018615548795, + "pose" : { + "translation" : { + "x" : 2.3275160092188085, + "y" : 0.514876020137125 + }, + "rotation" : { + "radians" : -0.8019547141012324 + } + }, + "curvature" : 0.24281777874977586 + }, { + "time" : 3.026940898356585, + "velocity" : 2.822049759494064, + "acceleration" : 0.6908039852530397, + "pose" : { + "translation" : { + "x" : 2.3731660254293274, + "y" : 0.4684119765579453 + }, + "rotation" : { + "radians" : -0.7866964853329266 + } + }, + "curvature" : 0.22613732914460744 + }, { + "time" : 3.050322026787958, + "velocity" : 2.8382015361941697, + "acceleration" : 0.5944336707391082, + "pose" : { + "translation" : { + "x" : 2.420237140398036, + "y" : 0.4219041096592999 + }, + "rotation" : { + "radians" : -0.772220104481907 + } + }, + "curvature" : 0.2118009615897061 + }, { + "time" : 3.0738960629383696, + "velocity" : 2.8522147370371953, + "acceleration" : 0.5085625308709665, + "pose" : { + "translation" : { + "x" : 2.4686103672150495, + "y" : 0.37544096780698055 + }, + "rotation" : { + "radians" : -0.7584370845254595 + } + }, + "curvature" : 0.19952992215767287 + }, { + "time" : 3.097629651885521, + "velocity" : 2.86428475109881, + "acceleration" : 0.4316967127192361, + "pose" : { + "translation" : { + "x" : 2.518162628360552, + "y" : 0.3291120623998669 + }, + "rotation" : { + "radians" : -0.7452660870351325 + } + }, + "curvature" : 0.18908811609138015 + }, { + "time" : 3.1214873211538543, + "velocity" : 2.874584028495092, + "acceleration" : 0.3624475659523509, + "pose" : { + "translation" : { + "x" : 2.5687675178050995, + "y" : 0.28300734236836433 + }, + "rotation" : { + "radians" : -0.7326322058073411 + } + }, + "curvature" : 0.1802768862425938 + }, { + "time" : 3.145431891692516, + "velocity" : 2.8832626798046044, + "acceleration" : 0.2995436811273533, + "pose" : { + "translation" : { + "x" : 2.6202960631099237, + "y" : 0.23721666867284164 + }, + "rotation" : { + "radians" : -0.7204662745977262 + } + }, + "curvature" : 0.17293027048351933 + }, { + "time" : 3.1694248840625208, + "velocity" : 2.890449629060376, + "acceleration" : 0.24183189107987368, + "pose" : { + "translation" : { + "x" : 2.6726174875272335, + "y" : 0.19182928880206873 + }, + "rotation" : { + "radians" : -0.708704216367118 + } + }, + "curvature" : 0.16691079279926538 + }, { + "time" : 3.193426917481795, + "velocity" : 2.8962540861919215, + "acceleration" : 0.18827218906255389, + "pose" : { + "translation" : { + "x" : 2.7255999721005186, + "y" : 0.14693331127165443 + }, + "rotation" : { + "radians" : -0.6972864449360306 + } + }, + "curvature" : 0.16210577857562763 + }, { + "time" : 3.217398100152016, + "velocity" : 2.9007671932276624, + "acceleration" : 0.13793017248481926, + "pose" : { + "translation" : { + "x" : 2.7791114177648524, + "y" : 0.10261518012248416 + }, + "rotation" : { + "radians" : -0.6861573260641398 + } + }, + "curvature" : 0.15842414582671857 + }, { + "time" : 3.241298409734759, + "velocity" : 2.9040637670508507, + "acceleration" : 0.08996972588049867, + "pose" : { + "translation" : { + "x" : 2.8330202074471944, + "y" : 0.0589591494191577 + }, + "rotation" : { + "radians" : -0.6752647030503031 + } + }, + "curvature" : 0.1557936000148212 + }, { + "time" : 3.2650880630305976, + "velocity" : 2.9062041156366694, + "acceleration" : 0.04364814484804298, + "pose" : { + "translation" : { + "x" : 2.8871959681666937, + "y" : 0.0160467577484269 + }, + "rotation" : { + "radians" : -0.6645594915329975 + } + }, + "curvature" : 0.15415814328341032 + }, { + "time" : 3.288727873894095, + "velocity" : 2.9072359495254196, + "acceleration" : -0.0016842852381663946, + "pose" : { + "translation" : { + "x" : 2.941510333134991, + "y" : -0.02604369728236655 + }, + "rotation" : { + "radians" : -0.6539953489714027 + } + }, + "curvature" : 0.15347579410401332 + }, { + "time" : 3.3121795982222406, + "velocity" : 2.907196450132324, + "acceleration" : -0.046578151707487234, + "pose" : { + "translation" : { + "x" : 2.995837703856523, + "y" : -0.06723768454685342 + }, + "rotation" : { + "radians" : -0.6435284261390796 + } + }, + "curvature" : 0.15371639662570263 + }, { + "time" : 3.335406264503977, + "velocity" : 2.9061145949465943, + "acceleration" : -0.09146977902103462, + "pose" : { + "translation" : { + "x" : 3.0500560122288234, + "y" : -0.10746496490119917 + }, + "rotation" : { + "radians" : -0.6331172107927046 + } + }, + "curvature" : 0.15485937730919708 + }, { + "time" : 3.358372487917268, + "velocity" : 2.904013879566033, + "acceleration" : -0.13665410312300072, + "pose" : { + "translation" : { + "x" : 3.1040474826428266, + "y" : -0.14666011668566625 + }, + "rotation" : { + "radians" : -0.6227224774786422 + } + }, + "curvature" : 0.15689127710744225 + }, { + "time" : 3.381044765324835, + "velocity" : 2.900915619831146, + "acceleration" : -2.1491915946775815, + "pose" : { + "translation" : { + "x" : 3.157699394083171, + "y" : -0.18476306122617636 + }, + "rotation" : { + "radians" : -0.6123073622363067 + } + }, + "curvature" : 0.15980284830484542 + }, { + "time" : 3.403563912604914, + "velocity" : 2.8525176577774944, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.2109048422285014, + "y" : -0.22171958833587269 + }, + "rotation" : { + "radians" : -0.6018375867750714 + } + }, + "curvature" : 0.16358545453108617 + }, { + "time" : 3.4492280218552347, + "velocity" : 2.6698612207762125, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.315582387420548, + "y" : -0.2920090449608779 + }, + "rotation" : { + "radians" : -0.5806125205758887 + } + }, + "curvature" : 0.1737031415493062 + }, { + "time" : 3.4961580129617453, + "velocity" : 2.48214125635017, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 3.4173701773397624, + "y" : -0.35723214386962354 + }, + "rotation" : { + "radians" : -0.558846041472928 + } + }, + "curvature" : 0.18697261071178092 + }, { + "time" : 3.544441517737483, + "velocity" : 2.2890072372472194, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 3.5157001126304266, + "y" : -0.4172200702665805 + }, + "rotation" : { + "radians" : -0.5364315370742367 + } + }, + "curvature" : 0.20264494363905447 + }, { + "time" : 3.594311089348581, + "velocity" : 2.0895289508028285, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.610170322670001, + "y" : -0.471948353301741 + }, + "rotation" : { + "radians" : -0.5134162602944821 + } + }, + "curvature" : 0.2190045005586516 + }, { + "time" : 3.6462406863199543, + "velocity" : 1.8818105629173347, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.7005695527788163, + "y" : -0.5215536821206115 + }, + "rotation" : { + "radians" : -0.4900919671398013 + } + }, + "curvature" : 0.23265913487493842 + }, { + "time" : 3.701128109083174, + "velocity" : 1.6622608718644565, + "acceleration" : -3.999999999999998, + "pose" : { + "translation" : { + "x" : 3.7869015514297644, + "y" : -0.5663507219142048 + }, + "rotation" : { + "radians" : -0.4671173179465116 + } + }, + "curvature" : 0.23773186828932136 + }, { + "time" : 3.7606892883012333, + "velocity" : 1.4240161549922183, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 3.8694094574579907, + "y" : -0.6068489299690327 + }, + "rotation" : { + "radians" : -0.4456529747120997 + } + }, + "curvature" : 0.2254664901452733 + }, { + "time" : 3.8285064753472717, + "velocity" : 1.1527474068080645, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 3.9486001872705856, + "y" : -0.6437693717170987 + }, + "rotation" : { + "radians" : -0.42745091338906643 + } + }, + "curvature" : 0.1853952813106193 + }, { + "time" : 3.9140667774226117, + "velocity" : 0.8105061985067056, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 4.025268822056276, + "y" : -0.67806153678589 + }, + "rotation" : { + "radians" : -0.414785534453574 + } + }, + "curvature" : 0.10957339292803865 + }, { + "time" : 4.116693327049288, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 4.100522994995117, + "y" : -0.7109201550483704 + }, + "rotation" : { + "radians" : -0.41008727651812055 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ], + "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ] + }, { + "x" : [ 1.825326681137085, -0.12263345718383789, 0.0 ], + "y" : [ 1.5449646711349487, -1.9010478258132935, 0.0 ] + }, { + "x" : [ 4.100522994995117, 2.3999733924865723, 0.0 ], + "y" : [ -0.7109201550483704, -1.0433523654937744, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : -3.1364759703203093 + } + }, { + "time" : 2.3099999483674765, + "rotation" : { + "radians" : 0.022477488801708422 + } + }, { + "time" : 4.029999909922481, + "rotation" : { + "radians" : -0.44615049906220317 + } + } ], + "reversed" : false, + "color" : 52.47058, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 2\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 2.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 4.000000000000001, + "pose" : { + "translation" : { + "x" : 4.100522994995117, + "y" : -0.7109201550483704 + }, + "rotation" : { + "radians" : 2.5857927494599444 + } + }, + "curvature" : -0.0 + }, { + "time" : 0.17905554113279049, + "velocity" : 0.716222164531162, + "acceleration" : 3.999999999999999, + "pose" : { + "translation" : { + "x" : 4.046051351866051, + "y" : -0.6770905534088945 + }, + "rotation" : { + "radians" : 2.5859290584771175 + } + }, + "curvature" : 0.004100780417876861 + }, { + "time" : 0.2532222147861238, + "velocity" : 1.0128888591444951, + "acceleration" : 3.9999999999999982, + "pose" : { + "translation" : { + "x" : 3.9915714570599867, + "y" : -0.6432753174175616 + }, + "rotation" : { + "radians" : 2.5862997183209484 + } + }, + "curvature" : 0.007316179708346309 + }, { + "time" : 0.3101283388167845, + "velocity" : 1.2405133552671377, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.9370801190687295, + "y" : -0.6094880375241134 + }, + "rotation" : { + "radians" : 2.5868491896599624 + } + }, + "curvature" : 0.009686266454747228 + }, { + "time" : 0.35809346841854367, + "velocity" : 1.4323738736741745, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.8825794348939553, + "y" : -0.5757414756226353 + }, + "rotation" : { + "radians" : 2.5875244773924204 + } + }, + "curvature" : 0.011252124346097734 + }, { + "time" : 0.4003365631482469, + "velocity" : 1.6013462525929871, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.828076535205502, + "y" : -0.5420475838784169 + }, + "rotation" : { + "radians" : 2.588275148867047 + } + }, + "curvature" : 0.012054310762550804 + }, { + "time" : 0.43850530180719144, + "velocity" : 1.7540212072287653, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.7735833294996617, + "y" : -0.5084175235548116 + }, + "rotation" : { + "radians" : 2.5890532903104244 + } + }, + "curvature" : 0.0121315702156112 + }, { + "time" : 0.4735755889182925, + "velocity" : 1.8943023556731697, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.719116251257474, + "y" : -0.4748616838400972 + }, + "rotation" : { + "radians" : 2.589813411890699 + } + }, + "curvature" : 0.01151977337607476 + }, { + "time" : 0.5061806573270884, + "velocity" : 2.024722629308353, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.6646960031030176, + "y" : -0.4413897006743355 + }, + "rotation" : { + "radians" : 2.590512310722669 + } + }, + "curvature" : 0.010251050076436605 + }, { + "time" : 0.5367579667986577, + "velocity" : 2.147031867194631, + "acceleration" : 4.000000000000007, + "pose" : { + "translation" : { + "x" : 3.610347301961702, + "y" : -0.4080104755762326 + }, + "rotation" : { + "radians" : 2.591108899964241 + } + }, + "curvature" : 0.008353084081511222 + }, { + "time" : 0.5656238557211262, + "velocity" : 2.2624954228845047, + "acceleration" : 3.9999999999999893, + "pose" : { + "translation" : { + "x" : 3.55609862421856, + "y" : -0.37473219446999906 + }, + "rotation" : { + "radians" : 2.5915640110153895 + } + }, + "curvature" : 0.0058485380718137905 + }, { + "time" : 0.6191140364610012, + "velocity" : 2.476456145844004, + "acceleration" : 4.0000000000000036, + "pose" : { + "translation" : { + "x" : 3.4480325127148035, + "y" : -0.308507742918664 + }, + "rotation" : { + "radians" : 2.5919013866336735 + } + }, + "curvature" : -9.17525897781337E-4 + }, { + "time" : 0.6679788967836321, + "velocity" : 2.671915587134528, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.340790984879593, + "y" : -0.24276823694478367 + }, + "rotation" : { + "radians" : 2.591240769502497 + } + }, + "curvature" : -0.009982710275132317 + }, { + "time" : 0.7130614555741249, + "velocity" : 2.8522458222964993, + "acceleration" : 3.5172385867566596, + "pose" : { + "translation" : { + "x" : 3.2347111984854564, + "y" : -0.17755532287992537 + }, + "rotation" : { + "radians" : 2.5893138286617203 + } + }, + "curvature" : -0.02138799605984592 + }, { + "time" : 0.7550700296469314, + "velocity" : 2.9999999999999996, + "acceleration" : 7.342027890777618E-15, + "pose" : { + "translation" : { + "x" : 3.130165997854956, + "y" : -0.11290100492229271 + }, + "rotation" : { + "radians" : 2.5858591271774563 + } + }, + "curvature" : -0.035290971114166826 + }, { + "time" : 0.7953939670709853, + "velocity" : 3.0, + "acceleration" : -7.485000429419822E-15, + "pose" : { + "translation" : { + "x" : 3.0275557589260416, + "y" : -0.04882824759624782 + }, + "rotation" : { + "radians" : 2.5806134504141105 + } + }, + "curvature" : -0.05197909891742402 + }, { + "time" : 0.8349476686405554, + "velocity" : 2.9999999999999996, + "acceleration" : -0.5537636550923151, + "pose" : { + "translation" : { + "x" : 2.9273002343174035, + "y" : 0.014648421788166388 + }, + "rotation" : { + "radians" : 2.573303027054985 + } + }, + "curvature" : -0.07188677132820555 + }, { + "time" : 0.87374969323478, + "velocity" : 2.97851284903572, + "acceleration" : -0.9070822146479386, + "pose" : { + "translation" : { + "x" : 2.829830398393824, + "y" : 0.07752231067570392 + }, + "rotation" : { + "radians" : 2.563634738472427 + } + }, + "curvature" : -0.095617170645365 + }, { + "time" : 0.9118975599042641, + "velocity" : 2.94390959765307, + "acceleration" : -2.262555771483939, + "pose" : { + "translation" : { + "x" : 2.73558029233153, + "y" : 0.13979395880639345 + }, + "rotation" : { + "radians" : 2.5512874700147776 + } + }, + "curvature" : -0.12396891770742956 + }, { + "time" : 0.9496761220339536, + "velocity" : 2.8584334938681764, + "acceleration" : -3.999999999999996, + "pose" : { + "translation" : { + "x" : 2.6449788691835465, + "y" : 0.20147053575601603 + }, + "rotation" : { + "radians" : 2.5359039299223154 + } + }, + "curvature" : -0.15796560005178947 + }, { + "time" : 0.9877492069346551, + "velocity" : 2.7061411542653704, + "acceleration" : -3.9999999999999956, + "pose" : { + "translation" : { + "x" : 2.558441838945047, + "y" : 0.26256523847658286 + }, + "rotation" : { + "radians" : 2.517083600091246 + } + }, + "curvature" : -0.19888275461263835 + }, { + "time" : 1.0265482053771444, + "velocity" : 2.550945160495413, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 2.4763635136187077, + "y" : 0.323096688836813 + }, + "rotation" : { + "radians" : 2.494378068434297 + } + }, + "curvature" : -0.2482604413289035 + }, { + "time" : 1.0661195848558005, + "velocity" : 2.3926596425807887, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.3991086522800593, + "y" : 0.383088331162611 + }, + "rotation" : { + "radians" : 2.4672909353168477 + } + }, + "curvature" : -0.3078783541033658 + }, { + "time" : 1.1065518464830886, + "velocity" : 2.230930596071637, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.3270043061428396, + "y" : 0.44256782977754483 + }, + "rotation" : { + "radians" : 2.4352859073052295 + } + }, + "curvature" : -0.37965241689117946 + }, { + "time" : 1.1479982964991189, + "velocity" : 2.065144796007515, + "acceleration" : -3.9999999999999973, + "pose" : { + "translation" : { + "x" : 2.2603316636243456, + "y" : 0.5015664665433235 + }, + "rotation" : { + "radians" : 2.397808651299899 + } + }, + "curvature" : -0.46538626343634765 + }, { + "time" : 1.1907134105418258, + "velocity" : 1.8942843398366878, + "acceleration" : -3.8004076554878155, + "pose" : { + "translation" : { + "x" : 2.199317895410786, + "y" : 0.5601185384002747 + }, + "rotation" : { + "radians" : 2.35433030289942 + } + }, + "curvature" : -0.5662827934211753 + }, { + "time" : 1.2350003317172529, + "velocity" : 1.725975985563609, + "acceleration" : -3.096610871868397, + "pose" : { + "translation" : { + "x" : 2.1441279995226346, + "y" : 0.6182607549078227 + }, + "rotation" : { + "radians" : 2.304422426440884 + } + }, + "curvature" : -0.6821096692467903 + }, { + "time" : 1.2808802623624265, + "velocity" : 1.5839036935271966, + "acceleration" : -2.455893226230777, + "pose" : { + "translation" : { + "x" : 2.0948566463799807, + "y" : 0.6760316357849661 + }, + "rotation" : { + "radians" : 2.24787278990788 + } + }, + "curvature" : -0.8099648281546794 + }, { + "time" : 1.3280319668435383, + "velocity" : 1.4681041418867986, + "acceleration" : -1.8499614404769908, + "pose" : { + "translation" : { + "x" : 2.0515200238678837, + "y" : 0.7334709084507551 + }, + "rotation" : { + "radians" : 2.184845091649205 + } + }, + "curvature" : -0.9427791639774807 + }, { + "time" : 1.376032015282584, + "velocity" : 1.3793059031335362, + "acceleration" : -1.248435182006515, + "pose" : { + "translation" : { + "x" : 2.014047682401724, + "y" : 0.7906189055647701 + }, + "rotation" : { + "radians" : 2.1160696333836517 + } + }, + "curvature" : -1.0680768788914654 + }, { + "time" : 1.424334569704738, + "velocity" : 1.3190032948121349, + "acceleration" : -0.6091506293120736, + "pose" : { + "translation" : { + "x" : 1.9822743799925568, + "y" : 0.8475159625675985 + }, + "rotation" : { + "radians" : 2.0430250430635404 + } + }, + "curvature" : -1.1679706858124066 + }, { + "time" : 1.4722549198410007, + "velocity" : 1.2898125833697756, + "acceleration" : 0.14219135021541737, + "pose" : { + "translation" : { + "x" : 1.955931927312463, + "y" : 0.9042018152213132 + }, + "rotation" : { + "radians" : 1.9680433024899477 + } + }, + "curvature" : -1.221435348671096 + }, { + "time" : 1.5189560337976418, + "velocity" : 1.2964530778198344, + "acceleration" : 1.1656913546554033, + "pose" : { + "translation" : { + "x" : 1.9346410327599024, + "y" : 0.9607149971499496 + }, + "rotation" : { + "radians" : 1.894265907226921 + } + }, + "curvature" : -1.2089548933496592 + }, { + "time" : 1.563428663287173, + "velocity" : 1.348294437534574, + "acceleration" : 2.8841346574602866, + "pose" : { + "translation" : { + "x" : 1.9179031475250667, + "y" : 1.017092237379984 + }, + "rotation" : { + "radians" : 1.8254232181396424 + } + }, + "curvature" : -1.1177745520566962 + }, { + "time" : 1.604436283065203, + "velocity" : 1.4665659349563442, + "acceleration" : 3.999999999999992, + "pose" : { + "translation" : { + "x" : 1.905092310655231, + "y" : 1.073367857880811 + }, + "rotation" : { + "radians" : 1.7654947807008987 + } + }, + "curvature" : -0.9447578680023896 + }, { + "time" : 1.6414523727107633, + "velocity" : 1.6146302935385846, + "acceleration" : 3.7005826612074797, + "pose" : { + "translation" : { + "x" : 1.8954469941201069, + "y" : 1.1295731711052213 + }, + "rotation" : { + "radians" : 1.7183797533566942 + } + }, + "curvature" : -0.6949978934363836 + }, { + "time" : 1.706643337288398, + "velocity" : 1.855874846721971, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.8818800449371338, + "y" : 1.2418794631958008 + }, + "rotation" : { + "radians" : 1.676778825261928 + } + }, + "curvature" : -0.0 + }, { + "time" : 1.7723136975830924, + "velocity" : 1.5931934055431933, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 1.8715964211490927, + "y" : 1.3546623770804445 + }, + "rotation" : { + "radians" : 1.632957005658238 + } + }, + "curvature" : -0.7254972162927384 + }, { + "time" : 1.810034487394077, + "velocity" : 1.4423102462992545, + "acceleration" : -2.710726662081756, + "pose" : { + "translation" : { + "x" : 1.8693755361295565, + "y" : 1.4118700819531647 + }, + "rotation" : { + "radians" : 1.5838413049635738 + } + }, + "curvature" : -0.9768015183979825 + }, { + "time" : 1.8519046258057839, + "velocity" : 1.3288117457615873, + "acceleration" : -1.1894816130443073, + "pose" : { + "translation" : { + "x" : 1.870371506531228, + "y" : 1.4698751626601734 + }, + "rotation" : { + "radians" : 1.521732134311164 + } + }, + "curvature" : -1.1507918750528034 + }, { + "time" : 1.8972897575518486, + "velocity" : 1.2748269660440499, + "acceleration" : -0.3477506556011637, + "pose" : { + "translation" : { + "x" : 1.8753421949347988, + "y" : 1.528748941692104 + }, + "rotation" : { + "radians" : 1.4504225267826032 + } + }, + "curvature" : -1.2503201227521346 + }, { + "time" : 1.9450499018565859, + "velocity" : 1.2582183445504713, + "acceleration" : 0.2016282832037308, + "pose" : { + "translation" : { + "x" : 1.8849010019640673, + "y" : 1.5884782075607973 + }, + "rotation" : { + "radians" : 1.3734581625763413 + } + }, + "curvature" : -1.28354671036822 + }, { + "time" : 1.9943208439648752, + "velocity" : 1.268152760019596, + "acceleration" : 0.5969274636685241, + "pose" : { + "translation" : { + "x" : 1.89952602502051, + "y" : 1.6489738267635623 + }, + "rotation" : { + "radians" : 1.293941372408038 + } + }, + "curvature" : -1.2635154627438518 + }, { + "time" : 2.0444402181362453, + "velocity" : 1.2980703909243658, + "acceleration" : 0.8935195397370149, + "pose" : { + "translation" : { + "x" : 1.9195692170178518, + "y" : 1.710079355747439 + }, + "rotation" : { + "radians" : 1.2143690380279577 + } + }, + "curvature" : -1.2059442083489496 + }, { + "time" : 2.094911061001386, + "velocity" : 1.3431670752113656, + "acceleration" : 1.1141266498975333, + "pose" : { + "translation" : { + "x" : 1.9452655451166372, + "y" : 1.7715796528734593 + }, + "rotation" : { + "radians" : 1.1365566560794031 + } + }, + "curvature" : -1.1263247530634717 + }, { + "time" : 2.1453768398619966, + "velocity" : 1.3993923443478076, + "acceleration" : 1.2676595640303998, + "pose" : { + "translation" : { + "x" : 1.9767421494588007, + "y" : 1.8332094903809093 + }, + "rotation" : { + "radians" : 1.061661442713639 + } + }, + "curvature" : -1.0376352482876874 + }, { + "time" : 2.1955990497229334, + "velocity" : 1.4630570090047659, + "acceleration" : 1.3576641534621845, + "pose" : { + "translation" : { + "x" : 2.0140275019022376, + "y" : 1.8946621663515906 + }, + "rotation" : { + "radians" : 0.9902801108323195 + } + }, + "curvature" : -0.949295026502291 + }, { + "time" : 2.2454352355231433, + "velocity" : 1.5307178120109917, + "acceleration" : 1.3867285924070238, + "pose" : { + "translation" : { + "x" : 2.057060564755375, + "y" : 1.9555981166740821 + }, + "rotation" : { + "radians" : 0.9225815953395504 + } + }, + "curvature" : -0.8672282806254888 + }, { + "time" : 2.2948177631789672, + "velocity" : 1.5991979750766532, + "acceleration" : 1.3589820685583802, + "pose" : { + "translation" : { + "x" : 2.1056999495117417, + "y" : 2.015653527008002 + }, + "rotation" : { + "radians" : 0.858440269383883 + } + }, + "curvature" : -0.7945463578337496 + }, { + "time" : 2.3437341392878004, + "velocity" : 1.6656744530674152, + "acceleration" : 1.2814168669463941, + "pose" : { + "translation" : { + "x" : 2.1597330755845405, + "y" : 2.0744489447482692 + }, + "rotation" : { + "radians" : 0.7975494729913294 + } + }, + "curvature" : -0.7323917673147309 + }, { + "time" : 2.3922093962778366, + "velocity" : 1.7277914650040085, + "acceleration" : 1.1642994999905252, + "pose" : { + "translation" : { + "x" : 2.2188853290412176, + "y" : 2.131597890989365 + }, + "rotation" : { + "radians" : 0.7395085520599514 + } + }, + "curvature" : -0.6806769668594456 + }, { + "time" : 2.4402906479447277, + "velocity" : 1.7837724422786883, + "acceleration" : 1.0208386073494335, + "pose" : { + "translation" : { + "x" : 2.2828292213380337, + "y" : 2.1867154724895954 + }, + "rotation" : { + "radians" : 0.6838850476440103 + } + }, + "curvature" : -0.6386233642001887 + }, { + "time" : 2.4880336196518704, + "velocity" : 1.8325103110269312, + "acceleration" : 0.8663296664660994, + "pose" : { + "translation" : { + "x" : 2.351193548054635, + "y" : 2.2394269936353517 + }, + "rotation" : { + "radians" : 0.6302573876108508 + } + }, + "curvature" : -0.6051051506550083 + }, { + "time" : 2.5354908013854165, + "velocity" : 1.873623875449575, + "acceleration" : 0.7170825323129972, + "pose" : { + "translation" : { + "x" : 2.4235725476286234, + "y" : 2.2893765684053733 + }, + "rotation" : { + "radians" : 0.5782441181597022 + } + }, + "curvature" : -0.5788404596526515 + }, { + "time" : 2.582700817287668, + "velocity" : 1.9074773532032985, + "acceleration" : 0.5894907207875506, + "pose" : { + "translation" : { + "x" : 2.4995350600901283, + "y" : 2.336235732335009 + }, + "rotation" : { + "radians" : 0.5275249535906203 + } + }, + "curvature" : -0.5584765250518172 + }, { + "time" : 2.629678599279987, + "velocity" : 1.935170319770951, + "acceleration" : 0.49960482001977663, + "pose" : { + "translation" : { + "x" : 2.578633685796376, + "y" : 2.3797120544804784 + }, + "rotation" : { + "radians" : 0.47785770284077494 + } + }, + "curvature" : -0.5426069030826571 + }, { + "time" : 2.6764059539315443, + "velocity" : 1.9585155313816427, + "acceleration" : 0.46356554662244476, + "pose" : { + "translation" : { + "x" : 2.6604139441662618, + "y" : 2.4195577493831344 + }, + "rotation" : { + "radians" : 0.42909391062732893 + } + }, + "curvature" : -0.5297484122773238 + }, { + "time" : 2.7228220905005966, + "velocity" : 1.9800324531023774, + "acceleration" : 0.4993320852380286, + "pose" : { + "translation" : { + "x" : 2.7444234324149193, + "y" : 2.455578289033724 + }, + "rotation" : { + "radians" : 0.3811949720767674 + } + }, + "curvature" : -0.5182974673057555 + }, { + "time" : 2.7688136027162047, + "velocity" : 2.0029974908002472, + "acceleration" : 0.6304938957131165, + "pose" : { + "translation" : { + "x" : 2.8302209842882924, + "y" : 2.487641014836651 + }, + "rotation" : { + "radians" : 0.33424953905468247 + } + }, + "curvature" : -0.5064806911118291 + }, { + "time" : 2.8142032161774035, + "velocity" : 2.031615365016311, + "acceleration" : 0.8939968521835171, + "pose" : { + "translation" : { + "x" : 2.9173858287977055, + "y" : 2.5156837495742366 + }, + "rotation" : { + "radians" : 0.28849219299419626 + } + }, + "curvature" : -0.4923123451071928 + }, { + "time" : 2.85873622335247, + "velocity" : 2.0714277332490862, + "acceleration" : 1.3565333753864743, + "pose" : { + "translation" : { + "x" : 3.005526748954434, + "y" : 2.539723409370982 + }, + "rotation" : { + "radians" : 0.24432259326770525 + } + }, + "curvature" : -0.4735699432966268 + }, { + "time" : 2.902062698889524, + "velocity" : 2.1302015433529657, + "acceleration" : 2.153155459162207, + "pose" : { + "translation" : { + "x" : 3.094291240504276, + "y" : 2.5598646156578297 + }, + "rotation" : { + "radians" : 0.20232364570316902 + } + }, + "curvature" : -0.4477981691549623 + }, { + "time" : 2.9437117576976832, + "velocity" : 2.2198784416947213, + "acceleration" : 3.592461951491249, + "pose" : { + "translation" : { + "x" : 3.183374670662122, + "y" : 2.5763083071364257 + }, + "rotation" : { + "radians" : 0.1632767221779342 + } + }, + "curvature" : -0.4123493520484391 + }, { + "time" : 2.9830497106114113, + "velocity" : 2.3611985407868445, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 3.2725294368465256, + "y" : 2.5893603517433803 + }, + "rotation" : { + "radians" : 0.12817160720952014 + } + }, + "curvature" : -0.36446740220852936 + }, { + "time" : 3.0198548135911913, + "velocity" : 2.508418952705964, + "acceleration" : 4.000000000000005, + "pose" : { + "translation" : { + "x" : 3.3615741254142755, + "y" : 2.599440158614531 + }, + "rotation" : { + "radians" : 0.0982084966435118 + } + }, + "curvature" : -0.30142499142103213 + }, { + "time" : 3.0544441058745386, + "velocity" : 2.646776121839353, + "acceleration" : 3.99999999999999, + "pose" : { + "translation" : { + "x" : 3.450402670394965, + "y" : 2.6070892900492026 + }, + "rotation" : { + "radians" : 0.07478856387691135 + } + }, + "curvature" : -0.22073732623662828 + }, { + "time" : 3.087179502133278, + "velocity" : 2.7777177068743097, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.538993512225563, + "y" : 2.6129800734744713 + }, + "rotation" : { + "radians" : 0.05948748242671234 + } + }, + "curvature" : -0.1205067561761745 + }, { + "time" : 3.118362856162708, + "velocity" : 2.9024511229920305, + "acceleration" : 3.2635486192884477, + "pose" : { + "translation" : { + "x" : 3.6274187564849854, + "y" : 2.617924213409424 + }, + "rotation" : { + "radians" : 0.0540017456962338 + } + }, + "curvature" : 0.0 + }, { + "time" : 3.1482532878331386, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.7155035994555945, + "y" : 2.6226850363620713 + }, + "rotation" : { + "radians" : 0.053981642702760804 + } + }, + "curvature" : -5.351308120085834E-4 + }, { + "time" : 3.1770862157675355, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.801876553490956, + "y" : 2.627348984328819 + }, + "rotation" : { + "radians" : 0.05389383309434626 + } + }, + "curvature" : -0.001601464316657459 + }, { + "time" : 3.2048908409093957, + "velocity" : 3.0, + "acceleration" : -1.1198792336370368E-14, + "pose" : { + "translation" : { + "x" : 3.885169715891969, + "y" : 2.631834930243855 + }, + "rotation" : { + "radians" : 0.05369200401278312 + } + }, + "curvature" : -0.0033948580681829725 + }, { + "time" : 3.231327573044508, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 3.964366333158978, + "y" : 2.6360779324022587 + }, + "rotation" : { + "radians" : 0.053319276839782104 + } + }, + "curvature" : -0.006242169384276816 + }, { + "time" : 3.256165149733738, + "velocity" : 2.9999999999999996, + "acceleration" : 1.2812741871263266E-14, + "pose" : { + "translation" : { + "x" : 4.038774281627788, + "y" : 2.640028140852877 + }, + "rotation" : { + "radians" : 0.0527039245624449 + } + }, + "curvature" : -0.01063132499803715 + }, { + "time" : 3.2792717943698393, + "velocity" : 3.0, + "acceleration" : -7.231938957008415E-15, + "pose" : { + "translation" : { + "x" : 4.107999548105681, + "y" : 2.6436497037911977 + }, + "rotation" : { + "radians" : 0.05175599940745225 + } + }, + "curvature" : -0.017237888913125436 + }, { + "time" : 3.320209567074138, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.230657418491319, + "y" : 2.6498269150033593 + }, + "rotation" : { + "radians" : 0.04841053128135275 + } + }, + "curvature" : -0.04051841469935257 + }, { + "time" : 3.3547406099731343, + "velocity" : 2.9999999999999996, + "acceleration" : -0.6325094018133772, + "pose" : { + "translation" : { + "x" : 4.334142312372251, + "y" : 2.654561157464741 + }, + "rotation" : { + "radians" : 0.0423299273622696 + } + }, + "curvature" : -0.08057010884102427 + }, { + "time" : 3.384584997268422, + "velocity" : 2.9811231444443704, + "acceleration" : -0.08050231493317403, + "pose" : { + "translation" : { + "x" : 4.4233291260534315, + "y" : 2.6579580020916183 + }, + "rotation" : { + "radians" : 0.03315653358683334 + } + }, + "curvature" : -0.1233537474620638 + }, { + "time" : 3.4121081263600206, + "velocity" : 2.978907468838292, + "acceleration" : 0.757187202067055, + "pose" : { + "translation" : { + "x" : 4.5053166644408975, + "y" : 2.6602420038625496 + }, + "rotation" : { + "radians" : 0.022490284597376595 + } + }, + "curvature" : -0.1268933627335242 + }, { + "time" : 3.4399645548682245, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.588579021394253, + "y" : 2.661721706390381 + }, + "rotation" : { + "radians" : 0.01374388263590382 + } + }, + "curvature" : -0.07804765164973368 + }, { + "time" : 3.471145768832688, + "velocity" : 3.0, + "acceleration" : -7.895171233701495E-15, + "pose" : { + "translation" : { + "x" : 4.682116960079156, + "y" : 2.6627546464942498 + }, + "rotation" : { + "radians" : 0.009189096860167837 + } + }, + "curvature" : -0.024057614430750256 + }, { + "time" : 3.508644572148012, + "velocity" : 2.9999999999999996, + "acceleration" : 1.3503955874947296E-14, + "pose" : { + "translation" : { + "x" : 4.794609293319809, + "y" : 2.6637123587715905 + }, + "rotation" : { + "radians" : 0.008349254243389597 + } + }, + "curvature" : 0.0037103968236503098 + }, { + "time" : 3.5305684791849585, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.860378613618231, + "y" : 2.6642743243984057 + }, + "rotation" : { + "radians" : 0.00879853748094375 + } + }, + "curvature" : 0.009166818258418976 + }, { + "time" : 3.554964721451485, + "velocity" : 3.0, + "acceleration" : -1.992727596873581, + "pose" : { + "translation" : { + "x" : 4.933564263951439, + "y" : 2.6649453801701384 + }, + "rotation" : { + "radians" : 0.009565324282873914 + } + }, + "curvature" : 0.011329196406191677 + }, { + "time" : 3.5822868057216533, + "velocity" : 2.9455545286707294, + "acceleration" : -3.9999999999999902, + "pose" : { + "translation" : { + "x" : 5.01478264645214, + "y" : 2.6657603208850844 + }, + "rotation" : { + "radians" : 0.010504868424232214 + } + }, + "curvature" : 0.011571381180452858 + }, { + "time" : 3.613394389130737, + "velocity" : 2.821124195034394, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.104470925172791, + "y" : 2.666748254559934 + }, + "rotation" : { + "radians" : 0.011513061014706312 + } + }, + "curvature" : 0.01080959208114291 + }, { + "time" : 3.649180845248966, + "velocity" : 2.6779783705614784, + "acceleration" : -4.000000000000004, + "pose" : { + "translation" : { + "x" : 5.202860506721613, + "y" : 2.6679315088226474 + }, + "rotation" : { + "radians" : 0.012518180765925502 + } + }, + "curvature" : 0.009593832950905505 + }, { + "time" : 3.690445008393871, + "velocity" : 2.5129217179818584, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.30995052089861, + "y" : 2.6693245373053287 + }, + "rotation" : { + "radians" : 0.01347191339238352 + } + }, + "curvature" : 0.008225115612607133 + }, { + "time" : 3.738242422456052, + "velocity" : 2.3217320617331336, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 5.42548130133158, + "y" : 2.6709328260371024 + }, + "rotation" : { + "radians" : 0.014341527416621765 + } + }, + "curvature" : 0.006849417564785735 + }, { + "time" : 3.7940970341308637, + "velocity" : 2.098313615033887, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.548907866112131, + "y" : 2.6727517998369876 + }, + "rotation" : { + "radians" : 0.015103596532319463 + } + }, + "curvature" : 0.005520155477322901 + }, { + "time" : 3.825756511779807, + "velocity" : 1.9716757044381135, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 5.6133272185697205, + "y" : 2.673735843651399 + }, + "rotation" : { + "radians" : 0.015438273175695332 + } + }, + "curvature" : 0.004873668640140536 + }, { + "time" : 3.860481196136515, + "velocity" : 1.8327769670112823, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.6793733984317, + "y" : 2.674765728706774 + }, + "rotation" : { + "radians" : 0.015738980768460056 + } + }, + "curvature" : 0.004234449415552546 + }, { + "time" : 3.89893588264187, + "velocity" : 1.678958220989862, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.746886228560693, + "y" : 2.6758375663932243 + }, + "rotation" : { + "radians" : 0.016003288340590872 + } + }, + "curvature" : 0.003596071353112922 + }, { + "time" : 3.9421404788446717, + "velocity" : 1.5061398361786555, + "acceleration" : -3.999999999999997, + "pose" : { + "translation" : { + "x" : 5.815682727217563, + "y" : 2.6769466342238957 + }, + "rotation" : { + "radians" : 0.016228495089680457 + } + }, + "curvature" : 0.0029497967820049952 + }, { + "time" : 3.991815920611223, + "velocity" : 1.3074380691124496, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 5.885556279331293, + "y" : 2.678087341659743 + }, + "rotation" : { + "radians" : 0.016411476850763638 + } + }, + "curvature" : 0.0022842194098207026 + }, { + "time" : 4.0513318851535365, + "velocity" : 1.0693742109431945, + "acceleration" : -3.9999999999999982, + "pose" : { + "translation" : { + "x" : 5.956275807768854, + "y" : 2.679253195934308 + }, + "rotation" : { + "radians" : 0.0165485050781744 + } + }, + "curvature" : 0.0015845831447143007 + }, { + "time" : 4.129431954649663, + "velocity" : 0.75697393295869, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.027584944605088, + "y" : 2.680436767878498 + }, + "rotation" : { + "radians" : 0.016635023032639307 + } + }, + "curvature" : 8.316618868490522E-4 + }, { + "time" : 4.318675437889335, + "velocity" : 0.0, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.099201202392578, + "y" : 2.6816296577453613 + }, + "rotation" : { + "radians" : 0.016665357555075345 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.100522994995117, -3.486077070236206, 0.0 ], + "y" : [ -0.7109201550483704, 2.165250778198242, 0.0 ] + }, { + "x" : [ 1.8818800449371338, -0.19111382961273193, 0.0 ], + "y" : [ 1.2418794631958008, 1.7965017557144165, 0.0 ] + }, { + "x" : [ 3.6274187564849854, 2.828535318374634, 0.0 ], + "y" : [ 2.617924213409424, 0.1528944969177246, 0.0 ] + }, { + "x" : [ 6.099201202392578, 4.586813449859619, 0.0 ], + "y" : [ 2.6816296577453613, 0.07644796371459961, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 2.584307420515759 + } + }, { + "time" : 1.699999962002039, + "rotation" : { + "radians" : 2.7676508674151563 + } + }, { + "time" : 3.1199999302625656, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 4.229999905452132, + "rotation" : { + "radians" : 0.00740696187012303 + } + } ], + "reversed" : false, + "color" : 286.3529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + } ] +} \ No newline at end of file diff --git a/src/main/java/frc/auton/BuddyAutoLeftHideBlue.java b/src/main/java/frc/auton/BuddyAutoLeftHideBlue.java new file mode 100644 index 00000000..ef29bba4 --- /dev/null +++ b/src/main/java/frc/auton/BuddyAutoLeftHideBlue.java @@ -0,0 +1,12 @@ +package frc.auton; + +import edu.wpi.first.wpilibj.Filesystem; +import frc.auton.guiauto.AbstractGuiAuto; + +import java.io.File; + +public class BuddyAutoLeftHideBlue extends AbstractGuiAuto { + public BuddyAutoLeftHideBlue() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/buddyautoleftsteal.json")); + } +} diff --git a/src/main/java/frc/auton/BuddyAutoLeftHideRed.java b/src/main/java/frc/auton/BuddyAutoLeftHideRed.java new file mode 100644 index 00000000..b39f57e0 --- /dev/null +++ b/src/main/java/frc/auton/BuddyAutoLeftHideRed.java @@ -0,0 +1,12 @@ +package frc.auton; + +import edu.wpi.first.wpilibj.Filesystem; +import frc.auton.guiauto.AbstractGuiAuto; + +import java.io.File; + +public class BuddyAutoLeftHideRed extends AbstractGuiAuto { + public BuddyAutoLeftHideRed() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/buddyautoleftsteal.json")); + } +} diff --git a/src/main/java/frc/auton/HangerCargoBlue.java b/src/main/java/frc/auton/HangerCargoBlue.java new file mode 100644 index 00000000..45c62df2 --- /dev/null +++ b/src/main/java/frc/auton/HangerCargoBlue.java @@ -0,0 +1,12 @@ +package frc.auton; + +import edu.wpi.first.wpilibj.Filesystem; +import frc.auton.guiauto.AbstractGuiAuto; + +import java.io.File; + +public class HangerCargoBlue extends AbstractGuiAuto { + public HangerCargoBlue() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/movecargotohanger.json")); + } +} diff --git a/src/main/java/frc/auton/HangerCargoRed.java b/src/main/java/frc/auton/HangerCargoRed.java new file mode 100644 index 00000000..fee9b47f --- /dev/null +++ b/src/main/java/frc/auton/HangerCargoRed.java @@ -0,0 +1,12 @@ +package frc.auton; + +import edu.wpi.first.wpilibj.Filesystem; +import frc.auton.guiauto.AbstractGuiAuto; + +import java.io.File; + +public class HangerCargoRed extends AbstractGuiAuto { + public HangerCargoRed() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/movecargotohanger.json")); + } +} diff --git a/src/main/java/frc/auton/HideBallsBlue.java b/src/main/java/frc/auton/HideCargoBlue.java similarity index 62% rename from src/main/java/frc/auton/HideBallsBlue.java rename to src/main/java/frc/auton/HideCargoBlue.java index 4d020950..56f9a93b 100644 --- a/src/main/java/frc/auton/HideBallsBlue.java +++ b/src/main/java/frc/auton/HideCargoBlue.java @@ -5,8 +5,8 @@ import java.io.File; -public class HideBallsBlue extends AbstractGuiAuto { - public HideBallsBlue() { - super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/hideballs.json")); +public class HideCargoBlue extends AbstractGuiAuto { + public HideCargoBlue() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/hidecargo.json")); } } diff --git a/src/main/java/frc/auton/HideBallsRed.java b/src/main/java/frc/auton/HideCargoRed.java similarity index 63% rename from src/main/java/frc/auton/HideBallsRed.java rename to src/main/java/frc/auton/HideCargoRed.java index cedb3354..07a69620 100644 --- a/src/main/java/frc/auton/HideBallsRed.java +++ b/src/main/java/frc/auton/HideCargoRed.java @@ -5,8 +5,8 @@ import java.io.File; -public class HideBallsRed extends AbstractGuiAuto { - public HideBallsRed() { - super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/hideballs.json")); +public class HideCargoRed extends AbstractGuiAuto { + public HideCargoRed() { + super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/hidecargo.json")); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b322ec36..95f25a49 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -126,11 +126,27 @@ public class Robot extends TimedRobot { @SuppressWarnings("NotNullFieldNotInitialized") @NotNull - private HideBallsBlue hideBallsBlue; + private HideCargoBlue hideCargoBlue; @SuppressWarnings("NotNullFieldNotInitialized") @NotNull - private HideBallsRed hideBallsRed; + private HideCargoRed hideCargoRed; + + @SuppressWarnings("NotNullFieldNotInitialized") + @NotNull + private BuddyAutoLeftHideBlue buddyAutoLeftHideBlue; + + @SuppressWarnings("NotNullFieldNotInitialized") + @NotNull + private BuddyAutoLeftHideRed buddyAutoLeftHideRed; + + @SuppressWarnings("NotNullFieldNotInitialized") + @NotNull + private HangerCargoBlue hangerCargoBlue; + + @SuppressWarnings("NotNullFieldNotInitialized") + @NotNull + private HangerCargoRed hangerCargoRed; @NotNull private static final String SHOOT_AND_MOVE_LEFT = "Shoot and Move Left"; @@ -144,7 +160,9 @@ public class Robot extends TimedRobot { @NotNull private static final String SHOOT_ONLY_RIGHT = "Shoot Only Right"; @NotNull private static final String SHOOT_ONLY_MID = "Shoot Only Mid"; @NotNull private static final String SHOOT_ONLY_LEFT = "Shoot Only Left"; - @NotNull private static final String HIDE_BALLS = "Hide Balls"; + @NotNull private static final String HIDE_CARGO = "Hide Cargo"; + @NotNull private static final String BUDDY_AUTO_LEFT_HIDE = "Buddy Auto Left Hide"; + @NotNull private static final String HANGER_CARGO = "Hanger Cargo"; private static final String RESET_POSE = "Reset Pose"; @@ -264,8 +282,13 @@ public void robotInit() { CompletableFuture.runAsync(() -> fiveBallRed = new FiveBallRed()).thenRun(this::incrementLoadedAutos); CompletableFuture.runAsync(() -> sixBallRed = new SixBallRed()).thenRun(this::incrementLoadedAutos); CompletableFuture.runAsync(() -> buddyAutoLeft = new BuddyAutoLeft()).thenRun(this::incrementLoadedAutos); - CompletableFuture.runAsync(() -> hideBallsBlue = new HideBallsBlue()).thenRun(this::incrementLoadedAutos); - CompletableFuture.runAsync(() -> hideBallsRed = new HideBallsRed()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> hideCargoBlue = new HideCargoBlue()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> hideCargoRed = new HideCargoRed()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> buddyAutoLeftHideBlue = new BuddyAutoLeftHideBlue()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> buddyAutoLeftHideRed = new BuddyAutoLeftHideRed()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> hangerCargoBlue = new HangerCargoBlue()).thenRun(this::incrementLoadedAutos); + CompletableFuture.runAsync(() -> hangerCargoRed = new HangerCargoRed()).thenRun(this::incrementLoadedAutos); + SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative); autoChooser.setDefaultOption(SHOOT_AND_MOVE_LEFT, SHOOT_AND_MOVE_LEFT); @@ -278,7 +301,9 @@ public void robotInit() { autoChooser.addOption(SHOOT_ONLY_LEFT, SHOOT_ONLY_LEFT); autoChooser.addOption(SHOOT_ONLY_MID, SHOOT_ONLY_MID); autoChooser.addOption(SHOOT_ONLY_RIGHT, SHOOT_ONLY_RIGHT); - autoChooser.addOption(HIDE_BALLS, HIDE_BALLS); + autoChooser.addOption(HIDE_CARGO, HIDE_CARGO); + autoChooser.addOption(HANGER_CARGO, HANGER_CARGO); + autoChooser.addOption(BUDDY_AUTO_LEFT_HIDE, BUDDY_AUTO_LEFT_HIDE); sideChooser.setDefaultOption(BLUE, BLUE); sideChooser.addOption(RED, RED); @@ -313,7 +338,7 @@ public void robotInit() { volatile boolean loadingAutos = true; public void incrementLoadedAutos() { - if (loadedAutos.incrementAndGet() == 12) { + if (loadedAutos.incrementAndGet() == 16) { loadingAutos = false; } } @@ -388,8 +413,14 @@ public void autonomousInit() { case BUDDY_AUTO_LEFT: selectedAuto = buddyAutoLeft; break; - case HIDE_BALLS: - selectedAuto = hideBallsBlue; + case HIDE_CARGO: + selectedAuto = hideCargoBlue; + break; + case BUDDY_AUTO_LEFT_HIDE: + selectedAuto = buddyAutoLeftHideBlue; + break; + case HANGER_CARGO: + selectedAuto = hangerCargoBlue; break; default: selectedAuto = shootAndMoveRight; @@ -427,8 +458,14 @@ public void autonomousInit() { case BUDDY_AUTO_LEFT: selectedAuto = buddyAutoLeft; break; - case HIDE_BALLS: - selectedAuto = hideBallsRed; + case HIDE_CARGO: + selectedAuto = hideCargoRed; + break; + case BUDDY_AUTO_LEFT_HIDE: + selectedAuto = buddyAutoLeftHideRed; + break; + case HANGER_CARGO: + selectedAuto = hangerCargoRed; break; default: selectedAuto = shootAndMoveRight; diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index c6de478f..882c6472 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -833,6 +833,7 @@ public void turnToAngle(double degrees) throws InterruptedException { setRotation(degrees); while (!isTurningDone()) { Thread.onSpinWait(); + if (Thread.interrupted()) throw new InterruptedException("Interrupted while turning"); } } } From 7a4738316ad7a52a04e4b91acabccbf7e3037d8a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 16:02:29 -0700 Subject: [PATCH 062/108] move the robot at the end of the 5 ball to a more ideal spot --- src/main/deploy/autos/blue/5ball.json | 2212 ++++++++++++++++-------- src/main/deploy/autos/red/5ball.json | 2272 +++++++++++++++++-------- 2 files changed, 3121 insertions(+), 1363 deletions(-) diff --git a/src/main/deploy/autos/blue/5ball.json b/src/main/deploy/autos/blue/5ball.json index 85ea1792..4aa2b6c7 100644 --- a/src/main/deploy/autos/blue/5ball.json +++ b/src/main/deploy/autos/blue/5ball.json @@ -250,452 +250,466 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 3.9999999999999987, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 7.626592636108398, "y" : -3.196845054626465 }, "rotation" : { - "radians" : 2.70041390445855 + "radians" : -3.128825218368557 } }, - "curvature" : -0.0 + "curvature" : 0.0 }, { - "time" : 0.23693937519075453, - "velocity" : 0.9477575007630178, - "acceleration" : 3.999999999999997, + "time" : 0.18074788653672832, + "velocity" : 0.7229915461469133, + "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 7.5252611777282254, - "y" : -3.148483353978918 + "x" : 7.5612559258704835, + "y" : -3.197459243636949 }, "rotation" : { - "radians" : 2.6889354034911954 + "radians" : -3.138740759458168 } }, - "curvature" : -0.17311628002444718 + "curvature" : -0.29172803877514997 }, { - "time" : 0.34179239143933526, - "velocity" : 1.3671695657573404, - "acceleration" : 4.000000000000002, + "time" : 0.25626538315297653, + "velocity" : 1.0250615326119064, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 7.416731443241588, - "y" : -3.094165712100221 + "x" : 7.495254533242587, + "y" : -3.196840137823301 }, "rotation" : { - "radians" : 2.666000997449467 + "radians" : 3.1176073408097276 } }, - "curvature" : -0.1843290944729718 + "curvature" : -0.5074451761358538 }, { - "time" : 0.3870859500027286, - "velocity" : 1.5483438000109138, - "acceleration" : 4.0, + "time" : 0.3151289706110068, + "velocity" : 1.2605158824440272, + "acceleration" : 4.000000000000004, "pose" : { "translation" : { - "x" : 7.358208917510225, - "y" : -3.0635932230767082 + "x" : 7.428047492778063, + "y" : -3.1939619357255324 }, "rotation" : { - "radians" : 2.6546580588501496 + "radians" : 3.0784656278152482 } }, - "curvature" : -0.1576700182253739 + "curvature" : -0.6425731706470493 }, { - "time" : 0.43004526564064915, - "velocity" : 1.720181062562596, - "acceleration" : 4.0, + "time" : 0.3658526486292684, + "velocity" : 1.463410594517074, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 7.29633205176151, - "y" : -3.0304232421231063 + "x" : 7.3592204221349675, + "y" : -3.1880118377084727 }, "rotation" : { - "radians" : 2.644758053448521 + "radians" : 3.0315419813222593 } }, - "curvature" : -0.12418290422011853 + "curvature" : -0.7042822762242509 }, { - "time" : 0.4714292106726367, - "velocity" : 1.8857168426905462, - "acceleration" : 4.000000000000005, + "time" : 0.4117606980978159, + "velocity" : 1.6470427923912636, + "acceleration" : 2.1723907907211393, "pose" : { "translation" : { - "x" : 7.2308898126944, - "y" : -2.994584240198975 + "x" : 7.288476303159229, + "y" : -3.178375355932097 }, "rotation" : { - "radians" : 2.6367892329110907 + "radians" : 2.980846049930411 } }, - "curvature" : -0.08996179996248393 + "curvature" : -0.7073398126437707 }, { - "time" : 0.5116473910205699, - "velocity" : 2.046589564082279, - "acceleration" : 4.000000000000005, + "time" : 0.4555105966830825, + "velocity" : 1.7420846691728806, + "acceleration" : 1.9989588140063956, "pose" : { "translation" : { - "x" : 7.16179740941152, - "y" : -2.956125016324222 + "x" : 7.215626262967817, + "y" : -3.1646216243218532 }, "rotation" : { - "radians" : 2.6309726297310103 + "radians" : 2.9296129513061087 } }, - "curvature" : -0.057945967404890976 + "curvature" : -0.6695533478285349 }, { - "time" : 0.5509102464902416, - "velocity" : 2.2036409859609662, - "acceleration" : 3.999999999999985, + "time" : 0.4987555436671162, + "velocity" : 1.828529537107854, + "acceleration" : 2.813152489265579, "pose" : { "translation" : { - "x" : 7.089084233776916, - "y" : -2.915201800386029 + "x" : 7.140580355031915, + "y" : -3.146488708538989 }, "rotation" : { - "radians" : 2.6273738323394795 + "radians" : 2.8802105895971133 } }, - "curvature" : -0.029106454170210898 + "curvature" : -0.6077426894646522 }, { - "time" : 0.5893084187633256, - "velocity" : 2.357233675053302, - "acceleration" : 4.0, + "time" : 0.5413750298351414, + "velocity" : 1.948424650712654, + "acceleration" : 3.6356664874107865, "pose" : { "translation" : { - "x" : 7.012881800773812, - "y" : -2.8720653559457787 + "x" : 7.063338340260088, + "y" : -3.1238689159508795 }, "rotation" : { - "radians" : 2.625981856643082 + "radians" : 2.834193414995866 } }, - "curvature" : -0.003356981490914264 + "curvature" : -0.5352497494731134 }, { - "time" : 0.6268587033194142, - "velocity" : 2.5074348132776563, - "acceleration" : 4.0, + "time" : 0.5828078541546968, + "velocity" : 2.0990605815700403, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.933411688862364, - "y" : -2.8270480830459803 + "x" : 6.983980468081455, + "y" : -3.0967941056013544 }, "rotation" : { - "radians" : 2.6267596915335263 + "radians" : 2.7924479125830106 } }, - "curvature" : 0.01988136359364883 + "curvature" : -0.46118349636323475 }, { - "time" : 0.6635324088349698, - "velocity" : 2.6541296353398787, - "acceleration" : 4.0, + "time" : 0.6228085797994793, + "velocity" : 2.259063484149171, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 6.850973480337416, - "y" : -2.780551121017197 + "x" : 6.902658257528856, + "y" : -3.0654209981810254 }, "rotation" : { - "radians" : 2.6296746402590294 + "radians" : 2.755369864098415 } }, - "curvature" : 0.04137650970352362 + "curvature" : -0.3908757756828451 }, { - "time" : 0.6992737012181571, - "velocity" : 2.797094804872628, - "acceleration" : 2.946517671736344, + "time" : 0.6614595623941697, + "velocity" : 2.413667414527932, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 6.765932701686253, - "y" : -2.7330314512849725 + "x" : 6.819585278322023, + "y" : -3.0300164859976135 }, "rotation" : { - "radians" : 2.634715682627438 + "radians" : 2.7230300588082508 } }, - "curvature" : 0.0619438539724856 + "curvature" : -0.32684828411643785 }, { - "time" : 0.7342311236047477, - "velocity" : 2.900097467693069, - "acceleration" : -3.9999999999999956, + "time" : 0.6988907922441839, + "velocity" : 2.563392333927989, + "acceleration" : 3.9999999999999907, "pose" : { "translation" : { - "x" : 6.678708763946361, - "y" : -2.6849890001767562 + "x" : 6.7350279319507536, + "y" : -2.990942942946276 }, "rotation" : { - "radians" : 2.6419030505364995 + "radians" : 2.6953074205444363 } }, - "curvature" : 0.08239451314240322 + "curvature" : -0.26979273017272876 }, { - "time" : 0.7699686049800885, - "velocity" : 2.757147542191706, - "acceleration" : -4.000000000000004, + "time" : 0.735158469971846, + "velocity" : 2.708463044838637, + "acceleration" : 1.279466838075468, "pose" : { "translation" : { - "x" : 6.589762903063175, - "y" : -2.6369537417288313 + "x" : 6.649296232758076, + "y" : -2.9486435344799347 }, "rotation" : { - "radians" : 2.651293318832483 + "radians" : 2.671985460234166 } }, - "curvature" : 0.1035289020309025 + "curvature" : -0.2193389226412271 }, { - "time" : 0.8079799488552681, - "velocity" : 2.6051021666909873, + "time" : 0.7708801601749081, + "velocity" : 2.75416776285346, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.499586120247841, - "y" : -2.5894728004932404 + "x" : 6.562734589023421, + "y" : -2.9036275275796015 }, "rotation" : { - "radians" : 2.6629818655214734 + "radians" : 2.65281690834138 } }, - "curvature" : 0.126150198530942 + "curvature" : -0.17457123350751053 }, { - "time" : 0.8484060299195538, - "velocity" : 2.4433978424338445, + "time" : 0.8078106139873205, + "velocity" : 2.606445947603811, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.4086871223349675, - "y" : -2.543097554344712 + "x" : 6.4757125840457945, + "y" : -2.8564556007247077 }, "rotation" : { - "radians" : 2.6771034833727265 + "radians" : 2.6375642644084403 } }, - "curvature" : 0.1510827102354415 + "curvature" : -0.13433724003134345 }, { - "time" : 0.8914612758849075, - "velocity" : 2.2711768585724297, - "acceleration" : -3.9999999999999956, + "time" : 0.8472976500866943, + "velocity" : 2.4484978032063154, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.317580262140382, - "y" : -2.498370737287587 + "x" : 6.388615757226944, + "y" : -2.80772515386343 }, "rotation" : { - "radians" : 2.6938310626262183 + "radians" : 2.62602361109316 } }, - "curvature" : 0.1791841330534715 + "curvature" : -0.09741150032488063 }, { - "time" : 0.9374815876639159, - "velocity" : 2.0870956114563963, - "acceleration" : -4.000000000000007, + "time" : 0.8895957780818, + "velocity" : 2.2793052912258926, + "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.226773478818885, - "y" : -2.455813542262746 + "x" : 6.30183638515453, + "y" : -2.7580556183830183 }, "rotation" : { - "radians" : 2.713371437701187 + "radians" : 2.618037394000371 } }, - "curvature" : 0.2113396676958131 + "curvature" : -0.06256913374757356 }, { - "time" : 0.9870098384364133, - "velocity" : 1.8889826083664063, - "acceleration" : -4.000000000000003, + "time" : 0.9350786097662884, + "velocity" : 2.097373964487939, + "acceleration" : -3.999999999999998, "pose" : { "translation" : { - "x" : 6.136756238222006, - "y" : -2.415912723954534 + "x" : 6.215764262685298, + "y" : -2.7080737670801227 }, "rotation" : { - "radians" : 2.7359565418118486 + "radians" : 2.6135000159251436 } }, - "curvature" : 0.24842042272290393 + "curvature" : -0.02860941649074867 }, { - "time" : 1.0409639720951336, - "velocity" : 1.6731660737315248, + "time" : 0.9843259494632479, + "velocity" : 1.900384605700101, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.047987473255759, - "y" : -2.3791077015976896 + "x" : 6.130777484028243, + "y" : -2.6583990241311213 }, "rotation" : { - "radians" : 2.761826815961614 + "radians" : 2.612358417756541 } }, - "curvature" : 0.29117840311887416 + "curvature" : 0.005641950266030671 }, { - "time" : 1.1010149453972793, - "velocity" : 1.4329621805229422, - "acceleration" : -3.9999999999999987, + "time" : 1.038295625522969, + "velocity" : 1.6845059014612167, + "acceleration" : -4.000000000000003, "pose" : { "translation" : { - "x" : 5.9608835242383975, - "y" : -2.3457776617842683 + "x" : 6.047233223827789, + "y" : -2.609628775062447 }, "rotation" : { - "radians" : 2.79120232287241 + "radians" : 2.6146083461531995 } }, - "curvature" : 0.34003617310478934 + "curvature" : 0.04132896947061391 }, { - "time" : 1.1706296628919548, - "velocity" : 1.1545033105442406, - "acceleration" : -3.9999999999999987, + "time" : 1.0987120643010575, + "velocity" : 1.4428401463488625, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.9654585182469475, + "y" : -2.562323676720915 + }, + "rotation" : { + "radians" : 2.620285612891302 + } + }, + "curvature" : 0.07953607724401594 + }, { + "time" : 1.1691474210025192, + "velocity" : 1.1610987195430156, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 5.875806079258169, - "y" : -2.3162286612705714 + "x" : 5.885741046050498, + "y" : -2.5169929672440503 }, "rotation" : { - "radians" : 2.8242353628684933 + "radians" : 2.6294501603955758 } }, - "curvature" : 0.3947146209017993 + "curvature" : 0.12120182277895979 }, { - "time" : 1.2592554905280149, + "time" : 1.259422100888273, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.79305011453107, - "y" : -2.2906807297840714 + "x" : 5.8083199096881515, + "y" : -2.4740797760304147 }, "rotation" : { - "radians" : 2.860937134348083 + "radians" : 2.642159027386621 } }, - "curvature" : 0.45363861905641456 + "curvature" : 0.1669290910240929 }, { - "time" : 1.363043401302631, + "time" : 1.3656883669621598, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.712831834758603, - "y" : -2.269254972830339 + "x" : 5.733376416377723, + "y" : -2.4339464337099344 }, "rotation" : { - "radians" : 2.901071430255972 + "radians" : 2.6584223838932473 } }, - "curvature" : 0.5130934535793734 + "curvature" : 0.2166266434826088 }, { - "time" : 1.4623685074995216, + "time" : 1.4673170381313625, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.63527661348553, - "y" : -2.251960674499969 + "x" : 5.661024859188302, + "y" : -2.396859782114227 }, "rotation" : { - "radians" : 2.944012822610768 + "radians" : 2.6781351093523047 } }, - "curvature" : 0.5662163704582089 + "curvature" : 0.2689053696942707 }, { - "time" : 1.5574160412808196, + "time" : 1.5642156155177847, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.560406933457628, - "y" : -2.2386824002755077 + "x" : 5.591303298123421, + "y" : -2.3629764842469285 }, "rotation" : { - "radians" : 2.988578192465073 + "radians" : 2.700976194613289 } }, - "curvature" : 0.6021348789892561 + "curvature" : 0.3201793855552225 }, { - "time" : 1.648541373811172, + "time" : 1.6564699229560516, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.488130326979444, - "y" : -2.229167099838378 + "x" : 5.5241643412042265, + "y" : -2.332328334254022 }, "rotation" : { - "radians" : 3.0328604090655755 + "radians" : 2.726272217468977 } }, - "curvature" : 0.6058749987539385 + "curvature" : 0.36354779991332803 }, { - "time" : 1.7362583003346943, + "time" : 1.7443554806111186, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.418227316272052, - "y" : -2.223011209875807 + "x" : 5.459465925552649, + "y" : -2.304807567394164 }, "rotation" : { - "radians" : 3.074117872291698 + "radians" : 2.7528334527109286 } }, - "curvature" : 0.5598690362019735 + "curvature" : 0.38783779721239137 }, { - "time" : 1.8212223392389115, + "time" : 1.8283441053954617, "velocity" : 0.8, - "acceleration" : -0.8705165062981772, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.350339353830805, - "y" : -2.2196477568877526 + "x" : 5.3969620984745745, + "y" : -2.2801521700090106 }, "rotation" : { - "radians" : 3.108791896480375 + "radians" : 2.7787959191919596 } }, - "curvature" : 0.44767291775872703 + "curvature" : 0.3776909643218038 }, { - "time" : 1.9083467094330548, - "velocity" : 0.7241567976451654, - "acceleration" : -4.0, + "time" : 1.9091062425604757, + "velocity" : 0.8, + "acceleration" : -2.5718750180081185, "pose" : { "translation" : { - "x" : 5.283956762783092, - "y" : -2.218333459993829 + "x" : 5.336293798543011, + "y" : -2.257931189493547 }, "rotation" : { - "radians" : 3.132720840556557 + "radians" : 2.8015387216773218 } }, - "curvature" : 0.2597205814128655 + "curvature" : 0.31603589357887024 }, { - "time" : 2.089385908844346, + "time" : 2.22016334852342, "velocity" : 0.0, - "acceleration" : -4.0, + "acceleration" : -2.5718750180081185, "pose" : { "translation" : { "x" : 5.218406677246094, "y" : -2.2181358337402344 }, "rotation" : { - "radians" : 3.141592653589793 + "radians" : 2.823947779781842 } }, "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 7.626592636108398, -1.5994892120361328, 0.0 ], - "y" : [ -3.196845054626465, 0.7553129196166992, 0.0 ] + "x" : [ 7.626592636108398, -2.086981773376465, 0.0 ], + "y" : [ -3.196845054626465, -0.026646852493286133, 0.0 ] }, { - "x" : [ 5.218406677246094, -2.0928425788879395, 0.0 ], - "y" : [ -2.2181358337402344, 0.0, 0.0 ] + "x" : [ 5.218406677246094, -1.8701248168945312, 0.0 ], + "y" : [ -2.2181358337402344, 0.6148552894592285, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : -3.1179661408942683 + "radians" : 2.9608083881863267 } }, { - "time" : 1.9999999552965164, + "time" : 2.109999952837825, "rotation" : { "radians" : -3.1066860685499065 } @@ -760,691 +774,649 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 3.999999999999999, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 5.218406677246094, "y" : -2.2181358337402344 }, "rotation" : { - "radians" : -3.134549682568558 + "radians" : 2.9473819457269834 } }, - "curvature" : 0.0 + "curvature" : -0.0 }, { - "time" : 0.24844379078421727, - "velocity" : 0.9937751631368689, - "acceleration" : 4.000000000000001, + "time" : 0.22577455195938007, + "velocity" : 0.9030982078375203, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.094960177412837, - "y" : -2.2188617840741998 + "x" : 5.118318357381213, + "y" : -2.198750699968514 }, "rotation" : { - "radians" : -3.1379555673033197 + "radians" : 2.9554310054193884 } }, - "curvature" : -0.052504130942746294 + "curvature" : 0.13027761823749012 }, { - "time" : 0.35211767066746935, - "velocity" : 1.4084706826698772, - "acceleration" : 3.9999999999999982, + "time" : 0.3382371271442293, + "velocity" : 1.3529485085769173, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 4.970435117519401, - "y" : -2.2188032852247943 + "x" : 4.993451572256163, + "y" : -2.176348690292798 }, "rotation" : { - "radians" : 3.136267041691601 + "radians" : 2.972591803682675 } }, - "curvature" : -0.08861071555917467 + "curvature" : 0.12784985955539324 }, { - "time" : 0.43268308957020346, - "velocity" : 1.7307323582808134, - "acceleration" : 3.999999999999993, + "time" : 0.3916828784133741, + "velocity" : 1.5667315136534963, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 4.843987783591622, - "y" : -2.2173570738123303 + "x" : 4.916478985341882, + "y" : -2.163593622500116 }, "rotation" : { - "radians" : 3.123617851022157 + "radians" : 2.9819601021983635 } }, - "curvature" : -0.10892063513271176 + "curvature" : 0.11259673461042294 }, { - "time" : 0.4682875420771055, - "velocity" : 1.8731501683084213, - "acceleration" : 3.9999999999999964, + "time" : 0.4451005174091182, + "velocity" : 1.7804020696364724, + "acceleration" : 3.999999999999998, "pose" : { "translation" : { - "x" : 4.779845521340216, - "y" : -2.2159758438113766 + "x" : 4.82815027252218, + "y" : -2.1498081765639654 }, "rotation" : { - "radians" : 3.1164533918962682 + "radians" : 2.9913800207169805 } }, - "curvature" : -0.11388685999396095 + "curvature" : 0.09893287394574607 }, { - "time" : 0.5017196993257746, - "velocity" : 2.0068787973030977, - "acceleration" : 4.0, + "time" : 0.49879827939468, + "velocity" : 1.9951931175787196, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 4.715013664295839, - "y" : -2.2141041512441006 + "x" : 4.727849331012521, + "y" : -2.1351214277652844 }, "rotation" : { - "radians" : 3.108986265289489 + "radians" : 3.000843152646543 } }, - "curvature" : -0.11591011471666389 + "curvature" : 0.08854693535519663 }, { - "time" : 0.5334095522803516, - "velocity" : 2.1336382091214054, - "acceleration" : 4.0, + "time" : 0.5527581649942036, + "velocity" : 2.211032659976814, + "acceleration" : 3.9999999999999964, "pose" : { "translation" : { - "x" : 4.649450957232703, - "y" : -2.2117159957400556 + "x" : 4.61541160941124, + "y" : -2.11974885687232 }, "rotation" : { - "radians" : 3.101385880033052 + "radians" : 3.010451193020663 } }, - "curvature" : -0.1153988635183254 + "curvature" : 0.08141460522576446 }, { - "time" : 0.563665676688337, - "velocity" : 2.254662706753347, - "acceleration" : 3.99999999999998, + "time" : 0.6067994538503118, + "velocity" : 2.4271978154012466, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.583128758540219, - "y" : -2.2087950520156383 + "x" : 4.491079470872819, + "y" : -2.1039811364393017 }, "rotation" : { - "radians" : 3.093802255722613 + "radians" : 3.0203520346564443 } }, - "curvature" : -0.11275046849580618 + "curvature" : 0.0770754880726607 }, { - "time" : 0.5927164103455597, - "velocity" : 2.3708656413822373, - "acceleration" : 3.9999999999999805, + "time" : 0.6337713815419612, + "velocity" : 2.5350855261678444, + "acceleration" : 3.999999999999981, "pose" : { "translation" : { - "x" : 4.51603045608554, - "y" : -2.2053342095085027 + "x" : 4.424629014592831, + "y" : -2.096057730950637 }, "rotation" : { - "radians" : 3.086366518935236 + "radians" : 3.025464239893527 } }, - "curvature" : -0.10833530395810449 + "curvature" : 0.07580950849707052 }, { - "time" : 0.6207345486414533, - "velocity" : 2.4829381945658113, + "time" : 0.6606632003839968, + "velocity" : 2.6426528015359865, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.448150883076096, - "y" : -2.201335112011975 + "x" : 4.355457556281181, + "y" : -2.088172917105112 }, "rotation" : { - "radians" : 3.0791919886169734 + "radians" : 3.030713652227927 } }, - "curvature" : -0.1024865610528908 + "curvature" : 0.07508525249190778 }, { - "time" : 0.647853002436567, - "velocity" : 2.591412009746266, + "time" : 0.687437732922965, + "velocity" : 2.749750931691859, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.379495733922141, - "y" : -2.1968076973094695 + "x" : 4.2836899587141986, + "y" : -2.0803791830604403 }, "rotation" : { - "radians" : 3.0723756619209617 + "radians" : 3.036123218975336 } }, - "curvature" : -0.09549473651105347 + "curvature" : 0.07486952079026583 }, { - "time" : 0.6741751852926068, - "velocity" : 2.6967007411704254, - "acceleration" : 4.000000000000006, + "time" : 0.7140574327285515, + "velocity" : 2.856229730914205, + "acceleration" : 4.0000000000000115, "pose" : { "translation" : { - "x" : 4.310080980099293, - "y" : -2.191769736808901 + "x" : 4.209468147422967, + "y" : -2.0727316138919605 }, "rotation" : { - "radians" : 3.065999941440726 + "radians" : 3.0417167398697336 } }, - "curvature" : -0.08760573546394629 + "curvature" : 0.07513898148074699 }, { - "time" : 0.69978216896706, - "velocity" : 2.7991286758682383, - "acceleration" : 4.000000000000006, + "time" : 0.7404848785000484, + "velocity" : 2.961939514000193, + "acceleration" : 1.4367050542224837, "pose" : { "translation" : { - "x" : 4.239932286011077, - "y" : -2.186246375177099 + "x" : 4.132949715792485, + "y" : -2.0652875411644693 }, "rotation" : { - "radians" : 3.060134476441395 + "radians" : 3.0475190269956305 } }, - "curvature" : -0.07902157653078037 + "curvature" : 0.07587984575547331 }, { - "time" : 0.7247377728559167, - "velocity" : 2.898951091423665, - "acceleration" : 3.9999999999999876, + "time" : 0.7669763882834565, + "velocity" : 3.0, + "acceleration" : -1.0982184022288134E-14, "pose" : { "translation" : { - "x" : 4.169084424851462, - "y" : -2.180269669974223 + "x" : 4.054306530160829, + "y" : -2.058106192504056 }, "rotation" : { - "radians" : 3.054838021258 + "radians" : 3.053556107860972 } }, - "curvature" : -0.06990281795605731 + "curvature" : 0.07708752042753088 }, { - "time" : 0.7490922849120007, - "velocity" : 2.996369139648001, - "acceleration" : 0.1503076705961156, + "time" : 0.7939345482116209, + "velocity" : 2.9999999999999996, + "acceleration" : 1.0755238227730279E-14, "pose" : { "translation" : { - "x" : 4.09758069446741, - "y" : -2.173878131288177 + "x" : 3.9737233349183256, + "y" : -2.0512483411699343 }, "rotation" : { - "radians" : 3.0501602415106173 + "radians" : 3.05985546264456 } }, - "curvature" : -0.06037198639248534 + "curvature" : 0.07876635846236638 }, { - "time" : 0.7732484729335392, + "time" : 0.8214615510510413, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.025472333221408, - "y" : -2.1671162613690242 + "x" : 3.891396357606709, + "y" : -2.044775955626278 }, "rotation" : { - "radians" : 3.0461434221192074 + "radians" : 3.0664462909649215 } }, - "curvature" : -0.05051745629810009 + "curvature" : 0.08092957341077114 }, { - "time" : 0.7975813919887572, + "time" : 0.8494883928631835, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -1.0404470748216723E-14, "pose" : { "translation" : { - "x" : 3.9528179358540214, - "y" : -2.1600340942633993 + "x" : 3.8075319140182913, + "y" : -2.0387518491140533 }, "rotation" : { - "radians" : 3.0428240499419688 + "radians" : 3.0733598063989405 } }, - "curvature" : -0.04039737992759309 + "curvature" : 0.08359935409570846 }, { - "time" : 0.822082461431673, - "velocity" : 3.0, + "time" : 0.8779434176691419, + "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.879682869346425, - "y" : -2.152686735448924 + "x" : 3.7223450132951257, + "y" : -2.0332393292228517 }, "rotation" : { - "radians" : 3.0402342583591766 + "radians" : 3.080629558866709 } }, - "curvature" : -0.030043396377312796 + "curvature" : 0.08680720147794485 }, { - "time" : 0.8467261259226331, - "velocity" : 3.0, + "time" : 0.9067528176974184, + "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.8061386887829496, - "y" : -2.145133901468623 - }, - "rotation" : { - "radians" : 3.038403131725035 - } - }, - "curvature" : -0.019463950237647885 - }, { - "time" : 0.8714847110618453, - "velocity" : 3.0, - "acceleration" : -1.19167595131066E-14, - "pose" : { - "translation" : { - "x" : 3.732262553213623, - "y" : -2.1374394595653357 + "x" : 3.636057963028172, + "y" : -2.028301847462725 }, "rotation" : { - "radians" : 3.037357874897631 + "radians" : 3.0882917861725874 } }, - "curvature" : -0.008647127304545333 + "curvature" : 0.09059450379888723 }, { - "time" : 0.896328669094879, + "time" : 0.9358411361147014, "velocity" : 2.9999999999999996, - "acceleration" : 1.1890816042388423E-14, - "pose" : { - "translation" : { - "x" : 3.6581366415167116, - "y" : -2.129670967316132 - }, - "rotation" : { - "radians" : 3.037124857643745 - } - }, - "curvature" : 0.002437030086679833 - }, { - "time" : 0.9212268318599895, - "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : 1.0107649040265085E-14, "pose" : { "translation" : { - "x" : 3.5838475682612625, - "y" : -2.121899212266726 + "x" : 3.5488989743564616, + "y" : -2.024002648836017 }, "rotation" : { - "radians" : 3.0377305461685578 + "radians" : 3.0963857966103348 } }, - "curvature" : 0.013834727878406873 + "curvature" : 0.09501336487557137 }, { - "time" : 0.9461466703200341, + "time" : 0.9651317725479596, "velocity" : 3.0, - "acceleration" : -1.1886172423716848E-14, - "pose" : { - "translation" : { - "x" : 3.509485799569643, - "y" : -2.114197751565891 - }, - "rotation" : { - "radians" : 3.039202334822319 - } - }, - "curvature" : 0.025607120355229326 - }, { - "time" : 0.9710545601502908, - "velocity" : 2.9999999999999996, - "acceleration" : 1.1908354632336836E-14, + "acceleration" : -0.06809488522901795, "pose" : { "translation" : { - "x" : 3.435145068980084, - "y" : -2.1066424515998747 + "x" : 3.461100767066263, + "y" : -2.0204044214092 }, "rotation" : { - "radians" : 3.041569290565269 + "radians" : 3.1049543846666157 } }, - "curvature" : 0.037829464706338994 + "curvature" : 0.10012770121562267 }, { - "time" : 0.9959160529739453, - "velocity" : 3.0, - "acceleration" : -1.1947468879569855E-14, + "time" : 0.9945573185420075, + "velocity" : 2.997996270822734, + "acceleration" : -0.184591356598183, "pose" : { "translation" : { - "x" : 3.360921793309222, - "y" : -2.0993110276268103 + "x" : 3.3728991746902466, + "y" : -2.0175689458847046 }, "rotation" : { - "radians" : 3.0448628212988202 + "radians" : 3.1140442814748748 } }, - "curvature" : 0.05059067862156035 + "curvature" : 0.10601462452471887 }, { - "time" : 1.0206961529447127, - "velocity" : 2.9999999999999996, - "acceleration" : 1.2003978639715924E-14, + "time" : 1.0240672639708144, + "velocity" : 2.9925489899628923, + "acceleration" : -0.20849217138745274, "pose" : { "translation" : { - "x" : 3.2869144885146397, - "y" : -2.0922825834111336 + "x" : 3.28453174960665, + "y" : -2.0155567451727556 }, "rotation" : { - "radians" : 3.0491172768608856 + "radians" : 3.1237066406836274 } }, - "curvature" : 0.063993240610446 + "curvature" : 0.11276612545130336 }, { - "time" : 1.04535959846717, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.0536051481517177, + "velocity" : 2.9863905723518247, + "acceleration" : -0.2602935739030425, "pose" : { "translation" : { - "x" : 3.213223185557407, - "y" : -2.0856371508579974 + "x" : 3.196236368138443, + "y" : -2.014426733963205 }, "rotation" : { - "radians" : 3.05437048840413 + "radians" : 3.13399755862567 } }, - "curvature" : 0.07815336280211393 + "curvature" : 0.12049107003628591 }, { - "time" : 1.0698711489159878, - "velocity" : 3.0, - "acceleration" : -0.37676977972820513, + "time" : 1.0831056442927933, + "velocity" : 2.9787117827793512, + "acceleration" : -0.3599966924344615, "pose" : { "translation" : { - "x" : 3.1399488462646232, - "y" : -2.079455229647685 + "x" : 3.1082498356524937, + "y" : -2.014235868297366 }, "rotation" : { - "radians" : 3.0606642480050854 + "radians" : -3.1382066823601824 } }, - "curvature" : 0.09320135214507283 + "curvature" : 0.1293175095895187 }, { - "time" : 1.0942331455034373, - "velocity" : 2.9908211359120074, - "acceleration" : -0.657806238307953, + "time" : 1.112515242001862, + "velocity" : 2.9681244248782583, + "acceleration" : -0.41435957373242843, "pose" : { "translation" : { - "x" : 3.06719277919196, - "y" : -2.0738173268700244 + "x" : 3.020806491658732, + "y" : -2.0150387951398443 }, "rotation" : { - "radians" : 3.0680447255833965 + "radians" : -3.1264678126986505 } }, - "curvature" : 0.10928205530053386 + "curvature" : 0.13939528367796553 }, { - "time" : 1.1184753330047406, - "velocity" : 2.974874473743419, - "acceleration" : -0.6954631021109505, + "time" : 1.14178182090318, + "velocity" : 2.9559975377201018, + "acceleration" : -0.47782084790928264, "pose" : { "translation" : { - "x" : 2.9950560554862022, - "y" : -2.068803496658802 + "x" : 2.9341368149093157, + "y" : -2.016887501950375 }, "rotation" : { - "radians" : 3.076562814361 + "radians" : -3.113896838891268 } }, - "curvature" : 0.12655525199728102 + "curvature" : 0.150898858830179 }, { - "time" : 1.142593788850363, - "velocity" : 2.9581009776228964, - "acceleration" : -0.7372656847547625, + "time" : 1.1708492320949808, + "velocity" : 2.942108522657908, + "acceleration" : -0.5518862628311226, "pose" : { "translation" : { - "x" : 2.923638924747787, - "y" : -2.0644928798261795 + "x" : 2.8484660284977963, + "y" : -2.019830966255654 }, "rotation" : { - "radians" : 3.0862743889098336 + "radians" : -3.1004122518647654 } }, - "curvature" : 0.14519581773365695 + "curvature" : 0.16403028405156617 }, { - "time" : 1.166561406144272, - "velocity" : 2.9404304758467625, - "acceleration" : -0.782997881082271, + "time" : 1.199665689597395, + "velocity" : 2.9262051156188686, + "acceleration" : -0.6381658186444317, "pose" : { "translation" : { - "x" : 2.853040230893349, - "y" : -2.060963243497106 + "x" : 2.764012704958283, + "y" : -2.023914805221171 }, "rotation" : { - "radians" : 3.0972404510187994 + "radians" : -3.085924825413159 } }, - "curvature" : 0.1653934156780358 + "curvature" : 0.179022046292927 }, { - "time" : 1.19035256084492, - "velocity" : 2.9218020521276546, - "acceleration" : -3.3968911862397824, + "time" : 1.2281844325226372, + "velocity" : 2.9080054286932713, + "acceleration" : -0.7382909496365782, "pose" : { "translation" : { - "x" : 2.7833568280182597, - "y" : -2.058290520743733 + "x" : 2.6809873713646084, + "y" : -2.029180925223045 }, "rotation" : { - "radians" : 3.1095271278230494 + "radians" : -3.0703372176489117 } }, - "curvature" : 0.1873513955323033 + "curvature" : 0.19613945786907577 }, { - "time" : 1.2141944862495007, - "velocity" : 2.840813625857848, - "acceleration" : -4.0, + "time" : 1.256364374900713, + "velocity" : 2.8872004322742577, + "acceleration" : -2.163128503876278, "pose" : { "translation" : { - "x" : 2.714682996259171, - "y" : -2.0565483502198276 + "x" : 2.5995911144294936, + "y" : -2.035667171419856 }, "rotation" : { - "radians" : 3.1232054735668577 + "radians" : -3.0535437643496612 } }, - "curvature" : 0.21128447059284522 + "curvature" : 0.215681980996833 }, { - "time" : 1.2383947736707137, - "velocity" : 2.7440124761729954, - "acceleration" : -3.9999999999999933, + "time" : 1.2843497892040483, + "velocity" : 2.826664384901926, + "acceleration" : -3.9999999999999942, "pose" : { "translation" : { - "x" : 2.647109857656554, - "y" : -2.0558076157951897 + "x" : 2.5200141856037135, + "y" : -2.04340697732448 }, "rotation" : { - "radians" : 3.1383510108008927 + "radians" : -3.035430580772711 } }, - "curvature" : 0.23741461164036604 + "curvature" : 0.2379825635975536 }, { - "time" : 1.263030112690642, - "velocity" : 2.645471120093282, - "acceleration" : -4.000000000000007, + "time" : 1.3125427756927424, + "velocity" : 2.7138924389471497, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.580724792017243, - "y" : -2.056135986190064 + "x" : 2.4424346061752615, + "y" : -2.052429014375921 }, "rotation" : { - "radians" : -3.1281423784148634 + "radians" : -3.0158761347705023 } }, - "curvature" : 0.26596443997087926 + "curvature" : 0.2634035923974734 }, { - "time" : 1.288125803683672, - "velocity" : 2.5450883561211617, - "acceleration" : -3.999999999999993, + "time" : 1.3411967497233397, + "velocity" : 2.5992765428247604, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.515610852776976, - "y" : -2.057597454609555 + "x" : 2.3670167723685154, + "y" : -2.0627568415111455 }, "rotation" : { - "radians" : -3.1098224705430115 + "radians" : -2.9947525137401847 } }, - "curvature" : 0.29714723181388075 + "curvature" : 0.29232743281102963 }, { - "time" : 1.313716123378181, - "velocity" : 2.4427270773431253, - "acceleration" : -4.0000000000000036, + "time" : 1.3703306161997593, + "velocity" : 2.482741076919082, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.451846182862937, - "y" : -2.0602518783780397 + "x" : 2.2939100604434017, + "y" : -2.074408554736916 }, "rotation" : { - "radians" : -3.08979235872868 + "radians" : -2.971927682130507 } }, - "curvature" : 0.33115248556629073 + "curvature" : 0.325138704477899 }, { - "time" : 1.3665755526787362, - "velocity" : 2.2312893601409045, - "acceleration" : -3.7762049239471107, + "time" : 1.3999769283633923, + "velocity" : 2.36415582826455, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.3286491653547543, - "y" : -2.069355579662364 + "x" : 2.2232474317945616, + "y" : -2.0873964367016242 }, "rotation" : { - "radians" : -3.044285665635533 + "radians" : -2.947269113517704 } }, - "curvature" : 0.40814258856204794 + "curvature" : 0.3621944652381181 }, { - "time" : 1.4220168387612644, - "velocity" : 2.021931702646101, - "acceleration" : -3.01178119185259, + "time" : 1.4301864235012918, + "velocity" : 2.2433178477129516, + "acceleration" : -3.8425061187661265, "pose" : { "translation" : { - "x" : 2.211638475515656, - "y" : -2.083825737131292 + "x" : 2.1551440380505156, + "y" : -2.1017266062671247 }, "rotation" : { - "radians" : -2.9910714005419226 + "radians" : -2.9206492753984246 } }, - "curvature" : 0.49703931266591117 + "curvature" : 0.40377746810107285 }, { - "time" : 1.4800423556994062, - "velocity" : 1.8471715420842816, - "acceleration" : -2.332000316761054, + "time" : 1.4609990554339194, + "velocity" : 2.1249201209765416, + "acceleration" : -3.5248780323742457, "pose" : { "translation" : { - "x" : 2.101202896434394, - "y" : -2.1039453603839817 + "x" : 2.0896958261728287, + "y" : -2.1173986680805683 }, "rotation" : { - "radians" : -2.929853651016197 + "radians" : -2.8919535310937237 } }, - "curvature" : 0.5955376808428134 + "curvature" : 0.45002691208795453 }, { - "time" : 1.5401431171375435, - "velocity" : 1.7070165473729648, - "acceleration" : -1.6920514012421186, + "time" : 1.4923979532320715, + "velocity" : 2.0142428358870714, + "acceleration" : -3.197919951626316, "pose" : { "translation" : { - "x" : 1.9975965658668429, - "y" : -2.1298870898317546 + "x" : 2.026978143555276, + "y" : -2.1344053621462358 }, "rotation" : { - "radians" : -2.860828817960625 + "radians" : -2.861091073527203 } }, - "curvature" : 0.6973458833437982 + "curvature" : 0.5008412010105917 }, { - "time" : 1.601639333661775, - "velocity" : 1.6029617880320504, - "acceleration" : -1.0471995948613984, + "time" : 1.5243220106712716, + "velocity" : 1.9121522556653892, + "acceleration" : -2.6890710106994757, "pose" : { "translation" : { - "x" : 1.9009202838373298, - "y" : -2.161698464999354 + "x" : 1.967044343123007, + "y" : -2.152732213397371 }, "rotation" : { - "radians" : -2.784982844676026 + "radians" : -2.8280094739823887 } }, - "curvature" : 0.7908194905212341 + "curvature" : 0.555749093954951 }, { - "time" : 1.6636359764771624, - "velocity" : 1.5380389287930096, - "acceleration" : -0.3329668372610245, + "time" : 1.5892933232552724, + "velocity" : 1.7374397824686587, + "acceleration" : -1.9497522202134734, "pose" : { "translation" : { - "x" : 1.8111028202399666, - "y" : -2.1992871928262048 + "x" : 1.855623458766786, + "y" : -2.1932503092648403 }, "rotation" : { - "radians" : -2.7043823448281255 + "radians" : -2.7552864354535944 } }, - "curvature" : 0.8589918589263511 + "curvature" : 0.6731381127503112 }, { - "time" : 1.7249832824094171, - "velocity" : 1.5176123103622623, - "acceleration" : 0.579231551390755, + "time" : 1.655072303925798, + "velocity" : 1.6091870688629224, + "acceleration" : -1.0749318224543505, "pose" : { "translation" : { - "x" : 1.7278822224399804, - "y" : -2.2424064159676718 + "x" : 1.7553671009011396, + "y" : -2.2386795374578696 }, "rotation" : { - "radians" : -2.622338159512215 + "radians" : -2.674931532448897 } }, - "curvature" : 0.8822710361525333 + "curvature" : 0.7847126172556117 }, { - "time" : 1.7842365157589062, - "velocity" : 1.5519336526402052, - "acceleration" : -1.7698952228100826, + "time" : 1.720231550281118, + "velocity" : 1.539145321428446, + "acceleration" : -1.1921125316242216, "pose" : { "translation" : { - "x" : 1.6507871228750446, - "y" : -2.2906399810963194 + "x" : 1.6657680121716112, + "y" : -2.288608611212112 }, "rotation" : { - "radians" : -2.543313789762975 + "radians" : -2.5902033764696597 } }, - "curvature" : 0.8436793169541906 + "curvature" : 0.8577573547258329 }, { - "time" : 1.8435847320921743, - "velocity" : 1.4468935280696547, + "time" : 1.784455858835107, + "velocity" : 1.4625827183663351, "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 1.5791180466566104, - "y" : -2.3433877072031706 + "x" : 1.5858331132455987, + "y" : -2.342477161522652 }, "rotation" : { - "radians" : -2.4725497583473715 + "radians" : -2.507140034918934 } }, - "curvature" : 0.7339789593751636 + "curvature" : 0.8472000723919605 }, { - "time" : 1.9104150039170715, - "velocity" : 1.1795724407700658, + "time" : 1.853732644359764, + "velocity" : 1.1854755762677067, "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 1.5119287191712374, - "y" : -2.399850653898966 + "x" : 1.5140388659856399, + "y" : -2.3995645234426775 }, "rotation" : { - "radians" : -2.415547341031068 + "radians" : -2.43458289692862 } }, - "curvature" : 0.5530196676431328 + "curvature" : 0.7096157054787944 }, { - "time" : 1.9969535534435008, - "velocity" : 0.8334182426643494, + "time" : 1.9414753768133155, + "velocity" : 0.8345046464535002, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 1.448007373681925, - "y" : -2.4590163897154227 + "x" : 1.4482866366226972, + "y" : -2.4589785223821536 }, "rotation" : { - "radians" : -2.377658907716917 + "radians" : -2.383232861630834 } }, - "curvature" : 0.3063025968420864 + "curvature" : 0.422926581935277 }, { - "time" : 2.205308114109588, + "time" : 2.1501015384266906, "velocity" : 0.0, "acceleration" : -4.0, "pose" : { @@ -1459,8 +1431,8 @@ "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 5.218406677246094, -3.944063901901245, 0.0 ], - "y" : [ -2.2181358337402344, -0.02777838706970215, 0.0 ] + "x" : [ 5.218406677246094, -1.5258679389953613, 0.0 ], + "y" : [ -2.2181358337402344, 0.3001227378845215, 0.0 ] }, { "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ], "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ] @@ -1471,7 +1443,7 @@ "radians" : -2.3736477827122884 } }, { - "time" : 2.1199999526143074, + "time" : 2.059999953955412, "rotation" : { "radians" : -2.3824463294692526 } @@ -1527,7 +1499,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.000000000000001, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { "x" : 1.3858580589294434, @@ -1539,245 +1511,343 @@ }, "curvature" : 0.0 }, { - "time" : 0.24972925414190453, - "velocity" : 0.9989170165676183, - "acceleration" : 3.9999999999999996, + "time" : 0.2506613508834166, + "velocity" : 1.0026454035336663, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 1.5105871258381285, - "y" : -2.51993284172886 + "x" : 1.5115202584765939, + "y" : -2.5197252612570082 }, "rotation" : { - "radians" : -0.006712112645976874 + "radians" : -0.0018522539500240682 } }, - "curvature" : -0.10052582439878316 + "curvature" : -0.026842794063227322 }, { - "time" : 0.3521794641124606, - "velocity" : 1.4087178564498426, + "time" : 0.30789720119155506, + "velocity" : 1.23158880476622, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 1.5754591548339363, + "y" : -2.519904416002504 + }, + "rotation" : { + "radians" : -0.003832966982418239 + } + }, + "curvature" : -0.03448311037462077 + }, { + "time" : 0.35684276742474663, + "velocity" : 1.4273710696989863, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 1.6405304865329526, + "y" : -2.520230419613654 + }, + "rotation" : { + "radians" : -0.006233177930525208 + } + }, + "curvature" : -0.03877108765930074 + }, { + "time" : 0.40065288496208495, + "velocity" : 1.6026115398483394, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 1.7069005447805807, + "y" : -2.5207311531334256 + }, + "rotation" : { + "radians" : -0.008873618264835237 + } + }, + "curvature" : -0.04040754941441055 + }, { + "time" : 0.44090317601445284, + "velocity" : 1.7636127040578107, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 1.633905754708394, - "y" : -2.521704127531848 + "x" : 1.7746427433321514, + "y" : -2.5214250950984933 }, "rotation" : { - "radians" : -0.023287541793374517 + "radians" : -0.011609841654107651 } }, - "curvature" : -0.16257509326920988 + "curvature" : -0.04010473649023733 }, { - "time" : 0.4296391530021032, - "velocity" : 1.718556612008413, - "acceleration" : 4.000000000000002, + "time" : 0.4784875189105668, + "velocity" : 1.9139500756422665, + "acceleration" : 3.9999999999999907, "pose" : { "translation" : { - "x" : 1.7549551743861684, - "y" : -2.5258047027587054 + "x" : 1.843746311953339, + "y" : -2.522322105081429 }, "rotation" : { - "radians" : -0.04499886535467417 + "radians" : -0.014331201202338068 } }, - "curvature" : -0.19036970718931495 + "curvature" : -0.03847972749251971 }, { - "time" : 0.49386749238771455, - "velocity" : 1.9754699695508586, - "acceleration" : 4.000000000000002, + "time" : 0.5139493740375787, + "velocity" : 2.055797496150314, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 1.9141249898821115, + "y" : -2.5234242072328925 + }, + "rotation" : { + "radians" : -0.016956454931120607 + } + }, + "curvature" : -0.03601472391776367 + }, { + "time" : 0.5476314929124355, + "velocity" : 2.190525971649741, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 1.985625719290681, + "y" : -2.5247263738238246 + }, + "rotation" : { + "radians" : -0.019428093132643293 + } + }, + "curvature" : -0.03305737128049612 + }, { + "time" : 0.5797531676739855, + "velocity" : 2.3190126706959413, + "acceleration" : 4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.0580373387474538, + "y" : -2.5262173087876363 + }, + "rotation" : { + "radians" : -0.021706661066398916 + } + }, + "curvature" : -0.02983965013490699 + }, { + "time" : 0.6104540649254343, + "velocity" : 2.4418162597017363, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 1.8733975745271891, - "y" : -2.5324842440895736 + "x" : 2.131099276678981, + "y" : -2.527880231262401 }, "rotation" : { - "radians" : -0.06754653878366729 + "radians" : -0.023765711637107785 } }, - "curvature" : -0.18401812984610977 + "curvature" : -0.026501856917817767 }, { - "time" : 0.5495862121288385, - "velocity" : 2.1983448485153545, - "acceleration" : 3.9999999999999925, + "time" : 0.6398209489868676, + "velocity" : 2.5592837959474695, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 1.989327162226573, - "y" : -2.5415022642969234 + "x" : 2.2045102448319085, + "y" : -2.529693659133045 }, "rotation" : { - "radians" : -0.08694366158642766 + "radians" : -0.025587608770040265 } }, - "curvature" : -0.14408315169979258 + "curvature" : -0.02311472481731095 }, { - "time" : 0.5993533211792574, - "velocity" : 2.3974132847170297, - "acceleration" : 3.999999999999988, + "time" : 0.6679049204495552, + "velocity" : 2.67161968179822, + "acceleration" : 2.6312398843848555, "pose" : { "translation" : { - "x" : 2.1031812186483876, - "y" : -2.5522348566009896 + "x" : 2.277936931734928, + "y" : -2.5316321925735394 }, "rotation" : { - "radians" : -0.09970719732043364 + "radians" : -0.027160172194753095 } }, - "curvature" : -0.07464450875370457 + "curvature" : -0.019696900519105796 }, { - "time" : 0.6447906242370012, - "velocity" : 2.5791624969480043, - "acceleration" : 3.999999999999996, + "time" : 0.6949126774005732, + "velocity" : 2.7426835690755107, + "acceleration" : -3.999999999999994, "pose" : { "translation" : { - "x" : 2.215651155655223, - "y" : -2.563781439025206 + "x" : 2.351022696160726, + "y" : -2.5336672975890906 }, "rotation" : { - "radians" : -0.10320311301994016 + "radians" : -0.02847404834663094 } }, - "curvature" : 0.015359194352358661 + "curvature" : -0.01622720956565302 }, { - "time" : 0.6870299154317897, - "velocity" : 2.7481196617271584, - "acceleration" : -3.222032479349847, + "time" : 0.7218404114156483, + "velocity" : 2.6349726330152103, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.327593572437763, - "y" : -2.5750714987516403 + "x" : 2.4233962605879356, + "y" : -2.5357680895583314 }, "rotation" : { - "radians" : -0.09599875439359641 + "radians" : -0.02952065953655597 } }, - "curvature" : 0.11269067821063196 + "curvature" : -0.012652107561005304 }, { - "time" : 0.7291079502753489, - "velocity" : 2.612542866793996, - "acceleration" : -3.999999999999996, + "time" : 0.7494857099529498, + "velocity" : 2.524391438866004, + "acceleration" : -3.999999999999994, "pose" : { "translation" : { - "x" : 2.439941312144356, - "y" : -2.5849713364764284 + "x" : 2.494680404663086, + "y" : -2.5379021167755127 }, "rotation" : { - "radians" : -0.0780986159927982 + "radians" : -0.030290590291302242 } }, - "curvature" : 0.20204168868166544 + "curvature" : -0.008889004344138316 }, { - "time" : 0.7742726789750473, - "velocity" : 2.4318839519952027, + "time" : 0.7777916661649981, + "velocity" : 2.411167614017811, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.553614518510585, - "y" : -2.5923908107652096 + "x" : 2.5645006586625527, + "y" : -2.540036143992694 + }, + "rotation" : { + "radians" : -0.030772296527561225 + } + }, + "curvature" : -0.004826154357885299 + }, { + "time" : 0.8066975299375645, + "velocity" : 2.2955441589275454, + "acceleration" : -4.000000000000007, + "pose" : { + "translation" : { + "x" : 2.6324939969545085, + "y" : -2.542136935961935 }, "rotation" : { - "radians" : -0.05097712062186278 + "radians" : -0.030951061958126948 } }, - "curvature" : 0.26929228317845294 + "curvature" : -3.197882350940787E-4 }, { - "time" : 0.8239555505599717, - "velocity" : 2.233152465655505, + "time" : 0.8361410171738428, + "velocity" : 2.177770209982432, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.6694316924888426, - "y" : -2.5963900824085613 + "x" : 2.698317531460873, + "y" : -2.544172040977486 }, "rotation" : { - "radians" : -0.017372193426654598 + "radians" : -0.030808177370938297 } }, - "curvature" : 0.3047342069576041 + "curvature" : 0.004810664209664993 }, { - "time" : 0.8798582869576448, - "velocity" : 2.0095415200648126, - "acceleration" : -4.000000000000002, + "time" : 0.8660614348369912, + "velocity" : 2.0580885393298387, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { - "x" : 2.788020748877898, - "y" : -2.5962863587774336 + "x" : 2.7616572051192634, + "y" : -2.5461105744179804 }, "rotation" : { - "radians" : 0.019122127471802145 + "radians" : -0.030320390272959595 } }, - "curvature" : 0.3048495236191097 + "curvature" : 0.010787761573423147 }, { - "time" : 0.9446429920459655, - "velocity" : 1.7504026997115296, + "time" : 0.9271262518256638, + "velocity" : 1.8138292713751483, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.9097300729524704, - "y" : -2.5917606381785845 + "x" : 2.8798250574927806, + "y" : -2.549586924763389 }, "rotation" : { - "radians" : 0.05453524825539118 + "radians" : -0.02819439651168064 } }, - "curvature" : 0.2715879998238006 + "curvature" : 0.02638800187200745 }, { - "time" : 1.0231687323056105, - "velocity" : 1.43629973867295, - "acceleration" : -3.9999999999999947, + "time" : 0.989659585589217, + "velocity" : 1.5636959363209353, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 3.0345395770928008, - "y" : -2.582964454210014 + "x" : 2.9853920694440603, + "y" : -2.552380026318133 }, "rotation" : { - "radians" : 0.08488478604076904 + "radians" : -0.02431565149909576 } }, - "curvature" : 0.20927822730800172 + "curvature" : 0.04885357015887642 }, { - "time" : 1.0706615087143077, - "velocity" : 1.2463286330381615, + "time" : 1.054053992446091, + "velocity" : 1.3061183088934392, "acceleration" : -4.000000000000002, "pose" : { "translation" : { - "x" : 3.0979779765917996, - "y" : -2.577167223823878 + "x" : 3.077770434158083, + "y" : -2.554379138452532 }, "rotation" : { - "radians" : 0.09693981571821227 + "radians" : -0.018496813941660972 } }, - "curvature" : 0.16817339591118868 + "curvature" : 0.07878050827674199 }, { - "time" : 1.1274499043473336, - "velocity" : 1.0191750505060577, + "time" : 1.1223816921391718, + "velocity" : 1.0328075101211158, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 3.1619717574142214, - "y" : -2.5706266201164 + "x" : 3.1576682134182192, + "y" : -2.5555738139373716 }, "rotation" : { - "radians" : 0.10625319207386785 + "radians" : -0.01102726873603195 } }, - "curvature" : 0.12027802306933545 + "curvature" : 0.10654928830990536 }, { - "time" : 1.2018998880559773, - "velocity" : 0.7213751156714835, + "time" : 1.2022085804462013, + "velocity" : 0.7134999568929983, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 3.22637516801138, - "y" : -2.5635414574337716 + "x" : 3.2273675283886405, + "y" : -2.556078972294017 }, "rotation" : { - "radians" : 0.11229090993712704 + "radians" : -0.003571825888394278 } }, - "curvature" : 0.06477002018967379 + "curvature" : 0.09616404801082394 }, { - "time" : 1.382243666973848, + "time" : 1.3805835696694508, "velocity" : 0.0, "acceleration" : -4.0, "pose" : { @@ -1786,7 +1856,7 @@ "y" : -2.5561599731445312 }, "rotation" : { - "radians" : 0.1144532134762521 + "radians" : 0.0 } }, "curvature" : 0.0 @@ -1795,8 +1865,8 @@ "x" : [ 1.3858580589294434, 2.0, 0.0 ], "y" : [ -2.519644260406494, 0.0, 0.0 ] }, { - "x" : [ 3.2910027503967285, 2.0693557262420654, 0.0 ], - "y" : [ -2.5561599731445312, 0.2378840446472168, 0.0 ] + "x" : [ 3.2910027503967285, 1.0, 0.0 ], + "y" : [ -2.5561599731445312, 0.0, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -1817,7 +1887,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 20\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF", + "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE", "closed" : false, "valid" : true, "sendableScript" : { @@ -1825,7 +1895,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "20" ], + "args" : [ "8" ], "argTypes" : [ "double" ], "reflection" : true }, { @@ -1838,7 +1908,809 @@ "args" : [ "OFF" ], "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true } ] } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 3.9999999999999996, + "pose" : { + "translation" : { + "x" : 3.2910027503967285, + "y" : -2.5561599731445312 + }, + "rotation" : { + "radians" : -0.2884106402522736 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2373794954493144, + "velocity" : 0.9495179817972574, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 3.3989983545955056, + "y" : -2.588374874732608 + }, + "rotation" : { + "radians" : -0.2921778608912438 + } + }, + "curvature" : -0.04364084615692234 + }, { + "time" : 0.29579824973185176, + "velocity" : 1.1831929989274066, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.458631632620488, + "y" : -2.606390396730127 + }, + "rotation" : { + "radians" : -0.29440901549838794 + } + }, + "curvature" : -0.02595134837823401 + }, { + "time" : 0.3491550793551233, + "velocity" : 1.3966203174204928, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.5244835324483574, + "y" : -2.6264017262656125 + }, + "rotation" : { + "radians" : -0.2953398995873586 + } + }, + "curvature" : -0.0013985693216967878 + }, { + "time" : 0.40043149369700565, + "velocity" : 1.6017259747880221, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 3.5980331007389807, + "y" : -2.6487579275583 + }, + "rotation" : { + "radians" : -0.2945501713020244 + } + }, + "curvature" : 0.020718850966936625 + }, { + "time" : 0.45101233132563245, + "velocity" : 1.8040493253025296, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.680485970962536, + "y" : -2.6736677892761236 + }, + "rotation" : { + "radians" : -0.2919955872089119 + } + }, + "curvature" : 0.0373242313505917 + }, { + "time" : 0.5015733373341933, + "velocity" : 2.0062933493367727, + "acceleration" : 3.9999999999999876, + "pose" : { + "translation" : { + "x" : 3.7727913068944403, + "y" : -2.70121209879796 + }, + "rotation" : { + "radians" : -0.2878084836673059 + } + }, + "curvature" : 0.04861193460092709 + }, { + "time" : 0.5524240768131252, + "velocity" : 2.2096963072525, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.8756587461102754, + "y" : -2.731355916475877 + }, + "rotation" : { + "radians" : -0.28217372185403494 + } + }, + "curvature" : 0.055832145694026246 + }, { + "time" : 0.6036702471548304, + "velocity" : 2.414680988619321, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.989575343480716, + "y" : -2.76396084989738 + }, + "rotation" : { + "radians" : -0.2752690347022763 + } + }, + "curvature" : 0.06027032950217638 + }, { + "time" : 0.6294411364762771, + "velocity" : 2.5177645459051075, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 4.050772588350831, + "y" : -2.7811177201369857 + }, + "rotation" : { + "radians" : -0.271388278211743 + } + }, + "curvature" : 0.06177303417946446 + }, { + "time" : 0.6553004906081608, + "velocity" : 2.621201962432642, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 4.114822514666457, + "y" : -2.798797328147657 + }, + "rotation" : { + "radians" : -0.26724328091034977 + } + }, + "curvature" : 0.06293282064413533 + }, { + "time" : 0.6812367197554138, + "velocity" : 2.7249468790216542, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.181730948050758, + "y" : -2.816958744670205 + }, + "rotation" : { + "radians" : -0.26284758822831167 + } + }, + "curvature" : 0.06382850159969175 + }, { + "time" : 0.7072359906843115, + "velocity" : 2.8289439627372452, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.25149297961314, + "y" : -2.835556876071827 + }, + "rotation" : { + "radians" : -0.25821295054670484 + } + }, + "curvature" : 0.06452392825617205 + }, { + "time" : 0.7332828563292066, + "velocity" : 2.9331314253168257, + "acceleration" : 2.5481050539821153, + "pose" : { + "translation" : { + "x" : 4.324093495433467, + "y" : -2.854542847916802 + }, + "rotation" : { + "radians" : -0.253349516956691 + } + }, + "curvature" : 0.06507034108588115 + }, { + "time" : 0.759525327996014, + "velocity" : 3.0, + "acceleration" : -1.1017744707610282E-14, + "pose" : { + "translation" : { + "x" : 4.399507706046279, + "y" : -2.8738643885371857 + }, + "rotation" : { + "radians" : -0.24826603822981036 + } + }, + "curvature" : 0.06550851193026688 + }, { + "time" : 0.7863964782259588, + "velocity" : 2.9999999999999996, + "acceleration" : 1.0659361723935359E-14, + "pose" : { + "translation" : { + "x" : 4.477701675925007, + "y" : -2.8934662126035047 + }, + "rotation" : { + "radians" : -0.2429700686181491 + } + }, + "curvature" : 0.06587060593952473 + }, { + "time" : 0.8141710749514438, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.558632852966191, + "y" : -2.913290404695452 + }, + "rotation" : { + "radians" : -0.23746816090539483 + } + }, + "curvature" : 0.06618175066974573 + }, { + "time" : 0.8428287933072273, + "velocity" : 3.0, + "acceleration" : -1.0029761020365324E-14, + "pose" : { + "translation" : { + "x" : 4.642250597973694, + "y" : -2.9332768028725846 + }, + "rotation" : { + "radians" : -0.23176605224269178 + } + }, + "curvature" : 0.06646132969406916 + }, { + "time" : 0.8723468917576519, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.728496714142921, + "y" : -2.9533633822450156 + }, + "rotation" : { + "radians" : -0.2258688402689826 + } + }, + "curvature" : 0.06672403127896764 + }, { + "time" : 0.9027004208083538, + "velocity" : 2.9999999999999996, + "acceleration" : 9.500654106330315E-15, + "pose" : { + "translation" : { + "x" : 4.817305976545033, + "y" : -2.9734866385441103 + }, + "rotation" : { + "radians" : -0.21978115021622427 + } + }, + "curvature" : 0.0669806864818383 + }, { + "time" : 0.9338624302784291, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.9086066616111665, + "y" : -2.9935819716931817 + }, + "rotation" : { + "radians" : -0.21350729439917845 + } + }, + "curvature" : 0.06723892993087238 + }, { + "time" : 0.9658041749806907, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.002321076616648, + "y" : -3.0135840693781866 + }, + "rotation" : { + "radians" : -0.20705142586622954 + } + }, + "curvature" : 0.06750371317272791 + }, { + "time" : 0.9984953186414615, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.098366089165211, + "y" : -3.0334272906184196 + }, + "rotation" : { + "radians" : -0.20041768816116357 + } + }, + "curvature" : 0.0677776962888439 + }, { + "time" : 1.0319041358746839, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.19665365667321, + "y" : -3.0530460493372082 + }, + "rotation" : { + "radians" : -0.1936103631941936 + } + }, + "curvature" : 0.06806153927030212 + }, { + "time" : 1.0659977120081605, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.297091355853844, + "y" : -3.072375197932608 + }, + "rotation" : { + "radians" : -0.18663401919376696 + } + }, + "curvature" : 0.0683541107864204 + }, { + "time" : 1.1007421405426991, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.399582912201364, + "y" : -3.0913504108481 + }, + "rotation" : { + "radians" : -0.17949366063978456 + } + }, + "curvature" : 0.0686526286409456 + }, { + "time" : 1.136102718008151, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.504028729475294, + "y" : -3.1099085681432825 + }, + "rotation" : { + "radians" : -0.17219488198191107 + } + }, + "curvature" : 0.06895274341837182 + }, { + "time" : 1.1720441359642337, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.61032641918465, + "y" : -3.127988139064569 + }, + "rotation" : { + "radians" : -0.16474402683304648 + } + }, + "curvature" : 0.06924857455843654 + }, { + "time" : 1.2085306698791871, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.718371330072152, + "y" : -3.145529565615881 + }, + "rotation" : { + "radians" : -0.15714835420132006 + } + }, + "curvature" : 0.06953270631102976 + }, { + "time" : 1.2455263646064414, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.828057077598442, + "y" : -3.162475646129346 + }, + "rotation" : { + "radians" : -0.149416213183665 + } + }, + "curvature" : 0.06979614966205201 + }, { + "time" : 1.2829952161694935, + "velocity" : 3.0, + "acceleration" : -7.810331647113909E-15, + "pose" : { + "translation" : { + "x" : 5.939276073426299, + "y" : -3.1787719188359915 + }, + "rotation" : { + "radians" : -0.1415572273868729 + } + }, + "curvature" : 0.07002827533191112 + }, { + "time" : 1.3209013495592037, + "velocity" : 2.9999999999999996, + "acceleration" : 7.728429806013464E-15, + "pose" : { + "translation" : { + "x" : 6.051920054904861, + "y" : -3.1943670454364383 + }, + "rotation" : { + "radians" : -0.13358249016273918 + } + }, + "curvature" : 0.07021672228872737 + }, { + "time" : 1.359209192247121, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.165880614553835, + "y" : -3.209213194671598 + }, + "rotation" : { + "radians" : -0.12550477153574247 + } + }, + "curvature" : 0.07034728585337109 + }, { + "time" : 1.397883643124852, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.281049729547716, + "y" : -3.2232664258933683 + }, + "rotation" : { + "radians" : -0.11733873745676503 + } + }, + "curvature" : 0.07040378937764218 + }, { + "time" : 1.4368902365948655, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.397320291200003, + "y" : -3.2364870726353274 + }, + "rotation" : { + "radians" : -0.10910118172585662 + } + }, + "curvature" : 0.0703679436328688 + }, { + "time" : 1.4761953015647518, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.514586634447419, + "y" : -3.2488401261834285 + }, + "rotation" : { + "radians" : -0.10081127058141126 + } + }, + "curvature" : 0.0702191984436713 + }, { + "time" : 1.515766115137486, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.63274506733412, + "y" : -3.260295619146696 + }, + "rotation" : { + "radians" : -0.0924907995421401 + } + }, + "curvature" : 0.06993459173630182 + }, { + "time" : 1.55557105084767, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.7516944004959205, + "y" : -3.2708290090279206 + }, + "rotation" : { + "radians" : -0.08416446160106997 + } + }, + "curvature" : 0.06948860204346692 + }, { + "time" : 1.5955797213714447, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.871336476644501, + "y" : -3.2804215617943555 + }, + "rotation" : { + "radians" : -0.07586012529605445 + } + }, + "curvature" : 0.06885301162269934 + }, { + "time" : 1.6357631157394636, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.991576700051632, + "y" : -3.289060735448409 + }, + "rotation" : { + "radians" : -0.06760912050688539 + } + }, + "curvature" : 0.06799678871171229 + }, { + "time" : 1.6760937312120248, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 7.1123245660333865, + "y" : -3.296740563598341 + }, + "rotation" : { + "radians" : -0.05944652904214673 + } + }, + "curvature" : 0.06688599907381494 + }, { + "time" : 1.7165457001373738, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 7.233494190434357, + "y" : -3.30346203902896 + }, + "rotation" : { + "radians" : -0.05141147616548517 + } + }, + "curvature" : 0.06548375889504356 + }, { + "time" : 1.757094912312602, + "velocity" : 3.0, + "acceleration" : -7.287757495654728E-15, + "pose" : { + "translation" : { + "x" : 7.355004839111871, + "y" : -3.3092334972723165 + }, + "rotation" : { + "radians" : -0.043547418155515716 + } + }, + "curvature" : 0.06375024330143432 + }, { + "time" : 1.7977191336056264, + "velocity" : 2.9999999999999996, + "acceleration" : 7.27794583372083E-15, + "pose" : { + "translation" : { + "x" : 7.476781457420211, + "y" : -3.314071000178397 + }, + "rotation" : { + "radians" : -0.03590241977860944 + } + }, + "curvature" : 0.061642767292292996 + }, { + "time" : 1.8383981218802636, + "velocity" : 3.0, + "acceleration" : -0.8549861225542413, + "pose" : { + "translation" : { + "x" : 7.598755199694828, + "y" : -3.3179987194858214 + }, + "rotation" : { + "radians" : -0.02852941415912364 + } + }, + "curvature" : 0.059115958757323624 + }, { + "time" : 1.8793527503450278, + "velocity" : 2.964984361008262, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 7.720863958736559, + "y" : -3.3210493203925378 + }, + "rotation" : { + "radians" : -0.021486435933739608 + } + }, + "curvature" : 0.05612204648295773 + }, { + "time" : 1.9217846589717384, + "velocity" : 2.7952567265014197, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 7.843052895295841, + "y" : -3.3232643451265176 + }, + "rotation" : { + "radians" : -0.014836816748393312 + } + }, + "curvature" : 0.05261128966471575 + }, { + "time" : 1.966973534915382, + "velocity" : 2.6145012227268447, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 7.965274967556932, + "y" : -3.3246945965164496 + }, + "rotation" : { + "radians" : -0.008649330067547475 + } + }, + "curvature" : 0.048532579406757856 + }, { + "time" : 2.0155229987855723, + "velocity" : 2.4203033672460843, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 8.087491460622125, + "y" : -3.325400521562436 + }, + "rotation" : { + "radians" : -0.0029982698845735147 + } + }, + "curvature" : 0.04383424692658223 + }, { + "time" : 2.0683070326652815, + "velocity" : 2.2091672317272475, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 8.209672515995964, + "y" : -3.3254525950066887 + }, + "rotation" : { + "radians" : 0.0020365547800811692 + } + }, + "curvature" : 0.03846511750567603 + }, { + "time" : 2.126672618228889, + "velocity" : 1.9757048894728169, + "acceleration" : -3.9999999999999982, + "pose" : { + "translation" : { + "x" : 8.331797661069459, + "y" : -3.324931702904223 + }, + "rotation" : { + "radians" : 0.006369930747651973 + } + }, + "curvature" : 0.032375853273367367 + }, { + "time" : 2.19289366493448, + "velocity" : 1.7108207026504523, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 8.453856338604307, + "y" : -3.3239295261935533 + }, + "rotation" : { + "radians" : 0.00991148144607044 + } + }, + "curvature" : 0.025520631044257825 + }, { + "time" : 2.2714115520395897, + "velocity" : 1.3967491542300143, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 8.575848436217107, + "y" : -3.3225489242673873 + }, + "rotation" : { + "radians" : 0.012566065756910601 + } + }, + "curvature" : 0.017859202608647286 + }, { + "time" : 2.373702045251545, + "velocity" : 0.9875871813821927, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 8.697784815863574, + "y" : -3.320904318543324 + }, + "rotation" : { + "radians" : 0.014234366939993203 + } + }, + "curvature" : 0.009359382499354215 + }, { + "time" : 2.620598840597093, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 8.819687843322754, + "y" : -3.319122076034546 + }, + "rotation" : { + "radians" : 0.014813707347434221 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 3.2910027503967285, 1.676727533340454, 0.0 ], + "y" : [ -2.5561599731445312, -0.49745607376098633, 0.0 ] + }, { + "x" : [ 8.819687843322754, 7.801394462585449, 0.0 ], + "y" : [ -3.319122076034546, 0.11557602882385254, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 2.5299999434500933, + "rotation" : { + "radians" : 0.0 + } + } ], + "reversed" : false, + "color" : 88.23529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/red/5ball.json b/src/main/deploy/autos/red/5ball.json index de3ff4dc..4aa2b6c7 100644 --- a/src/main/deploy/autos/red/5ball.json +++ b/src/main/deploy/autos/red/5ball.json @@ -257,445 +257,459 @@ "y" : -3.196845054626465 }, "rotation" : { - "radians" : 2.70041390445855 + "radians" : -3.128825218368557 } }, - "curvature" : -0.0 + "curvature" : 0.0 }, { - "time" : 0.23678198316332957, - "velocity" : 0.9471279326533183, - "acceleration" : 3.9999999999999973, + "time" : 0.18074788653672832, + "velocity" : 0.7229915461469133, + "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 7.5253763288478694, - "y" : -3.1485883100490355 + "x" : 7.5612559258704835, + "y" : -3.197459243636949 }, "rotation" : { - "radians" : 2.6900449899598926 + "radians" : -3.138740759458168 } }, - "curvature" : -0.15715157236253124 + "curvature" : -0.29172803877514997 }, { - "time" : 0.3409986064846093, - "velocity" : 1.363994425938437, - "acceleration" : 4.000000000000002, + "time" : 0.25626538315297653, + "velocity" : 1.0250615326119064, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 7.417564729417791, - "y" : -3.094925222234451 + "x" : 7.495254533242587, + "y" : -3.196840137823301 }, "rotation" : { - "radians" : 2.6692426474617976 + "radians" : 3.1176073408097276 } }, - "curvature" : -0.16963195910628098 + "curvature" : -0.5074451761358538 }, { - "time" : 0.3857824438824356, - "velocity" : 1.5431297755297422, - "acceleration" : 3.9999999999999933, + "time" : 0.3151289706110068, + "velocity" : 1.2605158824440272, + "acceleration" : 4.000000000000004, "pose" : { "translation" : { - "x" : 7.359754048377923, - "y" : -3.065001553835387 + "x" : 7.428047492778063, + "y" : -3.1939619357255324 }, "rotation" : { - "radians" : 2.658923020471842 + "radians" : 3.0784656278152482 } }, - "curvature" : -0.14581550944396268 + "curvature" : -0.6425731706470493 }, { - "time" : 0.4281176659650556, - "velocity" : 1.712470663860222, - "acceleration" : 4.0, + "time" : 0.3658526486292684, + "velocity" : 1.463410594517074, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 7.298863693187286, - "y" : -3.0327307414841016 + "x" : 7.3592204221349675, + "y" : -3.1880118377084727 }, "rotation" : { - "radians" : 2.6499291206925197 + "radians" : 3.0315419813222593 } }, - "curvature" : -0.11488482457954703 + "curvature" : -0.7042822762242509 }, { - "time" : 0.4687787361650137, - "velocity" : 1.8751149446600544, - "acceleration" : 4.000000000000009, + "time" : 0.4117606980978159, + "velocity" : 1.6470427923912636, + "acceleration" : 2.1723907907211393, "pose" : { "translation" : { - "x" : 7.234696646951008, - "y" : -2.998054031586136 + "x" : 7.288476303159229, + "y" : -3.178375355932097 }, "rotation" : { - "radians" : 2.642744382686732 + "radians" : 2.980846049930411 } }, - "curvature" : -0.08249583112954426 + "curvature" : -0.7073398126437707 }, { - "time" : 0.5081929441867301, - "velocity" : 2.0327717767469204, - "acceleration" : 3.9999999999999947, + "time" : 0.4555105966830825, + "velocity" : 1.7420846691728806, + "acceleration" : 1.9989588140063956, "pose" : { "translation" : { - "x" : 7.167170996312052, - "y" : -2.9610228459350765 + "x" : 7.215626262967817, + "y" : -3.1646216243218532 }, "rotation" : { - "radians" : 2.637607535762462 + "radians" : 2.9296129513061087 } }, - "curvature" : -0.05155148354596137 + "curvature" : -0.6695533478285349 }, { - "time" : 0.5465891606616753, - "velocity" : 2.186356642646701, - "acceleration" : 3.9999999999999893, + "time" : 0.4987555436671162, + "velocity" : 1.828529537107854, + "acceleration" : 2.813152489265579, "pose" : { "translation" : { - "x" : 7.096308985695558, - "y" : -2.92178689978671 + "x" : 7.140580355031915, + "y" : -3.146488708538989 }, "rotation" : { - "radians" : 2.6346096400057593 + "radians" : 2.8802105895971133 } }, - "curvature" : -0.023151408617227538 + "curvature" : -0.6077426894646522 }, { - "time" : 0.5840763060621219, - "velocity" : 2.336305224248487, - "acceleration" : 4.0, + "time" : 0.5413750298351414, + "velocity" : 1.948424650712654, + "acceleration" : 3.6356664874107865, "pose" : { "translation" : { - "x" : 7.0222260715531775, - "y" : -2.8805823199331826 + "x" : 7.063338340260088, + "y" : -3.1238689159508795 }, "rotation" : { - "radians" : 2.6337649653389965 + "radians" : 2.834193414995866 } }, - "curvature" : 0.0026288919328956535 + "curvature" : -0.5352497494731134 }, { - "time" : 0.6206887373405835, - "velocity" : 2.4827549493623335, - "acceleration" : 4.0, + "time" : 0.5828078541546968, + "velocity" : 2.0990605815700403, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.945119976607415, - "y" : -2.837719762777155 + "x" : 6.983980468081455, + "y" : -3.0967941056013544 }, "rotation" : { - "radians" : 2.6350579923694193 + "radians" : 2.7924479125830106 } }, - "curvature" : 0.02622781759990982 + "curvature" : -0.46118349636323475 }, { - "time" : 0.6564142795761782, - "velocity" : 2.625657118304712, + "time" : 0.6228085797994793, + "velocity" : 2.259063484149171, "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 6.865259744095965, - "y" : -2.7935725324059604 + "x" : 6.902658257528856, + "y" : -3.0654209981810254 }, "rotation" : { - "radians" : 2.638472613930082 + "radians" : 2.755369864098415 } }, - "curvature" : 0.048307452945012146 + "curvature" : -0.3908757756828451 }, { - "time" : 0.6912124020261066, - "velocity" : 2.764849608104426, - "acceleration" : 2.5754078048514004, + "time" : 0.6614595623941697, + "velocity" : 2.413667414527932, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 6.782974792016049, - "y" : -2.74856469866576 + "x" : 6.819585278322023, + "y" : -3.0300164859976135 }, "rotation" : { - "radians" : 2.644009420306148 + "radians" : 2.7230300588082508 } }, - "curvature" : 0.06960805639848362 + "curvature" : -0.32684828411643785 }, { - "time" : 0.725312002060465, - "velocity" : 2.8526699841752237, - "acceleration" : -4.000000000000009, + "time" : 0.6988907922441839, + "velocity" : 2.563392333927989, + "acceleration" : 3.9999999999999907, "pose" : { "translation" : { - "x" : 6.698643967368753, - "y" : -2.7031592152357007 + "x" : 6.7350279319507536, + "y" : -2.990942942946276 }, "rotation" : { - "radians" : 2.6516955032387415 + "radians" : 2.6953074205444363 } }, - "curvature" : 0.09088679864226243 + "curvature" : -0.26979273017272876 }, { - "time" : 0.7602301806429926, - "velocity" : 2.7129972698451126, - "acceleration" : -4.0, + "time" : 0.735158469971846, + "velocity" : 2.708463044838637, + "acceleration" : 1.279466838075468, "pose" : { "translation" : { - "x" : 6.6126846004033695, - "y" : -2.657846037702072 + "x" : 6.649296232758076, + "y" : -2.9486435344799347 }, "rotation" : { - "radians" : 2.66158969110175 + "radians" : 2.671985460234166 } }, - "curvature" : 0.11290092691377968 + "curvature" : -0.2193389226412271 }, { - "time" : 0.7973483461427653, - "velocity" : 2.564524607846022, - "acceleration" : -3.9999999999999956, + "time" : 0.7708801601749081, + "velocity" : 2.75416776285346, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.5255415588617325, - "y" : -2.6131302416324615 + "x" : 6.562734589023421, + "y" : -2.9036275275796015 }, "rotation" : { - "radians" : 2.6737848783502094 + "radians" : 2.65281690834138 } }, - "curvature" : 0.1364110321450652 + "curvature" : -0.17457123350751053 }, { - "time" : 0.8368127385519012, - "velocity" : 2.4066670382094784, + "time" : 0.8078106139873205, + "velocity" : 2.606445947603811, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.437676302222556, - "y" : -2.5695201406499137 + "x" : 6.4757125840457945, + "y" : -2.8564556007247077 }, "rotation" : { - "radians" : 2.6884081376830955 + "radians" : 2.6375642644084403 } }, - "curvature" : 0.162189062203917 + "curvature" : -0.13433724003134345 }, { - "time" : 0.8788429529768294, - "velocity" : 2.2385461805097657, - "acceleration" : -3.9999999999999956, + "time" : 0.8472976500866943, + "velocity" : 2.4484978032063154, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.349555935945773, - "y" : -2.527515404507085 + "x" : 6.388615757226944, + "y" : -2.80772515386343 }, "rotation" : { - "radians" : 2.7056184913490395 + "radians" : 2.62602361109316 } }, - "curvature" : 0.19101923638415347 + "curvature" : -0.09741150032488063 }, { - "time" : 0.9237789286790171, - "velocity" : 2.0588022777010146, - "acceleration" : -4.0, + "time" : 0.8895957780818, + "velocity" : 2.2793052912258926, + "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.261642265716873, - "y" : -2.487595177160401 + "x" : 6.30183638515453, + "y" : -2.7580556183830183 }, "rotation" : { - "radians" : 2.7256014363493466 + "radians" : 2.618037394000371 } }, - "curvature" : 0.2236791506698763 + "curvature" : -0.06256913374757356 }, { - "time" : 0.9721645748556083, - "velocity" : 1.86525969299465, + "time" : 0.9350786097662884, + "velocity" : 2.097373964487939, "acceleration" : -3.999999999999998, "pose" : { "translation" : { - "x" : 6.1743808516912395, - "y" : -2.4502061948442133 + "x" : 6.215764262685298, + "y" : -2.7080737670801227 }, "rotation" : { - "radians" : 2.748558448602262 + "radians" : 2.6135000159251436 } }, - "curvature" : 0.2608836831433858 + "curvature" : -0.02860941649074867 }, { - "time" : 1.02491166750831, - "velocity" : 1.6542713223838432, + "time" : 0.9843259494632479, + "velocity" : 1.900384605700101, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.088190062738491, - "y" : -2.415750904144957 + "x" : 6.130777484028243, + "y" : -2.6583990241311213 }, "rotation" : { - "radians" : 2.774688654056905 + "radians" : 2.612358417756541 } }, - "curvature" : 0.30316623147805977 + "curvature" : 0.005641950266030671 }, { - "time" : 1.083666813203764, - "velocity" : 1.4192507396020277, - "acceleration" : -3.999999999999996, + "time" : 1.038295625522969, + "velocity" : 1.6845059014612167, + "acceleration" : -4.000000000000003, "pose" : { "translation" : { - "x" : 6.003450130686815, - "y" : -2.384575580075307 + "x" : 6.047233223827789, + "y" : -2.609628775062447 }, "rotation" : { - "radians" : 2.804158659356016 + "radians" : 2.6146083461531995 } }, - "curvature" : 0.3506617141264975 + "curvature" : 0.04132896947061391 }, { - "time" : 1.1518177007752968, - "velocity" : 1.1466471893158965, - "acceleration" : -3.9999999999999987, + "time" : 1.0987120643010575, + "velocity" : 1.4428401463488625, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 5.9654585182469475, + "y" : -2.562323676720915 + }, + "rotation" : { + "radians" : 2.620285612891302 + } + }, + "curvature" : 0.07953607724401594 + }, { + "time" : 1.1691474210025192, + "velocity" : 1.1610987195430156, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 5.92049220456731, - "y" : -2.356958444148333 + "x" : 5.885741046050498, + "y" : -2.5169929672440503 }, "rotation" : { - "radians" : 2.8370553812686623 + "radians" : 2.6294501603955758 } }, - "curvature" : 0.40274814478615856 + "curvature" : 0.12120182277895979 }, { - "time" : 1.2384794981042708, + "time" : 1.259422100888273, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.839587404858321, - "y" : -2.3330977824516594 + "x" : 5.8083199096881515, + "y" : -2.4740797760304147 }, "rotation" : { - "radians" : 2.873316167854496 + "radians" : 2.642159027386621 } }, - "curvature" : 0.4575095057609104 + "curvature" : 0.1669290910240929 }, { - "time" : 1.3399219962409736, + "time" : 1.3656883669621598, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.7609358777297786, - "y" : -2.3131000637216204 + "x" : 5.733376416377723, + "y" : -2.4339464337099344 }, "rotation" : { - "radians" : 2.9126317023886243 + "radians" : 2.6584223838932473 } }, - "curvature" : 0.5110229996682454 + "curvature" : 0.2166266434826088 }, { - "time" : 1.4373809969681062, + "time" : 1.4673170381313625, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.684655849287537, - "y" : -2.2969680574174163 + "x" : 5.661024859188302, + "y" : -2.396859782114227 }, "rotation" : { - "radians" : 2.954321888441968 + "radians" : 2.6781351093523047 } }, - "curvature" : 0.5565765979019635 + "curvature" : 0.2689053696942707 }, { - "time" : 1.5310223088557995, + "time" : 1.5642156155177847, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.610772679817714, - "y" : -2.2845889517952713 + "x" : 5.591303298123421, + "y" : -2.3629764842469285 }, "rotation" : { - "radians" : 2.9971949967281697 + "radians" : 2.700976194613289 } }, - "curvature" : 0.5841027262519725 + "curvature" : 0.3201793855552225 }, { - "time" : 1.621162213950032, + "time" : 1.6564699229560516, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.539207918031025, - "y" : -2.27572247198259 + "x" : 5.5241643412042265, + "y" : -2.332328334254022 }, "rotation" : { - "radians" : 3.039416140548233 + "radians" : 2.726272217468977 } }, - "curvature" : 0.5803251759571693 + "curvature" : 0.36354779991332803 }, { - "time" : 1.7082570400969352, + "time" : 1.7443554806111186, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.469768355307124, - "y" : -2.2699889980521135 + "x" : 5.459465925552649, + "y" : -2.304807567394164 }, "rotation" : { - "radians" : 3.0784289185307894 + "radians" : 2.7528334527109286 } }, - "curvature" : 0.530208726702976 + "curvature" : 0.38783779721239137 }, { - "time" : 1.792889195170949, + "time" : 1.8283441053954617, "velocity" : 0.8, - "acceleration" : -0.8727668696952652, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.402135079938944, - "y" : -2.2668576830960774 + "x" : 5.3969620984745745, + "y" : -2.2801521700090106 }, "rotation" : { - "radians" : 3.110985106966813 + "radians" : 2.7787959191919596 } }, - "curvature" : 0.42007567627926556 + "curvature" : 0.3776909643218038 }, { - "time" : 1.879884801763329, - "velocity" : 0.7240731167571277, - "acceleration" : -4.0, + "time" : 1.9091062425604757, + "velocity" : 0.8, + "acceleration" : -2.5718750180081185, "pose" : { "translation" : { - "x" : 5.335852531377029, - "y" : -2.2656345713003674 + "x" : 5.336293798543011, + "y" : -2.257931189493547 }, "rotation" : { - "radians" : 3.133332651308909 + "radians" : 2.8015387216773218 } }, - "curvature" : 0.24218749049492908 + "curvature" : 0.31603589357887024 }, { - "time" : 2.060903080952611, + "time" : 2.22016334852342, "velocity" : 0.0, - "acceleration" : -4.0, + "acceleration" : -2.5718750180081185, "pose" : { "translation" : { - "x" : 5.270317554473877, - "y" : -2.2654507160186768 + "x" : 5.218406677246094, + "y" : -2.2181358337402344 }, "rotation" : { - "radians" : 3.141592653589793 + "radians" : 2.823947779781842 } }, "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 7.626592636108398, -1.5994892120361328, 0.0 ], - "y" : [ -3.196845054626465, 0.7553129196166992, 0.0 ] + "x" : [ 7.626592636108398, -2.086981773376465, 0.0 ], + "y" : [ -3.196845054626465, -0.026646852493286133, 0.0 ] }, { - "x" : [ 5.270317554473877, -2.0928425788879395, 0.0 ], - "y" : [ -2.2654507160186768, 0.0, 0.0 ] + "x" : [ 5.218406677246094, -1.8701248168945312, 0.0 ], + "y" : [ -2.2181358337402344, 0.6148552894592285, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : -3.1179661408942683 + "radians" : 2.9608083881863267 } }, { - "time" : 1.9699999559670687, + "time" : 2.109999952837825, "rotation" : { "radians" : -3.1066860685499065 } @@ -760,711 +774,655 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.000000000000001, - "pose" : { - "translation" : { - "x" : 5.270317554473877, - "y" : -2.2654507160186768 - }, - "rotation" : { - "radians" : -3.134549682568558 - } - }, - "curvature" : 0.0 - }, { - "time" : 0.24842369994777175, - "velocity" : 0.9936947997910872, - "acceleration" : 3.9999999999999996, - "pose" : { - "translation" : { - "x" : 5.1468907883983235, - "y" : -2.2661361676086145 - }, - "rotation" : { - "radians" : -3.13891982536029 - } - }, - "curvature" : -0.06757119451203722 - }, { - "time" : 0.3520110014258871, - "velocity" : 1.4080440057035486, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.022496396452425, - "y" : -2.2658095045383106 + "x" : 5.218406677246094, + "y" : -2.2181358337402344 }, "rotation" : { - "radians" : 3.1326792429030137 + "radians" : 2.9473819457269834 } }, - "curvature" : -0.11527041271105326 - }, { - "time" : 0.43240960818409263, - "velocity" : 1.7296384327363707, - "acceleration" : 3.9999999999999964, - "pose" : { - "translation" : { - "x" : 4.896381715565369, - "y" : -2.2636806035667334 - }, - "rotation" : { - "radians" : 3.116151981669022 - } - }, - "curvature" : -0.14367915368249928 + "curvature" : -0.0 }, { - "time" : 0.46790088797956003, - "velocity" : 1.8716035519182401, - "acceleration" : 3.9999999999999964, + "time" : 0.22577455195938007, + "velocity" : 0.9030982078375203, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.83250436842793, - "y" : -2.2617556996466606 + "x" : 5.118318357381213, + "y" : -2.198750699968514 }, "rotation" : { - "radians" : 3.1067000622999905 + "radians" : 2.9554310054193884 } }, - "curvature" : -0.15145737343185342 + "curvature" : 0.13027761823749012 }, { - "time" : 0.5012011234061037, - "velocity" : 2.0048044936244147, - "acceleration" : 3.9999999999999796, + "time" : 0.3382371271442293, + "velocity" : 1.3529485085769173, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 4.768012917062151, - "y" : -2.2591854071142734 + "x" : 4.993451572256163, + "y" : -2.176348690292798 }, "rotation" : { - "radians" : 3.096774078649599 + "radians" : 2.972591803682675 } }, - "curvature" : -0.15553616018271685 + "curvature" : 0.12784985955539324 }, { - "time" : 0.5327416181740833, - "velocity" : 2.1309664726963327, - "acceleration" : 3.9999999999999867, + "time" : 0.3916828784133741, + "velocity" : 1.5667315136534963, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 4.702871997726714, - "y" : -2.2559316347583964 + "x" : 4.916478985341882, + "y" : -2.163593622500116 }, "rotation" : { - "radians" : 3.0865855244923694 + "radians" : 2.9819601021983635 } }, - "curvature" : -0.15638926647036192 + "curvature" : 0.11259673461042294 }, { - "time" : 0.562832446555475, - "velocity" : 2.2513297862218993, - "acceleration" : 4.0, + "time" : 0.4451005174091182, + "velocity" : 1.7804020696364724, + "acceleration" : 3.999999999999998, "pose" : { "translation" : { - "x" : 4.637057769139531, - "y" : -2.251968205787257 + "x" : 4.82815027252218, + "y" : -2.1498081765639654 }, "rotation" : { - "radians" : 3.0763231525028454 + "radians" : 2.9913800207169805 } }, - "curvature" : -0.15448651685997875 + "curvature" : 0.09893287394574607 }, { - "time" : 0.5917035370956308, - "velocity" : 2.3668141483825225, - "acceleration" : 4.000000000000006, + "time" : 0.49879827939468, + "velocity" : 1.9951931175787196, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 4.570557373805121, - "y" : -2.2472803041573766 + "x" : 4.727849331012521, + "y" : -2.1351214277652844 }, "rotation" : { - "radians" : 3.066153211117251 + "radians" : 3.000843152646543 } }, - "curvature" : -0.15027440895945235 + "curvature" : 0.08854693535519663 }, { - "time" : 0.6195292935585188, - "velocity" : 2.4781171742340744, - "acceleration" : 3.9999999999999933, + "time" : 0.5527581649942036, + "velocity" : 2.211032659976814, + "acceleration" : 3.9999999999999964, "pose" : { "translation" : { - "x" : 4.50336839934198, - "y" : -2.2418639209024604 + "x" : 4.61541160941124, + "y" : -2.11974885687232 }, "rotation" : { - "radians" : 3.0562204023412582 + "radians" : 3.010451193020663 } }, - "curvature" : -0.14416258965427287 + "curvature" : 0.08141460522576446 }, { - "time" : 0.6464441948512846, - "velocity" : 2.5857767794051374, - "acceleration" : 4.000000000000006, + "time" : 0.6067994538503118, + "velocity" : 2.4271978154012466, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.435498339809959, - "y" : -2.235725300462292 + "x" : 4.491079470872819, + "y" : -2.1039811364393017 }, "rotation" : { - "radians" : 3.0466493652938835 + "radians" : 3.0203520346564443 } }, - "curvature" : -0.13651545432943565 + "curvature" : 0.0770754880726607 }, { - "time" : 0.6725531388457419, - "velocity" : 2.6902125553829666, - "acceleration" : 3.999999999999994, + "time" : 0.6337713815419612, + "velocity" : 2.5350855261678444, + "acceleration" : 3.999999999999981, "pose" : { "translation" : { - "x" : 4.3669640570376345, - "y" : -2.228880387011621 + "x" : 4.424629014592831, + "y" : -2.096057730950637 }, "rotation" : { - "radians" : 3.037546511729907 + "radians" : 3.025464239893527 } }, - "curvature" : -0.12764795403681967 + "curvature" : 0.07580950849707052 }, { - "time" : 0.6979385669929336, - "velocity" : 2.7917542679717333, + "time" : 0.6606632003839968, + "velocity" : 2.6426528015359865, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.297791241949682, - "y" : -2.221354270789056 - }, - "rotation" : { - "radians" : 3.029002066155314 - } - }, - "curvature" : -0.1178246535056758 - }, { - "time" : 0.7226655334792509, - "velocity" : 2.8906621339170027, - "acceleration" : 3.9999999999999876, - "pose" : { - "translation" : { - "x" : 4.2280138758942485, - "y" : -2.213180634425953 + "x" : 4.355457556281181, + "y" : -2.088172917105112 }, "rotation" : { - "radians" : 3.021092191458384 + "radians" : 3.030713652227927 } }, - "curvature" : -0.10726113181941763 + "curvature" : 0.07508525249190778 }, { - "time" : 0.7467854146298516, - "velocity" : 2.987141658519405, - "acceleration" : 0.5386070895034664, + "time" : 0.687437732922965, + "velocity" : 2.749750931691859, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.157673691970331, - "y" : -2.2044011992753108 + "x" : 4.2836899587141986, + "y" : -2.0803791830604403 }, "rotation" : { - "radians" : 3.0138811087611876 + "radians" : 3.036123218975336 } }, - "curvature" : -0.09612692548068995 + "curvature" : 0.07486952079026583 }, { - "time" : 0.7706587384890842, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 0.7140574327285515, + "velocity" : 2.856229730914205, + "acceleration" : 4.0000000000000115, "pose" : { "translation" : { - "x" : 4.086819636355145, - "y" : -2.1950651717406586 + "x" : 4.209468147422967, + "y" : -2.0727316138919605 }, "rotation" : { - "radians" : 3.0074231456653506 + "radians" : 3.0417167398697336 } }, - "curvature" : -0.08454935140914073 + "curvature" : 0.07513898148074699 }, { - "time" : 0.7946545748664725, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 0.7404848785000484, + "velocity" : 2.961939514000193, + "acceleration" : 1.4367050542224837, "pose" : { "translation" : { - "x" : 4.0155073296314985, - "y" : -2.1852286896049478 + "x" : 4.132949715792485, + "y" : -2.0652875411644693 }, "rotation" : { - "radians" : 3.001764669123353 + "radians" : 3.0475190269956305 } }, - "curvature" : -0.0726176921950453 + "curvature" : 0.07587984575547331 }, { - "time" : 0.818801615050494, + "time" : 0.7669763882834565, "velocity" : 3.0, - "acceleration" : -1.2196893018554146E-14, + "acceleration" : -1.0982184022288134E-14, "pose" : { "translation" : { - "x" : 3.943798528115167, - "y" : -2.1749542683594427 + "x" : 4.054306530160829, + "y" : -2.058106192504056 }, "rotation" : { - "radians" : 2.996945877314582 + "radians" : 3.053556107860972 } }, - "curvature" : -0.06038736183252478 + "curvature" : 0.07708752042753088 }, { - "time" : 0.843074966696934, + "time" : 0.7939345482116209, "velocity" : 2.9999999999999996, - "acceleration" : 1.2147236114840852E-14, + "acceleration" : 1.0755238227730279E-14, "pose" : { "translation" : { - "x" : 3.8717605851822676, - "y" : -2.1643102475326135 + "x" : 3.9737233349183256, + "y" : -2.0512483411699343 }, "rotation" : { - "radians" : 2.9930024391077152 + "radians" : 3.05985546264456 } }, - "curvature" : -0.04788378801169793 + "curvature" : 0.07876635846236638 }, { - "time" : 0.8674475458115909, + "time" : 0.8214615510510413, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.799465912596631, - "y" : -2.1533702370190255 + "x" : 3.891396357606709, + "y" : -2.044775955626278 }, "rotation" : { - "radians" : 2.9899669802209736 + "radians" : 3.0664462909649215 } }, - "curvature" : -0.03510584306122642 + "curvature" : 0.08092957341077114 }, { - "time" : 0.8918903193227681, + "time" : 0.8494883928631835, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -1.0404470748216723E-14, "pose" : { "translation" : { - "x" : 3.7269914418371743, - "y" : -2.14221256340823 + "x" : 3.8075319140182913, + "y" : -2.0387518491140533 }, "rotation" : { - "radians" : 2.987870422494782 + "radians" : 3.0733598063989405 } }, - "curvature" : -0.022028729814589355 + "curvature" : 0.08359935409570846 }, { - "time" : 0.9163725588181648, - "velocity" : 3.0, + "time" : 0.8779434176691419, + "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.6544180854252772, - "y" : -2.130919716313656 + "x" : 3.7223450132951257, + "y" : -2.0332393292228517 }, "rotation" : { - "radians" : 2.9867431872752266 + "radians" : 3.080629558866709 } }, - "curvature" : -0.008606283415062655 + "curvature" : 0.08680720147794485 }, { - "time" : 0.9408621043203094, - "velocity" : 3.0, + "time" : 0.9067528176974184, + "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.581830198252153, - "y" : -2.1195777947015015 + "x" : 3.636057963028172, + "y" : -2.028301847462725 }, "rotation" : { - "radians" : 2.9866162762632475 + "radians" : 3.0882917861725874 } }, - "curvature" : 0.005227310981712426 + "curvature" : 0.09059450379888723 }, { - "time" : 0.9653256371710356, - "velocity" : 3.0, - "acceleration" : -1.2131932184309055E-14, + "time" : 0.9358411361147014, + "velocity" : 2.9999999999999996, + "acceleration" : 1.0107649040265085E-14, "pose" : { "translation" : { - "x" : 3.509315038906223, - "y" : -2.108275953219625 + "x" : 3.5488989743564616, + "y" : -2.024002648836017 }, "rotation" : { - "radians" : 2.987522243738523 + "radians" : 3.0963857966103348 } }, - "curvature" : 0.019556357371493856 + "curvature" : 0.09501336487557137 }, { - "time" : 0.9897289612859104, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.9651317725479596, + "velocity" : 3.0, + "acceleration" : -0.06809488522901795, "pose" : { "translation" : { - "x" : 3.4369622310004906, - "y" : -2.097105848526434 + "x" : 3.461100767066263, + "y" : -2.0204044214092 }, "rotation" : { - "radians" : 2.9894960731470905 + "radians" : 3.1049543846666157 } }, - "curvature" : 0.03448299790096692 + "curvature" : 0.10012770121562267 }, { - "time" : 1.0140372922159826, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.9945573185420075, + "velocity" : 2.997996270822734, + "acceleration" : -0.184591356598183, "pose" : { "translation" : { - "x" : 3.3648632244999135, - "y" : -2.0861610856197785 + "x" : 3.3728991746902466, + "y" : -2.0175689458847046 }, "rotation" : { - "radians" : 2.9925759688659737 + "radians" : 3.1140442814748748 } }, - "curvature" : 0.050127008297662215 + "curvature" : 0.10601462452471887 }, { - "time" : 1.038215553610506, - "velocity" : 2.9999999999999996, - "acceleration" : 1.2329067798560187E-14, + "time" : 1.0240672639708144, + "velocity" : 2.9925489899628923, + "acceleration" : -0.20849217138745274, "pose" : { "translation" : { - "x" : 3.2931107570487783, - "y" : -2.075536664165842 + "x" : 3.28453174960665, + "y" : -2.0155567451727556 }, "rotation" : { - "radians" : 2.9968040706287984 + "radians" : 3.1237066406836274 } }, - "curvature" : 0.06662595941346228 + "curvature" : 0.11276612545130336 }, { - "time" : 1.0622286808047574, - "velocity" : 3.0, - "acceleration" : -0.31635458303853925, + "time" : 1.0536051481517177, + "velocity" : 2.9863905723518247, + "acceleration" : -0.2602935739030425, "pose" : { "translation" : { - "x" : 3.2217983152980736, - "y" : -2.0653284248280315 + "x" : 3.196236368138443, + "y" : -2.014426733963205 }, "rotation" : { - "radians" : 3.0022270936000854 + "radians" : 3.13399755862567 } }, - "curvature" : 0.08413564313074447 + "curvature" : 0.12049107003628591 }, { - "time" : 1.0860719059198862, - "velocity" : 2.9924570864604094, - "acceleration" : -0.8283229299033826, + "time" : 1.0831056442927933, + "velocity" : 2.9787117827793512, + "acceleration" : -0.3599966924344615, "pose" : { "translation" : { - "x" : 3.1510195962328633, - "y" : -2.0556324955958685 + "x" : 3.1082498356524937, + "y" : -2.014235868297366 }, "rotation" : { - "radians" : 3.0088968912951697 + "radians" : -3.1382066823601824 } }, - "curvature" : 0.10283063070655941 + "curvature" : 0.1293175095895187 }, { - "time" : 1.1097884594192817, - "velocity" : 2.97281212137858, - "acceleration" : -0.8752690242844084, + "time" : 1.112515242001862, + "velocity" : 2.9681244248782583, + "acceleration" : -0.41435957373242843, "pose" : { "translation" : { - "x" : 3.0808679684996605, - "y" : -2.04654473811388 + "x" : 3.020806491658732, + "y" : -2.0150387951398443 }, "rotation" : { - "radians" : 3.016870931220585 + "radians" : -3.1264678126986505 } }, - "curvature" : 0.12290478806154469 + "curvature" : 0.13939528367796553 }, { - "time" : 1.1333958560398858, - "velocity" : 2.952149298372569, - "acceleration" : -1.014196696356634, + "time" : 1.14178182090318, + "velocity" : 2.9559975377201018, + "acceleration" : -0.47782084790928264, "pose" : { "translation" : { - "x" : 3.0114359337338006, - "y" : -2.0381601940104916 + "x" : 2.9341368149093157, + "y" : -2.016887501950375 }, "rotation" : { - "radians" : 3.0262126639182343 + "radians" : -3.113896838891268 } }, - "curvature" : 0.1445715103185185 + "curvature" : 0.150898858830179 }, { - "time" : 1.156876767647848, - "velocity" : 2.9283350353923314, - "acceleration" : -3.999999999999987, + "time" : 1.1708492320949808, + "velocity" : 2.942108522657908, + "acceleration" : -0.5518862628311226, "pose" : { "translation" : { - "x" : 2.942814587886815, - "y" : -2.030572531226916 + "x" : 2.8484660284977963, + "y" : -2.019830966255654 }, "rotation" : { - "radians" : 3.0369917546143315 + "radians" : -3.1004122518647654 } }, - "curvature" : 0.16806335127014238 + "curvature" : 0.16403028405156617 }, { - "time" : 1.1804969687914266, - "velocity" : 2.8338542308180172, - "acceleration" : -4.000000000000006, + "time" : 1.199665689597395, + "velocity" : 2.9262051156188686, + "acceleration" : -0.6381658186444317, "pose" : { "translation" : { - "x" : 2.875093082553805, - "y" : -2.0238734903460447 + "x" : 2.764012704958283, + "y" : -2.023914805221171 }, "rotation" : { - "radians" : 3.049284132432743 + "radians" : -3.085924825413159 } }, - "curvature" : 0.19363060644268892 + "curvature" : 0.179022046292927 }, { - "time" : 1.2045405378384517, - "velocity" : 2.737679954629917, - "acceleration" : -4.0, + "time" : 1.2281844325226372, + "velocity" : 2.9080054286932713, + "acceleration" : -0.7382909496365782, "pose" : { "translation" : { - "x" : 2.8083580863008137, - "y" : -2.018152330921339 + "x" : 2.6809873713646084, + "y" : -2.029180925223045 }, "rotation" : { - "radians" : 3.0631717946866663 + "radians" : -3.0703372176489117 } }, - "curvature" : 0.22153825590519519 + "curvature" : 0.19613945786907577 }, { - "time" : 1.2290242925168728, - "velocity" : 2.639744935916233, - "acceleration" : -4.0, + "time" : 1.256364374900713, + "velocity" : 2.8872004322742577, + "acceleration" : -2.163128503876278, "pose" : { "translation" : { - "x" : 2.742693245992202, - "y" : -2.0134952778057222 + "x" : 2.5995911144294936, + "y" : -2.035667171419856 }, "rotation" : { - "radians" : 3.078742282815308 + "radians" : -3.0535437643496612 } }, - "curvature" : 0.25206048276332627 + "curvature" : 0.215681980996833 }, { - "time" : 1.2539716924931896, - "velocity" : 2.5399553360109657, - "acceleration" : -4.0, + "time" : 1.2843497892040483, + "velocity" : 2.826664384901926, + "acceleration" : -3.9999999999999942, "pose" : { "translation" : { - "x" : 2.6781786481180205, - "y" : -2.00998496748047 + "x" : 2.5200141856037135, + "y" : -2.04340697732448 }, "rotation" : { - "radians" : 3.096087722144318 + "radians" : -3.035430580772711 } }, - "curvature" : 0.28547175988586754 + "curvature" : 0.2379825635975536 }, { - "time" : 1.3053939989160892, - "velocity" : 2.3342661103193674, - "acceleration" : -3.9999999999999964, + "time" : 1.3125427756927424, + "velocity" : 2.7138924389471497, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.5528994917258387, - "y" : -2.006713857241266 + "x" : 2.4424346061752615, + "y" : -2.052429014375921 }, "rotation" : { - "radians" : 3.136484953437787 + "radians" : -3.0158761347705023 } }, - "curvature" : 0.3619731146711039 + "curvature" : 0.2634035923974734 }, { - "time" : 1.3592200941345929, - "velocity" : 2.1189617294453527, - "acceleration" : -3.689061390388735, + "time" : 1.3411967497233397, + "velocity" : 2.5992765428247604, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.433069631998663, - "y" : -2.0089072851188305 + "x" : 2.3670167723685154, + "y" : -2.0627568415111455 }, "rotation" : { - "radians" : -3.098071189329059 + "radians" : -2.9947525137401847 } }, - "curvature" : 0.45256136707383543 + "curvature" : 0.29232743281102963 }, { - "time" : 1.4159189374172112, - "velocity" : 1.9097962158117439, - "acceleration" : -2.8753503248023993, + "time" : 1.3703306161997593, + "velocity" : 2.482741076919082, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.3191466427738305, - "y" : -2.0170406871309297 + "x" : 2.2939100604434017, + "y" : -2.074408554736916 }, "rotation" : { - "radians" : -3.040577368313093 + "radians" : -2.971927682130507 } }, - "curvature" : 0.5571211509945629 + "curvature" : 0.325138704477899 }, { - "time" : 1.4754694675169775, - "velocity" : 1.7385675797472258, - "acceleration" : -2.1691436818120216, + "time" : 1.3999769283633923, + "velocity" : 2.36415582826455, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.211479419327574, - "y" : -2.0314789329225036 + "x" : 2.2232474317945616, + "y" : -2.0873964367016242 }, "rotation" : { - "radians" : -2.9739098912623314 + "radians" : -2.947269113517704 } }, - "curvature" : 0.6722650755346324 + "curvature" : 0.3621944652381181 }, { - "time" : 1.5372939266887176, - "velocity" : 1.6044614447534007, - "acceleration" : -1.5165826850953532, + "time" : 1.4301864235012918, + "velocity" : 2.2433178477129516, + "acceleration" : -3.8425061187661265, "pose" : { "translation" : { - "x" : 2.110290940850973, - "y" : -2.0524586082901806 + "x" : 2.1551440380505156, + "y" : -2.1017266062671247 }, "rotation" : { - "radians" : -2.898381476531495 + "radians" : -2.9206492753984246 } }, - "curvature" : 0.7893418563615717 + "curvature" : 0.40377746810107285 }, { - "time" : 1.6006283768070562, - "velocity" : 1.508409514333893, - "acceleration" : -1.032513464911367, + "time" : 1.4609990554339194, + "velocity" : 2.1249201209765416, + "acceleration" : -3.5248780323742457, "pose" : { "translation" : { - "x" : 2.0156610329259053, - "y" : -2.08007029770679 + "x" : 2.0896958261728287, + "y" : -2.1173986680805683 }, "rotation" : { - "radians" : -2.8152839089339228 + "radians" : -2.8919535310937237 } }, - "curvature" : 0.8930693351434651 + "curvature" : 0.45002691208795453 }, { - "time" : 1.6326245076366628, - "velocity" : 1.4753730784272583, - "acceleration" : -0.7012265516486391, + "time" : 1.4923979532320715, + "velocity" : 2.0142428358870714, + "acceleration" : -3.197919951626316, "pose" : { "translation" : { - "x" : 1.9707871028025834, - "y" : -2.0963471729342444 + "x" : 2.026978143555276, + "y" : -2.1344053621462358 }, "rotation" : { - "radians" : -2.77164828786782 + "radians" : -2.861091073527203 } }, - "curvature" : 0.9335121925643923 + "curvature" : 0.5008412010105917 }, { - "time" : 1.6646096086435074, - "velocity" : 1.4529442763440952, - "acceleration" : -0.35056590766525175, + "time" : 1.5243220106712716, + "velocity" : 1.9121522556653892, + "acceleration" : -2.6890710106994757, "pose" : { "translation" : { - "x" : 1.927509130000999, - "y" : -2.1142408668458756 + "x" : 1.967044343123007, + "y" : -2.152732213397371 }, "rotation" : { - "radians" : -2.727199104287848 + "radians" : -2.8280094739823887 } }, - "curvature" : 0.9625555150908901 + "curvature" : 0.555749093954951 }, { - "time" : 1.696419570306363, - "velocity" : 1.4417927882609594, - "acceleration" : 0.03358024482766757, + "time" : 1.5892933232552724, + "velocity" : 1.7374397824686587, + "acceleration" : -1.9497522202134734, "pose" : { "translation" : { - "x" : 1.8857891546813712, - "y" : -2.1337137498151666 + "x" : 1.855623458766786, + "y" : -2.1932503092648403 }, "rotation" : { - "radians" : -2.6824765979300107 + "radians" : -2.7552864354535944 } }, - "curvature" : 0.9775027904601952 + "curvature" : 0.6731381127503112 }, { - "time" : 1.7278732271361714, - "velocity" : 1.4428490097580298, - "acceleration" : 0.7353110075222659, + "time" : 1.655072303925798, + "velocity" : 1.6091870688629224, + "acceleration" : -1.0749318224543505, "pose" : { "translation" : { - "x" : 1.845577037867585, - "y" : -2.154715745106209 + "x" : 1.7553671009011396, + "y" : -2.2386795374578696 }, "rotation" : { - "radians" : -2.638094148131616 + "radians" : -2.674931532448897 } }, - "curvature" : 0.9760721742923996 + "curvature" : 0.7847126172556117 }, { - "time" : 1.7887154459118286, - "velocity" : 1.4875869629458485, - "acceleration" : -0.7264993431499159, + "time" : 1.720231550281118, + "velocity" : 1.539145321428446, + "acceleration" : -1.1921125316242216, "pose" : { "translation" : { - "x" : 1.7694116961356485, - "y" : -2.201041208136303 + "x" : 1.6657680121716112, + "y" : -2.288608611212112 }, "rotation" : { - "radians" : -2.5530618886531005 + "radians" : -2.5902033764696597 } }, - "curvature" : 0.9182458546356818 + "curvature" : 0.8577573547258329 }, { - "time" : 1.8485897368265631, - "velocity" : 1.444088329924727, - "acceleration" : -4.000000000000003, + "time" : 1.784455858835107, + "velocity" : 1.4625827183663351, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 1.6983479407097803, - "y" : -2.2525466603589237 + "x" : 1.5858331132455987, + "y" : -2.342477161522652 }, "rotation" : { - "radians" : -2.477748499042142 + "radians" : -2.507140034918934 } }, - "curvature" : 0.7850323243866926 + "curvature" : 0.8472000723919605 }, { - "time" : 1.9149904498064472, - "velocity" : 1.178485478005191, - "acceleration" : -4.000000000000001, + "time" : 1.853732644359764, + "velocity" : 1.1854755762677067, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 1.6314912662651295, - "y" : -2.308326917495606 + "x" : 1.5140388659856399, + "y" : -2.3995645234426775 }, "rotation" : { - "radians" : -2.417734046615386 + "radians" : -2.43458289692862 } }, - "curvature" : 0.5823128416173662 + "curvature" : 0.7096157054787944 }, { - "time" : 2.0013081447960555, - "velocity" : 0.8332146980467585, - "acceleration" : -3.9999999999999996, + "time" : 1.9414753768133155, + "velocity" : 0.8345046464535002, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 1.5677005887233548, - "y" : -2.3672244890911642 + "x" : 1.4482866366226972, + "y" : -2.4589785223821536 }, "rotation" : { - "radians" : -2.378189328750247 + "radians" : -2.383232861630834 } }, - "curvature" : 0.3190107323302002 + "curvature" : 0.422926581935277 }, { - "time" : 2.209611819307745, + "time" : 2.1501015384266906, "velocity" : 0.0, - "acceleration" : -3.9999999999999996, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 1.5055710077285767, - "y" : -2.427811861038208 + "x" : 1.3858580589294434, + "y" : -2.519644260406494 }, "rotation" : { "radians" : -2.3639463990252185 @@ -1473,11 +1431,11 @@ "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 5.270317554473877, -3.944063901901245, 0.0 ], - "y" : [ -2.2654507160186768, -0.02777838706970215, 0.0 ] + "x" : [ 5.218406677246094, -1.5258679389953613, 0.0 ], + "y" : [ -2.2181358337402344, 0.3001227378845215, 0.0 ] }, { - "x" : [ 1.5055710077285767, -1.9787755012512207, 0.0 ], - "y" : [ -2.427811861038208, -1.9483323097229004, 0.0 ] + "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ], + "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -1485,7 +1443,7 @@ "radians" : -2.3736477827122884 } }, { - "time" : 2.1199999526143074, + "time" : 2.059999953955412, "rotation" : { "radians" : -2.3824463294692526 } @@ -1541,11 +1499,11 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.000000000000001, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { - "x" : 1.5055710077285767, - "y" : -2.427811861038208 + "x" : 1.3858580589294434, + "y" : -2.519644260406494 }, "rotation" : { "radians" : 0.0 @@ -1553,223 +1511,349 @@ }, "curvature" : 0.0 }, { - "time" : 0.24913945161581508, - "velocity" : 0.9965578064632605, - "acceleration" : 3.999999999999997, + "time" : 0.2506613508834166, + "velocity" : 1.0026454035336663, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 1.6297117822080054, - "y" : -2.4280100629130175 + "x" : 1.5115202584765939, + "y" : -2.5197252612570082 }, "rotation" : { - "radians" : -0.004695040376802698 + "radians" : -0.0018522539500240682 } }, - "curvature" : -0.07299386363005499 + "curvature" : -0.026842794063227322 }, { - "time" : 0.3491337812502485, - "velocity" : 1.396535125000994, - "acceleration" : 3.999999999999997, + "time" : 0.30789720119155506, + "velocity" : 1.23158880476622, + "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 1.7493532588196103, - "y" : -2.4292461404693313 + "x" : 1.5754591548339363, + "y" : -2.519904416002504 }, "rotation" : { - "radians" : -0.017229353546914548 + "radians" : -0.003832966982418239 } }, - "curvature" : -0.136406473250297 + "curvature" : -0.03448311037462077 }, { - "time" : 0.42199180876246867, - "velocity" : 1.6879672350498744, - "acceleration" : 4.000000000000002, + "time" : 0.35684276742474663, + "velocity" : 1.4273710696989863, + "acceleration" : 3.9999999999999964, "pose" : { "translation" : { - "x" : 1.8616806058923885, - "y" : -2.4321694050941005 + "x" : 1.6405304865329526, + "y" : -2.520230419613654 }, "rotation" : { - "radians" : -0.03593834139063176 + "radians" : -0.006233177930525208 } }, - "curvature" : -0.19668533798708224 + "curvature" : -0.03877108765930074 }, { - "time" : 0.4796162909188312, - "velocity" : 1.9184651636753245, - "acceleration" : 3.9999999999999956, + "time" : 0.40065288496208495, + "velocity" : 1.6026115398483394, + "acceleration" : 3.9999999999999933, "pose" : { "translation" : { - "x" : 1.9654748018365353, - "y" : -2.4370610546320677 + "x" : 1.7069005447805807, + "y" : -2.5207311531334256 }, "rotation" : { - "radians" : -0.059144691931219094 + "radians" : -0.008873618264835237 } }, - "curvature" : -0.2480691940003244 + "curvature" : -0.04040754941441055 }, { - "time" : 0.5271053063257927, - "velocity" : 2.10842122530317, - "acceleration" : 3.999999999999995, + "time" : 0.44090317601445284, + "velocity" : 1.7636127040578107, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 2.0608466667965786, - "y" : -2.4438955256428017 + "x" : 1.7746427433321514, + "y" : -2.5214250950984933 }, "rotation" : { - "radians" : -0.08433384568667432 + "radians" : -0.011609841654107651 } }, - "curvature" : -0.27258387491895875 + "curvature" : -0.04010473649023733 }, { - "time" : 0.5675446352717414, - "velocity" : 2.270178541086965, - "acceleration" : 3.9999999999999947, + "time" : 0.4784875189105668, + "velocity" : 1.9139500756422665, + "acceleration" : 3.9999999999999907, "pose" : { "translation" : { - "x" : 2.1489708943045116, - "y" : -2.4524018456577323 + "x" : 1.843746311953339, + "y" : -2.522322105081429 }, "rotation" : { - "radians" : -0.10771687746023423 + "radians" : -0.014331201202338068 } }, - "curvature" : -0.24453210747816045 + "curvature" : -0.03847972749251971 }, { - "time" : 0.6031714577725938, - "velocity" : 2.412685831090375, - "acceleration" : 4.000000000000005, + "time" : 0.5139493740375787, + "velocity" : 2.055797496150314, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 2.231820082932927, - "y" : -2.4621249854371854 + "x" : 1.9141249898821115, + "y" : -2.5234242072328925 }, "rotation" : { - "radians" : -0.12458825964417336 + "radians" : -0.016956454931120607 } }, - "curvature" : -0.1479406947072193 + "curvature" : -0.03601472391776367 }, { - "time" : 0.6357585844969933, - "velocity" : 2.5430343379879727, - "acceleration" : -3.999999999999978, + "time" : 0.5476314929124355, + "velocity" : 2.190525971649741, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 2.3118987679481506, - "y" : -2.472487211227417 + "x" : 1.985625719290681, + "y" : -2.5247263738238246 }, "rotation" : { - "radians" : -0.1307784900417709 + "radians" : -0.019428093132643293 } }, - "curvature" : 0.0 + "curvature" : -0.03305737128049612 }, { - "time" : 0.6683457112213927, - "velocity" : 2.412685831090376, - "acceleration" : -4.000000000000005, + "time" : 0.5797531676739855, + "velocity" : 2.3190126706959413, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 2.391977452963374, - "y" : -2.4828494370176486 + "x" : 2.0580373387474538, + "y" : -2.5262173087876363 }, "rotation" : { - "radians" : -0.12458825964417336 + "radians" : -0.021706661066398916 } }, - "curvature" : 0.1479406947072193 + "curvature" : -0.02983965013490699 }, { - "time" : 0.7039725337222453, - "velocity" : 2.2701785410869655, - "acceleration" : -4.000000000000005, + "time" : 0.6104540649254343, + "velocity" : 2.4418162597017363, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 2.4748266415917897, - "y" : -2.4925725767971016 + "x" : 2.131099276678981, + "y" : -2.527880231262401 }, "rotation" : { - "radians" : -0.10771687746023423 + "radians" : -0.023765711637107785 } }, - "curvature" : 0.24453210747816045 + "curvature" : -0.026501856917817767 }, { - "time" : 0.7444118626681941, - "velocity" : 2.10842122530317, - "acceleration" : -3.999999999999998, + "time" : 0.6398209489868676, + "velocity" : 2.5592837959474695, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 2.2045102448319085, + "y" : -2.529693659133045 + }, + "rotation" : { + "radians" : -0.025587608770040265 + } + }, + "curvature" : -0.02311472481731095 + }, { + "time" : 0.6679049204495552, + "velocity" : 2.67161968179822, + "acceleration" : 2.6312398843848555, + "pose" : { + "translation" : { + "x" : 2.277936931734928, + "y" : -2.5316321925735394 + }, + "rotation" : { + "radians" : -0.027160172194753095 + } + }, + "curvature" : -0.019696900519105796 + }, { + "time" : 0.6949126774005732, + "velocity" : 2.7426835690755107, + "acceleration" : -3.999999999999994, "pose" : { "translation" : { - "x" : 2.5629508690997227, - "y" : -2.5010788968120323 + "x" : 2.351022696160726, + "y" : -2.5336672975890906 }, "rotation" : { - "radians" : -0.08433384568667432 + "radians" : -0.02847404834663094 } }, - "curvature" : 0.27258387491895875 + "curvature" : -0.01622720956565302 }, { - "time" : 0.7919008780751556, - "velocity" : 1.9184651636753245, + "time" : 0.7218404114156483, + "velocity" : 2.6349726330152103, + "acceleration" : -4.000000000000006, + "pose" : { + "translation" : { + "x" : 2.4233962605879356, + "y" : -2.5357680895583314 + }, + "rotation" : { + "radians" : -0.02952065953655597 + } + }, + "curvature" : -0.012652107561005304 + }, { + "time" : 0.7494857099529498, + "velocity" : 2.524391438866004, + "acceleration" : -3.999999999999994, + "pose" : { + "translation" : { + "x" : 2.494680404663086, + "y" : -2.5379021167755127 + }, + "rotation" : { + "radians" : -0.030290590291302242 + } + }, + "curvature" : -0.008889004344138316 + }, { + "time" : 0.7777916661649981, + "velocity" : 2.411167614017811, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.658322734059766, - "y" : -2.5079133678227663 + "x" : 2.5645006586625527, + "y" : -2.540036143992694 }, "rotation" : { - "radians" : -0.059144691931219094 + "radians" : -0.030772296527561225 } }, - "curvature" : 0.2480691940003244 + "curvature" : -0.004826154357885299 }, { - "time" : 0.8495253602315181, - "velocity" : 1.6879672350498744, - "acceleration" : -3.9999999999999982, + "time" : 0.8066975299375645, + "velocity" : 2.2955441589275454, + "acceleration" : -4.000000000000007, "pose" : { "translation" : { - "x" : 2.762116930003913, - "y" : -2.5128050173607335 + "x" : 2.6324939969545085, + "y" : -2.542136935961935 }, "rotation" : { - "radians" : -0.03593834139063176 + "radians" : -0.030951061958126948 } }, - "curvature" : 0.19668533798708224 + "curvature" : -3.197882350940787E-4 }, { - "time" : 0.9223833877437382, - "velocity" : 1.3965351250009943, - "acceleration" : -3.9999999999999996, + "time" : 0.8361410171738428, + "velocity" : 2.177770209982432, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.874444277076691, - "y" : -2.5157282819855027 + "x" : 2.698317531460873, + "y" : -2.544172040977486 }, "rotation" : { - "radians" : -0.017229353546914548 + "radians" : -0.030808177370938297 } }, - "curvature" : 0.136406473250297 + "curvature" : 0.004810664209664993 }, { - "time" : 1.0223777173781716, - "velocity" : 0.9965578064632606, - "acceleration" : -4.000000000000001, + "time" : 0.8660614348369912, + "velocity" : 2.0580885393298387, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 2.7616572051192634, + "y" : -2.5461105744179804 + }, + "rotation" : { + "radians" : -0.030320390272959595 + } + }, + "curvature" : 0.010787761573423147 + }, { + "time" : 0.9271262518256638, + "velocity" : 1.8138292713751483, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 2.8798250574927806, + "y" : -2.549586924763389 + }, + "rotation" : { + "radians" : -0.02819439651168064 + } + }, + "curvature" : 0.02638800187200745 + }, { + "time" : 0.989659585589217, + "velocity" : 1.5636959363209353, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 2.9853920694440603, + "y" : -2.552380026318133 + }, + "rotation" : { + "radians" : -0.02431565149909576 + } + }, + "curvature" : 0.04885357015887642 + }, { + "time" : 1.054053992446091, + "velocity" : 1.3061183088934392, + "acceleration" : -4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.077770434158083, + "y" : -2.554379138452532 + }, + "rotation" : { + "radians" : -0.018496813941660972 + } + }, + "curvature" : 0.07878050827674199 + }, { + "time" : 1.1223816921391718, + "velocity" : 1.0328075101211158, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 3.1576682134182192, + "y" : -2.5555738139373716 + }, + "rotation" : { + "radians" : -0.01102726873603195 + } + }, + "curvature" : 0.10654928830990536 + }, { + "time" : 1.2022085804462013, + "velocity" : 0.7134999568929983, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.994085753688296, - "y" : -2.5169643595418165 + "x" : 3.2273675283886405, + "y" : -2.556078972294017 }, "rotation" : { - "radians" : -0.004695040376802698 + "radians" : -0.003571825888394278 } }, - "curvature" : 0.07299386363005499 + "curvature" : 0.09616404801082394 }, { - "time" : 1.2715171689939868, + "time" : 1.3805835696694508, "velocity" : 0.0, - "acceleration" : -4.000000000000001, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 3.1182265281677246, - "y" : -2.517162561416626 + "x" : 3.2910027503967285, + "y" : -2.5561599731445312 }, "rotation" : { "radians" : 0.0 @@ -1778,11 +1862,11 @@ "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 1.5055710077285767, 2.0, 0.0 ], - "y" : [ -2.427811861038208, 0.0, 0.0 ] + "x" : [ 1.3858580589294434, 2.0, 0.0 ], + "y" : [ -2.519644260406494, 0.0, 0.0 ] }, { - "x" : [ 3.1182265281677246, 2.0, 0.0 ], - "y" : [ -2.517162561416626, 0.0, 0.0 ] + "x" : [ 3.2910027503967285, 1.0, 0.0 ], + "y" : [ -2.5561599731445312, 0.0, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -1790,7 +1874,7 @@ "radians" : -2.670353755551324 } }, { - "time" : 1.1799999736249447, + "time" : 1.289999971166253, "rotation" : { "radians" : -2.684023492023121 } @@ -1803,7 +1887,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 20\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF", + "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE", "closed" : false, "valid" : true, "sendableScript" : { @@ -1811,7 +1895,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "20" ], + "args" : [ "8" ], "argTypes" : [ "double" ], "reflection" : true }, { @@ -1824,7 +1908,809 @@ "args" : [ "OFF" ], "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true } ] } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 3.9999999999999996, + "pose" : { + "translation" : { + "x" : 3.2910027503967285, + "y" : -2.5561599731445312 + }, + "rotation" : { + "radians" : -0.2884106402522736 + } + }, + "curvature" : 0.0 + }, { + "time" : 0.2373794954493144, + "velocity" : 0.9495179817972574, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 3.3989983545955056, + "y" : -2.588374874732608 + }, + "rotation" : { + "radians" : -0.2921778608912438 + } + }, + "curvature" : -0.04364084615692234 + }, { + "time" : 0.29579824973185176, + "velocity" : 1.1831929989274066, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.458631632620488, + "y" : -2.606390396730127 + }, + "rotation" : { + "radians" : -0.29440901549838794 + } + }, + "curvature" : -0.02595134837823401 + }, { + "time" : 0.3491550793551233, + "velocity" : 1.3966203174204928, + "acceleration" : 4.000000000000002, + "pose" : { + "translation" : { + "x" : 3.5244835324483574, + "y" : -2.6264017262656125 + }, + "rotation" : { + "radians" : -0.2953398995873586 + } + }, + "curvature" : -0.0013985693216967878 + }, { + "time" : 0.40043149369700565, + "velocity" : 1.6017259747880221, + "acceleration" : 4.000000000000003, + "pose" : { + "translation" : { + "x" : 3.5980331007389807, + "y" : -2.6487579275583 + }, + "rotation" : { + "radians" : -0.2945501713020244 + } + }, + "curvature" : 0.020718850966936625 + }, { + "time" : 0.45101233132563245, + "velocity" : 1.8040493253025296, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.680485970962536, + "y" : -2.6736677892761236 + }, + "rotation" : { + "radians" : -0.2919955872089119 + } + }, + "curvature" : 0.0373242313505917 + }, { + "time" : 0.5015733373341933, + "velocity" : 2.0062933493367727, + "acceleration" : 3.9999999999999876, + "pose" : { + "translation" : { + "x" : 3.7727913068944403, + "y" : -2.70121209879796 + }, + "rotation" : { + "radians" : -0.2878084836673059 + } + }, + "curvature" : 0.04861193460092709 + }, { + "time" : 0.5524240768131252, + "velocity" : 2.2096963072525, + "acceleration" : 3.9999999999999964, + "pose" : { + "translation" : { + "x" : 3.8756587461102754, + "y" : -2.731355916475877 + }, + "rotation" : { + "radians" : -0.28217372185403494 + } + }, + "curvature" : 0.055832145694026246 + }, { + "time" : 0.6036702471548304, + "velocity" : 2.414680988619321, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 3.989575343480716, + "y" : -2.76396084989738 + }, + "rotation" : { + "radians" : -0.2752690347022763 + } + }, + "curvature" : 0.06027032950217638 + }, { + "time" : 0.6294411364762771, + "velocity" : 2.5177645459051075, + "acceleration" : 3.9999999999999933, + "pose" : { + "translation" : { + "x" : 4.050772588350831, + "y" : -2.7811177201369857 + }, + "rotation" : { + "radians" : -0.271388278211743 + } + }, + "curvature" : 0.06177303417946446 + }, { + "time" : 0.6553004906081608, + "velocity" : 2.621201962432642, + "acceleration" : 3.999999999999994, + "pose" : { + "translation" : { + "x" : 4.114822514666457, + "y" : -2.798797328147657 + }, + "rotation" : { + "radians" : -0.26724328091034977 + } + }, + "curvature" : 0.06293282064413533 + }, { + "time" : 0.6812367197554138, + "velocity" : 2.7249468790216542, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.181730948050758, + "y" : -2.816958744670205 + }, + "rotation" : { + "radians" : -0.26284758822831167 + } + }, + "curvature" : 0.06382850159969175 + }, { + "time" : 0.7072359906843115, + "velocity" : 2.8289439627372452, + "acceleration" : 4.0, + "pose" : { + "translation" : { + "x" : 4.25149297961314, + "y" : -2.835556876071827 + }, + "rotation" : { + "radians" : -0.25821295054670484 + } + }, + "curvature" : 0.06452392825617205 + }, { + "time" : 0.7332828563292066, + "velocity" : 2.9331314253168257, + "acceleration" : 2.5481050539821153, + "pose" : { + "translation" : { + "x" : 4.324093495433467, + "y" : -2.854542847916802 + }, + "rotation" : { + "radians" : -0.253349516956691 + } + }, + "curvature" : 0.06507034108588115 + }, { + "time" : 0.759525327996014, + "velocity" : 3.0, + "acceleration" : -1.1017744707610282E-14, + "pose" : { + "translation" : { + "x" : 4.399507706046279, + "y" : -2.8738643885371857 + }, + "rotation" : { + "radians" : -0.24826603822981036 + } + }, + "curvature" : 0.06550851193026688 + }, { + "time" : 0.7863964782259588, + "velocity" : 2.9999999999999996, + "acceleration" : 1.0659361723935359E-14, + "pose" : { + "translation" : { + "x" : 4.477701675925007, + "y" : -2.8934662126035047 + }, + "rotation" : { + "radians" : -0.2429700686181491 + } + }, + "curvature" : 0.06587060593952473 + }, { + "time" : 0.8141710749514438, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.558632852966191, + "y" : -2.913290404695452 + }, + "rotation" : { + "radians" : -0.23746816090539483 + } + }, + "curvature" : 0.06618175066974573 + }, { + "time" : 0.8428287933072273, + "velocity" : 3.0, + "acceleration" : -1.0029761020365324E-14, + "pose" : { + "translation" : { + "x" : 4.642250597973694, + "y" : -2.9332768028725846 + }, + "rotation" : { + "radians" : -0.23176605224269178 + } + }, + "curvature" : 0.06646132969406916 + }, { + "time" : 0.8723468917576519, + "velocity" : 2.9999999999999996, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.728496714142921, + "y" : -2.9533633822450156 + }, + "rotation" : { + "radians" : -0.2258688402689826 + } + }, + "curvature" : 0.06672403127896764 + }, { + "time" : 0.9027004208083538, + "velocity" : 2.9999999999999996, + "acceleration" : 9.500654106330315E-15, + "pose" : { + "translation" : { + "x" : 4.817305976545033, + "y" : -2.9734866385441103 + }, + "rotation" : { + "radians" : -0.21978115021622427 + } + }, + "curvature" : 0.0669806864818383 + }, { + "time" : 0.9338624302784291, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 4.9086066616111665, + "y" : -2.9935819716931817 + }, + "rotation" : { + "radians" : -0.21350729439917845 + } + }, + "curvature" : 0.06723892993087238 + }, { + "time" : 0.9658041749806907, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.002321076616648, + "y" : -3.0135840693781866 + }, + "rotation" : { + "radians" : -0.20705142586622954 + } + }, + "curvature" : 0.06750371317272791 + }, { + "time" : 0.9984953186414615, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.098366089165211, + "y" : -3.0334272906184196 + }, + "rotation" : { + "radians" : -0.20041768816116357 + } + }, + "curvature" : 0.0677776962888439 + }, { + "time" : 1.0319041358746839, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.19665365667321, + "y" : -3.0530460493372082 + }, + "rotation" : { + "radians" : -0.1936103631941936 + } + }, + "curvature" : 0.06806153927030212 + }, { + "time" : 1.0659977120081605, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.297091355853844, + "y" : -3.072375197932608 + }, + "rotation" : { + "radians" : -0.18663401919376696 + } + }, + "curvature" : 0.0683541107864204 + }, { + "time" : 1.1007421405426991, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.399582912201364, + "y" : -3.0913504108481 + }, + "rotation" : { + "radians" : -0.17949366063978456 + } + }, + "curvature" : 0.0686526286409456 + }, { + "time" : 1.136102718008151, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.504028729475294, + "y" : -3.1099085681432825 + }, + "rotation" : { + "radians" : -0.17219488198191107 + } + }, + "curvature" : 0.06895274341837182 + }, { + "time" : 1.1720441359642337, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.61032641918465, + "y" : -3.127988139064569 + }, + "rotation" : { + "radians" : -0.16474402683304648 + } + }, + "curvature" : 0.06924857455843654 + }, { + "time" : 1.2085306698791871, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.718371330072152, + "y" : -3.145529565615881 + }, + "rotation" : { + "radians" : -0.15714835420132006 + } + }, + "curvature" : 0.06953270631102976 + }, { + "time" : 1.2455263646064414, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 5.828057077598442, + "y" : -3.162475646129346 + }, + "rotation" : { + "radians" : -0.149416213183665 + } + }, + "curvature" : 0.06979614966205201 + }, { + "time" : 1.2829952161694935, + "velocity" : 3.0, + "acceleration" : -7.810331647113909E-15, + "pose" : { + "translation" : { + "x" : 5.939276073426299, + "y" : -3.1787719188359915 + }, + "rotation" : { + "radians" : -0.1415572273868729 + } + }, + "curvature" : 0.07002827533191112 + }, { + "time" : 1.3209013495592037, + "velocity" : 2.9999999999999996, + "acceleration" : 7.728429806013464E-15, + "pose" : { + "translation" : { + "x" : 6.051920054904861, + "y" : -3.1943670454364383 + }, + "rotation" : { + "radians" : -0.13358249016273918 + } + }, + "curvature" : 0.07021672228872737 + }, { + "time" : 1.359209192247121, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.165880614553835, + "y" : -3.209213194671598 + }, + "rotation" : { + "radians" : -0.12550477153574247 + } + }, + "curvature" : 0.07034728585337109 + }, { + "time" : 1.397883643124852, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.281049729547716, + "y" : -3.2232664258933683 + }, + "rotation" : { + "radians" : -0.11733873745676503 + } + }, + "curvature" : 0.07040378937764218 + }, { + "time" : 1.4368902365948655, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.397320291200003, + "y" : -3.2364870726353274 + }, + "rotation" : { + "radians" : -0.10910118172585662 + } + }, + "curvature" : 0.0703679436328688 + }, { + "time" : 1.4761953015647518, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.514586634447419, + "y" : -3.2488401261834285 + }, + "rotation" : { + "radians" : -0.10081127058141126 + } + }, + "curvature" : 0.0702191984436713 + }, { + "time" : 1.515766115137486, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.63274506733412, + "y" : -3.260295619146696 + }, + "rotation" : { + "radians" : -0.0924907995421401 + } + }, + "curvature" : 0.06993459173630182 + }, { + "time" : 1.55557105084767, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.7516944004959205, + "y" : -3.2708290090279206 + }, + "rotation" : { + "radians" : -0.08416446160106997 + } + }, + "curvature" : 0.06948860204346692 + }, { + "time" : 1.5955797213714447, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.871336476644501, + "y" : -3.2804215617943555 + }, + "rotation" : { + "radians" : -0.07586012529605445 + } + }, + "curvature" : 0.06885301162269934 + }, { + "time" : 1.6357631157394636, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 6.991576700051632, + "y" : -3.289060735448409 + }, + "rotation" : { + "radians" : -0.06760912050688539 + } + }, + "curvature" : 0.06799678871171229 + }, { + "time" : 1.6760937312120248, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 7.1123245660333865, + "y" : -3.296740563598341 + }, + "rotation" : { + "radians" : -0.05944652904214673 + } + }, + "curvature" : 0.06688599907381494 + }, { + "time" : 1.7165457001373738, + "velocity" : 3.0, + "acceleration" : 0.0, + "pose" : { + "translation" : { + "x" : 7.233494190434357, + "y" : -3.30346203902896 + }, + "rotation" : { + "radians" : -0.05141147616548517 + } + }, + "curvature" : 0.06548375889504356 + }, { + "time" : 1.757094912312602, + "velocity" : 3.0, + "acceleration" : -7.287757495654728E-15, + "pose" : { + "translation" : { + "x" : 7.355004839111871, + "y" : -3.3092334972723165 + }, + "rotation" : { + "radians" : -0.043547418155515716 + } + }, + "curvature" : 0.06375024330143432 + }, { + "time" : 1.7977191336056264, + "velocity" : 2.9999999999999996, + "acceleration" : 7.27794583372083E-15, + "pose" : { + "translation" : { + "x" : 7.476781457420211, + "y" : -3.314071000178397 + }, + "rotation" : { + "radians" : -0.03590241977860944 + } + }, + "curvature" : 0.061642767292292996 + }, { + "time" : 1.8383981218802636, + "velocity" : 3.0, + "acceleration" : -0.8549861225542413, + "pose" : { + "translation" : { + "x" : 7.598755199694828, + "y" : -3.3179987194858214 + }, + "rotation" : { + "radians" : -0.02852941415912364 + } + }, + "curvature" : 0.059115958757323624 + }, { + "time" : 1.8793527503450278, + "velocity" : 2.964984361008262, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 7.720863958736559, + "y" : -3.3210493203925378 + }, + "rotation" : { + "radians" : -0.021486435933739608 + } + }, + "curvature" : 0.05612204648295773 + }, { + "time" : 1.9217846589717384, + "velocity" : 2.7952567265014197, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 7.843052895295841, + "y" : -3.3232643451265176 + }, + "rotation" : { + "radians" : -0.014836816748393312 + } + }, + "curvature" : 0.05261128966471575 + }, { + "time" : 1.966973534915382, + "velocity" : 2.6145012227268447, + "acceleration" : -4.0000000000000036, + "pose" : { + "translation" : { + "x" : 7.965274967556932, + "y" : -3.3246945965164496 + }, + "rotation" : { + "radians" : -0.008649330067547475 + } + }, + "curvature" : 0.048532579406757856 + }, { + "time" : 2.0155229987855723, + "velocity" : 2.4203033672460843, + "acceleration" : -3.9999999999999964, + "pose" : { + "translation" : { + "x" : 8.087491460622125, + "y" : -3.325400521562436 + }, + "rotation" : { + "radians" : -0.0029982698845735147 + } + }, + "curvature" : 0.04383424692658223 + }, { + "time" : 2.0683070326652815, + "velocity" : 2.2091672317272475, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 8.209672515995964, + "y" : -3.3254525950066887 + }, + "rotation" : { + "radians" : 0.0020365547800811692 + } + }, + "curvature" : 0.03846511750567603 + }, { + "time" : 2.126672618228889, + "velocity" : 1.9757048894728169, + "acceleration" : -3.9999999999999982, + "pose" : { + "translation" : { + "x" : 8.331797661069459, + "y" : -3.324931702904223 + }, + "rotation" : { + "radians" : 0.006369930747651973 + } + }, + "curvature" : 0.032375853273367367 + }, { + "time" : 2.19289366493448, + "velocity" : 1.7108207026504523, + "acceleration" : -3.999999999999999, + "pose" : { + "translation" : { + "x" : 8.453856338604307, + "y" : -3.3239295261935533 + }, + "rotation" : { + "radians" : 0.00991148144607044 + } + }, + "curvature" : 0.025520631044257825 + }, { + "time" : 2.2714115520395897, + "velocity" : 1.3967491542300143, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 8.575848436217107, + "y" : -3.3225489242673873 + }, + "rotation" : { + "radians" : 0.012566065756910601 + } + }, + "curvature" : 0.017859202608647286 + }, { + "time" : 2.373702045251545, + "velocity" : 0.9875871813821927, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 8.697784815863574, + "y" : -3.320904318543324 + }, + "rotation" : { + "radians" : 0.014234366939993203 + } + }, + "curvature" : 0.009359382499354215 + }, { + "time" : 2.620598840597093, + "velocity" : 0.0, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 8.819687843322754, + "y" : -3.319122076034546 + }, + "rotation" : { + "radians" : 0.014813707347434221 + } + }, + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 3.2910027503967285, 1.676727533340454, 0.0 ], + "y" : [ -2.5561599731445312, -0.49745607376098633, 0.0 ] + }, { + "x" : [ 8.819687843322754, 7.801394462585449, 0.0 ], + "y" : [ -3.319122076034546, 0.11557602882385254, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : 0.0 + } + }, { + "time" : 2.5299999434500933, + "rotation" : { + "radians" : 0.0 + } + } ], + "reversed" : false, + "color" : 88.23529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] } ] } \ No newline at end of file From 7404551692cf3ab3d3bb6a03903f4da2cd6ed7d7 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 16:02:51 -0700 Subject: [PATCH 063/108] remove some unused vars in VisionManager --- src/main/java/frc/subsystem/VisionManager.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 5f9eeffc..b370c0ff 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -92,10 +92,8 @@ public void logData() { double timeFromLastShoot = Timer.getFPGATimestamp() - shooter.getLastShotTime(); double shooterLookAheadTime = 0.15 - timeFromLastShoot; - boolean justShot = false; if (shooterLookAheadTime < 0) { shooterLookAheadTime = 0.15; - justShot = true; } double turnDelay = 0.00; @@ -367,7 +365,6 @@ public boolean isVisionForcedOn() { @Override public void update() { robotPositionOffset = new Translation2d(); - Pose2d robotTrackerPose = robotTracker.getLatencyCompedPoseMeters(); Translation2d relativeGoalPos = getRelativeGoalTranslation(); double angleToTarget = Math.atan2(relativeGoalPos.getY(), relativeGoalPos.getX()); From e5f766ed2effdc491156999af2cffcbf29b3ab1c Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 21:16:58 -0700 Subject: [PATCH 064/108] tune new distance aprox and shoot and move --- src/main/deploy/shooter/shooterconfig.json | 50 ++++++++++--------- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/subsystem/VisionManager.java | 14 ++++-- src/main/java/frc/utility/Limelight.java | 20 +++++--- 4 files changed, 51 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 8ebaa834..69ec53f0 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -2,70 +2,74 @@ "shooterConfigs" : [ { "hoodEjectAngle" : 70.0, "flywheelSpeed" : 1800.0, - "distance" : 40.0 + "distance" : 45.0 }, { "hoodEjectAngle" : 62.0, "flywheelSpeed" : 1700.0, - "distance" : 60.0 + "distance" : 62.0 }, { "hoodEjectAngle" : 56.0, "flywheelSpeed" : 1700.0, - "distance" : 82.0 + "distance" : 81.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 1800.0, - "distance" : 106.0 + "flywheelSpeed" : 1900.0, + "distance" : 101.0 }, { "hoodEjectAngle" : 53.0, - "flywheelSpeed" : 1800.0, - "distance" : 115.0 + "flywheelSpeed" : 1900.0, + "distance" : 109.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 1900.0, - "distance" : 120.0 + "distance" : 113.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 1900.0, - "distance" : 133.0 + "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 2000.0, - "distance" : 144.0 + "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, - "distance" : 157.0 + "flywheelSpeed" : 2100.0, + "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 2200.0, - "distance" : 175.0 + "distance" : 160.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 2300.0, - "distance" : 190.0 + "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2420.0, - "distance" : 200.0 + "flywheelSpeed" : 2400.0, + "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2450.0, - "distance" : 210.0 + "flywheelSpeed" : 2400.0, + "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 2450.0, - "distance" : 225.0 + "distance" : 202.0 + }, { + "hoodEjectAngle" : 51.0, + "flywheelSpeed" : 2550.0, + "distance" : 224.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 2700.0, - "distance" : 250.0 + "distance" : 241.0 }, { "hoodEjectAngle" : 51.0, "flywheelSpeed" : 3000.0, - "distance" : 270.0 + "distance" : 265.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3300.0, - "distance" : 299.0 + "flywheelSpeed" : 3200.0, + "distance" : 300.0 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2b78ebf4..cad6602d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -165,7 +165,7 @@ public final class Constants { // TurnPID public static final double DEFAULT_TURN_P = 12.0; - public static final double DEFAULT_TURN_I = 4.0; + public static final double DEFAULT_TURN_I = 20.0; public static final double DEFAULT_TURN_D = 0.0; public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0; public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0; diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index b370c0ff..551af4ff 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -69,6 +69,8 @@ public void logData() { Vector3D correctVector = limelight.getCorrectTargetVector(); logData("New Distance", Math.hypot(correctVector.getX(), correctVector.getZ())); + logData("Old Distance ", limelight.getDistance() + Constants.GOAL_RADIUS_IN + 23); + Vector2d targetPx = limelight.getTargetPosInCameraPixels(); logData("py", targetPx.y); @@ -112,6 +114,8 @@ public void logData() { Math.abs((angleOf(getRelativeGoalTranslation()) .minus(robotTracker.getGyroAngle())).getRadians()) < allowedTurnError); + + logData("Acceleration", getAccel().getNorm()); } @@ -402,8 +406,10 @@ public void update() { if (limelight.areCornersTouchingEdge()) { logData("Using Vision Info", "Corners touching edge"); } else { - robotTracker.addVisionMeasurement(robotTranslation, - getLimelightTime()); + if (!DriverStation.isAutonomous()) { + robotTracker.addVisionMeasurement(robotTranslation, + getLimelightTime()); + } robotPositionOffset = new Translation2d(); logData("Using Vision Info", "Using Vision Info"); blinkinLED.setStatus(limelightUsingVisionStatus); @@ -523,9 +529,9 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat double timeOfFlightFrames; if (distance < 120) { - timeOfFlightFrames = ((0.02 / 30) * (distance - 120)) + (22.5 / 30); + timeOfFlightFrames = ((0.02 / 30) * (distance - 120)) + (22.0 / 30); } else { - timeOfFlightFrames = ((0.077 / 30) * (distance - 120)) + (22.5 / 30); + timeOfFlightFrames = ((0.07 / 30) * (distance - 120)) + (22.0 / 30); } //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542; diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 86c29462..514b4530 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -39,8 +39,9 @@ public final class Limelight extends AbstractSubsystem { volatile double angle = -38; - volatile double hOffset = 56; - volatile double depthOffset = 24; + volatile double hOffset = 43; + volatile double depthOffset = 34.5; + volatile double centerOffset = 6.9; public static @NotNull Limelight getInstance(String name) { LIMELIGHT_MAP_LOCK.readLock().lock(); @@ -166,9 +167,14 @@ private Limelight(String name) { hOffsetTable.addListener(event -> hOffset = event.getEntry().getDouble(0), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - NetworkTableEntry zOffsetTable = limelightGuiTable.getEntry("zOffset"); - zOffsetTable.setDouble(depthOffset); - zOffsetTable.addListener(event -> depthOffset = event.getEntry().getDouble(0), + NetworkTableEntry depthOffsetTable = limelightGuiTable.getEntry("depthOffset"); + depthOffsetTable.setDouble(depthOffset); + depthOffsetTable.addListener(event -> depthOffset = event.getEntry().getDouble(0), + EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + + NetworkTableEntry centerOffsetTable = limelightGuiTable.getEntry("centerOffset"); + centerOffsetTable.setDouble(centerOffset); + centerOffsetTable.addListener(event -> centerOffset = event.getEntry().getDouble(0), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); @@ -354,7 +360,7 @@ public Vector3D getCorrectTargetVector() { double k = hOffset / goalDir.getY(); return goalDir.scalarMultiply(k) - .add(new Vector3D(0, 0, 6.0867) + .add(new Vector3D(0, 0, centerOffset) //5.875 .add(new Vector3D(Math.sin(angle) * depthOffset, 0, Math.cos(angle) * depthOffset))); } else { return new Vector3D(0, 0, 0); @@ -375,7 +381,7 @@ public Vector2d[] getCorners() { public boolean areCornersTouchingEdge() { Vector2d[] corners = getCorners(); for (Vector2d corner : corners) { - if (corner.x < 30 || corner.x > 290 || corner.y < 30 || corner.y > 210) { + if (corner.x < 15 || corner.x > 320 - 15 || corner.y < 0 || corner.y > 240 - 15) { return true; } From c0f0362ca5abe499a7e39c180f8302e9f74b3329 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 21:16:33 -0700 Subject: [PATCH 065/108] invert controls to match up with intake camera in field relative mode --- src/main/java/frc/robot/Robot.java | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 95f25a49..7e7caa99 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -706,12 +706,22 @@ private void doNormalDriving() { } private ControllerDriveInputs getControllerDriveInputs() { - if (xbox.getRawButton(Controller.XboxButtons.X)) { - return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0), - -xbox.getRawAxis(4)).applyDeadZone(0.2, 0.2, 0.2, 0.2).squareInputs(); + if (useFieldRelative) { + if (xbox.getRawButton(Controller.XboxButtons.X)) { + return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0), + -xbox.getRawAxis(4)).applyDeadZone(0.2, 0.2, 0.2, 0.2).squareInputs(); + } else { + return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0), + -xbox.getRawAxis(4)).applyDeadZone(0.05, 0.05, 0.2, 0.2).squareInputs(); + } } else { - return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0), - -xbox.getRawAxis(4)).applyDeadZone(0.05, 0.05, 0.2, 0.2).squareInputs(); + if (xbox.getRawButton(Controller.XboxButtons.X)) { + return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0), + -xbox.getRawAxis(4)).applyDeadZone(0.2, 0.2, 0.2, 0.2).squareInputs(); + } else { + return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0), + -xbox.getRawAxis(4)).applyDeadZone(0.05, 0.05, 0.2, 0.2).squareInputs(); + } } } From 7b74d3017b7016ac80fdf97b1342d5bc31f1cf03 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 21:29:28 -0700 Subject: [PATCH 066/108] update limelight pipelines --- limelight/Intake blue.vpr | 68 +++++++++++++++++++ ...ePipeline 3_27_2022.vpr => Intake red.vpr} | 10 +-- ...oterPipeline 3_27_2022.vpr => Shooter.vpr} | 0 3 files changed, 73 insertions(+), 5 deletions(-) create mode 100644 limelight/Intake blue.vpr rename limelight/{IntakePipeline 3_27_2022.vpr => Intake red.vpr} (89%) rename limelight/{ShooterPipeline 3_27_2022.vpr => Shooter.vpr} (100%) diff --git a/limelight/Intake blue.vpr b/limelight/Intake blue.vpr new file mode 100644 index 00000000..4eb37ce1 --- /dev/null +++ b/limelight/Intake blue.vpr @@ -0,0 +1,68 @@ +area_max:100 +area_min:7.311616000000002 +area_similarity:0 +aspect_max:3.794947199999998 +aspect_min:0.3001249999999999 +black_level:0 +blue_balance:2500 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:100 +convexity_min:77.2 +corner_approx:5.000000 +crop_x_max:1 +crop_x_min:-1 +crop_y_max:0.13 +crop_y_min:-1 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:1 +exposure:1200 +force_convex:1 +hue_max:120 +hue_min:99 +image_flip:1 +image_source:0 +img_to_show:0 +intersection_filter:0 +invert_hue:0 +multigroup_max:7 +multigroup_min:1 +multigroup_rejector:0 +pipeline_led_enabled:0 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1260 +roi_x:0.000000 +roi_y:0.000000 +sat_max:255 +sat_min:127 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0.000000 +val_max:255 +val_min:43 +x_outlier_miqr:1.5 +y_max:1.000000 +y_min:-1.000000 +y_outlier_miqr:1.5 diff --git a/limelight/IntakePipeline 3_27_2022.vpr b/limelight/Intake red.vpr similarity index 89% rename from limelight/IntakePipeline 3_27_2022.vpr rename to limelight/Intake red.vpr index bca6d64f..79361b4a 100644 --- a/limelight/IntakePipeline 3_27_2022.vpr +++ b/limelight/Intake red.vpr @@ -1,19 +1,19 @@ area_max:100 -area_min:1.874161 +area_min:7.311616000000002 area_similarity:0 -aspect_max:3.681249012500001 -aspect_min:0.07206001250000002 +aspect_max:3.794947199999998 +aspect_min:0.3001249999999999 black_level:0 blue_balance:2500 calibration_type:0 contour_grouping:0 contour_sort_final:0 convexity_max:100 -convexity_min:58.6 +convexity_min:77.2 corner_approx:5.000000 crop_x_max:1 crop_x_min:-1 -crop_y_max:-0.07 +crop_y_max:0.13 crop_y_min:-1 cross_a_a:1 cross_a_x:0 diff --git a/limelight/ShooterPipeline 3_27_2022.vpr b/limelight/Shooter.vpr similarity index 100% rename from limelight/ShooterPipeline 3_27_2022.vpr rename to limelight/Shooter.vpr From 179cb13ff4809f4f8a30fa3d89f13bcb7c668326 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 29 Mar 2022 22:58:14 -0700 Subject: [PATCH 067/108] update autos to make them work for our field also tune shooting and moving a bit more --- src/main/deploy/autos/blue/5ball.json | 692 +++++----- src/main/deploy/autos/blue/hidecargo.json | 1201 +++++++++-------- .../deploy/autos/blue/movecargotohanger.json | 737 +++++----- src/main/deploy/autos/red/5ball.json | 692 +++++----- src/main/deploy/autos/red/hidecargo.json | 1201 +++++++++-------- .../deploy/autos/red/movecargotohanger.json | 737 +++++----- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/subsystem/Drive.java | 5 +- src/main/java/frc/subsystem/Hopper.java | 7 + .../java/frc/subsystem/VisionManager.java | 3 +- 11 files changed, 2734 insertions(+), 2546 deletions(-) diff --git a/src/main/deploy/autos/blue/5ball.json b/src/main/deploy/autos/blue/5ball.json index 4aa2b6c7..c0e07aab 100644 --- a/src/main/deploy/autos/blue/5ball.json +++ b/src/main/deploy/autos/blue/5ball.json @@ -212,7 +212,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 7", + "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 6", "closed" : false, "valid" : true, "sendableScript" : { @@ -220,7 +220,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "7" ], + "args" : [ "6" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -736,7 +736,7 @@ } ] }, { "type" : "script", - "script" : "VisionManager.shootBalls 7", + "script" : "VisionManager.shootBalls 3.5", "closed" : false, "valid" : true, "sendableScript" : { @@ -744,7 +744,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "7" ], + "args" : [ "3.5" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -1887,7 +1887,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE", + "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8.5\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE", "closed" : false, "valid" : true, "sendableScript" : { @@ -1895,7 +1895,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "8" ], + "args" : [ "8.5" ], "argTypes" : [ "double" ], "reflection" : true }, { @@ -1920,7 +1920,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 3.9999999999999996, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { "x" : 3.2910027503967285, @@ -1932,755 +1932,713 @@ }, "curvature" : 0.0 }, { - "time" : 0.2373794954493144, - "velocity" : 0.9495179817972574, - "acceleration" : 3.999999999999994, + "time" : 0.23642508906658902, + "velocity" : 0.9457003562663563, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 3.3989983545955056, - "y" : -2.588374874732608 + "x" : 3.3981667469918193, + "y" : -2.5879985818380646 }, "rotation" : { - "radians" : -0.2921778608912438 + "radians" : -0.2891712372126716 } }, - "curvature" : -0.04364084615692234 + "curvature" : -5.874320584109886E-4 }, { - "time" : 0.29579824973185176, - "velocity" : 1.1831929989274066, - "acceleration" : 4.000000000000002, + "time" : 0.3444359926496966, + "velocity" : 1.3777439705987866, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 3.458631632620488, - "y" : -2.606390396730127 + "x" : 3.5184656394194462, + "y" : -2.6236786987501546 }, "rotation" : { - "radians" : -0.29440901549838794 + "radians" : -0.286576361816551 } }, - "curvature" : -0.02595134837823401 + "curvature" : 0.04203266118630113 }, { - "time" : 0.3491550793551233, - "velocity" : 1.3966203174204928, - "acceleration" : 4.000000000000002, + "time" : 0.3927805454968463, + "velocity" : 1.5711221819873853, + "acceleration" : 3.9999999999999942, "pose" : { "translation" : { - "x" : 3.5244835324483574, - "y" : -2.6264017262656125 + "x" : 3.586874350548449, + "y" : -2.643708721189732 }, "rotation" : { - "radians" : -0.2953398995873586 + "radians" : -0.2828782585831171 } }, - "curvature" : -0.0013985693216967878 + "curvature" : 0.06060524122763304 }, { - "time" : 0.40043149369700565, - "velocity" : 1.6017259747880221, - "acceleration" : 4.000000000000003, + "time" : 0.43985280368645924, + "velocity" : 1.7594112147458367, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 3.5980331007389807, - "y" : -2.6487579275583 + "x" : 3.662202759596539, + "y" : -2.665394846008212 }, "rotation" : { - "radians" : -0.2945501713020244 + "radians" : -0.277565073334469 } }, - "curvature" : 0.020718850966936625 + "curvature" : 0.07384725863707324 }, { - "time" : 0.45101233132563245, - "velocity" : 1.8040493253025296, - "acceleration" : 4.0, + "time" : 0.4864477060306216, + "velocity" : 1.9457908241224862, + "acceleration" : 3.999999999999998, "pose" : { "translation" : { - "x" : 3.680485970962536, - "y" : -2.6736677892761236 + "x" : 3.745298805531398, + "y" : -2.688772057520943 }, "rotation" : { - "radians" : -0.2919955872089119 + "radians" : -0.2707950380725529 } }, - "curvature" : 0.0373242313505917 + "curvature" : 0.08213625833510316 }, { - "time" : 0.5015733373341933, - "velocity" : 2.0062933493367727, - "acceleration" : 3.9999999999999876, + "time" : 0.5329979230669721, + "velocity" : 2.131991692267888, + "acceleration" : 4.000000000000004, "pose" : { "translation" : { - "x" : 3.7727913068944403, - "y" : -2.70121209879796 + "x" : 3.8368513446766883, + "y" : -2.7137960128020495 }, "rotation" : { - "radians" : -0.2878084836673059 + "radians" : -0.2627587722278301 } }, - "curvature" : 0.04861193460092709 + "curvature" : 0.08660282922194644 }, { - "time" : 0.5524240768131252, - "velocity" : 2.2096963072525, - "acceleration" : 3.9999999999999964, + "time" : 0.5797279435183833, + "velocity" : 2.3189117740735328, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 3.8756587461102754, - "y" : -2.731355916475877 + "x" : 3.9373990498514004, + "y" : -2.740351675968121 }, "rotation" : { - "radians" : -0.28217372185403494 + "radians" : -0.25363830846069757 } }, - "curvature" : 0.055832145694026246 + "curvature" : 0.08841580171796991 }, { - "time" : 0.6036702471548304, - "velocity" : 2.414680988619321, + "time" : 0.6267362610549605, + "velocity" : 2.5069450442198415, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 3.989575343480716, - "y" : -2.76396084989738 + "x" : 4.047339309509198, + "y" : -2.7682619524618985 }, "rotation" : { - "radians" : -0.2752690347022763 + "radians" : -0.2435910468434367 } }, - "curvature" : 0.06027032950217638 + "curvature" : 0.0885183696531404 }, { - "time" : 0.6294411364762771, - "velocity" : 2.5177645459051075, - "acceleration" : 3.9999999999999933, + "time" : 0.6740431507892504, + "velocity" : 2.696172603157001, + "acceleration" : 3.9999999999999862, "pose" : { "translation" : { - "x" : 4.050772588350831, - "y" : -2.7811177201369857 + "x" : 4.166937126877762, + "y" : -2.7972963233359636 }, "rotation" : { - "radians" : -0.271388278211743 + "radians" : -0.2327470040418315 } }, - "curvature" : 0.06177303417946446 + "curvature" : 0.08758788738466654 }, { - "time" : 0.6553004906081608, - "velocity" : 2.621201962432642, - "acceleration" : 3.999999999999994, + "time" : 0.6978010763245834, + "velocity" : 2.791204305298333, + "acceleration" : 3.999999999999974, "pose" : { "translation" : { - "x" : 4.114822514666457, - "y" : -2.798797328147657 + "x" : 4.230406424149577, + "y" : -2.8121505236061024 }, "rotation" : { - "radians" : -0.26724328091034977 + "radians" : -0.22706006402416112 } }, - "curvature" : 0.06293282064413533 + "curvature" : 0.08688404973771191 }, { - "time" : 0.6812367197554138, - "velocity" : 2.7249468790216542, - "acceleration" : 4.0, + "time" : 0.7216202590268884, + "velocity" : 2.8864810361075524, + "acceleration" : 3.9999999999999747, "pose" : { "translation" : { - "x" : 4.181730948050758, - "y" : -2.816958744670205 + "x" : 4.29633401909814, + "y" : -2.827179479536426 }, "rotation" : { - "radians" : -0.26284758822831167 + "radians" : -0.22121199588447016 } }, - "curvature" : 0.06382850159969175 + "curvature" : 0.08607766603782731 }, { - "time" : 0.7072359906843115, - "velocity" : 2.8289439627372452, - "acceleration" : 4.0, + "time" : 0.74549257464285, + "velocity" : 2.9819702985713983, + "acceleration" : 0.7442098400976622, "pose" : { "translation" : { - "x" : 4.25149297961314, - "y" : -2.835556876071827 + "x" : 4.364719766680094, + "y" : -2.8423431956032283 }, "rotation" : { - "radians" : -0.25821295054670484 + "radians" : -0.21521310387858186 } }, - "curvature" : 0.06452392825617205 + "curvature" : 0.08519994915603576 }, { - "time" : 0.7332828563292066, - "velocity" : 2.9331314253168257, - "acceleration" : 2.5481050539821153, + "time" : 0.7697192113482115, + "velocity" : 2.9999999999999996, + "acceleration" : 1.1864982661111718E-14, "pose" : { "translation" : { - "x" : 4.324093495433467, - "y" : -2.854542847916802 + "x" : 4.435556916364085, + "y" : -2.8575999561866112 }, "rotation" : { - "radians" : -0.253349516956691 + "radians" : -0.2090728501840574 } }, - "curvature" : 0.06507034108588115 + "curvature" : 0.08427499675849662 }, { - "time" : 0.759525327996014, + "time" : 0.7946715843679995, "velocity" : 3.0, - "acceleration" : -1.1017744707610282E-14, + "acceleration" : -1.1500748104073835E-14, "pose" : { "translation" : { - "x" : 4.399507706046279, - "y" : -2.8738643885371857 + "x" : 4.508832390228868, + "y" : -2.8729065953918482 }, "rotation" : { - "radians" : -0.24826603822981036 + "radians" : -0.2028000166117804 } }, - "curvature" : 0.06550851193026688 + "curvature" : 0.08332106007566421 }, { - "time" : 0.7863964782259588, + "time" : 0.8204142117652993, "velocity" : 2.9999999999999996, - "acceleration" : 1.0659361723935359E-14, + "acceleration" : 1.1162447712385717E-14, "pose" : { "translation" : { - "x" : 4.477701675925007, - "y" : -2.8934662126035047 + "x" : 4.584527061061408, + "y" : -2.88821876687075 }, "rotation" : { - "radians" : -0.2429700686181491 + "radians" : -0.19640285659122264 } }, - "curvature" : 0.06587060593952473 + "curvature" : 0.08235160626948937 }, { - "time" : 0.8141710749514438, + "time" : 0.8469370211760767, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.558632852966191, - "y" : -2.913290404695452 + "x" : 4.6626160304549895, + "y" : -2.9034912136430293 }, "rotation" : { - "radians" : -0.23746816090539483 + "radians" : -0.18988923719000408 } }, - "curvature" : 0.06618175066974573 + "curvature" : 0.0813761949653504 }, { - "time" : 0.8428287933072273, + "time" : 0.874228259006809, "velocity" : 3.0, - "acceleration" : -1.0029761020365324E-14, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.642250597973694, - "y" : -2.9332768028725846 + "x" : 4.743068906907318, + "y" : -2.9186780379176653 }, "rotation" : { - "radians" : -0.23176605224269178 + "radians" : -0.18326677164075206 } }, - "curvature" : 0.06646132969406916 + "curvature" : 0.08040119291556454 }, { - "time" : 0.8723468917576519, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.9022745981913911, + "velocity" : 3.0, + "acceleration" : -1.0284611572537565E-14, "pose" : { "translation" : { - "x" : 4.728496714142921, - "y" : -2.9533633822450156 + "x" : 4.825850083918625, + "y" : -2.9337329709142717 }, "rotation" : { - "radians" : -0.2258688402689826 + "radians" : -0.17654294326329226 } }, - "curvature" : 0.06672403127896764 + "curvature" : 0.07943035076297603 }, { - "time" : 0.9027004208083538, + "time" : 0.9310612442543232, "velocity" : 2.9999999999999996, - "acceleration" : 9.500654106330315E-15, + "acceleration" : 1.0032243113209958E-14, "pose" : { "translation" : { - "x" : 4.817305976545033, - "y" : -2.9734866385441103 + "x" : 4.910919018089771, + "y" : -2.9486096426844597 }, "rotation" : { - "radians" : -0.21978115021622427 + "radians" : -0.16972522188764874 } }, - "curvature" : 0.0669806864818383 + "curvature" : 0.07846526410378302 }, { - "time" : 0.9338624302784291, + "time" : 0.9605720395861806, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.9086066616111665, - "y" : -2.9935819716931817 + "x" : 4.998230507220355, + "y" : -2.9632618519332032 }, "rotation" : { - "radians" : -0.21350729439917845 + "radians" : -0.16282117397282284 } }, - "curvature" : 0.06723892993087238 + "curvature" : 0.07750573849274361 }, { - "time" : 0.9658041749806907, + "time" : 0.990789565834994, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.002321076616648, - "y" : -3.0135840693781866 + "x" : 5.087734968406814, + "y" : -2.9776438358402046 }, "rotation" : { - "radians" : -0.20705142586622954 + "radians" : -0.15583856762569545 } }, - "curvature" : 0.06750371317272791 + "curvature" : 0.07655007526138907 }, { - "time" : 0.9984953186414615, + "time" : 1.0216952443158518, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.098366089165211, - "y" : -3.0334272906184196 + "x" : 5.179378716140528, + "y" : -2.991710539881261 }, "rotation" : { - "radians" : -0.20041768816116357 + "radians" : -0.14878547368600106 } }, - "curvature" : 0.0677776962888439 + "curvature" : 0.07559529234150263 }, { - "time" : 1.0319041358746839, + "time" : 1.0532694343409463, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.19665365667321, - "y" : -3.0530460493372082 + "x" : 5.273104240405928, + "y" : -3.005417887649628 }, "rotation" : { - "radians" : -0.1936103631941936 + "radians" : -0.14167036397863442 } }, - "curvature" : 0.06806153927030212 + "curvature" : 0.07463729185441531 }, { - "time" : 1.0659977120081605, + "time" : 1.0854915293736447, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -2.7038611726562314E-14, "pose" : { "translation" : { - "x" : 5.297091355853844, - "y" : -3.072375197932608 + "x" : 5.368850484778598, + "y" : -3.0187230506773854 }, "rotation" : { - "radians" : -0.18663401919376696 + "radians" : -0.13450220775752084 } }, - "curvature" : 0.0683541107864204 + "curvature" : 0.07367098410111733 }, { - "time" : 1.1007421405426991, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.1183400509132173, + "velocity" : 2.999999999999999, + "acceleration" : 2.6550284696523992E-14, "pose" : { "translation" : { - "x" : 5.399582912201364, - "y" : -3.0913504108481 + "x" : 5.466553124523379, + "y" : -3.031584718256802 }, "rotation" : { - "radians" : -0.17949366063978456 + "radians" : -0.1272905672847317 } }, - "curvature" : 0.0686526286409456 + "curvature" : 0.0726903757761293 }, { - "time" : 1.136102718008151, + "time" : 1.151792740021911, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -8.698956559016603E-15, "pose" : { "translation" : { - "x" : 5.504028729475294, - "y" : -3.1099085681432825 + "x" : 5.566144844692476, + "y" : -3.0439633672617026 }, "rotation" : { - "radians" : -0.17219488198191107 + "radians" : -0.1200456934097773 } }, - "curvature" : 0.06895274341837182 + "curvature" : 0.07168862870971174 }, { - "time" : 1.1720441359642337, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.1858266464134202, + "velocity" : 2.9999999999999996, + "acceleration" : 8.558717775961129E-15, "pose" : { "translation" : { - "x" : 5.61032641918465, - "y" : -3.127988139064569 + "x" : 5.66755561822356, + "y" : -3.0558215319688316 }, "rotation" : { - "radians" : -0.16474402683304648 + "radians" : -0.11277862194006867 } }, - "curvature" : 0.06924857455843654 + "curvature" : 0.0706580941863296 }, { - "time" : 1.2085306698791871, + "time" : 1.2204182150319014, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -8.428702249529382E-15, "pose" : { "translation" : { - "x" : 5.718371330072152, - "y" : -3.145529565615881 + "x" : 5.770712984037872, + "y" : -3.067124073879218 }, "rotation" : { - "radians" : -0.15714835420132006 + "radians" : -0.10550127152586215 } }, - "curvature" : 0.06953270631102976 + "curvature" : 0.06959032685672162 }, { - "time" : 1.2455263646064414, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.2555433700639118, + "velocity" : 2.9999999999999996, + "acceleration" : 8.308289645985856E-15, "pose" : { "translation" : { - "x" : 5.828057077598442, - "y" : -3.162475646129346 + "x" : 5.875542325138333, + "y" : -3.0778384515395416 }, "rotation" : { - "radians" : -0.149416213183665 + "radians" : -0.09822654372203397 } }, - "curvature" : 0.06979614966205201 + "curvature" : 0.06847608142197704 }, { - "time" : 1.2829952161694935, + "time" : 1.2911775963425318, "velocity" : 3.0, - "acceleration" : -7.810331647113909E-15, + "acceleration" : -8.19691056891909E-15, "pose" : { "translation" : { - "x" : 5.939276073426299, - "y" : -3.1787719188359915 + "x" : 5.9819671467076425, + "y" : -3.0879349903634985 }, "rotation" : { - "radians" : -0.1415572273868729 + "radians" : -0.0909684258345216 } }, - "curvature" : 0.07002827533191112 + "curvature" : 0.06730529458972327 }, { - "time" : 1.3209013495592037, + "time" : 1.327296018124022, "velocity" : 2.9999999999999996, - "acceleration" : 7.728429806013464E-15, - "pose" : { - "translation" : { - "x" : 6.051920054904861, - "y" : -3.1943670454364383 - }, - "rotation" : { - "radians" : -0.13358249016273918 - } - }, - "curvature" : 0.07021672228872737 - }, { - "time" : 1.359209192247121, - "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.165880614553835, - "y" : -3.209213194671598 + "x" : 6.089909354206384, + "y" : -3.097387152453166 }, "rotation" : { - "radians" : -0.12550477153574247 + "radians" : -0.08374209711050397 } }, - "curvature" : 0.07034728585337109 + "curvature" : 0.06606705425850384 }, { - "time" : 1.397883643124852, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.3638734752432662, + "velocity" : 2.9999999999999996, + "acceleration" : 7.999202988055673E-15, "pose" : { "translation" : { - "x" : 6.281049729547716, - "y" : -3.2232664258933683 + "x" : 6.199289531471132, + "y" : -3.1061718064203667 }, "rotation" : { - "radians" : -0.11733873745676503 + "radians" : -0.07656403878726785 } }, - "curvature" : 0.07040378937764218 + "curvature" : 0.06474955745525926 }, { - "time" : 1.4368902365948655, + "time" : 1.400884596685671, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.397320291200003, - "y" : -3.2364870726353274 + "x" : 6.310027218812557, + "y" : -3.114269497208035 }, "rotation" : { - "radians" : -0.10910118172585662 + "radians" : -0.06945214847377139 } }, - "curvature" : 0.0703679436328688 + "curvature" : 0.06334005821608486 }, { - "time" : 1.4761953015647518, + "time" : 1.438303871650907, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -7.831878599476166E-15, "pose" : { "translation" : { - "x" : 6.514586634447419, - "y" : -3.2488401261834285 + "x" : 6.422041191113526, + "y" : -3.1216647159115825 }, "rotation" : { - "radians" : -0.10081127058141126 + "radians" : -0.06242585929932353 } }, - "curvature" : 0.0702191984436713 + "curvature" : 0.06182480635063325 }, { - "time" : 1.515766115137486, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.4761057182287096, + "velocity" : 2.9999999999999996, + "acceleration" : 7.758609490328921E-15, "pose" : { "translation" : { - "x" : 6.63274506733412, - "y" : -3.260295619146696 + "x" : 6.535249735927209, + "y" : -3.1283461696002632 }, "rotation" : { - "radians" : -0.0924907995421401 + "radians" : -0.055506264223218596 } }, - "curvature" : 0.06993459173630182 + "curvature" : 0.06018897785906844 }, { - "time" : 1.55557105084767, + "time" : 1.5142645498598688, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.7516944004959205, - "y" : -3.2708290090279206 + "x" : 6.6495709315751865, + "y" : -3.1343070511385376 }, "rotation" : { - "radians" : -0.08416446160106997 + "radians" : -0.04871624585462448 } }, - "curvature" : 0.06948860204346692 + "curvature" : 0.05841659767581544 }, { - "time" : 1.5955797213714447, + "time" : 1.5527548398175375, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.871336476644501, - "y" : -3.2804215617943555 + "x" : 6.76492292524555, + "y" : -3.1395453090074383 }, "rotation" : { - "radians" : -0.07586012529605445 + "radians" : -0.0420806120796796 } }, - "curvature" : 0.06885301162269934 + "curvature" : 0.05649045540029702 }, { - "time" : 1.6357631157394636, + "time" : 1.5915511840162078, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.991576700051632, - "y" : -3.289060735448409 + "x" : 6.881224211091007, + "y" : -3.144063917125937 }, "rotation" : { - "radians" : -0.06760912050688539 + "radians" : -0.03562623772763141 } }, - "curvature" : 0.06799678871171229 + "curvature" : 0.05439201475116482 }, { - "time" : 1.6760937312120248, + "time" : 1.6306283625393438, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 7.1123245660333865, - "y" : -3.296740563598341 + "x" : 6.998393908326989, + "y" : -3.147871144672308 }, "rotation" : { - "radians" : -0.05944652904214673 + "radians" : -0.029382212423449006 } }, - "curvature" : 0.06688599907381494 + "curvature" : 0.05210131766377214 }, { - "time" : 1.7165457001373738, + "time" : 1.6699614003730316, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 7.233494190434357, - "y" : -3.30346203902896 + "x" : 7.116352039329751, + "y" : -3.150980825905494 }, "rotation" : { - "radians" : -0.05141147616548517 + "radians" : -0.02337999466219851 } }, - "curvature" : 0.06548375889504356 + "curvature" : 0.04959688426535946 }, { - "time" : 1.757094912312602, + "time" : 1.7095256279435234, "velocity" : 3.0, - "acceleration" : -7.287757495654728E-15, + "acceleration" : -1.1557888191446533, "pose" : { "translation" : { - "x" : 7.355004839111871, - "y" : -3.3092334972723165 + "x" : 7.235019807734481, + "y" : -3.1534126299864695 }, "rotation" : { - "radians" : -0.043547418155515716 + "radians" : -0.017653571989563004 } }, - "curvature" : 0.06375024330143432 + "curvature" : 0.04685561044273053 }, { - "time" : 1.7977191336056264, - "velocity" : 2.9999999999999996, - "acceleration" : 7.27794583372083E-15, + "time" : 1.749606195370589, + "velocity" : 2.953675328302824, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { - "x" : 7.476781457420211, - "y" : -3.314071000178397 + "x" : 7.354319876533402, + "y" : -3.1551923307996104 }, "rotation" : { - "radians" : -0.03590241977860944 + "radians" : -0.012239626968958514 } }, - "curvature" : 0.061642767292292996 + "curvature" : 0.04385266540863671 }, { - "time" : 1.8383981218802636, - "velocity" : 3.0, - "acceleration" : -0.8549861225542413, + "time" : 1.791367879808199, + "velocity" : 2.786628590552384, + "acceleration" : -3.9999999999999964, "pose" : { "translation" : { - "x" : 7.598755199694828, - "y" : -3.3179987194858214 + "x" : 7.4741766461738735, + "y" : -3.1563520767740556 }, "rotation" : { - "radians" : -0.02852941415912364 + "radians" : -0.007177708340391282 } }, - "curvature" : 0.059115958757323624 + "curvature" : 0.040561392634885035 }, { - "time" : 1.8793527503450278, - "velocity" : 2.964984361008262, + "time" : 1.8359816679429726, + "velocity" : 2.6081734380132895, "acceleration" : -3.9999999999999964, "pose" : { "translation" : { - "x" : 7.720863958736559, - "y" : -3.3210493203925378 + "x" : 7.594516532656503, + "y" : -3.156930660705073 }, "rotation" : { - "radians" : -0.021486435933739608 + "radians" : -0.002510406406084059 } }, - "curvature" : 0.05612204648295773 + "curvature" : 0.036953218827764284 }, { - "time" : 1.9217846589717384, - "velocity" : 2.7952567265014197, + "time" : 1.8840509532237966, + "velocity" : 2.415896296889994, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 7.843052895295841, - "y" : -3.3232643451265176 - }, - "rotation" : { - "radians" : -0.014836816748393312 - } - }, - "curvature" : 0.05261128966471575 - }, { - "time" : 1.966973534915382, - "velocity" : 2.6145012227268447, - "acceleration" : -4.0000000000000036, - "pose" : { - "translation" : { - "x" : 7.965274967556932, - "y" : -3.3246945965164496 + "x" : 7.715268245633247, + "y" : -3.1569737895754244 }, "rotation" : { - "radians" : -0.008649330067547475 + "radians" : 0.0017164688167075213 } }, - "curvature" : 0.048532579406757856 + "curvature" : 0.03299757736886057 }, { - "time" : 2.0155229987855723, - "velocity" : 2.4203033672460843, + "time" : 1.9364483118057918, + "velocity" : 2.206306862562013, "acceleration" : -3.9999999999999964, "pose" : { "translation" : { - "x" : 8.087491460622125, - "y" : -3.325400521562436 + "x" : 7.836363066505513, + "y" : -3.156534354376733 }, "rotation" : { - "radians" : -0.0029982698845735147 + "radians" : 0.005453708791556059 } }, - "curvature" : 0.04383424692658223 + "curvature" : 0.028661854946767117 }, { - "time" : 2.0683070326652815, - "velocity" : 2.2091672317272475, - "acceleration" : -4.000000000000005, + "time" : 1.9945178704913586, + "velocity" : 1.9740286278197459, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { - "x" : 8.209672515995964, - "y" : -3.3254525950066887 + "x" : 7.957735126522266, + "y" : -3.1556726999308466 }, "rotation" : { - "radians" : 0.0020365547800811692 + "radians" : 0.008648529987311705 } }, - "curvature" : 0.03846511750567603 + "curvature" : 0.023911373099727667 }, { - "time" : 2.126672618228889, - "velocity" : 1.9757048894728169, + "time" : 2.060528833894078, + "velocity" : 1.7099847742088685, "acceleration" : -3.9999999999999982, "pose" : { "translation" : { - "x" : 8.331797661069459, - "y" : -3.324931702904223 - }, - "rotation" : { - "radians" : 0.006369930747651973 - } - }, - "curvature" : 0.032375853273367367 - }, { - "time" : 2.19289366493448, - "velocity" : 1.7108207026504523, - "acceleration" : -3.999999999999999, - "pose" : { - "translation" : { - "x" : 8.453856338604307, - "y" : -3.3239295261935533 + "x" : 8.079321684878137, + "y" : -3.1544568947112026 }, "rotation" : { - "radians" : 0.00991148144607044 + "radians" : 0.011244403839107409 } }, - "curvature" : 0.025520631044257825 + "curvature" : 0.018709420234176016 }, { - "time" : 2.2714115520395897, - "velocity" : 1.3967491542300143, - "acceleration" : -4.000000000000001, + "time" : 2.1389153715933484, + "velocity" : 1.3964386234117863, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 8.575848436217107, - "y" : -3.3225489242673873 + "x" : 8.201063406811521, + "y" : -3.1529630006641938 }, "rotation" : { - "radians" : 0.012566065756910601 + "radians" : 0.013180898713490029 } }, - "curvature" : 0.017859202608647286 + "curvature" : 0.013017354553867993 }, { - "time" : 2.373702045251545, - "velocity" : 0.9875871813821927, - "acceleration" : -4.000000000000001, + "time" : 2.2411422808075967, + "velocity" : 0.9875309865547927, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 8.697784815863574, - "y" : -3.320904318543324 + "x" : 8.322904641702685, + "y" : -3.1512753430305347 }, "rotation" : { - "radians" : 0.014234366939993203 + "radians" : 0.01439354131558399 } }, - "curvature" : 0.009359382499354215 + "curvature" : 0.0067948044075592335 }, { - "time" : 2.620598840597093, + "time" : 2.488025027446295, "velocity" : 0.0, - "acceleration" : -4.000000000000001, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 8.819687843322754, - "y" : -3.319122076034546 + "x" : 8.444793701171875, + "y" : -3.149486780166626 }, "rotation" : { "radians" : 0.014813707347434221 @@ -2692,8 +2650,8 @@ "x" : [ 3.2910027503967285, 1.676727533340454, 0.0 ], "y" : [ -2.5561599731445312, -0.49745607376098633, 0.0 ] }, { - "x" : [ 8.819687843322754, 7.801394462585449, 0.0 ], - "y" : [ -3.319122076034546, 0.11557602882385254, 0.0 ] + "x" : [ 8.444793701171875, 7.801394462585449, 0.0 ], + "y" : [ -3.149486780166626, 0.11557602882385254, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -2701,7 +2659,7 @@ "radians" : 0.0 } }, { - "time" : 2.5299999434500933, + "time" : 2.3999999463558197, "rotation" : { "radians" : 0.0 } diff --git a/src/main/deploy/autos/blue/hidecargo.json b/src/main/deploy/autos/blue/hidecargo.json index 9932a2f5..16cbc83b 100644 --- a/src/main/deploy/autos/blue/hidecargo.json +++ b/src/main/deploy/autos/blue/hidecargo.json @@ -29,7 +29,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 2.351956220256751, + "acceleration" : 3.9999999999999996, "pose" : { "translation" : { "x" : 6.1293768882751465, @@ -41,9 +41,9 @@ }, "curvature" : -0.0 }, { - "time" : 0.3188834866654645, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.24452127463632814, + "velocity" : 0.9780850985453124, + "acceleration" : 3.9999999999999996, "pose" : { "translation" : { "x" : 6.039938468317359, @@ -55,9 +55,9 @@ }, "curvature" : -0.10063347960741297 }, { - "time" : 0.47317666597794406, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.3430023265109939, + "velocity" : 1.3720093060439753, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { "x" : 5.954285601692391, @@ -69,9 +69,9 @@ }, "curvature" : -0.1855623376134662 }, { - "time" : 0.6188308342429313, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.4150553084726476, + "velocity" : 1.6602212338905902, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 5.87496604004582, @@ -83,9 +83,9 @@ }, "curvature" : -0.26531768438500725 }, { - "time" : 0.7539530640358325, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.4721670734641104, + "velocity" : 1.8886682938564414, + "acceleration" : 3.999999999999993, "pose" : { "translation" : { "x" : 5.803342186380178, @@ -97,9 +97,9 @@ }, "curvature" : -0.3381985496587611 }, { - "time" : 0.8779901904625292, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5190911939858679, + "velocity" : 2.076364775943471, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 5.7397725735722815, @@ -111,9 +111,9 @@ }, "curvature" : -0.3907488038191573 }, { - "time" : 0.9914536504675615, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5585735987097502, + "velocity" : 2.2342943948390004, + "acceleration" : 0.26324528204804704, "pose" : { "translation" : { "x" : 5.683793342890567, @@ -125,9 +125,9 @@ }, "curvature" : -0.39766870774643054 }, { - "time" : 1.095639209373063, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5934744799766615, + "velocity" : 2.243481887171834, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.634299722512424, @@ -139,9 +139,9 @@ }, "curvature" : -0.32812227024366547 }, { - "time" : 1.1923614795366175, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6267989018195489, + "velocity" : 2.110184199800284, + "acceleration" : -3.999999999999997, "pose" : { "translation" : { "x" : 5.589727506041527, @@ -153,9 +153,9 @@ }, "curvature" : -0.16341344710143665 }, { - "time" : 1.2837283378298283, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6603386127840065, + "velocity" : 1.9760253559424537, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.548234531025173, @@ -167,9 +167,9 @@ }, "curvature" : 0.07956164338822755 }, { - "time" : 1.3719691365115938, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6950498794552016, + "velocity" : 1.8371802892576732, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { "x" : 5.507882157471613, @@ -181,9 +181,9 @@ }, "curvature" : 0.33953937734428047 }, { - "time" : 1.4592981567041168, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.732203292039014, + "velocity" : 1.6885666389224232, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.466816746367385, @@ -195,9 +195,9 @@ }, "curvature" : 0.5355105351904276 }, { - "time" : 1.5477626861241707, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.773517717518194, + "velocity" : 1.5233089370057031, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { "x" : 5.42345113819465, @@ -209,9 +209,9 @@ }, "curvature" : 0.6126486386243994 }, { - "time" : 1.6390285515859833, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8214715782483804, + "velocity" : 1.3314934940849577, + "acceleration" : -3.9999999999999982, "pose" : { "translation" : { "x" : 5.376646131448524, @@ -223,9 +223,9 @@ }, "curvature" : 0.568293999561127 }, { - "time" : 1.7340988034913118, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8802038597478065, + "velocity" : 1.0965643680872534, + "acceleration" : -3.999999999999999, "pose" : { "translation" : { "x" : 5.3258919611544115, @@ -237,9 +237,9 @@ }, "curvature" : 0.4354667748411578 }, { - "time" : 1.8330035326706784, - "velocity" : 0.75, - "acceleration" : -3.6944293888894246, + "time" : 0.9592448110914289, + "velocity" : 0.7804005627127639, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { "x" : 5.271489777385341, @@ -251,9 +251,9 @@ }, "curvature" : 0.24590943352090008 }, { - "time" : 2.0360118787648114, + "time" : 1.1543449517696198, "velocity" : 0.0, - "acceleration" : -3.6944293888894246, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { "x" : 5.214733123779297, @@ -278,7 +278,7 @@ "radians" : 2.37094272169677 } }, { - "time" : 1.939999956637621, + "time" : 1.0699999760836363, "rotation" : { "radians" : 2.370942522371697 } @@ -288,13 +288,10 @@ "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, - "constraints" : [ { - "type" : "MaxVelocityConstraint", - "maxVelocity" : 0.75 - } ] + "constraints" : [ ] }, { "type" : "script", - "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", + "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 7", "closed" : false, "valid" : true, "sendableScript" : { @@ -302,22 +299,27 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "10" ], + "args" : [ "7" ], "argTypes" : [ "double" ], "reflection" : true } ] } }, { "type" : "script", - "script" : "Drive.setAutoRotation 40", + "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "NONE", "delay" : 0.0, "commands" : [ { - "methodName" : "frc.subsystem.Drive.setAutoRotation", - "args" : [ "40" ], + "methodName" : "frc.subsystem.Hopper.setEjectOverride", + "args" : [ "true" ], + "argTypes" : [ "boolean" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Drive.turnToAngle", + "args" : [ "45" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -545,6 +547,56 @@ "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 1.3\nIntake.setIntakeSolState CLOSE\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.3, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "OFF" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "OFF" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "@t 3.36\nIntake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 3.36, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { @@ -2088,16 +2140,36 @@ } ], "reversed" : false, "color" : 52.47058, - "closed" : false, + "closed" : true, "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 0.89\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 0.89, + "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "REVERSE" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.000000000000001, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 4.012234210968018, @@ -2109,982 +2181,995 @@ }, "curvature" : 0.0 }, { - "time" : 0.18393581920253443, - "velocity" : 0.7357432768101378, - "acceleration" : 4.000000000000001, + "time" : 0.18393922011618816, + "velocity" : 0.7357568804647526, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 4.079866022458708, - "y" : -0.9175092776647826 + "x" : 4.079868928493841, + "y" : -0.9175222116396733 }, "rotation" : { - "radians" : 0.032422495075255596 + "radians" : 0.03184996886547707 } }, - "curvature" : 0.05303783852412645 + "curvature" : 0.03638401924476792 }, { - "time" : 0.2600740573066539, - "velocity" : 1.0402962292266158, - "acceleration" : 3.999999999999995, + "time" : 0.26009263225753565, + "velocity" : 1.0403705290301426, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 4.147437401812738, - "y" : -0.9151584665515866 + "x" : 4.1474601409882865, + "y" : -0.9152593300483591 }, "rotation" : { - "radians" : 0.03767860106119578 + "radians" : 0.035461574602982625 } }, - "curvature" : 0.10177004653056979 + "curvature" : 0.06998452771616054 }, { - "time" : 0.31842255095187355, - "velocity" : 1.2736902038074942, - "acceleration" : 3.9999999999999902, + "time" : 0.3184715235418354, + "velocity" : 1.2738860941673413, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.214887726988865, - "y" : -0.9123489750712632 + "x" : 4.214962774536021, + "y" : -0.9126807022060877 }, "rotation" : { - "radians" : 0.046083444431288076 + "radians" : 0.04125182940160068 } }, - "curvature" : 0.14663006175024168 + "curvature" : 0.1010514081151579 }, { - "time" : 0.36752120065207927, - "velocity" : 1.4700848026083166, - "acceleration" : 3.9999999999999933, + "time" : 0.36761655293009865, + "velocity" : 1.4704662117203942, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 4.28215626266865, - "y" : -0.9088819079360633 + "x" : 4.2823301801504385, + "y" : -0.9096479069208385 }, "rotation" : { - "radians" : 0.05737092549763 + "radians" : 0.04904809314793145 } }, - "curvature" : 0.18797575110518114 + "curvature" : 0.12981170585502438 }, { - "time" : 0.41067486026855743, - "velocity" : 1.642699441074229, + "time" : 0.4108314858481353, + "velocity" : 1.643325943392541, "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.349182263537044, - "y" : -0.904578393853863 + "x" : 4.349514283856005, + "y" : -0.9060353399753465 }, "rotation" : { - "radians" : 0.0712929378915065 + "radians" : 0.05868962955532622 } }, - "curvature" : 0.22608984719455819 + "curvature" : 0.15646782863946646 }, { - "time" : 0.4495779246156969, - "velocity" : 1.7983116984627867, - "acceleration" : 3.999999999999997, + "time" : 0.44980816479061647, + "velocity" : 1.7992326591624657, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.415905077562968, - "y" : -0.8992789224692164 + "x" : 4.416465736770604, + "y" : -0.9017298085207326 }, "rotation" : { - "radians" : 0.08761614166764932 + "radians" : 0.07002652572535734 } }, - "curvature" : 0.2611858070311432 + "curvature" : 0.1811979163878778 }, { - "time" : 0.48523766875483854, - "velocity" : 1.940950675019353, - "acceleration" : 3.9999999999999867, + "time" : 0.4855501313208, + "velocity" : 1.9422005252831998, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.482264249279902, - "y" : -0.8928426813044079 + "x" : 4.48313406518789, + "y" : -0.8966301254701357 }, "rotation" : { - "radians" : 0.10611908836229655 + "radians" : 0.08291864609124491 } }, - "curvature" : 0.29341692608996234 + "curvature" : 0.20415755775322134 }, { - "time" : 0.5183117595380216, - "velocity" : 2.0732470381520844, + "time" : 0.5187103847104131, + "velocity" : 2.0748415388416523, "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.548199623066466, - "y" : -0.8851468927005044 + "x" : 4.549467820659629, + "y" : -0.8906467038923438 }, "rotation" : { - "radians" : 0.12658982254096487 + "radians" : 0.0972346733352272 } }, - "curvature" : 0.32288731915441804 + "curvature" : 0.22548228467606032 }, { - "time" : 0.5492586656367637, - "velocity" : 2.197034662547053, - "acceleration" : 4.0, + "time" : 0.5497420218631486, + "velocity" : 2.1989680874525943, + "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.613651446427005, - "y" : -0.876086150758407 + "x" : 4.615414730078044, + "y" : -0.8837011514054263 }, "rotation" : { - "radians" : 0.14882402397746977 + "radians" : 0.1128512659439996 } }, - "curvature" : 0.3496638342427625 + "curvature" : 0.24529045745559744 }, { - "time" : 0.5784138965550384, - "velocity" : 2.3136555862201518, - "acceleration" : -1.6236735964798095, + "time" : 0.5789746947138208, + "velocity" : 2.315898778855283, + "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.678560473272171, - "y" : -0.8655717582799038 + "x" : 4.680921845758164, + "y" : -0.8757258645703647 }, "rotation" : { - "radians" : 0.17262371101994242 + "radians" : 0.12965234596402142 } }, - "curvature" : 0.3737882085974883 + "curvature" : 0.263686278415705 }, { - "time" : 0.6069779983301187, - "velocity" : 2.267276808360792, - "acceleration" : -1.8021731001922658, + "time" : 0.6066571501940509, + "velocity" : 2.4266286007762035, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.742868067199511, - "y" : -0.8535310637087223 + "x" : 4.745935695520172, + "y" : -0.8666636232846845 }, "rotation" : { - "radians" : 0.19779649220341822 + "radians" : 0.14752851931001348 } }, - "curvature" : 0.3952888936073791 + "curvature" : 0.2807627581933577 }, { - "time" : 0.6360217316741164, - "velocity" : 2.214934973399082, - "acceleration" : -1.4376529363116377, + "time" : 0.6329825891472886, + "velocity" : 2.531930356589154, + "acceleration" : 0.9017846067562265, "pose" : { "translation" : { - "x" : 4.80651630477405, - "y" : -0.8399067980715813 + "x" : 4.8104024327717525, + "y" : -0.8564671851760863 }, "rotation" : { - "radians" : 0.22415532600922183 + "radians" : 0.1663766232708782 } }, - "curvature" : 0.41419203433477625 + "curvature" : 0.29660451731621684 }, { - "time" : 0.6655393241600903, - "velocity" : 2.1724989198887714, - "acceleration" : -1.1396966079827222, + "time" : 0.6584873043239601, + "velocity" : 2.5549301161351785, + "acceleration" : -2.1207024597033652, "pose" : { "translation" : { - "x" : 4.869448078808871, - "y" : -0.824656411919243 + "x" : 4.87426798659043, + "y" : -0.8450988799960781 }, "rotation" : { - "radians" : 0.25151872624934885 + "radians" : 0.18609939066967865 } }, - "curvature" : 0.4305311361191539 + "curvature" : 0.3112903462074792 }, { - "time" : 0.6954246504047454, - "velocity" : 2.138438714939281, - "acceleration" : -0.8908554566969, + "time" : 0.683981890618659, + "velocity" : 2.500863684270891, + "acceleration" : -1.823821978528963, "pose" : { "translation" : { - "x" : 4.931607201645704, - "y" : -0.8077514122675655 + "x" : 4.937478211805924, + "y" : -0.8325302040136062 }, "rotation" : { - "radians" : 0.2797113326941941 + "radians" : 0.20660521679714633 } }, - "curvature" : 0.4443550158057545 + "curvature" : 0.3248954755335266 }, { - "time" : 0.725580967378834, - "velocity" : 2.111573795409033, - "acceleration" : -0.6793465022751951, + "time" : 0.7098179589134984, + "velocity" : 2.4537432950759874, + "acceleration" : -1.5828512441983833, "pose" : { "translation" : { - "x" : 4.992938508435509, - "y" : -0.7891766995385554 + "x" : 4.999979039082493, + "y" : -0.8187414144086866 }, "rotation" : { - "radians" : 0.3085647538618645 + "radians" : 0.22780801329582814 } }, - "curvature" : 0.45573373321158034 + "curvature" : 0.3374935302608176 }, { - "time" : 0.7559198081982444, - "velocity" : 2.0909632100152824, - "acceleration" : -0.4179681793139011, + "time" : 0.7359324293185369, + "velocity" : 2.4124079731037904, + "acceleration" : -1.3044976586946302, "pose" : { "translation" : { - "x" : 5.053387960419059, - "y" : -0.7689299045014195 + "x" : 5.0617166250012815, + "y" : -0.8037211236660369 }, "rotation" : { - "radians" : 0.33791858215085874 + "radians" : 0.24962713231241265 } }, - "curvature" : 0.4647623301686379 + "curvature" : 0.3491581576802025 }, { - "time" : 0.8167858290266697, - "velocity" : 2.0655231501075435, - "acceleration" : -0.1384063526866644, + "time" : 0.7887463969414447, + "velocity" : 2.343512275993333, + "acceleration" : -1.0334589786307198, "pose" : { "translation" : { - "x" : 5.171431395063053, - "y" : -0.7234702639619126 + "x" : 5.182688729168589, + "y" : -0.7699798315917139 }, "rotation" : { - "radians" : 0.39753225232864264 + "radians" : 0.29481885303544775 } }, - "curvature" : 0.47628134337229905 + "curvature" : 0.36998936253987214 }, { - "time" : 0.8775052043910777, - "velocity" : 2.0571192028259433, - "acceleration" : 0.07651708161584311, + "time" : 0.8420624190091038, + "velocity" : 2.288412354282637, + "acceleration" : -0.8513997934004414, "pose" : { "translation" : { - "x" : 5.28533164196233, - "y" : -0.6715829475066926 + "x" : 5.299973998423866, + "y" : -0.7313669207204043 }, "rotation" : { - "radians" : 0.457468911728272 + "radians" : 0.3416439837677253 } }, - "curvature" : 0.4801807958619656 + "curvature" : 0.3880209156918146 }, { - "time" : 0.9376075969896146, - "velocity" : 2.061718062505713, - "acceleration" : 0.23719713417019256, + "time" : 0.8955555645929367, + "velocity" : 2.242868301184222, + "acceleration" : -0.736658519915954, "pose" : { "translation" : { - "x" : 5.39470746489593, - "y" : -0.6136396617759576 + "x" : 5.413165126823472, + "y" : -0.6880507100495148 }, "rotation" : { - "radians" : 0.51683377374029 + "radians" : 0.389654433979741 } }, - "curvature" : 0.47804100648283726 + "curvature" : 0.403939345595272 }, { - "time" : 0.9967126677163254, - "velocity" : 2.0757376158970153, - "acceleration" : 0.3466981374198625, + "time" : 0.9489629734818855, + "velocity" : 2.203525278399543, + "acceleration" : -0.6767969498415867, "pose" : { "translation" : { - "x" : 5.49920556001598, - "y" : -0.5501521453061287 + "x" : 5.5218728309992, + "y" : -0.6402942652402999 }, "rotation" : { - "radians" : 0.5749352590162321 + "radians" : 0.43849160494770656 } }, - "curvature" : 0.471605425281159 + "curvature" : 0.41849245218579234 }, { - "time" : 1.0545224472486359, - "velocity" : 2.0957801587855203, - "acceleration" : 0.40356633939226144, + "time" : 1.0020763940584052, + "velocity" : 2.167578277357701, + "acceleration" : -0.6641591172113208, "pose" : { "translation" : { - "x" : 5.5985038608263835, - "y" : -0.48175095064352114 + "x" : 5.625730652793365, + "y" : -0.5884424192140703 }, "rotation" : { - "radians" : 0.6312847326123074 + "radians" : 0.48788654495705386 } }, - "curvature" : 0.46262836247598543 + "curvature" : 0.4324880614534242 }, { - "time" : 1.1108140478616002, - "velocity" : 2.1184975539834254, - "acceleration" : 0.40345921603853396, + "time" : 1.0547362906352467, + "velocity" : 2.1326037269347866, + "acceleration" : -0.693255970599057, "pose" : { "translation" : { - "x" : 5.692314843161512, - "y" : -0.4091642264580173 + "x" : 5.724399761893892, + "y" : -0.5329087927484011 }, "rotation" : { - "radians" : 0.6855833217165795 + "radians" : 0.5376585244703848 } }, - "curvature" : 0.4527597065949554 + "curvature" : 0.4467899297328142 }, { - "time" : 1.1654323160372946, - "velocity" : 2.1405337976429735, - "acceleration" : 0.34034527303420153, + "time" : 1.1068266505846867, + "velocity" : 2.0964917738891833, + "acceleration" : -0.7590255374673471, "pose" : { "translation" : { - "x" : 5.780388830164895, - "y" : -0.33319649965673825 + "x" : 5.817573758469408, + "y" : -0.4741628150733401 }, "rotation" : { - "radians" : 0.7377000747034669 + "radians" : 0.5877118587042041 } }, - "curvature" : 0.443485601912384 + "curvature" : 0.4623143546066941 }, { - "time" : 1.2182824818871505, - "velocity" : 2.1585211017690455, - "acceleration" : 0.20771430856564294, + "time" : 1.1582699222585076, + "velocity" : 2.0574450169578826, + "acceleration" : -0.85555708396171, "pose" : { "translation" : { - "x" : 5.862517297267914, - "y" : -0.25470745749771595 + "x" : 5.904983475804329, + "y" : -0.4127167444676161 }, "rotation" : { - "radians" : 0.7876462908989643 + "radians" : 0.638030997754264 } }, - "curvature" : 0.4361251242987918 + "curvature" : 0.4800287263774246 }, { - "time" : 1.2693229851547776, - "velocity" : 2.169122944614123, - "acceleration" : 1.0817549407792827E-4, + "time" : 1.20902166214638, + "velocity" : 2.014024006373431, + "acceleration" : -0.9751306653340864, "pose" : { "translation" : { - "x" : 5.938536177168487, - "y" : -0.17459072970356537 + "x" : 5.986401782933953, + "y" : -0.3491126888548468 }, "rotation" : { - "radians" : 0.8355501360445428 + "radians" : 0.6886738736416058 } }, - "curvature" : 0.431872317394874 + "curvature" : 0.500950042603538 }, { - "time" : 1.3185585791987948, - "velocity" : 2.169128270698835, - "acceleration" : -0.2848041321796146, + "time" : 1.259064520666594, + "velocity" : 1.9652256804493953, + "acceleration" : -1.1075634112905204, "pose" : { "translation" : { - "x" : 6.008329164809766, - "y" : -0.09375267057515657 + "x" : 6.061648387279547, + "y" : -0.2839096263997476 }, "rotation" : { - "radians" : 0.8816343285891601 + "radians" : 0.7397632167792167 } }, - "curvature" : 0.431870196556052 + "curvature" : 0.5261369978659279 }, { - "time" : 1.3660336232443555, - "velocity" : 2.15560718197925, - "acceleration" : -0.6435155746795979, + "time" : 1.3084011828915338, + "velocity" : 1.9105821985338527, + "acceleration" : -1.239915254926958, "pose" : { "translation" : { - "x" : 6.071831022358822, - "y" : -0.013091141105286752 + "x" : 6.130594637283437, + "y" : -0.2176704261043394 }, "rotation" : { - "radians" : 0.9261982414140092 + "radians" : 0.7914750099875708 } }, - "curvature" : 0.43730501698866603 + "curvature" : 0.5566628639559956 }, { - "time" : 1.4118252194909213, - "velocity" : 2.126139576605145, - "acceleration" : -1.063489692084961, + "time" : 1.3570458238963947, + "velocity" : 1.8502669660814803, + "acceleration" : -1.3565877307387766, "pose" : { "translation" : { - "x" : 6.12903088418534, - "y" : 0.06652570890764764 + "x" : 6.193168325044098, + "y" : -0.1509488684041571 }, "rotation" : { - "radians" : 0.9696044259991614 + "radians" : 0.8440224046461846 } }, - "curvature" : 0.4495108307569809 + "curvature" : 0.5935467253109423 }, { - "time" : 1.4560355521525066, - "velocity" : 2.0791223435359023, - "acceleration" : -1.52002157710405, + "time" : 1.4050135519869393, + "velocity" : 1.7851945346824336, + "acceleration" : -1.439730441538962, "pose" : { "translation" : { - "x" : 6.179975561840308, - "y" : 0.14428065874597884 + "x" : 6.249358488951245, + "y" : -0.0842766657644578 }, "rotation" : { - "radians" : 1.0122682566684933 + "radians" : 0.8976322459176475 } }, - "curvature" : 0.4700711657871874 + "curvature" : 0.6376063101064825 }, { - "time" : 1.4987824321910335, - "velocity" : 2.014146163523463, - "acceleration" : -1.974071718041697, + "time" : 1.4523072062995137, + "velocity" : 1.7171044208769999, + "acceleration" : -1.4696274540546912, "pose" : { "translation" : { - "x" : 6.224772849034707, - "y" : 0.21942863465909568 + "x" : 6.2992202163209186, + "y" : -0.018150483276428986 }, "rotation" : { - "radians" : 1.0546477790856124 + "radians" : 0.9525098498997762 } }, - "curvature" : 0.500889279611352 + "curvature" : 0.6891762313763988 }, { - "time" : 1.540186616781137, - "velocity" : 1.9324113337155617, - "acceleration" : -2.3720307789417294, + "time" : 1.4989007546283921, + "velocity" : 1.6486292630710562, + "acceleration" : -1.4243277798746545, "pose" : { "translation" : { - "x" : 6.263594826618203, - "y" : 0.2913179277497715 + "x" : 6.3428794460305795, + "y" : 0.04698104074660314 }, "rotation" : { - "radians" : 1.0972283316685527 + "radians" : 1.0087860125729582 } }, - "curvature" : 0.5441574151816366 + "curvature" : 0.7476144640522491 }, { - "time" : 1.580353908329549, - "velocity" : 1.8371332818560024, - "acceleration" : -2.6474197214125605, + "time" : 1.544718402885982, + "velocity" : 1.5833699138492454, + "acceleration" : -1.277051637682657, "pose" : { "translation" : { - "x" : 6.296681167557836, - "y" : 0.3594114118604921 + "x" : 6.380537771154195, + "y" : 0.11073027417296544 }, "rotation" : { - "radians" : 1.140492186777639 + "radians" : 1.0664389649669272 } }, - "curvature" : 0.6020636034649972 + "curvature" : 0.8105110247651124 }, { - "time" : 1.6193492670845173, - "velocity" : 1.7338962000445413, - "acceleration" : -2.721672977384236, + "time" : 1.589608352044505, + "velocity" : 1.5260431307608624, + "acceleration" : -0.9882350321042662, "pose" : { "translation" : { - "x" : 6.324342441916713, - "y" : 0.42330776145978355 + "x" : 6.41247724159733, + "y" : 0.17278357045642956 }, "rotation" : { - "radians" : 1.1848561982155952 + "radians" : 1.1251844168307794 } }, - "curvature" : 0.6758923235708024 + "curvature" : 0.8725495261033641 }, { - "time" : 1.6571590723066265, - "velocity" : 1.6309902748913652, - "acceleration" : -2.4945738768301835, + "time" : 1.6333097898626403, + "velocity" : 1.482855838955655, + "acceleration" : -0.4841929472411742, "pose" : { "translation" : { - "x" : 6.346963421832697, - "y" : 0.4827626695285403 + "x" : 6.4390651667322345, + "y" : 0.2329142480400037 }, "rotation" : { - "radians" : 1.2305496048186084 + "radians" : 1.1843313194749898 } }, - "curvature" : 0.7638726499983753 + "curvature" : 0.9241146185221707 }, { - "time" : 1.6936360168640041, - "velocity" : 1.539995841891948, - "acceleration" : -1.7970788234851456, + "time" : 1.6754107043468742, + "velocity" : 1.4624708730899851, + "acceleration" : -3.7808476405994242, "pose" : { "translation" : { - "x" : 6.3650063864971, - "y" : 0.5377100654463529 + "x" : 6.460758918032937, + "y" : 0.2909955697597244 }, "rotation" : { - "radians" : 1.2773923275414163 + "radians" : 1.2426127877010575 } }, - "curvature" : 0.8568101590190533 + "curvature" : 0.9500561047849169 }, { - "time" : 1.7284182440412232, - "velocity" : 1.477489437998118, - "acceleration" : 1.705632879833736, + "time" : 1.7178366346797787, + "velocity" : 1.3020648944905882, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.37901442713337, - "y" : 0.5882833328778361 + "x" : 6.478110731710331, + "y" : 0.3470137222484482 }, "rotation" : { - "radians" : 1.3244344674414545 + "radians" : 1.2980233032835509 } }, - "curvature" : 0.9308397775185885 + "curvature" : 0.9295171145481738 }, { - "time" : 1.7883248954688422, - "velocity" : 1.5796681923938034, - "acceleration" : 3.9999999999999862, + "time" : 1.7639291544880524, + "velocity" : 1.117694815257493, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 6.397521991248141, - "y" : 0.6779655956833608 + "x" : 6.491772511347264, + "y" : 0.40108079533964336 }, "rotation" : { - "radians" : 1.4085741509062624 + "radians" : 1.3477231257633573 } }, - "curvature" : 0.8143141037035398 + "curvature" : 0.838208547283802 }, { - "time" : 1.8363455674677351, - "velocity" : 1.7717508803893742, - "acceleration" : 4.000000000000008, + "time" : 1.8167467720854111, + "velocity" : 0.9064243448680579, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.408572673797607, - "y" : 0.7576718926429749 + "x" : 6.502500630533632, + "y" : 0.4534477614711818 }, "rotation" : { - "radians" : 1.4464418628084068 + "radians" : 1.3880989582767027 } }, - "curvature" : 0.0 + "curvature" : 0.6544921319595709 }, { - "time" : 1.879584211551695, - "velocity" : 1.9447054567252142, - "acceleration" : 3.999999999999995, + "time" : 2.0433528583024256, + "velocity" : 0.0, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.417938666279497, - "y" : 0.8374714001847678 + "x" : 6.51873254776001, + "y" : 0.5548575520515442 }, "rotation" : { - "radians" : 1.4673635124320494 + "radians" : 1.4247813178399886 } }, - "curvature" : 0.4361537758239235 + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], + "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] }, { - "time" : 1.9241999105096688, - "velocity" : 2.123168252557109, - "acceleration" : 4.000000000000008, + "x" : [ 6.51873254776001, 0.23627901077270508, 0.0 ], + "y" : [ 0.5548575520515442, 1.6066665649414062, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : -0.13962634015954636 + } + }, { + "time" : 1.9499999564141035, + "rotation" : { + "radians" : -0.1526493858119171 + } + } ], + "reversed" : false, + "color" : 286.3529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "sleep 2000", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "sleep", + "args" : [ "2000" ], + "argTypes" : [ "long" ], + "reflection" : false + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { - "x" : 6.425398940205923, - "y" : 0.9279097366234055 + "x" : 6.51873254776001, + "y" : 0.5548575520515442 }, "rotation" : { - "radians" : 1.509659326369691 + "radians" : 1.4550369939781984 } }, - "curvature" : 0.44722076145528444 + "curvature" : 0.0 }, { - "time" : 1.9728110085852033, - "velocity" : 2.317612644859248, - "acceleration" : 3.999999999999993, + "time" : 0.1788531666911449, + "velocity" : 0.7154126667645795, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 6.429642048575715, - "y" : 1.035761920819823 + "x" : 6.526029236446348, + "y" : 0.6184169998749507 }, "rotation" : { - "radians" : 1.550908753938559 + "radians" : 1.4593465391163816 } }, - "curvature" : 0.31730031892680094 + "curvature" : 0.13050101301586398 }, { - "time" : 1.9987893246112054, - "velocity" : 2.421525908963256, - "acceleration" : 4.000000000000006, + "time" : 0.25321186756390435, + "velocity" : 1.012847470255617, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.430305899986379, - "y" : 1.0973157606626955 + "x" : 6.532831722088758, + "y" : 0.6823114984076142 }, "rotation" : { - "radians" : 1.5684899825119734 + "radians" : 1.471236108776517 } }, - "curvature" : 0.25637867502172557 + "curvature" : 0.23504966165445965 }, { - "time" : 2.025781495555629, - "velocity" : 2.5294945927409493, - "acceleration" : 3.999999999999994, + "time" : 0.3106349893777712, + "velocity" : 1.2425399575110843, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.429927970748395, - "y" : 1.1641340877395123 + "x" : 6.538718402694414, + "y" : 0.746799069553143 }, "rotation" : { - "radians" : 1.583853790503598 + "radians" : 1.4891403961437595 } }, - "curvature" : 0.20594813300808948 + "curvature" : 0.31358925447041286 }, { - "time" : 2.0536364380792893, - "velocity" : 2.6409143628355918, - "acceleration" : 4.000000000000005, + "time" : 0.3594524552162631, + "velocity" : 1.437809820865052, + "acceleration" : 3.9999999999999964, "pose" : { "translation" : { - "x" : 6.42849117103934, - "y" : 1.2361304745226107 + "x" : 6.543342575678253, + "y" : 0.8120593882322282 }, "rotation" : { - "radians" : 1.597166880767735 + "radians" : 1.511568465726473 } }, - "curvature" : 0.16589805557058687 + "curvature" : 0.3681705103657191 }, { - "time" : 2.082166732992299, - "velocity" : 2.7550355424876307, - "acceleration" : 3.999999999999967, + "time" : 0.4028800665262472, + "velocity" : 1.611520266104988, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 6.4260035555757895, - "y" : 1.3130642882106258 + "x" : 6.546427705950194, + "y" : 0.8782000352682129 }, "rotation" : { - "radians" : 1.6086729537639617 + "radians" : 1.5371749251346154 } }, - "curvature" : 0.13471207633826426 + "curvature" : 0.4020595428820239 }, { - "time" : 2.1111641583855323, - "velocity" : 2.8710252440605633, - "acceleration" : 4.000000000000011, + "time" : 0.44254982612556115, + "velocity" : 1.7701993045022437, + "acceleration" : 3.9999999999999933, "pose" : { "translation" : { - "x" : 6.422495681211174, - "y" : 1.3945594657209313 + "x" : 6.547762694002358, + "y" : 0.9452627502726614 }, "rotation" : { - "radians" : 1.6186275742658227 + "radians" : 1.5647989424802173 } }, - "curvature" : 0.11061622885252656 + "curvature" : 0.41904605440094195 }, { - "time" : 2.1404115791378824, - "velocity" : 2.988014927069964, - "acceleration" : 0.40226107984247866, + "time" : 0.4794110889330761, + "velocity" : 1.9176443557323033, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.418017964533647, - "y" : 1.4801232886820799 + "x" : 6.547197143996286, + "y" : 1.0132296845309288 }, "rotation" : { - "radians" : 1.6272684320752597 + "radians" : 1.5934786052963947 } }, - "curvature" : 0.09202002758878106 + "curvature" : 0.42293973727499673 }, { - "time" : 2.170205843436901, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.5140610781999098, + "velocity" : 2.056244312799638, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.412638039463943, - "y" : 1.5691651584262445 + "x" : 6.544636631850153, + "y" : 1.08202965388773 }, "rotation" : { - "radians" : 1.6348043576150604 + "radians" : 1.622447172268418 } }, - "curvature" : 0.07763517137648186 + "curvature" : 0.41723748069828426 }, { - "time" : 2.200892251234442, - "velocity" : 2.9999999999999996, - "acceleration" : 9.430296455303456E-15, + "time" : 0.5468931637504397, + "velocity" : 2.187572655001758, + "acceleration" : 3.1633627206418256, "pose" : { "translation" : { - "x" : 6.406438114853245, - "y" : 1.6610153709816586 + "x" : 6.540037973325994, + "y" : 1.1515443916327097 }, "rotation" : { - "radians" : 1.6414135047463148 + "radians" : 1.651117099798838 } }, - "curvature" : 0.06646668401976608 + "curvature" : 0.4049376530908519 }, { - "time" : 2.232286754539466, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 0.5783519506192766, + "velocity" : 2.287088208619253, + "acceleration" : 1.870434307392289, "pose" : { "translation" : { - "x" : 6.399512332081045, - "y" : 1.7549438920650573 + "x" : 6.533404492116915, + "y" : 1.2216148013860106 }, "rotation" : { - "radians" : 1.6472455716072028 + "radians" : 1.6790569837278395 } }, - "curvature" : 0.05776403436142682 + "curvature" : 0.38847034737186514 }, { - "time" : 2.2641313884937704, - "velocity" : 3.0, - "acceleration" : -0.5220281507380499, + "time" : 0.608993635935249, + "velocity" : 2.3444014680705667, + "acceleration" : 2.165397894604544, "pose" : { "translation" : { - "x" : 6.3919641226530075, - "y" : 1.8501791320741177 + "x" : 6.524781287934317, + "y" : 1.2920472099838438 }, "rotation" : { - "radians" : 1.6524254678653565 + "radians" : 1.7059655876740503 } }, - "curvature" : 0.050967461408882746 + "curvature" : 0.36970875427049765 }, { - "time" : 2.2962499027431713, - "velocity" : 2.9832332314019316, - "acceleration" : -4.0, + "time" : 0.6390132720691309, + "velocity" : 2.409405924951669, + "acceleration" : 2.411756982149806, "pose" : { "translation" : { - "x" : 6.383903565798832, - "y" : 1.9459267210798998 + "x" : 6.514250504595111, + "y" : 1.3626196203640575 }, "rotation" : { - "radians" : 1.6570571945158592 + "radians" : 1.731646007957078 } }, - "curvature" : 0.04566147685699515 + "curvature" : 0.3500287810963245 }, { - "time" : 2.329098019757857, - "velocity" : 2.851840763343189, - "acceleration" : -4.000000000000004, + "time" : 0.668275787449217, + "velocity" : 2.479980000734858, + "acceleration" : 2.6202614568139766, "pose" : { "translation" : { - "x" : 6.375444746070116, - "y" : 2.0413882838192876 + "x" : 6.501926598108938, + "y" : 1.4330879644517065 }, "rotation" : { - "radians" : 1.6612273952864063 + "radians" : 1.7559819224996793 } }, - "curvature" : 0.04153801107443314 + "curvature" : 0.3303903442679772 }, { - "time" : 2.3631515088169963, - "velocity" : 2.7156268071066307, - "acceleration" : -3.999999999999995, + "time" : 0.6966740713701192, + "velocity" : 2.554390929532458, + "acceleration" : 2.7997101251293675, "pose" : { "translation" : { - "x" : 6.366703110938218, - "y" : 2.135780214687429 + "x" : 6.487951604765385, + "y" : 1.5031923560446216 }, "rotation" : { - "radians" : 1.6650083738352117 + "radians" : 1.7789169353574112 } }, - "curvature" : 0.038368183750837316 + "curvature" : 0.3114217758149773 }, { - "time" : 2.3983080444520524, - "velocity" : 2.5750006645664074, - "acceleration" : -4.0, + "time" : 0.7241232587242202, + "velocity" : 2.6312406972943077, + "acceleration" : 1.2935503366228165, "pose" : { "translation" : { - "x" : 6.357792828392121, - "y" : 2.228352452730178 + "x" : 6.4724904092212086, + "y" : 1.572663343698979 }, "rotation" : { - "radians" : 1.6684605177848264 + "radians" : 1.8004373323306622 } }, - "curvature" : 0.03598061733215407 + "curvature" : 0.2934962442471143 }, { - "time" : 2.4344694314979898, - "velocity" : 2.4303551163826573, - "acceleration" : -3.999999999999995, + "time" : 0.7507742479488775, + "velocity" : 2.665715093377194, + "acceleration" : -3.999999999999994, "pose" : { "translation" : { - "x" : 6.3488241445362945, - "y" : 2.318407256636533 + "x" : 6.455726012587547, + "y" : 1.6412281636148691 }, "rotation" : { - "radians" : 1.6716341182037762 + "radians" : 1.8205581086255052 } }, - "curvature" : 0.03424396145033817 + "curvature" : 0.27679481544391576 }, { - "time" : 2.4715493896338248, - "velocity" : 2.2820352838393174, - "acceleration" : -4.000000000000005, + "time" : 0.7774623046076926, + "velocity" : 2.5589628667419335, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 6.339900741188558, - "y" : 2.4053179797310804 + "x" : 6.437854800517144, + "y" : 1.7086169925218666 }, "rotation" : { - "radians" : 1.6745705658893675 + "radians" : 1.8393118764446346 } }, - "curvature" : 0.03305114421605748 + "curvature" : 0.2613545325842005 }, { - "time" : 2.509484949213048, - "velocity" : 2.1302930455224227, - "acceleration" : -4.0, + "time" : 0.8048451310611667, + "velocity" : 2.449431560928037, + "acceleration" : -3.9999999999999933, "pose" : { "translation" : { - "x" : 6.331117093477943, - "y" : 2.4885478449664333 + "x" : 6.419081811291562, + "y" : 1.7745692005645992 }, "rotation" : { - "radians" : 1.6773028650946311 + "radians" : 1.856740154150982 } }, - "curvature" : 0.03230248453046758 + "curvature" : 0.24710198818221107 }, { - "time" : 2.5482536718462243, - "velocity" : 1.9752181549897188, + "time" : 0.8329039549239173, + "velocity" : 2.3371962654770346, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.322555827442557, - "y" : 2.567668719915673 + "x" : 6.399616003908406, + "y" : 1.8388396041883173 }, "rotation" : { - "radians" : 1.6798553438041104 + "radians" : 1.8728865291753276 } }, - "curvature" : 0.03188409115100577 + "curvature" : 0.23387381346481986 }, { - "time" : 2.587900600923266, - "velocity" : 1.8166304386815513, - "acceleration" : -3.999999999999997, + "time" : 0.861625693206511, + "velocity" : 2.22230931234666, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { - "x" : 6.314285077627446, - "y" : 2.64237989176479 + "x" : 6.379665526168537, + "y" : 1.901204719024463 }, "rotation" : { - "radians" : 1.682242375809382 + "radians" : 1.8877912376445647 } }, - "curvature" : 0.031637223157170435 + "curvature" : 0.22142585640379966 }, { - "time" : 2.6285823403067847, - "velocity" : 1.6539034811474767, + "time" : 0.921065056241802, + "velocity" : 1.9845518602054957, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.306355844682457, - "y" : 2.712526842305124 + "x" : 6.339110703361712, + "y" : 2.0194711581041815 }, "rotation" : { - "radians" : 1.6844659116451925 + "radians" : 1.9139944386552825 } }, - "curvature" : 0.03131479782522635 + "curvature" : 0.19748286503535473 }, { - "time" : 2.6706436297897875, - "velocity" : 1.4856583232154645, - "acceleration" : -4.000000000000005, + "time" : 0.9834254421957473, + "velocity" : 1.7351103163897141, + "acceleration" : -3.999999999999998, "pose" : { "translation" : { - "x" : 6.298799352960103, - "y" : 2.7781200229258047 + "x" : 6.298886488657445, + "y" : 2.1282522362307645 }, "rotation" : { - "radians" : 1.6865117620775738 + "radians" : 1.9354586912044225 } }, - "curvature" : 0.03052674983936547 + "curvature" : 0.17157685598367362 }, { - "time" : 2.71476243308551, - "velocity" : 1.3091831100325741, - "acceleration" : -3.9999999999999996, + "time" : 1.0497222979942022, + "velocity" : 1.4699228931958945, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.260146206276204, + "y" : 2.2271790434867285 + }, + "rotation" : { + "radians" : 1.9520508222111153 + } + }, + "curvature" : 0.13862722514403217 + }, { + "time" : 1.1228723260834714, + "velocity" : 1.1773227808388174, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 6.291624408113421, - "y" : 2.839353629606194 + "x" : 6.223575937663554, + "y" : 2.316830133948315 }, "rotation" : { - "radians" : 1.6883451359021284 + "radians" : 1.9634500574230802 } }, - "curvature" : 0.028694423143857225 + "curvature" : 0.09407105630824328 }, { - "time" : 2.815943111560705, - "velocity" : 0.904460396131795, + "time" : 1.2119347577273158, + "velocity" : 0.8210730542634398, "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.278326433749044, - "y" : 2.9505502779693416 + "x" : 6.189243100281146, + "y" : 2.3989316180237097 }, "rotation" : { - "radians" : 1.6911174188705012 + "radians" : 1.969501422562912 } }, - "curvature" : 0.018977039717131546 + "curvature" : 0.0409346875176687 }, { - "time" : 3.0420582105936536, + "time" : 1.4172030212931759, "velocity" : 0.0, "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.265983581542969, - "y" : 3.052058696746826 + "x" : 6.156445026397705, + "y" : 2.4765572547912598 }, "rotation" : { - "radians" : 1.6921482303347304 + "radians" : 1.9710366419843302 } }, "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], - "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + "x" : [ 6.51873254776001, 0.23627471923828125, 0.0 ], + "y" : [ 0.5548575520515442, 2.0319605469703674, 0.0 ] }, { - "x" : [ 6.408572673797607, 0.15555095672607422, 0.0 ], - "y" : [ 0.7576718926429749, 1.2444130182266235, 0.0 ] - }, { - "x" : [ 6.265983581542969, -0.19443941116333008, 0.0 ], - "y" : [ 3.052058696746826, 1.5944044589996338, 0.0 ] + "x" : [ 6.156445026397705, -0.519803524017334, 0.0 ], + "y" : [ 2.4765572547912598, 1.2286276817321777, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : -0.13962634015954636 - } - }, { - "time" : 1.8299999590963125, - "rotation" : { - "radians" : 0.4537856055185257 + "radians" : 0.4704038872114269 } }, { - "time" : 2.9499999340623617, + "time" : 1.3299999702721834, "rotation" : { - "radians" : 0.4537856055185257 + "radians" : 0.8760571821323136 } } ], "reversed" : false, - "color" : 286.3529, + "color" : 74.117645, "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] - }, { - "type" : "script", - "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "sleep", - "args" : [ "1000" ], - "argTypes" : [ "long" ], - "reflection" : false - }, { - "methodName" : "frc.subsystem.Intake.setWantedIntakeState", - "args" : [ "EJECT" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], - "reflection" : true - } ] - } - }, { - "type" : "script", - "script" : "Intake.setIntakeSolState CLOSE", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "frc.subsystem.Intake.setIntakeSolState", - "args" : [ "CLOSE" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], - "reflection" : true - } ] - } } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/blue/movecargotohanger.json b/src/main/deploy/autos/blue/movecargotohanger.json index 8eb15ca5..7607216c 100644 --- a/src/main/deploy/autos/blue/movecargotohanger.json +++ b/src/main/deploy/autos/blue/movecargotohanger.json @@ -24,263 +24,320 @@ "reflection" : true } ] } + }, { + "type" : "script", + "script" : "@t 0.01\nDrive.setAutoRotation 173", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 0.01, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.setAutoRotation", + "args" : [ "173" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 2.351956220256751, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.1293768882751465, - "y" : 1.0937262773513794 + "x" : 6.6141839027404785, + "y" : 1.5894602537155151 }, "rotation" : { - "radians" : 2.4178655244299407 + "radians" : 2.389889403743233 } }, "curvature" : -0.0 }, { - "time" : 0.3188834866654645, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.2489252451296045, + "velocity" : 0.995700980518418, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 6.039938468317359, - "y" : 1.173101702633744 + "x" : 6.523176953443453, + "y" : 1.6735778746319738 }, "rotation" : { - "radians" : 2.411607121974174 + "radians" : 2.4064673708251867 } }, - "curvature" : -0.10063347960741297 + "curvature" : 0.25911994232595476 }, { - "time" : 0.47317666597794406, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.3496677436327589, + "velocity" : 1.3986709745310357, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 5.954285601692391, - "y" : 1.2509135147447523 + "x" : 6.432098919045529, + "y" : 1.7526409019337734 }, "rotation" : { - "radians" : 2.395009715228919 + "radians" : 2.4514777463969604 } }, - "curvature" : -0.1855623376134662 + "curvature" : 0.48603924486171285 }, { - "time" : 0.6188308342429313, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.4240995092316322, + "velocity" : 1.6963980369265288, + "acceleration" : -2.790576412342658, "pose" : { "translation" : { - "x" : 5.87496604004582, - "y" : 1.3260262403808838 + "x" : 6.340960895459375, + "y" : 1.823079613135178 }, "rotation" : { - "radians" : 2.3703948601089895 + "radians" : 2.519876340424447 } }, - "curvature" : -0.26531768438500725 + "curvature" : 0.701310596977904 }, { - "time" : 0.7539530640358325, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.49210983031570427, + "velocity" : 1.5066100391134667, + "acceleration" : -1.7711769053800082, "pose" : { "translation" : { - "x" : 5.803342186380178, - "y" : 1.3977211365709081 + "x" : 6.249847903382033, + "y" : 1.8827598318457603 }, "rotation" : { - "radians" : 2.339767645326564 + "radians" : 2.607059147103057 } }, - "curvature" : -0.3381985496587611 + "curvature" : 0.895203949702412 }, { - "time" : 0.8779901904625292, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5274607298209384, + "velocity" : 1.4439973423253865, + "acceleration" : -1.205251380120216, "pose" : { "translation" : { - "x" : 5.7397725735722815, - "y" : 1.4656408262000014 + "x" : 6.204343319524796, + "y" : 1.9082412143392418 }, "rotation" : { - "radians" : 2.3056558386592805 + "radians" : 2.6558796477618074 } }, - "curvature" : -0.3907488038191573 + "curvature" : 0.9745203573253068 }, { - "time" : 0.9914536504675615, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.563119080457446, + "velocity" : 1.401020066007925, + "acceleration" : -0.687574960710818, "pose" : { "translation" : { - "x" : 5.683793342890567, - "y" : 1.529733933533862 + "x" : 6.15890158834145, + "y" : 1.9307801831205325 }, "rotation" : { - "radians" : 2.271678768601324 + "radians" : 2.706939505672123 } }, - "curvature" : -0.39766870774643054 + "curvature" : 1.0352255751207677 }, { - "time" : 1.095639209373063, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5987086013693859, + "velocity" : 1.376549602565181, + "acceleration" : -0.17765980611680962, "pose" : { "translation" : { - "x" : 5.634299722512424, - "y" : 1.5901997197428273 + "x" : 6.113546424879203, + "y" : 1.9504234287532398 }, "rotation" : { - "radians" : 2.242697343821613 + "radians" : 2.7591326119824173 } }, - "curvature" : -0.32812227024366547 + "curvature" : 1.0723584365876502 }, { - "time" : 1.1923614795366175, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6338600425589757, + "velocity" : 1.3703046043387122, + "acceleration" : 0.36242756952241223, "pose" : { "translation" : { - "x" : 5.589727506041527, - "y" : 1.6474327184259892 + "x" : 6.068302920743008, + "y" : 1.9672693488100776 }, "rotation" : { - "radians" : 2.224227521884603 + "radians" : 2.81126068578529 } }, - "curvature" : -0.16341344710143665 + "curvature" : 1.0821549980493446 }, { - "time" : 1.2837283378298283, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6682116558661673, + "velocity" : 1.3827545760588114, + "acceleration" : 0.9785292578911144, "pose" : { "translation" : { - "x" : 5.548234531025173, - "y" : 1.7019673711353107 + "x" : 6.023197003472021, + "y" : 1.9814617121025577 }, "rotation" : { - "radians" : 2.2209763317101947 + "radians" : 2.862095726190118 } }, - "curvature" : 0.07956164338822755 + "curvature" : 1.0627558269737196 }, { - "time" : 1.3719691365115938, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.7014108163316237, + "velocity" : 1.4152409259116825, + "acceleration" : 2.2441271164822805, "pose" : { "translation" : { - "x" : 5.507882157471613, - "y" : 1.7544226628997421 + "x" : 5.978254895916052, + "y" : 1.9931833229106815 }, "rotation" : { - "radians" : 2.234944043969041 + "radians" : 2.91045196330826 } }, - "curvature" : 0.33953937734428047 + "curvature" : 1.0145254489555913 }, { - "time" : 1.4592981567041168, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.7626515005088358, + "velocity" : 1.5526728059056913, + "acceleration" : 3.9999999999999925, "pose" : { "translation" : { - "x" : 5.466816746367385, - "y" : 1.805446757749337 + "x" : 5.888965234160423, + "y" : 2.010102666914463 }, "rotation" : { - "radians" : 2.264150133653273 + "radians" : 2.9955886329173094 } }, - "curvature" : 0.5355105351904276 + "curvature" : 0.8428762374068068 }, { - "time" : 1.5477626861241707, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8162079335949602, + "velocity" : 1.7668985382501887, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.42345113819465, - "y" : 1.8556616342393681 + "x" : 5.800629080792987, + "y" : 2.0200297651595065 }, "rotation" : { - "radians" : 2.30296870864004 + "radians" : 3.0601867848614575 } }, - "curvature" : 0.6126486386243994 + "curvature" : 0.6011409133906767 }, { - "time" : 1.6390285515859833, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8631605793414101, + "velocity" : 1.9547091212359882, + "acceleration" : -2.958595604807148, "pose" : { "translation" : { - "x" : 5.376646131448524, - "y" : 1.9056077209744444 + "x" : 5.713411706194165, + "y" : 2.025186080179992 }, "rotation" : { - "radians" : 2.343996330815072 + "radians" : 3.1009099300153964 } }, - "curvature" : 0.568293999561127 + "curvature" : 0.3280169918222083 }, { - "time" : 1.7340988034913118, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.9087395705485002, + "velocity" : 1.8198593181791478, + "acceleration" : -3.9999999999999973, "pose" : { "translation" : { - "x" : 5.3258919611544115, - "y" : 1.9556885321326263 + "x" : 5.627431205854464, + "y" : 2.027809408056328 }, "rotation" : { - "radians" : 2.3801609988469874 + "radians" : 3.1173842319085643 } }, - "curvature" : 0.4354667748411578 + "curvature" : 0.05731406395107486 }, { - "time" : 1.8330035326706784, - "velocity" : 0.75, - "acceleration" : -3.6944293888894246, + "time" : 0.957952684084819, + "velocity" : 1.6230068640338726, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { - "x" : 5.271489777385341, - "y" : 2.006115302989542 + "x" : 5.5427412004210055, + "y" : 2.0299511337652802 }, "rotation" : { - "radians" : 2.405692727133013 + "radians" : 3.1118848820659286 } }, - "curvature" : 0.24590943352090008 + "curvature" : -0.17922803729629205 }, { - "time" : 2.0360118787648114, + "time" : 1.0131511813558638, + "velocity" : 1.4022128749496938, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.459313535744059, + "y" : 2.0332734865301063 + }, + "rotation" : { + "radians" : 3.0893774981886746 + } + }, + "curvature" : -0.3450488004527902 + }, { + "time" : 1.077964961333034, + "velocity" : 1.1429577550410128, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.377020982923568, + "y" : 2.0388467951706843 + }, + "rotation" : { + "radians" : 3.0578354549476097 + } + }, + "curvature" : -0.3971872754778253 + }, { + "time" : 1.161849251134293, + "velocity" : 0.8074205958359769, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.295619938355685, + "y" : 2.0469467434536455 + }, + "rotation" : { + "radians" : 3.0283983068361184 + } + }, + "curvature" : -0.2932585838727471 + }, { + "time" : 1.3637044000932872, "velocity" : 0.0, - "acceleration" : -3.6944293888894246, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.214733123779297, "y" : 2.056851625442505 }, "rotation" : { - "radians" : 2.4154850346736763 + "radians" : 3.0150708953014127 } }, "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ], - "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ] + "x" : [ 6.6141839027404785, -1.4558343887329102, 0.0 ], + "y" : [ 1.5894602537155151, 1.360889196395874, 0.0 ] }, { - "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ], - "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ] + "x" : [ 5.214733123779297, -1.292677402496338, 0.0 ], + "y" : [ 2.056851625442505, 0.1644301414489746, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : 2.37094272169677 + "radians" : 2.3941697598804894 } }, { - "time" : 1.939999956637621, + "time" : 1.269999971613288, "rotation" : { - "radians" : 2.370942522371697 + "radians" : 3.0222255927586548 } } ], "reversed" : false, @@ -288,10 +345,7 @@ "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, - "constraints" : [ { - "type" : "MaxVelocityConstraint", - "maxVelocity" : 0.75 - } ] + "constraints" : [ ] }, { "type" : "script", "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", @@ -309,15 +363,20 @@ } }, { "type" : "script", - "script" : "Drive.turnToAngle 40", + "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "NONE", "delay" : 0.0, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setEjectOverride", + "args" : [ "true" ], + "argTypes" : [ "boolean" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Drive.turnToAngle", - "args" : [ "40" ], + "args" : [ "45" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -547,13 +606,18 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "@t 1.3\nIntake.setWantedIntakeState EJECT", + "script" : "@t 1\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "TIME", - "delay" : 1.3, + "delay" : 1.0, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "REVERSE" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Intake.setWantedIntakeState", "args" : [ "EJECT" ], "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], @@ -562,13 +626,18 @@ } }, { "type" : "script", - "script" : "@t 1.6\nIntake.setWantedIntakeState INTAKE", + "script" : "@t 1.6\nHopper.setHopperState ON\nIntake.setWantedIntakeState INTAKE", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "TIME", "delay" : 1.6, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Intake.setWantedIntakeState", "args" : [ "INTAKE" ], "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], @@ -2040,19 +2109,39 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "@t 2\nIntake.setWantedIntakeState EJECT", - "closed" : false, + "script" : "@t 2\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT", + "closed" : true, "valid" : true, "sendableScript" : { "delayType" : "TIME", "delay" : 2.0, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "REVERSE" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Intake.setWantedIntakeState", "args" : [ "EJECT" ], "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], "reflection" : true } ] } + }, { + "type" : "script", + "script" : "@t 3\nIntake.setWantedIntakeState OFF", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 3.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "OFF" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { @@ -2996,7 +3085,7 @@ }, { "time" : 3.118362856162708, "velocity" : 2.9024511229920305, - "acceleration" : 3.2635486192884477, + "acceleration" : 3.2578770345335637, "pose" : { "translation" : { "x" : 3.6274187564849854, @@ -3008,408 +3097,366 @@ }, "curvature" : 0.0 }, { - "time" : 3.1482532878331386, + "time" : 3.148305323595932, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -1.0150458753684738E-14, "pose" : { "translation" : { - "x" : 3.7155035994555945, - "y" : 2.6226850363620713 + "x" : 3.715657867815885, + "y" : 2.6226762181125665 }, "rotation" : { - "radians" : 0.053981642702760804 + "radians" : 0.05341425500982433 } }, - "curvature" : -5.351308120085834E-4 + "curvature" : -0.012753175869738934 }, { - "time" : 3.1770862157675355, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 3.1774724263267227, + "velocity" : 2.9999999999999996, + "acceleration" : 1.0337019039886706E-14, "pose" : { "translation" : { - "x" : 3.801876553490956, - "y" : 2.627348984328819 + "x" : 3.803037680920397, + "y" : 2.6272856863245124 }, "rotation" : { - "radians" : 0.05389383309434626 + "radians" : 0.05184775131197311 } }, - "curvature" : -0.001601464316657459 + "curvature" : -0.022669788781850372 }, { - "time" : 3.2048908409093957, + "time" : 3.2061131274893104, "velocity" : 3.0, - "acceleration" : -1.1198792336370368E-14, - "pose" : { - "translation" : { - "x" : 3.885169715891969, - "y" : 2.631834930243855 - }, - "rotation" : { - "radians" : 0.05369200401278312 - } - }, - "curvature" : -0.0033948580681829725 - }, { - "time" : 3.231327573044508, - "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.964366333158978, - "y" : 2.6360779324022587 - }, - "rotation" : { - "radians" : 0.053319276839782104 - } - }, - "curvature" : -0.006242169384276816 - }, { - "time" : 3.256165149733738, - "velocity" : 2.9999999999999996, - "acceleration" : 1.2812741871263266E-14, - "pose" : { - "translation" : { - "x" : 4.038774281627788, - "y" : 2.640028140852877 + "x" : 3.888849129236597, + "y" : 2.631644952964443 }, "rotation" : { - "radians" : 0.0527039245624449 + "radians" : 0.04955597015846323 } }, - "curvature" : -0.01063132499803715 + "curvature" : -0.03035674043346665 }, { - "time" : 3.2792717943698393, + "time" : 3.2340415748283977, "velocity" : 3.0, - "acceleration" : -7.231938957008415E-15, - "pose" : { - "translation" : { - "x" : 4.107999548105681, - "y" : 2.6436497037911977 - }, - "rotation" : { - "radians" : 0.05175599940745225 - } - }, - "curvature" : -0.017237888913125436 - }, { - "time" : 3.320209567074138, - "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.230657418491319, - "y" : 2.6498269150033593 + "x" : 3.9725371670283494, + "y" : 2.6356817669948214 }, "rotation" : { - "radians" : 0.04841053128135275 + "radians" : 0.04676376501044413 } }, - "curvature" : -0.04051841469935257 + "curvature" : -0.03596921557348524 }, { - "time" : 3.3547406099731343, - "velocity" : 2.9999999999999996, - "acceleration" : -0.6325094018133772, + "time" : 3.261120207908762, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.334142312372251, - "y" : 2.654561157464741 + "x" : 4.053689914296321, + "y" : 2.639356401176549 }, "rotation" : { - "radians" : 0.0423299273622696 + "radians" : 0.043689807233283716 } }, - "curvature" : -0.08057010884102427 + "curvature" : -0.03932125691085411 }, { - "time" : 3.384584997268422, - "velocity" : 2.9811231444443704, - "acceleration" : -0.08050231493317403, + "time" : 3.2872560287661248, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.4233291260534315, - "y" : 2.6579580020916183 + "x" : 4.1320278016889915, + "y" : 2.642658761824123 }, "rotation" : { - "radians" : 0.03315653358683334 + "radians" : 0.040562671415580655 } }, - "curvature" : -0.1233537474620638 + "curvature" : -0.039940377095716434 }, { - "time" : 3.4121081263600206, - "velocity" : 2.978907468838292, - "acceleration" : 0.757187202067055, + "time" : 3.3123968620373043, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.5053166644408975, - "y" : 2.6602420038625496 + "x" : 4.207392715413668, + "y" : 2.6456054985607906 }, "rotation" : { - "radians" : 0.022490284597376595 + "radians" : 0.03763198896608686 } }, - "curvature" : -0.1268933627335242 + "curvature" : -0.03710839155892508 }, { - "time" : 3.4399645548682245, + "time" : 3.336527620370588, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.588579021394253, - "y" : 2.661721706390381 + "x" : 4.279737142147496, + "y" : 2.6482371140737087 }, "rotation" : { - "radians" : 0.01374388263590382 + "radians" : 0.035174735467647336 } }, - "curvature" : -0.07804765164973368 + "curvature" : -0.02992149474512993 }, { - "time" : 3.471145768832688, + "time" : 3.359666591601313, "velocity" : 3.0, - "acceleration" : -7.895171233701495E-15, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.682116960079156, - "y" : 2.6627546464942498 + "x" : 4.349113313948472, + "y" : 2.6506150738690977 }, "rotation" : { - "radians" : 0.009189096860167837 + "radians" : 0.033495618460887784 } }, - "curvature" : -0.024057614430750256 + "curvature" : -0.017400083112939065 }, { - "time" : 3.508644572148012, - "velocity" : 2.9999999999999996, - "acceleration" : 1.3503955874947296E-14, + "time" : 3.3818617651034137, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.794609293319809, - "y" : 2.6637123587715905 + "x" : 4.415662353166454, + "y" : 2.6528189160273996 }, "rotation" : { - "radians" : 0.008349254243389597 + "radians" : 0.03291987664459664 } }, - "curvature" : 0.0037103968236503098 + "curvature" : 0.0013263625292214375 }, { - "time" : 3.5305684791849585, + "time" : 3.423739530881724, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -0.8175000091130344, "pose" : { "translation" : { - "x" : 4.860378613618231, - "y" : 2.6642743243984057 + "x" : 4.5412228441782645, + "y" : 2.657095421156555 }, "rotation" : { - "radians" : 0.00879853748094375 + "radians" : 0.0363729513028002 } }, - "curvature" : 0.009166818258418976 + "curvature" : 0.05866331262890874 }, { - "time" : 3.554964721451485, - "velocity" : 3.0, - "acceleration" : -1.992727596873581, + "time" : 3.4632152976715145, + "velocity" : 2.967728560289602, + "acceleration" : -1.882760480943012, "pose" : { "translation" : { - "x" : 4.933564263951439, - "y" : 2.6649453801701384 + "x" : 4.658912906437536, + "y" : 2.6619545562850817 }, "rotation" : { - "radians" : 0.009565324282873914 + "radians" : 0.047679751894998845 } }, - "curvature" : 0.011329196406191677 + "curvature" : 0.13592977249614052 }, { - "time" : 3.5822868057216533, - "velocity" : 2.9455545286707294, - "acceleration" : -3.9999999999999902, + "time" : 3.5018404054419703, + "velocity" : 2.8950067338072225, + "acceleration" : -1.9238632002001914, "pose" : { "translation" : { - "x" : 5.01478264645214, - "y" : 2.6657603208850844 + "x" : 4.771954350173473, + "y" : 2.668388433754444 }, "rotation" : { - "radians" : 0.010504868424232214 + "radians" : 0.0674512871959646 } }, - "curvature" : 0.011571381180452858 + "curvature" : 0.2106703311228447 }, { - "time" : 3.613394389130737, - "velocity" : 2.821124195034394, - "acceleration" : -4.0, + "time" : 3.541164793474475, + "velocity" : 2.8193519908010933, + "acceleration" : -3.999999999999996, "pose" : { "translation" : { - "x" : 5.104470925172791, - "y" : 2.666748254559934 + "x" : 4.883949141742278, + "y" : 2.677400863588673 }, "rotation" : { - "radians" : 0.011513061014706312 + "radians" : 0.09396342350209999 } }, - "curvature" : 0.01080959208114291 + "curvature" : 0.2540441775959243 }, { - "time" : 3.649180845248966, - "velocity" : 2.6779783705614784, - "acceleration" : -4.000000000000004, + "time" : 3.583307920417398, + "velocity" : 2.6507794830294023, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.202860506721613, - "y" : 2.6679315088226474 + "x" : 4.998532040779537, + "y" : 2.689914865659375 }, "rotation" : { - "radians" : 0.012518180765925502 + "radians" : 0.12355864176280444 } }, - "curvature" : 0.009593832950905505 + "curvature" : 0.25210444079517663 }, { - "time" : 3.690445008393871, - "velocity" : 2.5129217179818584, - "acceleration" : -4.0, + "time" : 3.630910520336883, + "velocity" : 2.460369083351462, + "acceleration" : -3.999999999999993, "pose" : { "translation" : { - "x" : 5.30995052089861, - "y" : 2.6693245373053287 + "x" : 5.119023237352621, + "y" : 2.706680181850743 }, "rotation" : { - "radians" : 0.01347191339238352 + "radians" : 0.1521781406035511 } }, - "curvature" : 0.008225115612607133 + "curvature" : 0.21456009552095912 }, { - "time" : 3.738242422456052, - "velocity" : 2.3217320617331336, - "acceleration" : -3.9999999999999964, + "time" : 3.6575544532910382, + "velocity" : 2.3537933515348413, + "acceleration" : -3.9999999999999933, "pose" : { "translation" : { - "x" : 5.42548130133158, - "y" : 2.6709328260371024 + "x" : 5.182350507669064, + "y" : 2.7168212357026604 }, "rotation" : { - "radians" : 0.014341527416621765 + "radians" : 0.16512628632297052 } }, - "curvature" : 0.006849417564785735 + "curvature" : 0.18899817754880754 }, { - "time" : 3.7940970341308637, - "velocity" : 2.098313615033887, + "time" : 3.6866111335997718, + "velocity" : 2.237566630299908, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.548907866112131, - "y" : 2.6727517998369876 + "x" : 5.2480809891130775, + "y" : 2.728180788224563 }, "rotation" : { - "radians" : 0.015103596532319463 + "radians" : 0.1768351792035555 } }, - "curvature" : 0.005520155477322901 + "curvature" : 0.16220610323939916 }, { - "time" : 3.825756511779807, - "velocity" : 1.9716757044381135, - "acceleration" : -4.0000000000000036, + "time" : 3.718563991190063, + "velocity" : 2.109755199938744, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.6133272185697205, - "y" : 2.673735843651399 + "x" : 5.3163865477339485, + "y" : 2.740762636728995 }, "rotation" : { - "radians" : 0.015438273175695332 + "radians" : 0.1871757577997712 } }, - "curvature" : 0.004873668640140536 + "curvature" : 0.13593088502288128 }, { - "time" : 3.860481196136515, - "velocity" : 1.8327769670112823, - "acceleration" : -4.0, + "time" : 3.754021980661885, + "velocity" : 1.967923242051456, + "acceleration" : -4.000000000000003, "pose" : { "translation" : { - "x" : 5.6793733984317, - "y" : 2.674765728706774 + "x" : 5.387354258449022, + "y" : 2.7545424071852267 }, "rotation" : { - "radians" : 0.015738980768460056 + "radians" : 0.19609326514864456 } }, - "curvature" : 0.004234449415552546 + "curvature" : 0.11126066622925063 }, { - "time" : 3.89893588264187, - "velocity" : 1.678958220989862, + "time" : 3.7938015783178165, + "velocity" : 1.8088048514277302, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.746886228560693, - "y" : 2.6758375663932243 + "x" : 5.460975549954718, + "y" : 2.7694646639744107 }, "rotation" : { - "radians" : 0.016003288340590872 + "radians" : 0.20358486920868646 } }, - "curvature" : 0.003596071353112922 + "curvature" : 0.08871019500715766 }, { - "time" : 3.9421404788446717, - "velocity" : 1.5061398361786555, - "acceleration" : -3.999999999999997, + "time" : 3.8390908801882153, + "velocity" : 1.627647643946134, + "acceleration" : -3.9999999999999973, "pose" : { "translation" : { - "x" : 5.815682727217563, - "y" : 2.6769466342238957 + "x" : 5.537135349637538, + "y" : 2.7854400196447386 }, "rotation" : { - "radians" : 0.016228495089680457 + "radians" : 0.20967774359389213 } }, - "curvature" : 0.0029497967820049952 + "curvature" : 0.06834921616165193 }, { - "time" : 3.991815920611223, - "velocity" : 1.3074380691124496, - "acceleration" : -4.000000000000002, + "time" : 3.8918213921686315, + "velocity" : 1.4167255960244693, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.885556279331293, - "y" : 2.678087341659743 + "x" : 5.615601228485083, + "y" : 2.8023422446665975 }, "rotation" : { - "radians" : 0.016411476850763638 + "radians" : 0.21440925415138234 } }, - "curvature" : 0.0022842194098207026 + "curvature" : 0.049921154171936793 }, { - "time" : 4.0513318851535365, - "velocity" : 1.0693742109431945, - "acceleration" : -3.9999999999999982, + "time" : 3.955692162693215, + "velocity" : 1.1612425139261353, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.956275807768854, - "y" : 2.679253195934308 + "x" : 5.696012545997064, + "y" : 2.8200053771877265 }, "rotation" : { - "radians" : 0.0165485050781744 + "radians" : 0.21780924678420455 } }, - "curvature" : 0.0015845831447143007 + "curvature" : 0.03292187698251918 }, { - "time" : 4.129431954649663, - "velocity" : 0.75697393295869, + "time" : 4.040210129462593, + "velocity" : 0.8231706468486228, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.027584944605088, - "y" : 2.680436767878498 + "x" : 5.777869595096313, + "y" : 2.838220832788373 }, "rotation" : { - "radians" : 0.016635023032639307 + "radians" : 0.21988311898862645 } }, - "curvature" : 8.316618868490522E-4 + "curvature" : 0.016620346183280694 }, { - "time" : 4.318675437889335, + "time" : 4.246002791174749, "velocity" : 0.0, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.099201202392578, - "y" : 2.6816296577453613 + "x" : 5.860522747039795, + "y" : 2.85673451423645 }, "rotation" : { - "radians" : 0.016665357555075345 + "radians" : 0.22059308726779464 } }, "curvature" : 0.0 @@ -3424,8 +3471,8 @@ "x" : [ 3.6274187564849854, 2.828535318374634, 0.0 ], "y" : [ 2.617924213409424, 0.1528944969177246, 0.0 ] }, { - "x" : [ 6.099201202392578, 4.586813449859619, 0.0 ], - "y" : [ 2.6816296577453613, 0.07644796371459961, 0.0 ] + "x" : [ 5.860522747039795, 2.649440288543701, 0.0 ], + "y" : [ 2.85673451423645, 0.5941164493560791, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -3440,12 +3487,12 @@ }, { "time" : 3.1199999302625656, "rotation" : { - "radians" : 0.0 + "radians" : 0.4494112193892231 } }, { - "time" : 4.229999905452132, + "time" : 4.159999907016754, "rotation" : { - "radians" : 0.00740696187012303 + "radians" : 0.46364803834258117 } } ], "reversed" : false, diff --git a/src/main/deploy/autos/red/5ball.json b/src/main/deploy/autos/red/5ball.json index 4aa2b6c7..c0e07aab 100644 --- a/src/main/deploy/autos/red/5ball.json +++ b/src/main/deploy/autos/red/5ball.json @@ -212,7 +212,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 7", + "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 6", "closed" : false, "valid" : true, "sendableScript" : { @@ -220,7 +220,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "7" ], + "args" : [ "6" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -736,7 +736,7 @@ } ] }, { "type" : "script", - "script" : "VisionManager.shootBalls 7", + "script" : "VisionManager.shootBalls 3.5", "closed" : false, "valid" : true, "sendableScript" : { @@ -744,7 +744,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "7" ], + "args" : [ "3.5" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -1887,7 +1887,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE", + "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8.5\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE", "closed" : false, "valid" : true, "sendableScript" : { @@ -1895,7 +1895,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "8" ], + "args" : [ "8.5" ], "argTypes" : [ "double" ], "reflection" : true }, { @@ -1920,7 +1920,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 3.9999999999999996, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { "x" : 3.2910027503967285, @@ -1932,755 +1932,713 @@ }, "curvature" : 0.0 }, { - "time" : 0.2373794954493144, - "velocity" : 0.9495179817972574, - "acceleration" : 3.999999999999994, + "time" : 0.23642508906658902, + "velocity" : 0.9457003562663563, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 3.3989983545955056, - "y" : -2.588374874732608 + "x" : 3.3981667469918193, + "y" : -2.5879985818380646 }, "rotation" : { - "radians" : -0.2921778608912438 + "radians" : -0.2891712372126716 } }, - "curvature" : -0.04364084615692234 + "curvature" : -5.874320584109886E-4 }, { - "time" : 0.29579824973185176, - "velocity" : 1.1831929989274066, - "acceleration" : 4.000000000000002, + "time" : 0.3444359926496966, + "velocity" : 1.3777439705987866, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 3.458631632620488, - "y" : -2.606390396730127 + "x" : 3.5184656394194462, + "y" : -2.6236786987501546 }, "rotation" : { - "radians" : -0.29440901549838794 + "radians" : -0.286576361816551 } }, - "curvature" : -0.02595134837823401 + "curvature" : 0.04203266118630113 }, { - "time" : 0.3491550793551233, - "velocity" : 1.3966203174204928, - "acceleration" : 4.000000000000002, + "time" : 0.3927805454968463, + "velocity" : 1.5711221819873853, + "acceleration" : 3.9999999999999942, "pose" : { "translation" : { - "x" : 3.5244835324483574, - "y" : -2.6264017262656125 + "x" : 3.586874350548449, + "y" : -2.643708721189732 }, "rotation" : { - "radians" : -0.2953398995873586 + "radians" : -0.2828782585831171 } }, - "curvature" : -0.0013985693216967878 + "curvature" : 0.06060524122763304 }, { - "time" : 0.40043149369700565, - "velocity" : 1.6017259747880221, - "acceleration" : 4.000000000000003, + "time" : 0.43985280368645924, + "velocity" : 1.7594112147458367, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 3.5980331007389807, - "y" : -2.6487579275583 + "x" : 3.662202759596539, + "y" : -2.665394846008212 }, "rotation" : { - "radians" : -0.2945501713020244 + "radians" : -0.277565073334469 } }, - "curvature" : 0.020718850966936625 + "curvature" : 0.07384725863707324 }, { - "time" : 0.45101233132563245, - "velocity" : 1.8040493253025296, - "acceleration" : 4.0, + "time" : 0.4864477060306216, + "velocity" : 1.9457908241224862, + "acceleration" : 3.999999999999998, "pose" : { "translation" : { - "x" : 3.680485970962536, - "y" : -2.6736677892761236 + "x" : 3.745298805531398, + "y" : -2.688772057520943 }, "rotation" : { - "radians" : -0.2919955872089119 + "radians" : -0.2707950380725529 } }, - "curvature" : 0.0373242313505917 + "curvature" : 0.08213625833510316 }, { - "time" : 0.5015733373341933, - "velocity" : 2.0062933493367727, - "acceleration" : 3.9999999999999876, + "time" : 0.5329979230669721, + "velocity" : 2.131991692267888, + "acceleration" : 4.000000000000004, "pose" : { "translation" : { - "x" : 3.7727913068944403, - "y" : -2.70121209879796 + "x" : 3.8368513446766883, + "y" : -2.7137960128020495 }, "rotation" : { - "radians" : -0.2878084836673059 + "radians" : -0.2627587722278301 } }, - "curvature" : 0.04861193460092709 + "curvature" : 0.08660282922194644 }, { - "time" : 0.5524240768131252, - "velocity" : 2.2096963072525, - "acceleration" : 3.9999999999999964, + "time" : 0.5797279435183833, + "velocity" : 2.3189117740735328, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 3.8756587461102754, - "y" : -2.731355916475877 + "x" : 3.9373990498514004, + "y" : -2.740351675968121 }, "rotation" : { - "radians" : -0.28217372185403494 + "radians" : -0.25363830846069757 } }, - "curvature" : 0.055832145694026246 + "curvature" : 0.08841580171796991 }, { - "time" : 0.6036702471548304, - "velocity" : 2.414680988619321, + "time" : 0.6267362610549605, + "velocity" : 2.5069450442198415, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 3.989575343480716, - "y" : -2.76396084989738 + "x" : 4.047339309509198, + "y" : -2.7682619524618985 }, "rotation" : { - "radians" : -0.2752690347022763 + "radians" : -0.2435910468434367 } }, - "curvature" : 0.06027032950217638 + "curvature" : 0.0885183696531404 }, { - "time" : 0.6294411364762771, - "velocity" : 2.5177645459051075, - "acceleration" : 3.9999999999999933, + "time" : 0.6740431507892504, + "velocity" : 2.696172603157001, + "acceleration" : 3.9999999999999862, "pose" : { "translation" : { - "x" : 4.050772588350831, - "y" : -2.7811177201369857 + "x" : 4.166937126877762, + "y" : -2.7972963233359636 }, "rotation" : { - "radians" : -0.271388278211743 + "radians" : -0.2327470040418315 } }, - "curvature" : 0.06177303417946446 + "curvature" : 0.08758788738466654 }, { - "time" : 0.6553004906081608, - "velocity" : 2.621201962432642, - "acceleration" : 3.999999999999994, + "time" : 0.6978010763245834, + "velocity" : 2.791204305298333, + "acceleration" : 3.999999999999974, "pose" : { "translation" : { - "x" : 4.114822514666457, - "y" : -2.798797328147657 + "x" : 4.230406424149577, + "y" : -2.8121505236061024 }, "rotation" : { - "radians" : -0.26724328091034977 + "radians" : -0.22706006402416112 } }, - "curvature" : 0.06293282064413533 + "curvature" : 0.08688404973771191 }, { - "time" : 0.6812367197554138, - "velocity" : 2.7249468790216542, - "acceleration" : 4.0, + "time" : 0.7216202590268884, + "velocity" : 2.8864810361075524, + "acceleration" : 3.9999999999999747, "pose" : { "translation" : { - "x" : 4.181730948050758, - "y" : -2.816958744670205 + "x" : 4.29633401909814, + "y" : -2.827179479536426 }, "rotation" : { - "radians" : -0.26284758822831167 + "radians" : -0.22121199588447016 } }, - "curvature" : 0.06382850159969175 + "curvature" : 0.08607766603782731 }, { - "time" : 0.7072359906843115, - "velocity" : 2.8289439627372452, - "acceleration" : 4.0, + "time" : 0.74549257464285, + "velocity" : 2.9819702985713983, + "acceleration" : 0.7442098400976622, "pose" : { "translation" : { - "x" : 4.25149297961314, - "y" : -2.835556876071827 + "x" : 4.364719766680094, + "y" : -2.8423431956032283 }, "rotation" : { - "radians" : -0.25821295054670484 + "radians" : -0.21521310387858186 } }, - "curvature" : 0.06452392825617205 + "curvature" : 0.08519994915603576 }, { - "time" : 0.7332828563292066, - "velocity" : 2.9331314253168257, - "acceleration" : 2.5481050539821153, + "time" : 0.7697192113482115, + "velocity" : 2.9999999999999996, + "acceleration" : 1.1864982661111718E-14, "pose" : { "translation" : { - "x" : 4.324093495433467, - "y" : -2.854542847916802 + "x" : 4.435556916364085, + "y" : -2.8575999561866112 }, "rotation" : { - "radians" : -0.253349516956691 + "radians" : -0.2090728501840574 } }, - "curvature" : 0.06507034108588115 + "curvature" : 0.08427499675849662 }, { - "time" : 0.759525327996014, + "time" : 0.7946715843679995, "velocity" : 3.0, - "acceleration" : -1.1017744707610282E-14, + "acceleration" : -1.1500748104073835E-14, "pose" : { "translation" : { - "x" : 4.399507706046279, - "y" : -2.8738643885371857 + "x" : 4.508832390228868, + "y" : -2.8729065953918482 }, "rotation" : { - "radians" : -0.24826603822981036 + "radians" : -0.2028000166117804 } }, - "curvature" : 0.06550851193026688 + "curvature" : 0.08332106007566421 }, { - "time" : 0.7863964782259588, + "time" : 0.8204142117652993, "velocity" : 2.9999999999999996, - "acceleration" : 1.0659361723935359E-14, + "acceleration" : 1.1162447712385717E-14, "pose" : { "translation" : { - "x" : 4.477701675925007, - "y" : -2.8934662126035047 + "x" : 4.584527061061408, + "y" : -2.88821876687075 }, "rotation" : { - "radians" : -0.2429700686181491 + "radians" : -0.19640285659122264 } }, - "curvature" : 0.06587060593952473 + "curvature" : 0.08235160626948937 }, { - "time" : 0.8141710749514438, + "time" : 0.8469370211760767, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.558632852966191, - "y" : -2.913290404695452 + "x" : 4.6626160304549895, + "y" : -2.9034912136430293 }, "rotation" : { - "radians" : -0.23746816090539483 + "radians" : -0.18988923719000408 } }, - "curvature" : 0.06618175066974573 + "curvature" : 0.0813761949653504 }, { - "time" : 0.8428287933072273, + "time" : 0.874228259006809, "velocity" : 3.0, - "acceleration" : -1.0029761020365324E-14, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.642250597973694, - "y" : -2.9332768028725846 + "x" : 4.743068906907318, + "y" : -2.9186780379176653 }, "rotation" : { - "radians" : -0.23176605224269178 + "radians" : -0.18326677164075206 } }, - "curvature" : 0.06646132969406916 + "curvature" : 0.08040119291556454 }, { - "time" : 0.8723468917576519, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.9022745981913911, + "velocity" : 3.0, + "acceleration" : -1.0284611572537565E-14, "pose" : { "translation" : { - "x" : 4.728496714142921, - "y" : -2.9533633822450156 + "x" : 4.825850083918625, + "y" : -2.9337329709142717 }, "rotation" : { - "radians" : -0.2258688402689826 + "radians" : -0.17654294326329226 } }, - "curvature" : 0.06672403127896764 + "curvature" : 0.07943035076297603 }, { - "time" : 0.9027004208083538, + "time" : 0.9310612442543232, "velocity" : 2.9999999999999996, - "acceleration" : 9.500654106330315E-15, + "acceleration" : 1.0032243113209958E-14, "pose" : { "translation" : { - "x" : 4.817305976545033, - "y" : -2.9734866385441103 + "x" : 4.910919018089771, + "y" : -2.9486096426844597 }, "rotation" : { - "radians" : -0.21978115021622427 + "radians" : -0.16972522188764874 } }, - "curvature" : 0.0669806864818383 + "curvature" : 0.07846526410378302 }, { - "time" : 0.9338624302784291, + "time" : 0.9605720395861806, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.9086066616111665, - "y" : -2.9935819716931817 + "x" : 4.998230507220355, + "y" : -2.9632618519332032 }, "rotation" : { - "radians" : -0.21350729439917845 + "radians" : -0.16282117397282284 } }, - "curvature" : 0.06723892993087238 + "curvature" : 0.07750573849274361 }, { - "time" : 0.9658041749806907, + "time" : 0.990789565834994, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.002321076616648, - "y" : -3.0135840693781866 + "x" : 5.087734968406814, + "y" : -2.9776438358402046 }, "rotation" : { - "radians" : -0.20705142586622954 + "radians" : -0.15583856762569545 } }, - "curvature" : 0.06750371317272791 + "curvature" : 0.07655007526138907 }, { - "time" : 0.9984953186414615, + "time" : 1.0216952443158518, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.098366089165211, - "y" : -3.0334272906184196 + "x" : 5.179378716140528, + "y" : -2.991710539881261 }, "rotation" : { - "radians" : -0.20041768816116357 + "radians" : -0.14878547368600106 } }, - "curvature" : 0.0677776962888439 + "curvature" : 0.07559529234150263 }, { - "time" : 1.0319041358746839, + "time" : 1.0532694343409463, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.19665365667321, - "y" : -3.0530460493372082 + "x" : 5.273104240405928, + "y" : -3.005417887649628 }, "rotation" : { - "radians" : -0.1936103631941936 + "radians" : -0.14167036397863442 } }, - "curvature" : 0.06806153927030212 + "curvature" : 0.07463729185441531 }, { - "time" : 1.0659977120081605, + "time" : 1.0854915293736447, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -2.7038611726562314E-14, "pose" : { "translation" : { - "x" : 5.297091355853844, - "y" : -3.072375197932608 + "x" : 5.368850484778598, + "y" : -3.0187230506773854 }, "rotation" : { - "radians" : -0.18663401919376696 + "radians" : -0.13450220775752084 } }, - "curvature" : 0.0683541107864204 + "curvature" : 0.07367098410111733 }, { - "time" : 1.1007421405426991, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.1183400509132173, + "velocity" : 2.999999999999999, + "acceleration" : 2.6550284696523992E-14, "pose" : { "translation" : { - "x" : 5.399582912201364, - "y" : -3.0913504108481 + "x" : 5.466553124523379, + "y" : -3.031584718256802 }, "rotation" : { - "radians" : -0.17949366063978456 + "radians" : -0.1272905672847317 } }, - "curvature" : 0.0686526286409456 + "curvature" : 0.0726903757761293 }, { - "time" : 1.136102718008151, + "time" : 1.151792740021911, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -8.698956559016603E-15, "pose" : { "translation" : { - "x" : 5.504028729475294, - "y" : -3.1099085681432825 + "x" : 5.566144844692476, + "y" : -3.0439633672617026 }, "rotation" : { - "radians" : -0.17219488198191107 + "radians" : -0.1200456934097773 } }, - "curvature" : 0.06895274341837182 + "curvature" : 0.07168862870971174 }, { - "time" : 1.1720441359642337, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.1858266464134202, + "velocity" : 2.9999999999999996, + "acceleration" : 8.558717775961129E-15, "pose" : { "translation" : { - "x" : 5.61032641918465, - "y" : -3.127988139064569 + "x" : 5.66755561822356, + "y" : -3.0558215319688316 }, "rotation" : { - "radians" : -0.16474402683304648 + "radians" : -0.11277862194006867 } }, - "curvature" : 0.06924857455843654 + "curvature" : 0.0706580941863296 }, { - "time" : 1.2085306698791871, + "time" : 1.2204182150319014, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -8.428702249529382E-15, "pose" : { "translation" : { - "x" : 5.718371330072152, - "y" : -3.145529565615881 + "x" : 5.770712984037872, + "y" : -3.067124073879218 }, "rotation" : { - "radians" : -0.15714835420132006 + "radians" : -0.10550127152586215 } }, - "curvature" : 0.06953270631102976 + "curvature" : 0.06959032685672162 }, { - "time" : 1.2455263646064414, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.2555433700639118, + "velocity" : 2.9999999999999996, + "acceleration" : 8.308289645985856E-15, "pose" : { "translation" : { - "x" : 5.828057077598442, - "y" : -3.162475646129346 + "x" : 5.875542325138333, + "y" : -3.0778384515395416 }, "rotation" : { - "radians" : -0.149416213183665 + "radians" : -0.09822654372203397 } }, - "curvature" : 0.06979614966205201 + "curvature" : 0.06847608142197704 }, { - "time" : 1.2829952161694935, + "time" : 1.2911775963425318, "velocity" : 3.0, - "acceleration" : -7.810331647113909E-15, + "acceleration" : -8.19691056891909E-15, "pose" : { "translation" : { - "x" : 5.939276073426299, - "y" : -3.1787719188359915 + "x" : 5.9819671467076425, + "y" : -3.0879349903634985 }, "rotation" : { - "radians" : -0.1415572273868729 + "radians" : -0.0909684258345216 } }, - "curvature" : 0.07002827533191112 + "curvature" : 0.06730529458972327 }, { - "time" : 1.3209013495592037, + "time" : 1.327296018124022, "velocity" : 2.9999999999999996, - "acceleration" : 7.728429806013464E-15, - "pose" : { - "translation" : { - "x" : 6.051920054904861, - "y" : -3.1943670454364383 - }, - "rotation" : { - "radians" : -0.13358249016273918 - } - }, - "curvature" : 0.07021672228872737 - }, { - "time" : 1.359209192247121, - "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.165880614553835, - "y" : -3.209213194671598 + "x" : 6.089909354206384, + "y" : -3.097387152453166 }, "rotation" : { - "radians" : -0.12550477153574247 + "radians" : -0.08374209711050397 } }, - "curvature" : 0.07034728585337109 + "curvature" : 0.06606705425850384 }, { - "time" : 1.397883643124852, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.3638734752432662, + "velocity" : 2.9999999999999996, + "acceleration" : 7.999202988055673E-15, "pose" : { "translation" : { - "x" : 6.281049729547716, - "y" : -3.2232664258933683 + "x" : 6.199289531471132, + "y" : -3.1061718064203667 }, "rotation" : { - "radians" : -0.11733873745676503 + "radians" : -0.07656403878726785 } }, - "curvature" : 0.07040378937764218 + "curvature" : 0.06474955745525926 }, { - "time" : 1.4368902365948655, + "time" : 1.400884596685671, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.397320291200003, - "y" : -3.2364870726353274 + "x" : 6.310027218812557, + "y" : -3.114269497208035 }, "rotation" : { - "radians" : -0.10910118172585662 + "radians" : -0.06945214847377139 } }, - "curvature" : 0.0703679436328688 + "curvature" : 0.06334005821608486 }, { - "time" : 1.4761953015647518, + "time" : 1.438303871650907, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -7.831878599476166E-15, "pose" : { "translation" : { - "x" : 6.514586634447419, - "y" : -3.2488401261834285 + "x" : 6.422041191113526, + "y" : -3.1216647159115825 }, "rotation" : { - "radians" : -0.10081127058141126 + "radians" : -0.06242585929932353 } }, - "curvature" : 0.0702191984436713 + "curvature" : 0.06182480635063325 }, { - "time" : 1.515766115137486, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 1.4761057182287096, + "velocity" : 2.9999999999999996, + "acceleration" : 7.758609490328921E-15, "pose" : { "translation" : { - "x" : 6.63274506733412, - "y" : -3.260295619146696 + "x" : 6.535249735927209, + "y" : -3.1283461696002632 }, "rotation" : { - "radians" : -0.0924907995421401 + "radians" : -0.055506264223218596 } }, - "curvature" : 0.06993459173630182 + "curvature" : 0.06018897785906844 }, { - "time" : 1.55557105084767, + "time" : 1.5142645498598688, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.7516944004959205, - "y" : -3.2708290090279206 + "x" : 6.6495709315751865, + "y" : -3.1343070511385376 }, "rotation" : { - "radians" : -0.08416446160106997 + "radians" : -0.04871624585462448 } }, - "curvature" : 0.06948860204346692 + "curvature" : 0.05841659767581544 }, { - "time" : 1.5955797213714447, + "time" : 1.5527548398175375, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.871336476644501, - "y" : -3.2804215617943555 + "x" : 6.76492292524555, + "y" : -3.1395453090074383 }, "rotation" : { - "radians" : -0.07586012529605445 + "radians" : -0.0420806120796796 } }, - "curvature" : 0.06885301162269934 + "curvature" : 0.05649045540029702 }, { - "time" : 1.6357631157394636, + "time" : 1.5915511840162078, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 6.991576700051632, - "y" : -3.289060735448409 + "x" : 6.881224211091007, + "y" : -3.144063917125937 }, "rotation" : { - "radians" : -0.06760912050688539 + "radians" : -0.03562623772763141 } }, - "curvature" : 0.06799678871171229 + "curvature" : 0.05439201475116482 }, { - "time" : 1.6760937312120248, + "time" : 1.6306283625393438, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 7.1123245660333865, - "y" : -3.296740563598341 + "x" : 6.998393908326989, + "y" : -3.147871144672308 }, "rotation" : { - "radians" : -0.05944652904214673 + "radians" : -0.029382212423449006 } }, - "curvature" : 0.06688599907381494 + "curvature" : 0.05210131766377214 }, { - "time" : 1.7165457001373738, + "time" : 1.6699614003730316, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 7.233494190434357, - "y" : -3.30346203902896 + "x" : 7.116352039329751, + "y" : -3.150980825905494 }, "rotation" : { - "radians" : -0.05141147616548517 + "radians" : -0.02337999466219851 } }, - "curvature" : 0.06548375889504356 + "curvature" : 0.04959688426535946 }, { - "time" : 1.757094912312602, + "time" : 1.7095256279435234, "velocity" : 3.0, - "acceleration" : -7.287757495654728E-15, + "acceleration" : -1.1557888191446533, "pose" : { "translation" : { - "x" : 7.355004839111871, - "y" : -3.3092334972723165 + "x" : 7.235019807734481, + "y" : -3.1534126299864695 }, "rotation" : { - "radians" : -0.043547418155515716 + "radians" : -0.017653571989563004 } }, - "curvature" : 0.06375024330143432 + "curvature" : 0.04685561044273053 }, { - "time" : 1.7977191336056264, - "velocity" : 2.9999999999999996, - "acceleration" : 7.27794583372083E-15, + "time" : 1.749606195370589, + "velocity" : 2.953675328302824, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { - "x" : 7.476781457420211, - "y" : -3.314071000178397 + "x" : 7.354319876533402, + "y" : -3.1551923307996104 }, "rotation" : { - "radians" : -0.03590241977860944 + "radians" : -0.012239626968958514 } }, - "curvature" : 0.061642767292292996 + "curvature" : 0.04385266540863671 }, { - "time" : 1.8383981218802636, - "velocity" : 3.0, - "acceleration" : -0.8549861225542413, + "time" : 1.791367879808199, + "velocity" : 2.786628590552384, + "acceleration" : -3.9999999999999964, "pose" : { "translation" : { - "x" : 7.598755199694828, - "y" : -3.3179987194858214 + "x" : 7.4741766461738735, + "y" : -3.1563520767740556 }, "rotation" : { - "radians" : -0.02852941415912364 + "radians" : -0.007177708340391282 } }, - "curvature" : 0.059115958757323624 + "curvature" : 0.040561392634885035 }, { - "time" : 1.8793527503450278, - "velocity" : 2.964984361008262, + "time" : 1.8359816679429726, + "velocity" : 2.6081734380132895, "acceleration" : -3.9999999999999964, "pose" : { "translation" : { - "x" : 7.720863958736559, - "y" : -3.3210493203925378 + "x" : 7.594516532656503, + "y" : -3.156930660705073 }, "rotation" : { - "radians" : -0.021486435933739608 + "radians" : -0.002510406406084059 } }, - "curvature" : 0.05612204648295773 + "curvature" : 0.036953218827764284 }, { - "time" : 1.9217846589717384, - "velocity" : 2.7952567265014197, + "time" : 1.8840509532237966, + "velocity" : 2.415896296889994, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 7.843052895295841, - "y" : -3.3232643451265176 - }, - "rotation" : { - "radians" : -0.014836816748393312 - } - }, - "curvature" : 0.05261128966471575 - }, { - "time" : 1.966973534915382, - "velocity" : 2.6145012227268447, - "acceleration" : -4.0000000000000036, - "pose" : { - "translation" : { - "x" : 7.965274967556932, - "y" : -3.3246945965164496 + "x" : 7.715268245633247, + "y" : -3.1569737895754244 }, "rotation" : { - "radians" : -0.008649330067547475 + "radians" : 0.0017164688167075213 } }, - "curvature" : 0.048532579406757856 + "curvature" : 0.03299757736886057 }, { - "time" : 2.0155229987855723, - "velocity" : 2.4203033672460843, + "time" : 1.9364483118057918, + "velocity" : 2.206306862562013, "acceleration" : -3.9999999999999964, "pose" : { "translation" : { - "x" : 8.087491460622125, - "y" : -3.325400521562436 + "x" : 7.836363066505513, + "y" : -3.156534354376733 }, "rotation" : { - "radians" : -0.0029982698845735147 + "radians" : 0.005453708791556059 } }, - "curvature" : 0.04383424692658223 + "curvature" : 0.028661854946767117 }, { - "time" : 2.0683070326652815, - "velocity" : 2.2091672317272475, - "acceleration" : -4.000000000000005, + "time" : 1.9945178704913586, + "velocity" : 1.9740286278197459, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { - "x" : 8.209672515995964, - "y" : -3.3254525950066887 + "x" : 7.957735126522266, + "y" : -3.1556726999308466 }, "rotation" : { - "radians" : 0.0020365547800811692 + "radians" : 0.008648529987311705 } }, - "curvature" : 0.03846511750567603 + "curvature" : 0.023911373099727667 }, { - "time" : 2.126672618228889, - "velocity" : 1.9757048894728169, + "time" : 2.060528833894078, + "velocity" : 1.7099847742088685, "acceleration" : -3.9999999999999982, "pose" : { "translation" : { - "x" : 8.331797661069459, - "y" : -3.324931702904223 - }, - "rotation" : { - "radians" : 0.006369930747651973 - } - }, - "curvature" : 0.032375853273367367 - }, { - "time" : 2.19289366493448, - "velocity" : 1.7108207026504523, - "acceleration" : -3.999999999999999, - "pose" : { - "translation" : { - "x" : 8.453856338604307, - "y" : -3.3239295261935533 + "x" : 8.079321684878137, + "y" : -3.1544568947112026 }, "rotation" : { - "radians" : 0.00991148144607044 + "radians" : 0.011244403839107409 } }, - "curvature" : 0.025520631044257825 + "curvature" : 0.018709420234176016 }, { - "time" : 2.2714115520395897, - "velocity" : 1.3967491542300143, - "acceleration" : -4.000000000000001, + "time" : 2.1389153715933484, + "velocity" : 1.3964386234117863, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 8.575848436217107, - "y" : -3.3225489242673873 + "x" : 8.201063406811521, + "y" : -3.1529630006641938 }, "rotation" : { - "radians" : 0.012566065756910601 + "radians" : 0.013180898713490029 } }, - "curvature" : 0.017859202608647286 + "curvature" : 0.013017354553867993 }, { - "time" : 2.373702045251545, - "velocity" : 0.9875871813821927, - "acceleration" : -4.000000000000001, + "time" : 2.2411422808075967, + "velocity" : 0.9875309865547927, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 8.697784815863574, - "y" : -3.320904318543324 + "x" : 8.322904641702685, + "y" : -3.1512753430305347 }, "rotation" : { - "radians" : 0.014234366939993203 + "radians" : 0.01439354131558399 } }, - "curvature" : 0.009359382499354215 + "curvature" : 0.0067948044075592335 }, { - "time" : 2.620598840597093, + "time" : 2.488025027446295, "velocity" : 0.0, - "acceleration" : -4.000000000000001, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 8.819687843322754, - "y" : -3.319122076034546 + "x" : 8.444793701171875, + "y" : -3.149486780166626 }, "rotation" : { "radians" : 0.014813707347434221 @@ -2692,8 +2650,8 @@ "x" : [ 3.2910027503967285, 1.676727533340454, 0.0 ], "y" : [ -2.5561599731445312, -0.49745607376098633, 0.0 ] }, { - "x" : [ 8.819687843322754, 7.801394462585449, 0.0 ], - "y" : [ -3.319122076034546, 0.11557602882385254, 0.0 ] + "x" : [ 8.444793701171875, 7.801394462585449, 0.0 ], + "y" : [ -3.149486780166626, 0.11557602882385254, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -2701,7 +2659,7 @@ "radians" : 0.0 } }, { - "time" : 2.5299999434500933, + "time" : 2.3999999463558197, "rotation" : { "radians" : 0.0 } diff --git a/src/main/deploy/autos/red/hidecargo.json b/src/main/deploy/autos/red/hidecargo.json index 9932a2f5..16cbc83b 100644 --- a/src/main/deploy/autos/red/hidecargo.json +++ b/src/main/deploy/autos/red/hidecargo.json @@ -29,7 +29,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 2.351956220256751, + "acceleration" : 3.9999999999999996, "pose" : { "translation" : { "x" : 6.1293768882751465, @@ -41,9 +41,9 @@ }, "curvature" : -0.0 }, { - "time" : 0.3188834866654645, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.24452127463632814, + "velocity" : 0.9780850985453124, + "acceleration" : 3.9999999999999996, "pose" : { "translation" : { "x" : 6.039938468317359, @@ -55,9 +55,9 @@ }, "curvature" : -0.10063347960741297 }, { - "time" : 0.47317666597794406, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.3430023265109939, + "velocity" : 1.3720093060439753, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { "x" : 5.954285601692391, @@ -69,9 +69,9 @@ }, "curvature" : -0.1855623376134662 }, { - "time" : 0.6188308342429313, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.4150553084726476, + "velocity" : 1.6602212338905902, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 5.87496604004582, @@ -83,9 +83,9 @@ }, "curvature" : -0.26531768438500725 }, { - "time" : 0.7539530640358325, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.4721670734641104, + "velocity" : 1.8886682938564414, + "acceleration" : 3.999999999999993, "pose" : { "translation" : { "x" : 5.803342186380178, @@ -97,9 +97,9 @@ }, "curvature" : -0.3381985496587611 }, { - "time" : 0.8779901904625292, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5190911939858679, + "velocity" : 2.076364775943471, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 5.7397725735722815, @@ -111,9 +111,9 @@ }, "curvature" : -0.3907488038191573 }, { - "time" : 0.9914536504675615, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5585735987097502, + "velocity" : 2.2342943948390004, + "acceleration" : 0.26324528204804704, "pose" : { "translation" : { "x" : 5.683793342890567, @@ -125,9 +125,9 @@ }, "curvature" : -0.39766870774643054 }, { - "time" : 1.095639209373063, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5934744799766615, + "velocity" : 2.243481887171834, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.634299722512424, @@ -139,9 +139,9 @@ }, "curvature" : -0.32812227024366547 }, { - "time" : 1.1923614795366175, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6267989018195489, + "velocity" : 2.110184199800284, + "acceleration" : -3.999999999999997, "pose" : { "translation" : { "x" : 5.589727506041527, @@ -153,9 +153,9 @@ }, "curvature" : -0.16341344710143665 }, { - "time" : 1.2837283378298283, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6603386127840065, + "velocity" : 1.9760253559424537, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.548234531025173, @@ -167,9 +167,9 @@ }, "curvature" : 0.07956164338822755 }, { - "time" : 1.3719691365115938, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6950498794552016, + "velocity" : 1.8371802892576732, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { "x" : 5.507882157471613, @@ -181,9 +181,9 @@ }, "curvature" : 0.33953937734428047 }, { - "time" : 1.4592981567041168, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.732203292039014, + "velocity" : 1.6885666389224232, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.466816746367385, @@ -195,9 +195,9 @@ }, "curvature" : 0.5355105351904276 }, { - "time" : 1.5477626861241707, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.773517717518194, + "velocity" : 1.5233089370057031, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { "x" : 5.42345113819465, @@ -209,9 +209,9 @@ }, "curvature" : 0.6126486386243994 }, { - "time" : 1.6390285515859833, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8214715782483804, + "velocity" : 1.3314934940849577, + "acceleration" : -3.9999999999999982, "pose" : { "translation" : { "x" : 5.376646131448524, @@ -223,9 +223,9 @@ }, "curvature" : 0.568293999561127 }, { - "time" : 1.7340988034913118, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8802038597478065, + "velocity" : 1.0965643680872534, + "acceleration" : -3.999999999999999, "pose" : { "translation" : { "x" : 5.3258919611544115, @@ -237,9 +237,9 @@ }, "curvature" : 0.4354667748411578 }, { - "time" : 1.8330035326706784, - "velocity" : 0.75, - "acceleration" : -3.6944293888894246, + "time" : 0.9592448110914289, + "velocity" : 0.7804005627127639, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { "x" : 5.271489777385341, @@ -251,9 +251,9 @@ }, "curvature" : 0.24590943352090008 }, { - "time" : 2.0360118787648114, + "time" : 1.1543449517696198, "velocity" : 0.0, - "acceleration" : -3.6944293888894246, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { "x" : 5.214733123779297, @@ -278,7 +278,7 @@ "radians" : 2.37094272169677 } }, { - "time" : 1.939999956637621, + "time" : 1.0699999760836363, "rotation" : { "radians" : 2.370942522371697 } @@ -288,13 +288,10 @@ "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, - "constraints" : [ { - "type" : "MaxVelocityConstraint", - "maxVelocity" : 0.75 - } ] + "constraints" : [ ] }, { "type" : "script", - "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", + "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 7", "closed" : false, "valid" : true, "sendableScript" : { @@ -302,22 +299,27 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "10" ], + "args" : [ "7" ], "argTypes" : [ "double" ], "reflection" : true } ] } }, { "type" : "script", - "script" : "Drive.setAutoRotation 40", + "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "NONE", "delay" : 0.0, "commands" : [ { - "methodName" : "frc.subsystem.Drive.setAutoRotation", - "args" : [ "40" ], + "methodName" : "frc.subsystem.Hopper.setEjectOverride", + "args" : [ "true" ], + "argTypes" : [ "boolean" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Drive.turnToAngle", + "args" : [ "45" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -545,6 +547,56 @@ "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 1.3\nIntake.setIntakeSolState CLOSE\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 1.3, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "CLOSE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "OFF" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "OFF" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } + }, { + "type" : "script", + "script" : "@t 3.36\nIntake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 3.36, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setIntakeSolState", + "args" : [ "OPEN" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "INTAKE" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { @@ -2088,16 +2140,36 @@ } ], "reversed" : false, "color" : 52.47058, - "closed" : false, + "closed" : true, "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] + }, { + "type" : "script", + "script" : "@t 0.89\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 0.89, + "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "REVERSE" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "EJECT" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.000000000000001, + "acceleration" : 4.0, "pose" : { "translation" : { "x" : 4.012234210968018, @@ -2109,982 +2181,995 @@ }, "curvature" : 0.0 }, { - "time" : 0.18393581920253443, - "velocity" : 0.7357432768101378, - "acceleration" : 4.000000000000001, + "time" : 0.18393922011618816, + "velocity" : 0.7357568804647526, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 4.079866022458708, - "y" : -0.9175092776647826 + "x" : 4.079868928493841, + "y" : -0.9175222116396733 }, "rotation" : { - "radians" : 0.032422495075255596 + "radians" : 0.03184996886547707 } }, - "curvature" : 0.05303783852412645 + "curvature" : 0.03638401924476792 }, { - "time" : 0.2600740573066539, - "velocity" : 1.0402962292266158, - "acceleration" : 3.999999999999995, + "time" : 0.26009263225753565, + "velocity" : 1.0403705290301426, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 4.147437401812738, - "y" : -0.9151584665515866 + "x" : 4.1474601409882865, + "y" : -0.9152593300483591 }, "rotation" : { - "radians" : 0.03767860106119578 + "radians" : 0.035461574602982625 } }, - "curvature" : 0.10177004653056979 + "curvature" : 0.06998452771616054 }, { - "time" : 0.31842255095187355, - "velocity" : 1.2736902038074942, - "acceleration" : 3.9999999999999902, + "time" : 0.3184715235418354, + "velocity" : 1.2738860941673413, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.214887726988865, - "y" : -0.9123489750712632 + "x" : 4.214962774536021, + "y" : -0.9126807022060877 }, "rotation" : { - "radians" : 0.046083444431288076 + "radians" : 0.04125182940160068 } }, - "curvature" : 0.14663006175024168 + "curvature" : 0.1010514081151579 }, { - "time" : 0.36752120065207927, - "velocity" : 1.4700848026083166, - "acceleration" : 3.9999999999999933, + "time" : 0.36761655293009865, + "velocity" : 1.4704662117203942, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 4.28215626266865, - "y" : -0.9088819079360633 + "x" : 4.2823301801504385, + "y" : -0.9096479069208385 }, "rotation" : { - "radians" : 0.05737092549763 + "radians" : 0.04904809314793145 } }, - "curvature" : 0.18797575110518114 + "curvature" : 0.12981170585502438 }, { - "time" : 0.41067486026855743, - "velocity" : 1.642699441074229, + "time" : 0.4108314858481353, + "velocity" : 1.643325943392541, "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 4.349182263537044, - "y" : -0.904578393853863 + "x" : 4.349514283856005, + "y" : -0.9060353399753465 }, "rotation" : { - "radians" : 0.0712929378915065 + "radians" : 0.05868962955532622 } }, - "curvature" : 0.22608984719455819 + "curvature" : 0.15646782863946646 }, { - "time" : 0.4495779246156969, - "velocity" : 1.7983116984627867, - "acceleration" : 3.999999999999997, + "time" : 0.44980816479061647, + "velocity" : 1.7992326591624657, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.415905077562968, - "y" : -0.8992789224692164 + "x" : 4.416465736770604, + "y" : -0.9017298085207326 }, "rotation" : { - "radians" : 0.08761614166764932 + "radians" : 0.07002652572535734 } }, - "curvature" : 0.2611858070311432 + "curvature" : 0.1811979163878778 }, { - "time" : 0.48523766875483854, - "velocity" : 1.940950675019353, - "acceleration" : 3.9999999999999867, + "time" : 0.4855501313208, + "velocity" : 1.9422005252831998, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.482264249279902, - "y" : -0.8928426813044079 + "x" : 4.48313406518789, + "y" : -0.8966301254701357 }, "rotation" : { - "radians" : 0.10611908836229655 + "radians" : 0.08291864609124491 } }, - "curvature" : 0.29341692608996234 + "curvature" : 0.20415755775322134 }, { - "time" : 0.5183117595380216, - "velocity" : 2.0732470381520844, + "time" : 0.5187103847104131, + "velocity" : 2.0748415388416523, "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.548199623066466, - "y" : -0.8851468927005044 + "x" : 4.549467820659629, + "y" : -0.8906467038923438 }, "rotation" : { - "radians" : 0.12658982254096487 + "radians" : 0.0972346733352272 } }, - "curvature" : 0.32288731915441804 + "curvature" : 0.22548228467606032 }, { - "time" : 0.5492586656367637, - "velocity" : 2.197034662547053, - "acceleration" : 4.0, + "time" : 0.5497420218631486, + "velocity" : 2.1989680874525943, + "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.613651446427005, - "y" : -0.876086150758407 + "x" : 4.615414730078044, + "y" : -0.8837011514054263 }, "rotation" : { - "radians" : 0.14882402397746977 + "radians" : 0.1128512659439996 } }, - "curvature" : 0.3496638342427625 + "curvature" : 0.24529045745559744 }, { - "time" : 0.5784138965550384, - "velocity" : 2.3136555862201518, - "acceleration" : -1.6236735964798095, + "time" : 0.5789746947138208, + "velocity" : 2.315898778855283, + "acceleration" : 4.000000000000007, "pose" : { "translation" : { - "x" : 4.678560473272171, - "y" : -0.8655717582799038 + "x" : 4.680921845758164, + "y" : -0.8757258645703647 }, "rotation" : { - "radians" : 0.17262371101994242 + "radians" : 0.12965234596402142 } }, - "curvature" : 0.3737882085974883 + "curvature" : 0.263686278415705 }, { - "time" : 0.6069779983301187, - "velocity" : 2.267276808360792, - "acceleration" : -1.8021731001922658, + "time" : 0.6066571501940509, + "velocity" : 2.4266286007762035, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.742868067199511, - "y" : -0.8535310637087223 + "x" : 4.745935695520172, + "y" : -0.8666636232846845 }, "rotation" : { - "radians" : 0.19779649220341822 + "radians" : 0.14752851931001348 } }, - "curvature" : 0.3952888936073791 + "curvature" : 0.2807627581933577 }, { - "time" : 0.6360217316741164, - "velocity" : 2.214934973399082, - "acceleration" : -1.4376529363116377, + "time" : 0.6329825891472886, + "velocity" : 2.531930356589154, + "acceleration" : 0.9017846067562265, "pose" : { "translation" : { - "x" : 4.80651630477405, - "y" : -0.8399067980715813 + "x" : 4.8104024327717525, + "y" : -0.8564671851760863 }, "rotation" : { - "radians" : 0.22415532600922183 + "radians" : 0.1663766232708782 } }, - "curvature" : 0.41419203433477625 + "curvature" : 0.29660451731621684 }, { - "time" : 0.6655393241600903, - "velocity" : 2.1724989198887714, - "acceleration" : -1.1396966079827222, + "time" : 0.6584873043239601, + "velocity" : 2.5549301161351785, + "acceleration" : -2.1207024597033652, "pose" : { "translation" : { - "x" : 4.869448078808871, - "y" : -0.824656411919243 + "x" : 4.87426798659043, + "y" : -0.8450988799960781 }, "rotation" : { - "radians" : 0.25151872624934885 + "radians" : 0.18609939066967865 } }, - "curvature" : 0.4305311361191539 + "curvature" : 0.3112903462074792 }, { - "time" : 0.6954246504047454, - "velocity" : 2.138438714939281, - "acceleration" : -0.8908554566969, + "time" : 0.683981890618659, + "velocity" : 2.500863684270891, + "acceleration" : -1.823821978528963, "pose" : { "translation" : { - "x" : 4.931607201645704, - "y" : -0.8077514122675655 + "x" : 4.937478211805924, + "y" : -0.8325302040136062 }, "rotation" : { - "radians" : 0.2797113326941941 + "radians" : 0.20660521679714633 } }, - "curvature" : 0.4443550158057545 + "curvature" : 0.3248954755335266 }, { - "time" : 0.725580967378834, - "velocity" : 2.111573795409033, - "acceleration" : -0.6793465022751951, + "time" : 0.7098179589134984, + "velocity" : 2.4537432950759874, + "acceleration" : -1.5828512441983833, "pose" : { "translation" : { - "x" : 4.992938508435509, - "y" : -0.7891766995385554 + "x" : 4.999979039082493, + "y" : -0.8187414144086866 }, "rotation" : { - "radians" : 0.3085647538618645 + "radians" : 0.22780801329582814 } }, - "curvature" : 0.45573373321158034 + "curvature" : 0.3374935302608176 }, { - "time" : 0.7559198081982444, - "velocity" : 2.0909632100152824, - "acceleration" : -0.4179681793139011, + "time" : 0.7359324293185369, + "velocity" : 2.4124079731037904, + "acceleration" : -1.3044976586946302, "pose" : { "translation" : { - "x" : 5.053387960419059, - "y" : -0.7689299045014195 + "x" : 5.0617166250012815, + "y" : -0.8037211236660369 }, "rotation" : { - "radians" : 0.33791858215085874 + "radians" : 0.24962713231241265 } }, - "curvature" : 0.4647623301686379 + "curvature" : 0.3491581576802025 }, { - "time" : 0.8167858290266697, - "velocity" : 2.0655231501075435, - "acceleration" : -0.1384063526866644, + "time" : 0.7887463969414447, + "velocity" : 2.343512275993333, + "acceleration" : -1.0334589786307198, "pose" : { "translation" : { - "x" : 5.171431395063053, - "y" : -0.7234702639619126 + "x" : 5.182688729168589, + "y" : -0.7699798315917139 }, "rotation" : { - "radians" : 0.39753225232864264 + "radians" : 0.29481885303544775 } }, - "curvature" : 0.47628134337229905 + "curvature" : 0.36998936253987214 }, { - "time" : 0.8775052043910777, - "velocity" : 2.0571192028259433, - "acceleration" : 0.07651708161584311, + "time" : 0.8420624190091038, + "velocity" : 2.288412354282637, + "acceleration" : -0.8513997934004414, "pose" : { "translation" : { - "x" : 5.28533164196233, - "y" : -0.6715829475066926 + "x" : 5.299973998423866, + "y" : -0.7313669207204043 }, "rotation" : { - "radians" : 0.457468911728272 + "radians" : 0.3416439837677253 } }, - "curvature" : 0.4801807958619656 + "curvature" : 0.3880209156918146 }, { - "time" : 0.9376075969896146, - "velocity" : 2.061718062505713, - "acceleration" : 0.23719713417019256, + "time" : 0.8955555645929367, + "velocity" : 2.242868301184222, + "acceleration" : -0.736658519915954, "pose" : { "translation" : { - "x" : 5.39470746489593, - "y" : -0.6136396617759576 + "x" : 5.413165126823472, + "y" : -0.6880507100495148 }, "rotation" : { - "radians" : 0.51683377374029 + "radians" : 0.389654433979741 } }, - "curvature" : 0.47804100648283726 + "curvature" : 0.403939345595272 }, { - "time" : 0.9967126677163254, - "velocity" : 2.0757376158970153, - "acceleration" : 0.3466981374198625, + "time" : 0.9489629734818855, + "velocity" : 2.203525278399543, + "acceleration" : -0.6767969498415867, "pose" : { "translation" : { - "x" : 5.49920556001598, - "y" : -0.5501521453061287 + "x" : 5.5218728309992, + "y" : -0.6402942652402999 }, "rotation" : { - "radians" : 0.5749352590162321 + "radians" : 0.43849160494770656 } }, - "curvature" : 0.471605425281159 + "curvature" : 0.41849245218579234 }, { - "time" : 1.0545224472486359, - "velocity" : 2.0957801587855203, - "acceleration" : 0.40356633939226144, + "time" : 1.0020763940584052, + "velocity" : 2.167578277357701, + "acceleration" : -0.6641591172113208, "pose" : { "translation" : { - "x" : 5.5985038608263835, - "y" : -0.48175095064352114 + "x" : 5.625730652793365, + "y" : -0.5884424192140703 }, "rotation" : { - "radians" : 0.6312847326123074 + "radians" : 0.48788654495705386 } }, - "curvature" : 0.46262836247598543 + "curvature" : 0.4324880614534242 }, { - "time" : 1.1108140478616002, - "velocity" : 2.1184975539834254, - "acceleration" : 0.40345921603853396, + "time" : 1.0547362906352467, + "velocity" : 2.1326037269347866, + "acceleration" : -0.693255970599057, "pose" : { "translation" : { - "x" : 5.692314843161512, - "y" : -0.4091642264580173 + "x" : 5.724399761893892, + "y" : -0.5329087927484011 }, "rotation" : { - "radians" : 0.6855833217165795 + "radians" : 0.5376585244703848 } }, - "curvature" : 0.4527597065949554 + "curvature" : 0.4467899297328142 }, { - "time" : 1.1654323160372946, - "velocity" : 2.1405337976429735, - "acceleration" : 0.34034527303420153, + "time" : 1.1068266505846867, + "velocity" : 2.0964917738891833, + "acceleration" : -0.7590255374673471, "pose" : { "translation" : { - "x" : 5.780388830164895, - "y" : -0.33319649965673825 + "x" : 5.817573758469408, + "y" : -0.4741628150733401 }, "rotation" : { - "radians" : 0.7377000747034669 + "radians" : 0.5877118587042041 } }, - "curvature" : 0.443485601912384 + "curvature" : 0.4623143546066941 }, { - "time" : 1.2182824818871505, - "velocity" : 2.1585211017690455, - "acceleration" : 0.20771430856564294, + "time" : 1.1582699222585076, + "velocity" : 2.0574450169578826, + "acceleration" : -0.85555708396171, "pose" : { "translation" : { - "x" : 5.862517297267914, - "y" : -0.25470745749771595 + "x" : 5.904983475804329, + "y" : -0.4127167444676161 }, "rotation" : { - "radians" : 0.7876462908989643 + "radians" : 0.638030997754264 } }, - "curvature" : 0.4361251242987918 + "curvature" : 0.4800287263774246 }, { - "time" : 1.2693229851547776, - "velocity" : 2.169122944614123, - "acceleration" : 1.0817549407792827E-4, + "time" : 1.20902166214638, + "velocity" : 2.014024006373431, + "acceleration" : -0.9751306653340864, "pose" : { "translation" : { - "x" : 5.938536177168487, - "y" : -0.17459072970356537 + "x" : 5.986401782933953, + "y" : -0.3491126888548468 }, "rotation" : { - "radians" : 0.8355501360445428 + "radians" : 0.6886738736416058 } }, - "curvature" : 0.431872317394874 + "curvature" : 0.500950042603538 }, { - "time" : 1.3185585791987948, - "velocity" : 2.169128270698835, - "acceleration" : -0.2848041321796146, + "time" : 1.259064520666594, + "velocity" : 1.9652256804493953, + "acceleration" : -1.1075634112905204, "pose" : { "translation" : { - "x" : 6.008329164809766, - "y" : -0.09375267057515657 + "x" : 6.061648387279547, + "y" : -0.2839096263997476 }, "rotation" : { - "radians" : 0.8816343285891601 + "radians" : 0.7397632167792167 } }, - "curvature" : 0.431870196556052 + "curvature" : 0.5261369978659279 }, { - "time" : 1.3660336232443555, - "velocity" : 2.15560718197925, - "acceleration" : -0.6435155746795979, + "time" : 1.3084011828915338, + "velocity" : 1.9105821985338527, + "acceleration" : -1.239915254926958, "pose" : { "translation" : { - "x" : 6.071831022358822, - "y" : -0.013091141105286752 + "x" : 6.130594637283437, + "y" : -0.2176704261043394 }, "rotation" : { - "radians" : 0.9261982414140092 + "radians" : 0.7914750099875708 } }, - "curvature" : 0.43730501698866603 + "curvature" : 0.5566628639559956 }, { - "time" : 1.4118252194909213, - "velocity" : 2.126139576605145, - "acceleration" : -1.063489692084961, + "time" : 1.3570458238963947, + "velocity" : 1.8502669660814803, + "acceleration" : -1.3565877307387766, "pose" : { "translation" : { - "x" : 6.12903088418534, - "y" : 0.06652570890764764 + "x" : 6.193168325044098, + "y" : -0.1509488684041571 }, "rotation" : { - "radians" : 0.9696044259991614 + "radians" : 0.8440224046461846 } }, - "curvature" : 0.4495108307569809 + "curvature" : 0.5935467253109423 }, { - "time" : 1.4560355521525066, - "velocity" : 2.0791223435359023, - "acceleration" : -1.52002157710405, + "time" : 1.4050135519869393, + "velocity" : 1.7851945346824336, + "acceleration" : -1.439730441538962, "pose" : { "translation" : { - "x" : 6.179975561840308, - "y" : 0.14428065874597884 + "x" : 6.249358488951245, + "y" : -0.0842766657644578 }, "rotation" : { - "radians" : 1.0122682566684933 + "radians" : 0.8976322459176475 } }, - "curvature" : 0.4700711657871874 + "curvature" : 0.6376063101064825 }, { - "time" : 1.4987824321910335, - "velocity" : 2.014146163523463, - "acceleration" : -1.974071718041697, + "time" : 1.4523072062995137, + "velocity" : 1.7171044208769999, + "acceleration" : -1.4696274540546912, "pose" : { "translation" : { - "x" : 6.224772849034707, - "y" : 0.21942863465909568 + "x" : 6.2992202163209186, + "y" : -0.018150483276428986 }, "rotation" : { - "radians" : 1.0546477790856124 + "radians" : 0.9525098498997762 } }, - "curvature" : 0.500889279611352 + "curvature" : 0.6891762313763988 }, { - "time" : 1.540186616781137, - "velocity" : 1.9324113337155617, - "acceleration" : -2.3720307789417294, + "time" : 1.4989007546283921, + "velocity" : 1.6486292630710562, + "acceleration" : -1.4243277798746545, "pose" : { "translation" : { - "x" : 6.263594826618203, - "y" : 0.2913179277497715 + "x" : 6.3428794460305795, + "y" : 0.04698104074660314 }, "rotation" : { - "radians" : 1.0972283316685527 + "radians" : 1.0087860125729582 } }, - "curvature" : 0.5441574151816366 + "curvature" : 0.7476144640522491 }, { - "time" : 1.580353908329549, - "velocity" : 1.8371332818560024, - "acceleration" : -2.6474197214125605, + "time" : 1.544718402885982, + "velocity" : 1.5833699138492454, + "acceleration" : -1.277051637682657, "pose" : { "translation" : { - "x" : 6.296681167557836, - "y" : 0.3594114118604921 + "x" : 6.380537771154195, + "y" : 0.11073027417296544 }, "rotation" : { - "radians" : 1.140492186777639 + "radians" : 1.0664389649669272 } }, - "curvature" : 0.6020636034649972 + "curvature" : 0.8105110247651124 }, { - "time" : 1.6193492670845173, - "velocity" : 1.7338962000445413, - "acceleration" : -2.721672977384236, + "time" : 1.589608352044505, + "velocity" : 1.5260431307608624, + "acceleration" : -0.9882350321042662, "pose" : { "translation" : { - "x" : 6.324342441916713, - "y" : 0.42330776145978355 + "x" : 6.41247724159733, + "y" : 0.17278357045642956 }, "rotation" : { - "radians" : 1.1848561982155952 + "radians" : 1.1251844168307794 } }, - "curvature" : 0.6758923235708024 + "curvature" : 0.8725495261033641 }, { - "time" : 1.6571590723066265, - "velocity" : 1.6309902748913652, - "acceleration" : -2.4945738768301835, + "time" : 1.6333097898626403, + "velocity" : 1.482855838955655, + "acceleration" : -0.4841929472411742, "pose" : { "translation" : { - "x" : 6.346963421832697, - "y" : 0.4827626695285403 + "x" : 6.4390651667322345, + "y" : 0.2329142480400037 }, "rotation" : { - "radians" : 1.2305496048186084 + "radians" : 1.1843313194749898 } }, - "curvature" : 0.7638726499983753 + "curvature" : 0.9241146185221707 }, { - "time" : 1.6936360168640041, - "velocity" : 1.539995841891948, - "acceleration" : -1.7970788234851456, + "time" : 1.6754107043468742, + "velocity" : 1.4624708730899851, + "acceleration" : -3.7808476405994242, "pose" : { "translation" : { - "x" : 6.3650063864971, - "y" : 0.5377100654463529 + "x" : 6.460758918032937, + "y" : 0.2909955697597244 }, "rotation" : { - "radians" : 1.2773923275414163 + "radians" : 1.2426127877010575 } }, - "curvature" : 0.8568101590190533 + "curvature" : 0.9500561047849169 }, { - "time" : 1.7284182440412232, - "velocity" : 1.477489437998118, - "acceleration" : 1.705632879833736, + "time" : 1.7178366346797787, + "velocity" : 1.3020648944905882, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.37901442713337, - "y" : 0.5882833328778361 + "x" : 6.478110731710331, + "y" : 0.3470137222484482 }, "rotation" : { - "radians" : 1.3244344674414545 + "radians" : 1.2980233032835509 } }, - "curvature" : 0.9308397775185885 + "curvature" : 0.9295171145481738 }, { - "time" : 1.7883248954688422, - "velocity" : 1.5796681923938034, - "acceleration" : 3.9999999999999862, + "time" : 1.7639291544880524, + "velocity" : 1.117694815257493, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 6.397521991248141, - "y" : 0.6779655956833608 + "x" : 6.491772511347264, + "y" : 0.40108079533964336 }, "rotation" : { - "radians" : 1.4085741509062624 + "radians" : 1.3477231257633573 } }, - "curvature" : 0.8143141037035398 + "curvature" : 0.838208547283802 }, { - "time" : 1.8363455674677351, - "velocity" : 1.7717508803893742, - "acceleration" : 4.000000000000008, + "time" : 1.8167467720854111, + "velocity" : 0.9064243448680579, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.408572673797607, - "y" : 0.7576718926429749 + "x" : 6.502500630533632, + "y" : 0.4534477614711818 }, "rotation" : { - "radians" : 1.4464418628084068 + "radians" : 1.3880989582767027 } }, - "curvature" : 0.0 + "curvature" : 0.6544921319595709 }, { - "time" : 1.879584211551695, - "velocity" : 1.9447054567252142, - "acceleration" : 3.999999999999995, + "time" : 2.0433528583024256, + "velocity" : 0.0, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.417938666279497, - "y" : 0.8374714001847678 + "x" : 6.51873254776001, + "y" : 0.5548575520515442 }, "rotation" : { - "radians" : 1.4673635124320494 + "radians" : 1.4247813178399886 } }, - "curvature" : 0.4361537758239235 + "curvature" : 0.0 + } ], + "pointList" : [ { + "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], + "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] }, { - "time" : 1.9241999105096688, - "velocity" : 2.123168252557109, - "acceleration" : 4.000000000000008, + "x" : [ 6.51873254776001, 0.23627901077270508, 0.0 ], + "y" : [ 0.5548575520515442, 1.6066665649414062, 0.0 ] + } ], + "rotations" : [ { + "time" : 0.0, + "rotation" : { + "radians" : -0.13962634015954636 + } + }, { + "time" : 1.9499999564141035, + "rotation" : { + "radians" : -0.1526493858119171 + } + } ], + "reversed" : false, + "color" : 286.3529, + "closed" : false, + "velocityStart" : 0.0, + "velocityEnd" : 0.0, + "constraints" : [ ] + }, { + "type" : "script", + "script" : "sleep 2000", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "NONE", + "delay" : 0.0, + "commands" : [ { + "methodName" : "sleep", + "args" : [ "2000" ], + "argTypes" : [ "long" ], + "reflection" : false + } ] + } + }, { + "type" : "trajectory", + "states" : [ { + "time" : 0.0, + "velocity" : 0.0, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { - "x" : 6.425398940205923, - "y" : 0.9279097366234055 + "x" : 6.51873254776001, + "y" : 0.5548575520515442 }, "rotation" : { - "radians" : 1.509659326369691 + "radians" : 1.4550369939781984 } }, - "curvature" : 0.44722076145528444 + "curvature" : 0.0 }, { - "time" : 1.9728110085852033, - "velocity" : 2.317612644859248, - "acceleration" : 3.999999999999993, + "time" : 0.1788531666911449, + "velocity" : 0.7154126667645795, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 6.429642048575715, - "y" : 1.035761920819823 + "x" : 6.526029236446348, + "y" : 0.6184169998749507 }, "rotation" : { - "radians" : 1.550908753938559 + "radians" : 1.4593465391163816 } }, - "curvature" : 0.31730031892680094 + "curvature" : 0.13050101301586398 }, { - "time" : 1.9987893246112054, - "velocity" : 2.421525908963256, - "acceleration" : 4.000000000000006, + "time" : 0.25321186756390435, + "velocity" : 1.012847470255617, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.430305899986379, - "y" : 1.0973157606626955 + "x" : 6.532831722088758, + "y" : 0.6823114984076142 }, "rotation" : { - "radians" : 1.5684899825119734 + "radians" : 1.471236108776517 } }, - "curvature" : 0.25637867502172557 + "curvature" : 0.23504966165445965 }, { - "time" : 2.025781495555629, - "velocity" : 2.5294945927409493, - "acceleration" : 3.999999999999994, + "time" : 0.3106349893777712, + "velocity" : 1.2425399575110843, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.429927970748395, - "y" : 1.1641340877395123 + "x" : 6.538718402694414, + "y" : 0.746799069553143 }, "rotation" : { - "radians" : 1.583853790503598 + "radians" : 1.4891403961437595 } }, - "curvature" : 0.20594813300808948 + "curvature" : 0.31358925447041286 }, { - "time" : 2.0536364380792893, - "velocity" : 2.6409143628355918, - "acceleration" : 4.000000000000005, + "time" : 0.3594524552162631, + "velocity" : 1.437809820865052, + "acceleration" : 3.9999999999999964, "pose" : { "translation" : { - "x" : 6.42849117103934, - "y" : 1.2361304745226107 + "x" : 6.543342575678253, + "y" : 0.8120593882322282 }, "rotation" : { - "radians" : 1.597166880767735 + "radians" : 1.511568465726473 } }, - "curvature" : 0.16589805557058687 + "curvature" : 0.3681705103657191 }, { - "time" : 2.082166732992299, - "velocity" : 2.7550355424876307, - "acceleration" : 3.999999999999967, + "time" : 0.4028800665262472, + "velocity" : 1.611520266104988, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 6.4260035555757895, - "y" : 1.3130642882106258 + "x" : 6.546427705950194, + "y" : 0.8782000352682129 }, "rotation" : { - "radians" : 1.6086729537639617 + "radians" : 1.5371749251346154 } }, - "curvature" : 0.13471207633826426 + "curvature" : 0.4020595428820239 }, { - "time" : 2.1111641583855323, - "velocity" : 2.8710252440605633, - "acceleration" : 4.000000000000011, + "time" : 0.44254982612556115, + "velocity" : 1.7701993045022437, + "acceleration" : 3.9999999999999933, "pose" : { "translation" : { - "x" : 6.422495681211174, - "y" : 1.3945594657209313 + "x" : 6.547762694002358, + "y" : 0.9452627502726614 }, "rotation" : { - "radians" : 1.6186275742658227 + "radians" : 1.5647989424802173 } }, - "curvature" : 0.11061622885252656 + "curvature" : 0.41904605440094195 }, { - "time" : 2.1404115791378824, - "velocity" : 2.988014927069964, - "acceleration" : 0.40226107984247866, + "time" : 0.4794110889330761, + "velocity" : 1.9176443557323033, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.418017964533647, - "y" : 1.4801232886820799 + "x" : 6.547197143996286, + "y" : 1.0132296845309288 }, "rotation" : { - "radians" : 1.6272684320752597 + "radians" : 1.5934786052963947 } }, - "curvature" : 0.09202002758878106 + "curvature" : 0.42293973727499673 }, { - "time" : 2.170205843436901, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.5140610781999098, + "velocity" : 2.056244312799638, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.412638039463943, - "y" : 1.5691651584262445 + "x" : 6.544636631850153, + "y" : 1.08202965388773 }, "rotation" : { - "radians" : 1.6348043576150604 + "radians" : 1.622447172268418 } }, - "curvature" : 0.07763517137648186 + "curvature" : 0.41723748069828426 }, { - "time" : 2.200892251234442, - "velocity" : 2.9999999999999996, - "acceleration" : 9.430296455303456E-15, + "time" : 0.5468931637504397, + "velocity" : 2.187572655001758, + "acceleration" : 3.1633627206418256, "pose" : { "translation" : { - "x" : 6.406438114853245, - "y" : 1.6610153709816586 + "x" : 6.540037973325994, + "y" : 1.1515443916327097 }, "rotation" : { - "radians" : 1.6414135047463148 + "radians" : 1.651117099798838 } }, - "curvature" : 0.06646668401976608 + "curvature" : 0.4049376530908519 }, { - "time" : 2.232286754539466, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 0.5783519506192766, + "velocity" : 2.287088208619253, + "acceleration" : 1.870434307392289, "pose" : { "translation" : { - "x" : 6.399512332081045, - "y" : 1.7549438920650573 + "x" : 6.533404492116915, + "y" : 1.2216148013860106 }, "rotation" : { - "radians" : 1.6472455716072028 + "radians" : 1.6790569837278395 } }, - "curvature" : 0.05776403436142682 + "curvature" : 0.38847034737186514 }, { - "time" : 2.2641313884937704, - "velocity" : 3.0, - "acceleration" : -0.5220281507380499, + "time" : 0.608993635935249, + "velocity" : 2.3444014680705667, + "acceleration" : 2.165397894604544, "pose" : { "translation" : { - "x" : 6.3919641226530075, - "y" : 1.8501791320741177 + "x" : 6.524781287934317, + "y" : 1.2920472099838438 }, "rotation" : { - "radians" : 1.6524254678653565 + "radians" : 1.7059655876740503 } }, - "curvature" : 0.050967461408882746 + "curvature" : 0.36970875427049765 }, { - "time" : 2.2962499027431713, - "velocity" : 2.9832332314019316, - "acceleration" : -4.0, + "time" : 0.6390132720691309, + "velocity" : 2.409405924951669, + "acceleration" : 2.411756982149806, "pose" : { "translation" : { - "x" : 6.383903565798832, - "y" : 1.9459267210798998 + "x" : 6.514250504595111, + "y" : 1.3626196203640575 }, "rotation" : { - "radians" : 1.6570571945158592 + "radians" : 1.731646007957078 } }, - "curvature" : 0.04566147685699515 + "curvature" : 0.3500287810963245 }, { - "time" : 2.329098019757857, - "velocity" : 2.851840763343189, - "acceleration" : -4.000000000000004, + "time" : 0.668275787449217, + "velocity" : 2.479980000734858, + "acceleration" : 2.6202614568139766, "pose" : { "translation" : { - "x" : 6.375444746070116, - "y" : 2.0413882838192876 + "x" : 6.501926598108938, + "y" : 1.4330879644517065 }, "rotation" : { - "radians" : 1.6612273952864063 + "radians" : 1.7559819224996793 } }, - "curvature" : 0.04153801107443314 + "curvature" : 0.3303903442679772 }, { - "time" : 2.3631515088169963, - "velocity" : 2.7156268071066307, - "acceleration" : -3.999999999999995, + "time" : 0.6966740713701192, + "velocity" : 2.554390929532458, + "acceleration" : 2.7997101251293675, "pose" : { "translation" : { - "x" : 6.366703110938218, - "y" : 2.135780214687429 + "x" : 6.487951604765385, + "y" : 1.5031923560446216 }, "rotation" : { - "radians" : 1.6650083738352117 + "radians" : 1.7789169353574112 } }, - "curvature" : 0.038368183750837316 + "curvature" : 0.3114217758149773 }, { - "time" : 2.3983080444520524, - "velocity" : 2.5750006645664074, - "acceleration" : -4.0, + "time" : 0.7241232587242202, + "velocity" : 2.6312406972943077, + "acceleration" : 1.2935503366228165, "pose" : { "translation" : { - "x" : 6.357792828392121, - "y" : 2.228352452730178 + "x" : 6.4724904092212086, + "y" : 1.572663343698979 }, "rotation" : { - "radians" : 1.6684605177848264 + "radians" : 1.8004373323306622 } }, - "curvature" : 0.03598061733215407 + "curvature" : 0.2934962442471143 }, { - "time" : 2.4344694314979898, - "velocity" : 2.4303551163826573, - "acceleration" : -3.999999999999995, + "time" : 0.7507742479488775, + "velocity" : 2.665715093377194, + "acceleration" : -3.999999999999994, "pose" : { "translation" : { - "x" : 6.3488241445362945, - "y" : 2.318407256636533 + "x" : 6.455726012587547, + "y" : 1.6412281636148691 }, "rotation" : { - "radians" : 1.6716341182037762 + "radians" : 1.8205581086255052 } }, - "curvature" : 0.03424396145033817 + "curvature" : 0.27679481544391576 }, { - "time" : 2.4715493896338248, - "velocity" : 2.2820352838393174, - "acceleration" : -4.000000000000005, + "time" : 0.7774623046076926, + "velocity" : 2.5589628667419335, + "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 6.339900741188558, - "y" : 2.4053179797310804 + "x" : 6.437854800517144, + "y" : 1.7086169925218666 }, "rotation" : { - "radians" : 1.6745705658893675 + "radians" : 1.8393118764446346 } }, - "curvature" : 0.03305114421605748 + "curvature" : 0.2613545325842005 }, { - "time" : 2.509484949213048, - "velocity" : 2.1302930455224227, - "acceleration" : -4.0, + "time" : 0.8048451310611667, + "velocity" : 2.449431560928037, + "acceleration" : -3.9999999999999933, "pose" : { "translation" : { - "x" : 6.331117093477943, - "y" : 2.4885478449664333 + "x" : 6.419081811291562, + "y" : 1.7745692005645992 }, "rotation" : { - "radians" : 1.6773028650946311 + "radians" : 1.856740154150982 } }, - "curvature" : 0.03230248453046758 + "curvature" : 0.24710198818221107 }, { - "time" : 2.5482536718462243, - "velocity" : 1.9752181549897188, + "time" : 0.8329039549239173, + "velocity" : 2.3371962654770346, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.322555827442557, - "y" : 2.567668719915673 + "x" : 6.399616003908406, + "y" : 1.8388396041883173 }, "rotation" : { - "radians" : 1.6798553438041104 + "radians" : 1.8728865291753276 } }, - "curvature" : 0.03188409115100577 + "curvature" : 0.23387381346481986 }, { - "time" : 2.587900600923266, - "velocity" : 1.8166304386815513, - "acceleration" : -3.999999999999997, + "time" : 0.861625693206511, + "velocity" : 2.22230931234666, + "acceleration" : -4.000000000000002, "pose" : { "translation" : { - "x" : 6.314285077627446, - "y" : 2.64237989176479 + "x" : 6.379665526168537, + "y" : 1.901204719024463 }, "rotation" : { - "radians" : 1.682242375809382 + "radians" : 1.8877912376445647 } }, - "curvature" : 0.031637223157170435 + "curvature" : 0.22142585640379966 }, { - "time" : 2.6285823403067847, - "velocity" : 1.6539034811474767, + "time" : 0.921065056241802, + "velocity" : 1.9845518602054957, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.306355844682457, - "y" : 2.712526842305124 + "x" : 6.339110703361712, + "y" : 2.0194711581041815 }, "rotation" : { - "radians" : 1.6844659116451925 + "radians" : 1.9139944386552825 } }, - "curvature" : 0.03131479782522635 + "curvature" : 0.19748286503535473 }, { - "time" : 2.6706436297897875, - "velocity" : 1.4856583232154645, - "acceleration" : -4.000000000000005, + "time" : 0.9834254421957473, + "velocity" : 1.7351103163897141, + "acceleration" : -3.999999999999998, "pose" : { "translation" : { - "x" : 6.298799352960103, - "y" : 2.7781200229258047 + "x" : 6.298886488657445, + "y" : 2.1282522362307645 }, "rotation" : { - "radians" : 1.6865117620775738 + "radians" : 1.9354586912044225 } }, - "curvature" : 0.03052674983936547 + "curvature" : 0.17157685598367362 }, { - "time" : 2.71476243308551, - "velocity" : 1.3091831100325741, - "acceleration" : -3.9999999999999996, + "time" : 1.0497222979942022, + "velocity" : 1.4699228931958945, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 6.260146206276204, + "y" : 2.2271790434867285 + }, + "rotation" : { + "radians" : 1.9520508222111153 + } + }, + "curvature" : 0.13862722514403217 + }, { + "time" : 1.1228723260834714, + "velocity" : 1.1773227808388174, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { - "x" : 6.291624408113421, - "y" : 2.839353629606194 + "x" : 6.223575937663554, + "y" : 2.316830133948315 }, "rotation" : { - "radians" : 1.6883451359021284 + "radians" : 1.9634500574230802 } }, - "curvature" : 0.028694423143857225 + "curvature" : 0.09407105630824328 }, { - "time" : 2.815943111560705, - "velocity" : 0.904460396131795, + "time" : 1.2119347577273158, + "velocity" : 0.8210730542634398, "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.278326433749044, - "y" : 2.9505502779693416 + "x" : 6.189243100281146, + "y" : 2.3989316180237097 }, "rotation" : { - "radians" : 1.6911174188705012 + "radians" : 1.969501422562912 } }, - "curvature" : 0.018977039717131546 + "curvature" : 0.0409346875176687 }, { - "time" : 3.0420582105936536, + "time" : 1.4172030212931759, "velocity" : 0.0, "acceleration" : -3.9999999999999996, "pose" : { "translation" : { - "x" : 6.265983581542969, - "y" : 3.052058696746826 + "x" : 6.156445026397705, + "y" : 2.4765572547912598 }, "rotation" : { - "radians" : 1.6921482303347304 + "radians" : 1.9710366419843302 } }, "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ], - "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ] + "x" : [ 6.51873254776001, 0.23627471923828125, 0.0 ], + "y" : [ 0.5548575520515442, 2.0319605469703674, 0.0 ] }, { - "x" : [ 6.408572673797607, 0.15555095672607422, 0.0 ], - "y" : [ 0.7576718926429749, 1.2444130182266235, 0.0 ] - }, { - "x" : [ 6.265983581542969, -0.19443941116333008, 0.0 ], - "y" : [ 3.052058696746826, 1.5944044589996338, 0.0 ] + "x" : [ 6.156445026397705, -0.519803524017334, 0.0 ], + "y" : [ 2.4765572547912598, 1.2286276817321777, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : -0.13962634015954636 - } - }, { - "time" : 1.8299999590963125, - "rotation" : { - "radians" : 0.4537856055185257 + "radians" : 0.4704038872114269 } }, { - "time" : 2.9499999340623617, + "time" : 1.3299999702721834, "rotation" : { - "radians" : 0.4537856055185257 + "radians" : 0.8760571821323136 } } ], "reversed" : false, - "color" : 286.3529, + "color" : 74.117645, "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, "constraints" : [ ] - }, { - "type" : "script", - "script" : "sleep 1000\nIntake.setWantedIntakeState EJECT", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "sleep", - "args" : [ "1000" ], - "argTypes" : [ "long" ], - "reflection" : false - }, { - "methodName" : "frc.subsystem.Intake.setWantedIntakeState", - "args" : [ "EJECT" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], - "reflection" : true - } ] - } - }, { - "type" : "script", - "script" : "Intake.setIntakeSolState CLOSE", - "closed" : false, - "valid" : true, - "sendableScript" : { - "delayType" : "NONE", - "delay" : 0.0, - "commands" : [ { - "methodName" : "frc.subsystem.Intake.setIntakeSolState", - "args" : [ "CLOSE" ], - "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ], - "reflection" : true - } ] - } } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/red/movecargotohanger.json b/src/main/deploy/autos/red/movecargotohanger.json index 8eb15ca5..7607216c 100644 --- a/src/main/deploy/autos/red/movecargotohanger.json +++ b/src/main/deploy/autos/red/movecargotohanger.json @@ -24,263 +24,320 @@ "reflection" : true } ] } + }, { + "type" : "script", + "script" : "@t 0.01\nDrive.setAutoRotation 173", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 0.01, + "commands" : [ { + "methodName" : "frc.subsystem.Drive.setAutoRotation", + "args" : [ "173" ], + "argTypes" : [ "double" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 2.351956220256751, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.1293768882751465, - "y" : 1.0937262773513794 + "x" : 6.6141839027404785, + "y" : 1.5894602537155151 }, "rotation" : { - "radians" : 2.4178655244299407 + "radians" : 2.389889403743233 } }, "curvature" : -0.0 }, { - "time" : 0.3188834866654645, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.2489252451296045, + "velocity" : 0.995700980518418, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 6.039938468317359, - "y" : 1.173101702633744 + "x" : 6.523176953443453, + "y" : 1.6735778746319738 }, "rotation" : { - "radians" : 2.411607121974174 + "radians" : 2.4064673708251867 } }, - "curvature" : -0.10063347960741297 + "curvature" : 0.25911994232595476 }, { - "time" : 0.47317666597794406, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.3496677436327589, + "velocity" : 1.3986709745310357, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 5.954285601692391, - "y" : 1.2509135147447523 + "x" : 6.432098919045529, + "y" : 1.7526409019337734 }, "rotation" : { - "radians" : 2.395009715228919 + "radians" : 2.4514777463969604 } }, - "curvature" : -0.1855623376134662 + "curvature" : 0.48603924486171285 }, { - "time" : 0.6188308342429313, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.4240995092316322, + "velocity" : 1.6963980369265288, + "acceleration" : -2.790576412342658, "pose" : { "translation" : { - "x" : 5.87496604004582, - "y" : 1.3260262403808838 + "x" : 6.340960895459375, + "y" : 1.823079613135178 }, "rotation" : { - "radians" : 2.3703948601089895 + "radians" : 2.519876340424447 } }, - "curvature" : -0.26531768438500725 + "curvature" : 0.701310596977904 }, { - "time" : 0.7539530640358325, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.49210983031570427, + "velocity" : 1.5066100391134667, + "acceleration" : -1.7711769053800082, "pose" : { "translation" : { - "x" : 5.803342186380178, - "y" : 1.3977211365709081 + "x" : 6.249847903382033, + "y" : 1.8827598318457603 }, "rotation" : { - "radians" : 2.339767645326564 + "radians" : 2.607059147103057 } }, - "curvature" : -0.3381985496587611 + "curvature" : 0.895203949702412 }, { - "time" : 0.8779901904625292, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5274607298209384, + "velocity" : 1.4439973423253865, + "acceleration" : -1.205251380120216, "pose" : { "translation" : { - "x" : 5.7397725735722815, - "y" : 1.4656408262000014 + "x" : 6.204343319524796, + "y" : 1.9082412143392418 }, "rotation" : { - "radians" : 2.3056558386592805 + "radians" : 2.6558796477618074 } }, - "curvature" : -0.3907488038191573 + "curvature" : 0.9745203573253068 }, { - "time" : 0.9914536504675615, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.563119080457446, + "velocity" : 1.401020066007925, + "acceleration" : -0.687574960710818, "pose" : { "translation" : { - "x" : 5.683793342890567, - "y" : 1.529733933533862 + "x" : 6.15890158834145, + "y" : 1.9307801831205325 }, "rotation" : { - "radians" : 2.271678768601324 + "radians" : 2.706939505672123 } }, - "curvature" : -0.39766870774643054 + "curvature" : 1.0352255751207677 }, { - "time" : 1.095639209373063, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.5987086013693859, + "velocity" : 1.376549602565181, + "acceleration" : -0.17765980611680962, "pose" : { "translation" : { - "x" : 5.634299722512424, - "y" : 1.5901997197428273 + "x" : 6.113546424879203, + "y" : 1.9504234287532398 }, "rotation" : { - "radians" : 2.242697343821613 + "radians" : 2.7591326119824173 } }, - "curvature" : -0.32812227024366547 + "curvature" : 1.0723584365876502 }, { - "time" : 1.1923614795366175, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6338600425589757, + "velocity" : 1.3703046043387122, + "acceleration" : 0.36242756952241223, "pose" : { "translation" : { - "x" : 5.589727506041527, - "y" : 1.6474327184259892 + "x" : 6.068302920743008, + "y" : 1.9672693488100776 }, "rotation" : { - "radians" : 2.224227521884603 + "radians" : 2.81126068578529 } }, - "curvature" : -0.16341344710143665 + "curvature" : 1.0821549980493446 }, { - "time" : 1.2837283378298283, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.6682116558661673, + "velocity" : 1.3827545760588114, + "acceleration" : 0.9785292578911144, "pose" : { "translation" : { - "x" : 5.548234531025173, - "y" : 1.7019673711353107 + "x" : 6.023197003472021, + "y" : 1.9814617121025577 }, "rotation" : { - "radians" : 2.2209763317101947 + "radians" : 2.862095726190118 } }, - "curvature" : 0.07956164338822755 + "curvature" : 1.0627558269737196 }, { - "time" : 1.3719691365115938, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.7014108163316237, + "velocity" : 1.4152409259116825, + "acceleration" : 2.2441271164822805, "pose" : { "translation" : { - "x" : 5.507882157471613, - "y" : 1.7544226628997421 + "x" : 5.978254895916052, + "y" : 1.9931833229106815 }, "rotation" : { - "radians" : 2.234944043969041 + "radians" : 2.91045196330826 } }, - "curvature" : 0.33953937734428047 + "curvature" : 1.0145254489555913 }, { - "time" : 1.4592981567041168, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.7626515005088358, + "velocity" : 1.5526728059056913, + "acceleration" : 3.9999999999999925, "pose" : { "translation" : { - "x" : 5.466816746367385, - "y" : 1.805446757749337 + "x" : 5.888965234160423, + "y" : 2.010102666914463 }, "rotation" : { - "radians" : 2.264150133653273 + "radians" : 2.9955886329173094 } }, - "curvature" : 0.5355105351904276 + "curvature" : 0.8428762374068068 }, { - "time" : 1.5477626861241707, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8162079335949602, + "velocity" : 1.7668985382501887, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.42345113819465, - "y" : 1.8556616342393681 + "x" : 5.800629080792987, + "y" : 2.0200297651595065 }, "rotation" : { - "radians" : 2.30296870864004 + "radians" : 3.0601867848614575 } }, - "curvature" : 0.6126486386243994 + "curvature" : 0.6011409133906767 }, { - "time" : 1.6390285515859833, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.8631605793414101, + "velocity" : 1.9547091212359882, + "acceleration" : -2.958595604807148, "pose" : { "translation" : { - "x" : 5.376646131448524, - "y" : 1.9056077209744444 + "x" : 5.713411706194165, + "y" : 2.025186080179992 }, "rotation" : { - "radians" : 2.343996330815072 + "radians" : 3.1009099300153964 } }, - "curvature" : 0.568293999561127 + "curvature" : 0.3280169918222083 }, { - "time" : 1.7340988034913118, - "velocity" : 0.75, - "acceleration" : 0.0, + "time" : 0.9087395705485002, + "velocity" : 1.8198593181791478, + "acceleration" : -3.9999999999999973, "pose" : { "translation" : { - "x" : 5.3258919611544115, - "y" : 1.9556885321326263 + "x" : 5.627431205854464, + "y" : 2.027809408056328 }, "rotation" : { - "radians" : 2.3801609988469874 + "radians" : 3.1173842319085643 } }, - "curvature" : 0.4354667748411578 + "curvature" : 0.05731406395107486 }, { - "time" : 1.8330035326706784, - "velocity" : 0.75, - "acceleration" : -3.6944293888894246, + "time" : 0.957952684084819, + "velocity" : 1.6230068640338726, + "acceleration" : -4.0000000000000036, "pose" : { "translation" : { - "x" : 5.271489777385341, - "y" : 2.006115302989542 + "x" : 5.5427412004210055, + "y" : 2.0299511337652802 }, "rotation" : { - "radians" : 2.405692727133013 + "radians" : 3.1118848820659286 } }, - "curvature" : 0.24590943352090008 + "curvature" : -0.17922803729629205 }, { - "time" : 2.0360118787648114, + "time" : 1.0131511813558638, + "velocity" : 1.4022128749496938, + "acceleration" : -3.9999999999999987, + "pose" : { + "translation" : { + "x" : 5.459313535744059, + "y" : 2.0332734865301063 + }, + "rotation" : { + "radians" : 3.0893774981886746 + } + }, + "curvature" : -0.3450488004527902 + }, { + "time" : 1.077964961333034, + "velocity" : 1.1429577550410128, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.377020982923568, + "y" : 2.0388467951706843 + }, + "rotation" : { + "radians" : 3.0578354549476097 + } + }, + "curvature" : -0.3971872754778253 + }, { + "time" : 1.161849251134293, + "velocity" : 0.8074205958359769, + "acceleration" : -4.0, + "pose" : { + "translation" : { + "x" : 5.295619938355685, + "y" : 2.0469467434536455 + }, + "rotation" : { + "radians" : 3.0283983068361184 + } + }, + "curvature" : -0.2932585838727471 + }, { + "time" : 1.3637044000932872, "velocity" : 0.0, - "acceleration" : -3.6944293888894246, + "acceleration" : -4.0, "pose" : { "translation" : { "x" : 5.214733123779297, "y" : 2.056851625442505 }, "rotation" : { - "radians" : 2.4154850346736763 + "radians" : 3.0150708953014127 } }, "curvature" : -0.0 } ], "pointList" : [ { - "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ], - "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ] + "x" : [ 6.6141839027404785, -1.4558343887329102, 0.0 ], + "y" : [ 1.5894602537155151, 1.360889196395874, 0.0 ] }, { - "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ], - "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ] + "x" : [ 5.214733123779297, -1.292677402496338, 0.0 ], + "y" : [ 2.056851625442505, 0.1644301414489746, 0.0 ] } ], "rotations" : [ { "time" : 0.0, "rotation" : { - "radians" : 2.37094272169677 + "radians" : 2.3941697598804894 } }, { - "time" : 1.939999956637621, + "time" : 1.269999971613288, "rotation" : { - "radians" : 2.370942522371697 + "radians" : 3.0222255927586548 } } ], "reversed" : false, @@ -288,10 +345,7 @@ "closed" : false, "velocityStart" : 0.0, "velocityEnd" : 0.0, - "constraints" : [ { - "type" : "MaxVelocityConstraint", - "maxVelocity" : 0.75 - } ] + "constraints" : [ ] }, { "type" : "script", "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10", @@ -309,15 +363,20 @@ } }, { "type" : "script", - "script" : "Drive.turnToAngle 40", + "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "NONE", "delay" : 0.0, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setEjectOverride", + "args" : [ "true" ], + "argTypes" : [ "boolean" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Drive.turnToAngle", - "args" : [ "40" ], + "args" : [ "45" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -547,13 +606,18 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "@t 1.3\nIntake.setWantedIntakeState EJECT", + "script" : "@t 1\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "TIME", - "delay" : 1.3, + "delay" : 1.0, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "REVERSE" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Intake.setWantedIntakeState", "args" : [ "EJECT" ], "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], @@ -562,13 +626,18 @@ } }, { "type" : "script", - "script" : "@t 1.6\nIntake.setWantedIntakeState INTAKE", + "script" : "@t 1.6\nHopper.setHopperState ON\nIntake.setWantedIntakeState INTAKE", "closed" : false, "valid" : true, "sendableScript" : { "delayType" : "TIME", "delay" : 1.6, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "ON" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Intake.setWantedIntakeState", "args" : [ "INTAKE" ], "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], @@ -2040,19 +2109,39 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "@t 2\nIntake.setWantedIntakeState EJECT", - "closed" : false, + "script" : "@t 2\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT", + "closed" : true, "valid" : true, "sendableScript" : { "delayType" : "TIME", "delay" : 2.0, "commands" : [ { + "methodName" : "frc.subsystem.Hopper.setHopperState", + "args" : [ "REVERSE" ], + "argTypes" : [ "frc.subsystem.Hopper$HopperState" ], + "reflection" : true + }, { "methodName" : "frc.subsystem.Intake.setWantedIntakeState", "args" : [ "EJECT" ], "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], "reflection" : true } ] } + }, { + "type" : "script", + "script" : "@t 3\nIntake.setWantedIntakeState OFF", + "closed" : false, + "valid" : true, + "sendableScript" : { + "delayType" : "TIME", + "delay" : 3.0, + "commands" : [ { + "methodName" : "frc.subsystem.Intake.setWantedIntakeState", + "args" : [ "OFF" ], + "argTypes" : [ "frc.subsystem.Intake$IntakeState" ], + "reflection" : true + } ] + } }, { "type" : "trajectory", "states" : [ { @@ -2996,7 +3085,7 @@ }, { "time" : 3.118362856162708, "velocity" : 2.9024511229920305, - "acceleration" : 3.2635486192884477, + "acceleration" : 3.2578770345335637, "pose" : { "translation" : { "x" : 3.6274187564849854, @@ -3008,408 +3097,366 @@ }, "curvature" : 0.0 }, { - "time" : 3.1482532878331386, + "time" : 3.148305323595932, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -1.0150458753684738E-14, "pose" : { "translation" : { - "x" : 3.7155035994555945, - "y" : 2.6226850363620713 + "x" : 3.715657867815885, + "y" : 2.6226762181125665 }, "rotation" : { - "radians" : 0.053981642702760804 + "radians" : 0.05341425500982433 } }, - "curvature" : -5.351308120085834E-4 + "curvature" : -0.012753175869738934 }, { - "time" : 3.1770862157675355, - "velocity" : 3.0, - "acceleration" : 0.0, + "time" : 3.1774724263267227, + "velocity" : 2.9999999999999996, + "acceleration" : 1.0337019039886706E-14, "pose" : { "translation" : { - "x" : 3.801876553490956, - "y" : 2.627348984328819 + "x" : 3.803037680920397, + "y" : 2.6272856863245124 }, "rotation" : { - "radians" : 0.05389383309434626 + "radians" : 0.05184775131197311 } }, - "curvature" : -0.001601464316657459 + "curvature" : -0.022669788781850372 }, { - "time" : 3.2048908409093957, + "time" : 3.2061131274893104, "velocity" : 3.0, - "acceleration" : -1.1198792336370368E-14, - "pose" : { - "translation" : { - "x" : 3.885169715891969, - "y" : 2.631834930243855 - }, - "rotation" : { - "radians" : 0.05369200401278312 - } - }, - "curvature" : -0.0033948580681829725 - }, { - "time" : 3.231327573044508, - "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.964366333158978, - "y" : 2.6360779324022587 - }, - "rotation" : { - "radians" : 0.053319276839782104 - } - }, - "curvature" : -0.006242169384276816 - }, { - "time" : 3.256165149733738, - "velocity" : 2.9999999999999996, - "acceleration" : 1.2812741871263266E-14, - "pose" : { - "translation" : { - "x" : 4.038774281627788, - "y" : 2.640028140852877 + "x" : 3.888849129236597, + "y" : 2.631644952964443 }, "rotation" : { - "radians" : 0.0527039245624449 + "radians" : 0.04955597015846323 } }, - "curvature" : -0.01063132499803715 + "curvature" : -0.03035674043346665 }, { - "time" : 3.2792717943698393, + "time" : 3.2340415748283977, "velocity" : 3.0, - "acceleration" : -7.231938957008415E-15, - "pose" : { - "translation" : { - "x" : 4.107999548105681, - "y" : 2.6436497037911977 - }, - "rotation" : { - "radians" : 0.05175599940745225 - } - }, - "curvature" : -0.017237888913125436 - }, { - "time" : 3.320209567074138, - "velocity" : 2.9999999999999996, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.230657418491319, - "y" : 2.6498269150033593 + "x" : 3.9725371670283494, + "y" : 2.6356817669948214 }, "rotation" : { - "radians" : 0.04841053128135275 + "radians" : 0.04676376501044413 } }, - "curvature" : -0.04051841469935257 + "curvature" : -0.03596921557348524 }, { - "time" : 3.3547406099731343, - "velocity" : 2.9999999999999996, - "acceleration" : -0.6325094018133772, + "time" : 3.261120207908762, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.334142312372251, - "y" : 2.654561157464741 + "x" : 4.053689914296321, + "y" : 2.639356401176549 }, "rotation" : { - "radians" : 0.0423299273622696 + "radians" : 0.043689807233283716 } }, - "curvature" : -0.08057010884102427 + "curvature" : -0.03932125691085411 }, { - "time" : 3.384584997268422, - "velocity" : 2.9811231444443704, - "acceleration" : -0.08050231493317403, + "time" : 3.2872560287661248, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.4233291260534315, - "y" : 2.6579580020916183 + "x" : 4.1320278016889915, + "y" : 2.642658761824123 }, "rotation" : { - "radians" : 0.03315653358683334 + "radians" : 0.040562671415580655 } }, - "curvature" : -0.1233537474620638 + "curvature" : -0.039940377095716434 }, { - "time" : 3.4121081263600206, - "velocity" : 2.978907468838292, - "acceleration" : 0.757187202067055, + "time" : 3.3123968620373043, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.5053166644408975, - "y" : 2.6602420038625496 + "x" : 4.207392715413668, + "y" : 2.6456054985607906 }, "rotation" : { - "radians" : 0.022490284597376595 + "radians" : 0.03763198896608686 } }, - "curvature" : -0.1268933627335242 + "curvature" : -0.03710839155892508 }, { - "time" : 3.4399645548682245, + "time" : 3.336527620370588, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.588579021394253, - "y" : 2.661721706390381 + "x" : 4.279737142147496, + "y" : 2.6482371140737087 }, "rotation" : { - "radians" : 0.01374388263590382 + "radians" : 0.035174735467647336 } }, - "curvature" : -0.07804765164973368 + "curvature" : -0.02992149474512993 }, { - "time" : 3.471145768832688, + "time" : 3.359666591601313, "velocity" : 3.0, - "acceleration" : -7.895171233701495E-15, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.682116960079156, - "y" : 2.6627546464942498 + "x" : 4.349113313948472, + "y" : 2.6506150738690977 }, "rotation" : { - "radians" : 0.009189096860167837 + "radians" : 0.033495618460887784 } }, - "curvature" : -0.024057614430750256 + "curvature" : -0.017400083112939065 }, { - "time" : 3.508644572148012, - "velocity" : 2.9999999999999996, - "acceleration" : 1.3503955874947296E-14, + "time" : 3.3818617651034137, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.794609293319809, - "y" : 2.6637123587715905 + "x" : 4.415662353166454, + "y" : 2.6528189160273996 }, "rotation" : { - "radians" : 0.008349254243389597 + "radians" : 0.03291987664459664 } }, - "curvature" : 0.0037103968236503098 + "curvature" : 0.0013263625292214375 }, { - "time" : 3.5305684791849585, + "time" : 3.423739530881724, "velocity" : 3.0, - "acceleration" : 0.0, + "acceleration" : -0.8175000091130344, "pose" : { "translation" : { - "x" : 4.860378613618231, - "y" : 2.6642743243984057 + "x" : 4.5412228441782645, + "y" : 2.657095421156555 }, "rotation" : { - "radians" : 0.00879853748094375 + "radians" : 0.0363729513028002 } }, - "curvature" : 0.009166818258418976 + "curvature" : 0.05866331262890874 }, { - "time" : 3.554964721451485, - "velocity" : 3.0, - "acceleration" : -1.992727596873581, + "time" : 3.4632152976715145, + "velocity" : 2.967728560289602, + "acceleration" : -1.882760480943012, "pose" : { "translation" : { - "x" : 4.933564263951439, - "y" : 2.6649453801701384 + "x" : 4.658912906437536, + "y" : 2.6619545562850817 }, "rotation" : { - "radians" : 0.009565324282873914 + "radians" : 0.047679751894998845 } }, - "curvature" : 0.011329196406191677 + "curvature" : 0.13592977249614052 }, { - "time" : 3.5822868057216533, - "velocity" : 2.9455545286707294, - "acceleration" : -3.9999999999999902, + "time" : 3.5018404054419703, + "velocity" : 2.8950067338072225, + "acceleration" : -1.9238632002001914, "pose" : { "translation" : { - "x" : 5.01478264645214, - "y" : 2.6657603208850844 + "x" : 4.771954350173473, + "y" : 2.668388433754444 }, "rotation" : { - "radians" : 0.010504868424232214 + "radians" : 0.0674512871959646 } }, - "curvature" : 0.011571381180452858 + "curvature" : 0.2106703311228447 }, { - "time" : 3.613394389130737, - "velocity" : 2.821124195034394, - "acceleration" : -4.0, + "time" : 3.541164793474475, + "velocity" : 2.8193519908010933, + "acceleration" : -3.999999999999996, "pose" : { "translation" : { - "x" : 5.104470925172791, - "y" : 2.666748254559934 + "x" : 4.883949141742278, + "y" : 2.677400863588673 }, "rotation" : { - "radians" : 0.011513061014706312 + "radians" : 0.09396342350209999 } }, - "curvature" : 0.01080959208114291 + "curvature" : 0.2540441775959243 }, { - "time" : 3.649180845248966, - "velocity" : 2.6779783705614784, - "acceleration" : -4.000000000000004, + "time" : 3.583307920417398, + "velocity" : 2.6507794830294023, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.202860506721613, - "y" : 2.6679315088226474 + "x" : 4.998532040779537, + "y" : 2.689914865659375 }, "rotation" : { - "radians" : 0.012518180765925502 + "radians" : 0.12355864176280444 } }, - "curvature" : 0.009593832950905505 + "curvature" : 0.25210444079517663 }, { - "time" : 3.690445008393871, - "velocity" : 2.5129217179818584, - "acceleration" : -4.0, + "time" : 3.630910520336883, + "velocity" : 2.460369083351462, + "acceleration" : -3.999999999999993, "pose" : { "translation" : { - "x" : 5.30995052089861, - "y" : 2.6693245373053287 + "x" : 5.119023237352621, + "y" : 2.706680181850743 }, "rotation" : { - "radians" : 0.01347191339238352 + "radians" : 0.1521781406035511 } }, - "curvature" : 0.008225115612607133 + "curvature" : 0.21456009552095912 }, { - "time" : 3.738242422456052, - "velocity" : 2.3217320617331336, - "acceleration" : -3.9999999999999964, + "time" : 3.6575544532910382, + "velocity" : 2.3537933515348413, + "acceleration" : -3.9999999999999933, "pose" : { "translation" : { - "x" : 5.42548130133158, - "y" : 2.6709328260371024 + "x" : 5.182350507669064, + "y" : 2.7168212357026604 }, "rotation" : { - "radians" : 0.014341527416621765 + "radians" : 0.16512628632297052 } }, - "curvature" : 0.006849417564785735 + "curvature" : 0.18899817754880754 }, { - "time" : 3.7940970341308637, - "velocity" : 2.098313615033887, + "time" : 3.6866111335997718, + "velocity" : 2.237566630299908, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.548907866112131, - "y" : 2.6727517998369876 + "x" : 5.2480809891130775, + "y" : 2.728180788224563 }, "rotation" : { - "radians" : 0.015103596532319463 + "radians" : 0.1768351792035555 } }, - "curvature" : 0.005520155477322901 + "curvature" : 0.16220610323939916 }, { - "time" : 3.825756511779807, - "velocity" : 1.9716757044381135, - "acceleration" : -4.0000000000000036, + "time" : 3.718563991190063, + "velocity" : 2.109755199938744, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.6133272185697205, - "y" : 2.673735843651399 + "x" : 5.3163865477339485, + "y" : 2.740762636728995 }, "rotation" : { - "radians" : 0.015438273175695332 + "radians" : 0.1871757577997712 } }, - "curvature" : 0.004873668640140536 + "curvature" : 0.13593088502288128 }, { - "time" : 3.860481196136515, - "velocity" : 1.8327769670112823, - "acceleration" : -4.0, + "time" : 3.754021980661885, + "velocity" : 1.967923242051456, + "acceleration" : -4.000000000000003, "pose" : { "translation" : { - "x" : 5.6793733984317, - "y" : 2.674765728706774 + "x" : 5.387354258449022, + "y" : 2.7545424071852267 }, "rotation" : { - "radians" : 0.015738980768460056 + "radians" : 0.19609326514864456 } }, - "curvature" : 0.004234449415552546 + "curvature" : 0.11126066622925063 }, { - "time" : 3.89893588264187, - "velocity" : 1.678958220989862, + "time" : 3.7938015783178165, + "velocity" : 1.8088048514277302, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.746886228560693, - "y" : 2.6758375663932243 + "x" : 5.460975549954718, + "y" : 2.7694646639744107 }, "rotation" : { - "radians" : 0.016003288340590872 + "radians" : 0.20358486920868646 } }, - "curvature" : 0.003596071353112922 + "curvature" : 0.08871019500715766 }, { - "time" : 3.9421404788446717, - "velocity" : 1.5061398361786555, - "acceleration" : -3.999999999999997, + "time" : 3.8390908801882153, + "velocity" : 1.627647643946134, + "acceleration" : -3.9999999999999973, "pose" : { "translation" : { - "x" : 5.815682727217563, - "y" : 2.6769466342238957 + "x" : 5.537135349637538, + "y" : 2.7854400196447386 }, "rotation" : { - "radians" : 0.016228495089680457 + "radians" : 0.20967774359389213 } }, - "curvature" : 0.0029497967820049952 + "curvature" : 0.06834921616165193 }, { - "time" : 3.991815920611223, - "velocity" : 1.3074380691124496, - "acceleration" : -4.000000000000002, + "time" : 3.8918213921686315, + "velocity" : 1.4167255960244693, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.885556279331293, - "y" : 2.678087341659743 + "x" : 5.615601228485083, + "y" : 2.8023422446665975 }, "rotation" : { - "radians" : 0.016411476850763638 + "radians" : 0.21440925415138234 } }, - "curvature" : 0.0022842194098207026 + "curvature" : 0.049921154171936793 }, { - "time" : 4.0513318851535365, - "velocity" : 1.0693742109431945, - "acceleration" : -3.9999999999999982, + "time" : 3.955692162693215, + "velocity" : 1.1612425139261353, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.956275807768854, - "y" : 2.679253195934308 + "x" : 5.696012545997064, + "y" : 2.8200053771877265 }, "rotation" : { - "radians" : 0.0165485050781744 + "radians" : 0.21780924678420455 } }, - "curvature" : 0.0015845831447143007 + "curvature" : 0.03292187698251918 }, { - "time" : 4.129431954649663, - "velocity" : 0.75697393295869, + "time" : 4.040210129462593, + "velocity" : 0.8231706468486228, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.027584944605088, - "y" : 2.680436767878498 + "x" : 5.777869595096313, + "y" : 2.838220832788373 }, "rotation" : { - "radians" : 0.016635023032639307 + "radians" : 0.21988311898862645 } }, - "curvature" : 8.316618868490522E-4 + "curvature" : 0.016620346183280694 }, { - "time" : 4.318675437889335, + "time" : 4.246002791174749, "velocity" : 0.0, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.099201202392578, - "y" : 2.6816296577453613 + "x" : 5.860522747039795, + "y" : 2.85673451423645 }, "rotation" : { - "radians" : 0.016665357555075345 + "radians" : 0.22059308726779464 } }, "curvature" : 0.0 @@ -3424,8 +3471,8 @@ "x" : [ 3.6274187564849854, 2.828535318374634, 0.0 ], "y" : [ 2.617924213409424, 0.1528944969177246, 0.0 ] }, { - "x" : [ 6.099201202392578, 4.586813449859619, 0.0 ], - "y" : [ 2.6816296577453613, 0.07644796371459961, 0.0 ] + "x" : [ 5.860522747039795, 2.649440288543701, 0.0 ], + "y" : [ 2.85673451423645, 0.5941164493560791, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -3440,12 +3487,12 @@ }, { "time" : 3.1199999302625656, "rotation" : { - "radians" : 0.0 + "radians" : 0.4494112193892231 } }, { - "time" : 4.229999905452132, + "time" : 4.159999907016754, "rotation" : { - "radians" : 0.00740696187012303 + "radians" : 0.46364803834258117 } } ], "reversed" : false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cad6602d..270333da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -165,8 +165,8 @@ public final class Constants { // TurnPID public static final double DEFAULT_TURN_P = 12.0; - public static final double DEFAULT_TURN_I = 20.0; - public static final double DEFAULT_TURN_D = 0.0; + public static final double DEFAULT_TURN_I = 15.0; + public static final double DEFAULT_TURN_D = 0.5; public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0; public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7e7caa99..13b5df49 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -508,6 +508,7 @@ public void teleopInit() { SmartDashboard.putBoolean("Field Relative Enabled", true); drive.useFieldRelative = true; SmartDashboard.putBoolean("Drive Field Relative Allowed", true); + hopper.setEjectOverride(false); drive.configBrake(); } diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 882c6472..f472acf6 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -672,7 +672,7 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, turnPID.setSetpoint(goal.position); - if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate || turnPID.getPositionError() > Math.toRadians(30)) { + if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate || turnPID.getPositionError() > Math.toRadians(7)) { turnPID.reset(); } lastTurnUpdate = Timer.getFPGATimestamp(); @@ -832,8 +832,7 @@ public void turnToAngle(double degrees) throws InterruptedException { System.out.println("Turning to " + degrees); setRotation(degrees); while (!isTurningDone()) { - Thread.onSpinWait(); - if (Thread.interrupted()) throw new InterruptedException("Interrupted while turning"); + Thread.sleep(20); } } } diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java index 362be1b1..242eec89 100644 --- a/src/main/java/frc/subsystem/Hopper.java +++ b/src/main/java/frc/subsystem/Hopper.java @@ -111,6 +111,13 @@ public void toggleEjectOverride() { disableEject = !disableEject; } + /** + * Toggles disableEject + */ + public void setEjectOverride(boolean ejectOverride) { + disableEject = ejectOverride; + } + /** * Updates state of outtake based on color sensor and intake direction */ diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 551af4ff..4fc82158 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -170,6 +170,7 @@ private void tryToShoot(Translation2d aimToPosition, double targetAngularSpeed, < getAllowedTurnError(aimToPosition.getNorm()) && Math.abs(robotTracker.getLatencyCompedChassisSpeeds().omegaRadiansPerSecond - targetAngularSpeed) < Math.toRadians(8) + && getAccel().getNorm() < 1 && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck) && Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) { //@formatter:on @@ -531,7 +532,7 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat if (distance < 120) { timeOfFlightFrames = ((0.02 / 30) * (distance - 120)) + (22.0 / 30); } else { - timeOfFlightFrames = ((0.07 / 30) * (distance - 120)) + (22.0 / 30); + timeOfFlightFrames = ((0.071 / 30) * (distance - 120)) + (22.0 / 30); } //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542; From b6ccccfe39bfc2ae2ea59027746be9826f8f091a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Wed, 30 Mar 2022 09:28:59 -0700 Subject: [PATCH 068/108] fix synchronization issues in drive --- src/main/java/frc/subsystem/Drive.java | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index f472acf6..e1aabee1 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -288,7 +288,7 @@ public void setTeleop() { setDriveState(DriveState.TELEOP); } - synchronized public SwerveModuleState @NotNull [] getSwerveModuleStates() { + public @NotNull SwerveModuleState[] getSwerveModuleStates() { SwerveModuleState[] swerveModuleState = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { SwerveModuleState moduleState = new SwerveModuleState( @@ -573,7 +573,12 @@ private void updateRamsete() { } public void setAutoRotation(@NotNull Rotation2d rotation) { - autoTargetHeading = rotation; + currentAutoTrajectoryLock.lock(); + try { + autoTargetHeading = rotation; + } finally { + currentAutoTrajectoryLock.unlock(); + } System.out.println("new rotation" + rotation.getDegrees()); } @@ -621,9 +626,12 @@ public synchronized void setRotation(double angle) { } - public synchronized boolean isTurningDone() { - double error = wantedHeading.minus(RobotTracker.getInstance().getGyroAngle()).getDegrees(); - return (Math.abs(error) < Constants.MAX_TURN_ERROR); + public boolean isTurningDone() { + Rotation2d currentHeading = RobotTracker.getInstance().getGyroAngle(); + synchronized (this) { + double error = wantedHeading.minus(currentHeading).getDegrees(); + return (Math.abs(error) < Constants.MAX_TURN_ERROR); + } } double turnMinSpeed = 0; From a64b95ab5177a3c71648e899513ad3709db5f633 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Wed, 30 Mar 2022 17:25:37 -0700 Subject: [PATCH 069/108] don't acquire the write lock in RobotTracker while we're accessing methods from drive. --- src/main/java/frc/subsystem/Drive.java | 12 ++++++------ src/main/java/frc/subsystem/RobotTracker.java | 10 +++++++--- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index e1aabee1..ab74fcb6 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -96,7 +96,7 @@ public enum DriveState { private boolean isAiming = false; - private @NotNull final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( + private @NotNull static final SwerveDriveKinematics SWERVE_DRIVE_KINEMATICS = new SwerveDriveKinematics( Constants.SWERVE_LEFT_FRONT_LOCATION, Constants.SWERVE_LEFT_BACK_LOCATION, Constants.SWERVE_RIGHT_FRONT_LOCATION, @@ -276,8 +276,8 @@ private double getSwerveDriveVelocity(int motorNum) { return swerveDriveMotors[motorNum].getSelectedSensorVelocity() * Constants.FALCON_ENCODER_TICKS_PER_100_MS_TO_RPM * Constants.SWERVE_DRIVE_MOTOR_REDUCTION; } - public @NotNull SwerveDriveKinematics getSwerveDriveKinematics() { - return swerveKinematics; + public static @NotNull SwerveDriveKinematics getSwerveDriveKinematics() { + return SWERVE_DRIVE_KINEMATICS; } public synchronized void setDriveState(@NotNull DriveState driveState) { @@ -349,7 +349,7 @@ public void swerveDrive(ChassisSpeeds chassisSpeeds) { SmartDashboard.putNumber("Drive Command Y Velocity", chassisSpeeds.vyMetersPerSecond); SmartDashboard.putNumber("Drive Command Rotation", chassisSpeeds.omegaRadiansPerSecond); - SwerveModuleState[] moduleStates = swerveKinematics.toSwerveModuleStates(chassisSpeeds); + SwerveModuleState[] moduleStates = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); boolean rotate = chassisSpeeds.vxMetersPerSecond != 0 || chassisSpeeds.vyMetersPerSecond != 0 || @@ -614,14 +614,14 @@ synchronized public boolean isAiming() { return isAiming; } - public synchronized void setRotation(Rotation2d angle) { + public void setRotation(Rotation2d angle) { wantedHeading = angle; driveState = DriveState.TURN; rotateAuto = true; isAiming = !isTurningDone(); } - public synchronized void setRotation(double angle) { + public void setRotation(double angle) { setRotation(Rotation2d.fromDegrees(angle)); } diff --git a/src/main/java/frc/subsystem/RobotTracker.java b/src/main/java/frc/subsystem/RobotTracker.java index 1fda1826..999096f6 100644 --- a/src/main/java/frc/subsystem/RobotTracker.java +++ b/src/main/java/frc/subsystem/RobotTracker.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.WPIUtilJNI; @@ -50,6 +51,8 @@ public final class RobotTracker extends AbstractSubsystem { private double maxGyroRoll = 0; private double minGyroRoll = 0; + private final @NotNull SwerveDriveKinematics swerveDriveKinematics = Drive.getSwerveDriveKinematics(); + private final ReentrantReadWriteLock lock = new ReentrantReadWriteLock(); private Translation2d acceleration = new Translation2d(); @@ -70,7 +73,7 @@ private RobotTracker() { // (Constants.ROBOT_TRACKER_PERIOD * 2) / 1000.0d); //Only update every other tick swerveDriveOdometry = new SwerveDriveOdometry( - drive.getSwerveDriveKinematics(), + swerveDriveKinematics, gyroSensor.getRotation2d(), new Pose2d() @@ -151,14 +154,15 @@ public void update() { } if (updateNextTick) { - updateOdometry(time, rawGyroSensor, drive.getSwerveModuleStates()); + SwerveModuleState[] swerveModuleStates = drive.getSwerveModuleStates(); + updateOdometry(time, rawGyroSensor, swerveModuleStates); lock.writeLock().lock(); try { latestEstimatedPose = swerveDriveOdometry.getPoseMeters(); //gyroOffset = latestEstimatedPose.getRotation().minus(rawGyroSensor); latestChassisSpeeds = getRotatedSpeeds( - drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()), + swerveDriveKinematics.toChassisSpeeds(swerveModuleStates), getGyroAngle()); latencyCompensatedChassisSpeeds = latestChassisSpeeds; From 9fc377026586a1d76f74e1ec64281b62d1235727 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Wed, 30 Mar 2022 17:27:06 -0700 Subject: [PATCH 070/108] add pressing the LEFT_CLICK button on the xbox controller to toggle filed relative mode --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 13b5df49..cdd2b864 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -595,7 +595,7 @@ public void teleopPeriodic() { hopper.toggleEjectOverride(); } - if (xbox.getRisingEdge(Controller.XboxButtons.START)) { + if (xbox.getRisingEdge(XboxButtons.LEFT_CLICK) || xbox.getRisingEdge(Controller.XboxButtons.START)) { useFieldRelative = !useFieldRelative; System.out.println("Field relative: " + useFieldRelative); SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative); From 8f4dd6e639fdc6880f3b13f6abf2769fd42312c7 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Wed, 30 Mar 2022 20:24:55 -0700 Subject: [PATCH 071/108] reduce d value in turnPID --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 270333da..cce81ef4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -166,7 +166,7 @@ public final class Constants { public static final double DEFAULT_TURN_P = 12.0; public static final double DEFAULT_TURN_I = 15.0; - public static final double DEFAULT_TURN_D = 0.5; + public static final double DEFAULT_TURN_D = 0.25; public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0; public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0; From 52cfd101220ecfd1fabe1219f2e5a23db51aab72 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Wed, 30 Mar 2022 21:43:50 -0700 Subject: [PATCH 072/108] disable web dashboard code reduce allocations in Limelight --- .../java/frc/subsystem/DashboardHandler.java | 115 ++++++++---------- .../java/frc/subsystem/VisionManager.java | 2 +- src/main/java/frc/utility/Limelight.java | 29 ++--- 3 files changed, 68 insertions(+), 78 deletions(-) diff --git a/src/main/java/frc/subsystem/DashboardHandler.java b/src/main/java/frc/subsystem/DashboardHandler.java index c71ba86a..00707b31 100644 --- a/src/main/java/frc/subsystem/DashboardHandler.java +++ b/src/main/java/frc/subsystem/DashboardHandler.java @@ -3,19 +3,15 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; -import frc.utility.Serializer; import frc.utility.Timer; import frc.utility.net.DashboardConnection; import frc.utility.net.PacketHandler; -import frc.utility.net.SendableLog; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import java.io.IOException; import java.net.*; -import java.nio.charset.StandardCharsets; import java.util.HashMap; -import java.util.Iterator; import java.util.Map; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.locks.ReadWriteLock; @@ -100,73 +96,66 @@ public void log(@NotNull String key, @NotNull Object value, boolean logToNetwork } public void pushLog() { + String json; + LOG_DATA_MAP_LOCK.writeLock().lock(); // Ensure that no other writes happen while we're serializing try { - String json; - LOG_DATA_MAP_LOCK.writeLock().lock(); // Ensure that no other writes happen while we're serializing - try { - SendableLog log = new SendableLog(LOG_DATA_MAP); - json = 'd' + Serializer.serializeToString(log); - - for (Map.Entry entry : LOG_DATA_MAP_OBJECTS.entrySet()) { - //@formatter:off - String key = entry.getKey(); - Class cl = entry.getValue().getClass(); - Object value = entry.getValue(); - if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); - else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); - else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); - else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); - else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); - else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); - else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); - else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); - else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); - else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); - else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); - else SmartDashboard.putString(key, value.toString()); - //@formatter:on - } - } finally { - LOG_DATA_MAP_LOCK.writeLock().unlock(); - } - - synchronized (dashboardConnections) { - Iterator iterator = dashboardConnections.values().iterator(); - while (iterator.hasNext()) { - DashboardConnection entry = iterator.next(); - if (Timer.getFPGATimestamp() > entry.timeoutTime) { - // If we haven't received a keepalive in a while, remove the connection - entry.close(); - iterator.remove(); - System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " timed out"); - } else { - byte[] bytes = json.getBytes(StandardCharsets.ISO_8859_1); - try { - DatagramPacket packet = new DatagramPacket(bytes, bytes.length); - - entry.datagramSocket.send(packet); - System.out.println( - "sending packet of " + packet.getLength() + " length to " + entry.datagramSocket.getInetAddress()); - } catch (PortUnreachableException e) { - System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " got an error"); - entry.close(); - iterator.remove(); // Remove the connection if it's unreachable - } - } - } - } - } catch (IOException e) { - if (Timer.getFPGATimestamp() > nextAllowedErrorTime) { // Don't spam the driver station - DriverStation.reportError("Could not send data to web dashboard", false); - nextAllowedErrorTime = Timer.getFPGATimestamp() + 5; + //SendableLog log = new SendableLog(LOG_DATA_MAP); + //json = 'd' + Serializer.serializeToString(log); + + for (Map.Entry entry : LOG_DATA_MAP_OBJECTS.entrySet()) { + //@formatter:off + String key = entry.getKey(); + Class cl = entry.getValue().getClass(); + Object value = entry.getValue(); + if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value); + else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value); + else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value); + else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value); + else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value); + else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value); + else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value); + else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value); + else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value); + else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value); + else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value); + else SmartDashboard.putString(key, value.toString()); + //@formatter:on } + } finally { + LOG_DATA_MAP_LOCK.writeLock().unlock(); } + +// synchronized (dashboardConnections) { +// Iterator iterator = dashboardConnections.values().iterator(); +// while (iterator.hasNext()) { +// DashboardConnection entry = iterator.next(); +// if (Timer.getFPGATimestamp() > entry.timeoutTime) { +// // If we haven't received a keepalive in a while, remove the connection +// entry.close(); +// iterator.remove(); +// System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " timed out"); +// } else { +// byte[] bytes = json.getBytes(StandardCharsets.ISO_8859_1); +// try { +// DatagramPacket packet = new DatagramPacket(bytes, bytes.length); +// +// entry.datagramSocket.send(packet); +// System.out.println( +// "sending packet of " + packet.getLength() + " length to " + entry.datagramSocket.getInetAddress()); +// } catch (PortUnreachableException e) { +// System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " got an error"); +// entry.close(); +// iterator.remove(); // Remove the connection if it's unreachable +// } +// } +// } +// } } byte[] receivedBytes = new byte[65535]; private void handleDashboardPackets() { - if (receivingSocket == null) return; + if (receivingSocket == null || true) return; for (int i = 0; i < 100; i++) { // Receive up to 100 packets at a time try { diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 4fc82158..93677102 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -49,7 +49,7 @@ public void setShooterConfig(ShooterConfig shooterConfig) { } private VisionManager() { - super(Constants.VISION_MANAGER_PERIOD, 2); + super(Constants.VISION_MANAGER_PERIOD, 4); } public static @NotNull VisionManager getInstance() { diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 514b4530..926cd333 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -38,10 +38,12 @@ public final class Limelight extends AbstractSubsystem { } - volatile double angle = -38; + volatile Rotation cameraRotation = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, Math.toRadians(-38), 0, + 0); + volatile double hOffset = 43; volatile double depthOffset = 34.5; - volatile double centerOffset = 6.9; + volatile Vector3D centerOffset = new Vector3D(0, 0, 6.9); public static @NotNull Limelight getInstance(String name) { LIMELIGHT_MAP_LOCK.readLock().lock(); @@ -158,8 +160,10 @@ private Limelight(String name) { limelightGuiTable = NetworkTableInstance.getDefault().getTable(name + "gui"); NetworkTableEntry angleTable = limelightGuiTable.getEntry("angle"); - angleTable.setDouble(angle); - angleTable.addListener(event -> angle = event.getEntry().getDouble(0), + angleTable.setDouble(cameraRotation.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[0]); + angleTable.addListener(event -> cameraRotation + = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, + Math.toRadians(event.getEntry().getDouble(0)), 0, 0), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); NetworkTableEntry hOffsetTable = limelightGuiTable.getEntry("hOffset"); @@ -173,8 +177,8 @@ private Limelight(String name) { EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); NetworkTableEntry centerOffsetTable = limelightGuiTable.getEntry("centerOffset"); - centerOffsetTable.setDouble(centerOffset); - centerOffsetTable.addListener(event -> centerOffset = event.getEntry().getDouble(0), + centerOffsetTable.setDouble(centerOffset.getZ()); + centerOffsetTable.addListener(event -> centerOffset = new Vector3D(0, 0, event.getEntry().getDouble(0)), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); @@ -330,6 +334,7 @@ public Vector2d getTargetPosInCameraPixels() { 0, 0, 1 ); + private static final MatBuilder THREE_BY_ONE_MAT_BUILDER = new MatBuilder<>(Nat.N3(), Nat.N1()); /** * @return The Correct distance from the limelight to the target in IN. This correctly compensates for the angle of the camera * to ensure a consistent distance as the robot rotates. @@ -342,11 +347,7 @@ public Vector3D getCorrectTargetVector() { double py = targetPosInCameraPixels.y; double px = targetPosInCameraPixels.x; - Matrix cameraUnitVector = CAMERA_MATRIX_INVERSE.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(px, py, 1)); - - - Rotation cameraRotation = - new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, Math.toRadians(angle), 0, 0); + Matrix cameraUnitVector = CAMERA_MATRIX_INVERSE.times(THREE_BY_ONE_MAT_BUILDER.fill(px, py, 1)); Vector3D vector3D = new Vector3D( cameraUnitVector.get(0, 0), @@ -360,10 +361,10 @@ public Vector3D getCorrectTargetVector() { double k = hOffset / goalDir.getY(); return goalDir.scalarMultiply(k) - .add(new Vector3D(0, 0, centerOffset) //5.875 - .add(new Vector3D(Math.sin(angle) * depthOffset, 0, Math.cos(angle) * depthOffset))); + .add(centerOffset) //5.875 + .add(new Vector3D(Math.sin(angle) * depthOffset, 0, Math.cos(angle) * depthOffset)); } else { - return new Vector3D(0, 0, 0); + return Vector3D.ZERO; } } From f48739629c765f608ed1ebb48e0a6a87669c5c71 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 31 Mar 2022 15:37:59 -0700 Subject: [PATCH 073/108] add the calculated velocity to the turnPID's output also reduce I since the calculated turn velocity will be taking most of it's place --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/subsystem/Drive.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cce81ef4..82ac2111 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -165,7 +165,7 @@ public final class Constants { // TurnPID public static final double DEFAULT_TURN_P = 12.0; - public static final double DEFAULT_TURN_I = 15.0; + public static final double DEFAULT_TURN_I = 5.0; public static final double DEFAULT_TURN_D = 0.25; public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0; public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0; diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index ab74fcb6..f7c2c480 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -684,7 +684,7 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, turnPID.reset(); } lastTurnUpdate = Timer.getFPGATimestamp(); - double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()); + double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()) + goal.velocity; // System.out.println( // "turn error: " + Math.toDegrees(turnPID.getPositionError()) + " delta speed: " + Math.toDegrees(pidDeltaSpeed)); From f15c47acf34e139c9f2dcca819b48fe267738a61 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 31 Mar 2022 15:56:32 -0700 Subject: [PATCH 074/108] decrease the point where point where we transition from constantish to linear on our tof model --- src/main/java/frc/subsystem/VisionManager.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 93677102..8b4e369d 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -530,9 +530,9 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat double timeOfFlightFrames; if (distance < 120) { - timeOfFlightFrames = ((0.02 / 30) * (distance - 120)) + (22.0 / 30); + timeOfFlightFrames = ((0.02 / 30) * (distance - 113)) + (22.0 / 30); } else { - timeOfFlightFrames = ((0.071 / 30) * (distance - 120)) + (22.0 / 30); + timeOfFlightFrames = ((0.071 / 30) * (distance - 113)) + (22.0 / 30); } //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542; From ddfbdff6bcd7b49796879a23e7afc79628becddb Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 14:20:51 -0700 Subject: [PATCH 075/108] update shooter setting. after practice 11 --- src/main/deploy/shooter/shooterconfig.json | 14 +++++++------- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/subsystem/VisionManager.java | 4 ++-- src/main/java/frc/utility/Limelight.java | 4 ++-- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 69ec53f0..c1cfd820 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -25,7 +25,7 @@ "distance" : 113.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1900.0, + "flywheelSpeed" : 1950.0, "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, @@ -33,7 +33,7 @@ "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2200.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, @@ -45,7 +45,7 @@ "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2400.0, + "flywheelSpeed" : 2300.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, @@ -53,11 +53,11 @@ "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2450.0, + "flywheelSpeed" : 2500.0, "distance" : 202.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2550.0, + "flywheelSpeed" : 2500.0, "distance" : 224.0 }, { "hoodEjectAngle" : 51.0, @@ -65,11 +65,11 @@ "distance" : 241.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3000.0, + "flywheelSpeed" : 2700.0, "distance" : 265.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3200.0, + "flywheelSpeed" : 3000.0, "distance" : 300.0 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 82ac2111..0a0f7e63 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -165,8 +165,8 @@ public final class Constants { // TurnPID public static final double DEFAULT_TURN_P = 12.0; - public static final double DEFAULT_TURN_I = 5.0; - public static final double DEFAULT_TURN_D = 0.25; + public static final double DEFAULT_TURN_I = 15.0; + public static final double DEFAULT_TURN_D = 0.4; public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0; public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0; diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 8b4e369d..de00d402 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -135,7 +135,7 @@ public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean us double targetAngle = angleOf(aimToPosition).getRadians(); // Get the angle that will be used in the future to calculate the end velocity of the turn - Translation2d futureAimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay + (justShot ? 0.0 : 0.1)); + Translation2d futureAimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay + 0.1); double futureTargetAngle = angleOf(futureAimToPosition).getRadians(); drive.updateTurn(controllerDriveInputs, @@ -181,7 +181,7 @@ && getAccel().getNorm() < 1 checksPassedLastTime = true; System.out.println( "Shooting at " + (150 - DriverStation.getMatchTime()) + " " - + visionLookUpTable.getShooterPreset(Units.metersToInches(getDistanceToTarget()))); + + aimToPosition.getNorm()); } } else { lastChecksFailedTime = Timer.getFPGATimestamp(); diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 926cd333..9a51f1c2 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -41,8 +41,8 @@ public final class Limelight extends AbstractSubsystem { volatile Rotation cameraRotation = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, Math.toRadians(-38), 0, 0); - volatile double hOffset = 43; - volatile double depthOffset = 34.5; + volatile double hOffset = 38; + volatile double depthOffset = 44; volatile Vector3D centerOffset = new Vector3D(0, 0, 6.9); public static @NotNull Limelight getInstance(String name) { From 5283d616c0a29c34e403591d3ad15baa3f78b0aa Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 16:07:39 -0700 Subject: [PATCH 076/108] decrease the clamping amount in the climb --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0a0f7e63..91a7dcf5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -498,7 +498,7 @@ public final class Constants { /** * Length to grab on high and traversal bars */ - public static final double CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION = (27.2 - 4.5) * CLIMBER_ENCODER_TICKS_PER_INCH; + public static final double CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION = (27.2 - 4.0) * CLIMBER_ENCODER_TICKS_PER_INCH; /** * How long only one of the sensor switches can be closed for before the climb will pause From 6bec2a0b052f8ccfa5fbd64d69c5215706f7ad15 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 16:08:17 -0700 Subject: [PATCH 077/108] enable the brake on the elevator before we release from the last bar --- src/main/java/frc/subsystem/Climber.java | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index cf651513..bd1cbb24 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -177,6 +177,7 @@ public enum ClimbState { LOWER_ELEVATOR_ARM_TILL_PIVOT_ARM_CONTACT( new ClimbStep( (cl) -> { + cl.setBrakeState(BrakeState.FREE); cl.climberMotor.set(ControlMode.PercentOutput, -Constants.CLIMBER_MOTOR_MAX_OUTPUT); cl.setClawState(ClawState.UNLATCHED); }, @@ -187,6 +188,7 @@ public enum ClimbState { new ClimbStep( (cl) -> { + cl.setBrakeState(BrakeState.FREE); cl.climberMotor.set(ControlMode.MotionMagic, Constants.CLIMBER_GRAB_ON_FIRST_BAR_EXTENSION); cl.setClawState(ClawState.UNLATCHED); }, @@ -401,11 +403,30 @@ public enum ClimbState { }, (cl) -> cl.climberMotor.getSelectedSensorPosition() - (0.3 * CLIMBER_ENCODER_TICKS_PER_INCH) < CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION, // TODO: Determine if we want this - (cl) -> {}, + (cl) -> { + cl.stopClimberMotor(); + cl.setBrakeState(BrakeState.BRAKING); + }, true ) ), + WAIT_FOR_BRAKE_TIME( + new ClimbStep( + (cl) -> cl.data = Timer.getFPGATimestamp(), + (cl) -> Timer.getFPGATimestamp() - cl.data > 0.3, + (cl) -> {}, + false + ), + + new ClimbStep( + (cl) -> cl.data = Timer.getFPGATimestamp(), + (cl) -> Timer.getFPGATimestamp() - cl.data > 0.3, + (cl) -> {}, + false + ) + ), + /** * Unlatches the pivot arm so that the robot is supported by the elevator arm. */ From 7489bca67ba5316bfa1f55d9b89247733cb4d6c1 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 16:58:17 -0700 Subject: [PATCH 078/108] adjust climber more --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/subsystem/Climber.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91a7dcf5..b25dbef9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -498,7 +498,7 @@ public final class Constants { /** * Length to grab on high and traversal bars */ - public static final double CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION = (27.2 - 4.0) * CLIMBER_ENCODER_TICKS_PER_INCH; + public static final double CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION = (27.2 - 3.5) * CLIMBER_ENCODER_TICKS_PER_INCH; /** * How long only one of the sensor switches can be closed for before the climb will pause diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index bd1cbb24..6f6954b0 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -414,15 +414,15 @@ public enum ClimbState { WAIT_FOR_BRAKE_TIME( new ClimbStep( (cl) -> cl.data = Timer.getFPGATimestamp(), - (cl) -> Timer.getFPGATimestamp() - cl.data > 0.3, - (cl) -> {}, + (cl) -> Timer.getFPGATimestamp() - cl.data > 0.1, + (cl) -> cl.climberMotor.set(ControlMode.PercentOutput, 0), false ), new ClimbStep( (cl) -> cl.data = Timer.getFPGATimestamp(), - (cl) -> Timer.getFPGATimestamp() - cl.data > 0.3, - (cl) -> {}, + (cl) -> Timer.getFPGATimestamp() - cl.data > 0.1, + (cl) -> cl.climberMotor.set(ControlMode.PercentOutput, 0), false ) ), From 40721a978339117503d95496297b44b0b829274a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 18:44:51 -0700 Subject: [PATCH 079/108] properly reset the brake state when we resume a climb --- src/main/java/frc/subsystem/Climber.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index 6f6954b0..574ff287 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -83,6 +83,7 @@ public static Climber getInstance() { private boolean isPaused = false; private double pausedClimberSetpoint; + private @NotNull BrakeState pausedBrakeState = BrakeState.BRAKING; private ControlMode pausedClimberMode; private boolean sensorClimb = true; @@ -616,6 +617,7 @@ public synchronized void pauseClimb() { } else { pausedClimberSetpoint = climberMotor.getClosedLoopTarget(); } + pausedBrakeState = getBrakeState(); climberMotor.set(ControlMode.PercentOutput, 0); setBrakeState(BrakeState.BRAKING); } @@ -634,7 +636,7 @@ private synchronized void stopClimberMotor() { */ public synchronized void resumeClimb() { isPaused = false; - setBrakeState(BrakeState.FREE); + setBrakeState(pausedBrakeState); climberMotor.set(pausedClimberMode, pausedClimberSetpoint); otherPivotingArmMustContactByTime = Double.MAX_VALUE; } From a296dbb88a97c235977bbc0917be22dd68a95fe9 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 18:55:39 -0700 Subject: [PATCH 080/108] increase wait after grabbing bar --- src/main/java/frc/subsystem/Climber.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index 574ff287..792e6ff8 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -415,14 +415,14 @@ public enum ClimbState { WAIT_FOR_BRAKE_TIME( new ClimbStep( (cl) -> cl.data = Timer.getFPGATimestamp(), - (cl) -> Timer.getFPGATimestamp() - cl.data > 0.1, + (cl) -> Timer.getFPGATimestamp() - cl.data > 0.25, (cl) -> cl.climberMotor.set(ControlMode.PercentOutput, 0), false ), new ClimbStep( (cl) -> cl.data = Timer.getFPGATimestamp(), - (cl) -> Timer.getFPGATimestamp() - cl.data > 0.1, + (cl) -> Timer.getFPGATimestamp() - cl.data > 0.25, (cl) -> cl.climberMotor.set(ControlMode.PercentOutput, 0), false ) From f1464c72212f76d570d82687890b7c3b1253e15f Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 19:01:40 -0700 Subject: [PATCH 081/108] lazy init vision manager fix --- .../java/frc/subsystem/VisionManager.java | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index de00d402..b27525fd 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -26,15 +26,18 @@ import org.jetbrains.annotations.NotNull; import java.util.HashSet; +import java.util.Objects; import java.util.Optional; import java.util.Set; +import java.util.concurrent.locks.ReentrantReadWriteLock; import static frc.robot.Constants.GOAL_POSITION; import static frc.robot.Constants.MAX_SHOOT_SPEED; import static frc.utility.geometry.GeometryUtils.angleOf; public final class VisionManager extends AbstractSubsystem { - private static final @NotNull VisionManager instance = new VisionManager(); + private static final ReentrantReadWriteLock VISION_MANGER_INSTANCE_LOCK = new ReentrantReadWriteLock(); + private static VisionManager instance; private final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); private final @NotNull Limelight limelight = Limelight.getInstance(); @@ -53,7 +56,21 @@ private VisionManager() { } public static @NotNull VisionManager getInstance() { - return instance; + VISION_MANGER_INSTANCE_LOCK.readLock().lock(); + try { + if (instance != null) { + return instance; + } + } finally { + VISION_MANGER_INSTANCE_LOCK.readLock().unlock(); + } + + VISION_MANGER_INSTANCE_LOCK.writeLock().lock(); + try { + return Objects.requireNonNullElseGet(instance, () -> instance = new VisionManager()); + } finally { + VISION_MANGER_INSTANCE_LOCK.writeLock().unlock(); + } } @Override From 342818985752cbef4d9201e715eb2b9d51eb4144 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 19:02:07 -0700 Subject: [PATCH 082/108] remove field access for vision manager --- src/main/java/frc/robot/Robot.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cdd2b864..2977f027 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -184,7 +184,6 @@ public class Robot extends TimedRobot { private final Intake intake = Intake.getInstance(); private final Shooter shooter = Shooter.getInstance(); private final Climber climber = Climber.getInstance(); - private final VisionManager visionManager = VisionManager.getInstance(); //Inputs private final static Controller xbox = new Controller(0); @@ -236,7 +235,7 @@ public static void setRumble(RumbleType type, double value) { ShooterConfig shooterConfig = (ShooterConfig) Serializer.deserialize(shooterConfigEntry.getString(null), ShooterConfig.class); Collections.sort(shooterConfig.getShooterConfigs()); - visionManager.setShooterConfig(shooterConfig); + VisionManager.getInstance().setShooterConfig(shooterConfig); System.out.println(shooterConfig.getShooterConfigs()); } catch (IOException e) { //Should never happen. The gui should never upload invalid data. @@ -352,6 +351,7 @@ public void incrementLoadedAutos() { */ @Override public void robotPeriodic() { + VisionManager visionManager = VisionManager.getInstance(); SmartDashboard.putNumber("Match Timestamp", DriverStation.getMatchTime()); if (!DriverStation.isEnabled()) { xbox.update(); @@ -521,6 +521,7 @@ public void teleopInit() { */ @Override public void teleopPeriodic() { + VisionManager visionManager = VisionManager.getInstance(); xbox.update(); stick.update(); buttonPanel.update(); @@ -670,6 +671,7 @@ public void teleopPeriodic() { } private void runShooter() { + VisionManager visionManager = VisionManager.getInstance(); if (buttonPanel.getRisingEdge(1)) { shooterPreset = visionManager.visionLookUpTable.getShooterPreset(299); } else if (buttonPanel.getRisingEdge(2)) { @@ -833,7 +835,7 @@ private void startSubsystems() { hopper.start(); shooter.start(); climber.start(); - visionManager.start(); + VisionManager.getInstance().start(); DashboardHandler.getInstance().start(); } From efe49ff67fd292ddbd5813cb3012b91d34f14268 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 19:06:09 -0700 Subject: [PATCH 083/108] update shooter config last practice match --- src/main/deploy/shooter/shooterconfig.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index c1cfd820..f44061ab 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -29,23 +29,23 @@ "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 2050.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2200.0, + "flywheelSpeed" : 2250.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2200.0, + "flywheelSpeed" : 2350.0, "distance" : 160.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2300.0, + "flywheelSpeed" : 2400.0, "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2300.0, + "flywheelSpeed" : 2400.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, From aa8d03f54644ed2a9e3e3332080c31518fb53363 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 19:20:25 -0700 Subject: [PATCH 084/108] increase hopper speed --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b25dbef9..416c6ea4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -219,7 +219,7 @@ public final class Constants { //Hopper Constants public static final int HOPPER_PERIOD = 10; - public static final double HOPPER_SPEED = .5; + public static final double HOPPER_SPEED = 1.0; public static final int HOPPER_MOTOR_ID = 30; public static final int HOPPER_CURRENT_LIMIT = 35; public static final String INTAKE_LIMELIGHT_NAME = "limelight-intake"; From 9d92ce3e5426def8ccc447ff811f2cd4b8bd8f81 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 1 Apr 2022 19:20:39 -0700 Subject: [PATCH 085/108] update intake limelight pipelines --- limelight/Intake blue.vpr | 6 +++--- limelight/Intake red.vpr | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/limelight/Intake blue.vpr b/limelight/Intake blue.vpr index 4eb37ce1..f595568b 100644 --- a/limelight/Intake blue.vpr +++ b/limelight/Intake blue.vpr @@ -29,7 +29,7 @@ dual_close_sort_origin:0 erosion_steps:1 exposure:1200 force_convex:1 -hue_max:120 +hue_max:129 hue_min:99 image_flip:1 image_source:0 @@ -43,11 +43,11 @@ pipeline_led_enabled:0 pipeline_led_power:100 pipeline_res:0 pipeline_type:0 -red_balance:1260 +red_balance:1988 roi_x:0.000000 roi_y:0.000000 sat_max:255 -sat_min:127 +sat_min:107 send_corners:0 send_raw_contours:0 solve3d:0 diff --git a/limelight/Intake red.vpr b/limelight/Intake red.vpr index 79361b4a..f5540c46 100644 --- a/limelight/Intake red.vpr +++ b/limelight/Intake red.vpr @@ -29,13 +29,13 @@ dual_close_sort_origin:0 erosion_steps:0 exposure:1200 force_convex:1 -hue_max:154 -hue_min:4 +hue_max:175 +hue_min:154 image_flip:1 image_source:0 img_to_show:0 intersection_filter:0 -invert_hue:1 +invert_hue:0 multigroup_max:7 multigroup_min:1 multigroup_rejector:0 @@ -43,11 +43,11 @@ pipeline_led_enabled:0 pipeline_led_power:100 pipeline_res:0 pipeline_type:0 -red_balance:1260 +red_balance:1646 roi_x:0.000000 roi_y:0.000000 sat_max:255 -sat_min:229 +sat_min:202 send_corners:0 send_raw_contours:0 solve3d:0 From c4a68f62e00665b7fc7fadaf85a1fba75d6e0437 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 09:22:36 -0700 Subject: [PATCH 086/108] Display the distance of shots in inches --- src/main/java/frc/subsystem/VisionManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index b27525fd..360e516d 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -198,7 +198,7 @@ && getAccel().getNorm() < 1 checksPassedLastTime = true; System.out.println( "Shooting at " + (150 - DriverStation.getMatchTime()) + " " - + aimToPosition.getNorm()); + + Units.metersToInches(aimToPosition.getNorm())); } } else { lastChecksFailedTime = Timer.getFPGATimestamp(); From fea840600d929d3f94483bc0f21c33eac5ac267a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 09:22:59 -0700 Subject: [PATCH 087/108] update shooter quals 1 --- src/main/deploy/shooter/shooterconfig.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index f44061ab..3839e7c2 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -33,7 +33,7 @@ "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2250.0, + "flywheelSpeed" : 2200.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, @@ -53,11 +53,11 @@ "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2500.0, + "flywheelSpeed" : 2550.0, "distance" : 202.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2500.0, + "flywheelSpeed" : 2600.0, "distance" : 224.0 }, { "hoodEjectAngle" : 51.0, From 96794cd578a6b8c3f67f1d52445e64fedebce717 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 10:38:33 -0700 Subject: [PATCH 088/108] update shooter config qual 8 --- src/main/deploy/shooter/shooterconfig.json | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 3839e7c2..19402075 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -21,15 +21,15 @@ "distance" : 109.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1900.0, + "flywheelSpeed" : 1950.0, "distance" : 113.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1950.0, + "flywheelSpeed" : 2000.0, "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2050.0, + "flywheelSpeed" : 2100.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, @@ -45,19 +45,19 @@ "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2400.0, + "flywheelSpeed" : 2450.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2400.0, + "flywheelSpeed" : 2525.0, "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2550.0, + "flywheelSpeed" : 2600.0, "distance" : 202.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2600.0, + "flywheelSpeed" : 2650.0, "distance" : 224.0 }, { "hoodEjectAngle" : 51.0, @@ -65,7 +65,7 @@ "distance" : 241.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2700.0, + "flywheelSpeed" : 2750.0, "distance" : 265.0 }, { "hoodEjectAngle" : 51.0, From 55d40961b73f6159881282a753ff50a1d5bcfdd4 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 12:01:00 -0700 Subject: [PATCH 089/108] unpause the climb whenever we force advance & always start the climb on the first iteration. --- src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/subsystem/Climber.java | 8 ++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2977f027..958c78b3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -626,7 +626,7 @@ public void teleopPeriodic() { climber.setClimberMotor(0); } - if (buttonPanel.getRisingEdge(12)) { + if (buttonPanel.getRisingEdge(12) && buttonPanel.getRawButton(9)) { climber.forceAdvanceStep(); } @@ -645,6 +645,7 @@ public void teleopPeriodic() { if (buttonPanel.getRisingEdge(11)) { climber.stopClimb(); + climber.stopClimb(); } diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index 792e6ff8..15b20183 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -91,6 +91,7 @@ public static Climber getInstance() { private boolean advanceStep = false; private boolean skipChecks = false; private boolean ranEndAction = false; + private boolean startingClimb = true; public enum ClawState { LATCHED, UNLATCHED @@ -159,15 +160,16 @@ public enum ClimbState { new ClimbStep( (cl) -> {}, (cl) -> { + if (cl.startingClimb) return true; AHRS gyro = RobotTracker.getInstance().getGyro(); - if (Math.abs(gyro.getRoll()) < 15 && Math.abs(RobotTracker.getInstance().getGyroRollVelocity()) < 2) { + if (Math.abs(gyro.getRoll()) < 15 && Math.abs(RobotTracker.getInstance().getGyroRollVelocity()) < 5) { return true; } else { return gyro.getRoll() > 20 && gyro.getRoll() < 40 && RobotTracker.getInstance().getGyroRollVelocity() > 0; } }, - (cl) -> {}, + (cl) -> cl.startingClimb = false, false ) ), @@ -649,6 +651,7 @@ public synchronized void deployClimb() { setBrakeState(BrakeState.FREE); setClawState(ClawState.UNLATCHED); setPivotState(PivotState.INLINE); + startingClimb = true; } /** @@ -668,6 +671,7 @@ public synchronized void advanceStep() { * This method will also work regardless of whether the robot is in step-by-mode or not. */ public synchronized void forceAdvanceStep() { + if (isPaused) resumeClimb(); advanceStep = true; skipChecks = true; } From 0ddf5dc49ebecc77e22674f5511ddef5e961d36b Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 12:14:00 -0700 Subject: [PATCH 090/108] more climber changes to ensure that it always works --- src/main/java/frc/subsystem/Climber.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index 15b20183..c57f2a32 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -647,6 +647,7 @@ public synchronized void resumeClimb() { * Sets the climber in the correct state to initiate a climb and moves the elevator arm to the up above the high bar. */ public synchronized void deployClimb() { + timesRun = 0; climberMotor.set(ControlMode.MotionMagic, Constants.CLIMBER_DEPLOY_HEIGHT); setBrakeState(BrakeState.FREE); setClawState(ClawState.UNLATCHED); @@ -710,13 +711,13 @@ public synchronized void update() { if (!isPaused) { - if (climberMotor.getSelectedSensorPosition() < Constants.MIN_CLIMBER_ELEVATOR_HEIGHT - && climberMotor.getSelectedSensorVelocity() < 0) { - stopClimb(); - } else if (climberMotor.getSelectedSensorPosition() > Constants.MAX_CLIMBER_ELEVATOR_HEIGHT - && climberMotor.getSelectedSensorVelocity() > 0) { - stopClimb(); - } +// if (climberMotor.getSelectedSensorPosition() < Constants.MIN_CLIMBER_ELEVATOR_HEIGHT +// && climberMotor.getSelectedSensorVelocity() < 0) { +// stopClimb(); +// } else if (climberMotor.getSelectedSensorPosition() > Constants.MAX_CLIMBER_ELEVATOR_HEIGHT +// && climberMotor.getSelectedSensorVelocity() > 0) { +// stopClimb(); +// } if (currentClimbStep.waitCondition.apply(this)) { currentClimbStep.endAction.accept(this); From afd8740cdcae89a5dc4d45fee36e359103e98943 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 12:26:07 -0700 Subject: [PATCH 091/108] reset the pause sstate when starting the climb --- src/main/java/frc/subsystem/Climber.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java index c57f2a32..8ce23390 100644 --- a/src/main/java/frc/subsystem/Climber.java +++ b/src/main/java/frc/subsystem/Climber.java @@ -590,6 +590,8 @@ private Climber() { * Starts the automated climb sequence and deactivates the brake. */ public synchronized void startClimb() { + isPaused = false; + otherPivotingArmMustContactByTime = Double.MAX_VALUE; climbState = ClimbState.START_CLIMB; setBrakeState(BrakeState.FREE); } @@ -638,9 +640,9 @@ private synchronized void stopClimberMotor() { */ public synchronized void resumeClimb() { isPaused = false; + otherPivotingArmMustContactByTime = Double.MAX_VALUE; setBrakeState(pausedBrakeState); climberMotor.set(pausedClimberMode, pausedClimberSetpoint); - otherPivotingArmMustContactByTime = Double.MAX_VALUE; } /** From 9b5fdba35a6d24bb28eb418db716fd5f02f0a621 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 14:42:32 -0700 Subject: [PATCH 092/108] increase strictness when culling targets --- src/main/deploy/shooter/shooterconfig.json | 2 +- src/main/java/frc/robot/Constants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 19402075..e1eb7984 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -1,7 +1,7 @@ { "shooterConfigs" : [ { "hoodEjectAngle" : 70.0, - "flywheelSpeed" : 1800.0, + "flywheelSpeed" : 1950.0, "distance" : 45.0 }, { "hoodEjectAngle" : 62.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 416c6ea4..2d2859e7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,7 +42,7 @@ public final class Constants { * Relative position of the limelight from the center of the robot. */ public static final Translation2d LIMELIGHT_CENTER_OFFSET = new Translation2d(-0.684, 0); //TODO: CHANGE - public static final double VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED = Math.pow(4, 2); //TODO: CHANGE + public static final double VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED = Math.pow(2, 2); //TODO: CHANGE // This is in inches public static final double VISION_DISTANCE_BEFORE_ERROR_TIGHTENING = 200; From d772c27e0579ecc5fe083d0105d859ac04202ed8 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 14:43:00 -0700 Subject: [PATCH 093/108] adjust autos to make them get the second ball a bit lower --- src/main/deploy/autos/blue/5ball.json | 1102 +++++++++-------- src/main/deploy/autos/red/5ball.json | 1054 ++++++++-------- .../java/frc/subsystem/VisionManager.java | 2 +- 3 files changed, 1128 insertions(+), 1030 deletions(-) diff --git a/src/main/deploy/autos/blue/5ball.json b/src/main/deploy/autos/blue/5ball.json index c0e07aab..c9be3e67 100644 --- a/src/main/deploy/autos/blue/5ball.json +++ b/src/main/deploy/autos/blue/5ball.json @@ -212,7 +212,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 6", + "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 5", "closed" : false, "valid" : true, "sendableScript" : { @@ -220,7 +220,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "6" ], + "args" : [ "5" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -250,7 +250,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.0, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { "x" : 7.626592636108398, @@ -262,433 +262,433 @@ }, "curvature" : 0.0 }, { - "time" : 0.18074788653672832, - "velocity" : 0.7229915461469133, - "acceleration" : 4.000000000000002, + "time" : 0.18070546442899316, + "velocity" : 0.7228218577159725, + "acceleration" : 4.000000000000001, "pose" : { "translation" : { - "x" : 7.5612559258704835, - "y" : -3.197459243636949 + "x" : 7.561287415918969, + "y" : -3.1975411300314462 }, "rotation" : { - "radians" : -3.138740759458168 + "radians" : -3.1350447706508953 } }, - "curvature" : -0.29172803877514997 + "curvature" : -0.18371794738756367 }, { - "time" : 0.25626538315297653, - "velocity" : 1.0250615326119064, - "acceleration" : 3.999999999999997, + "time" : 0.2560291531859158, + "velocity" : 1.0241166127436632, + "acceleration" : 4.000000000000002, "pose" : { "translation" : { - "x" : 7.495254533242587, - "y" : -3.196840137823301 + "x" : 7.495494536036858, + "y" : -3.197464238592829 }, "rotation" : { - "radians" : 3.1176073408097276 + "radians" : 3.1311609020207034 } }, - "curvature" : -0.5074451761358538 + "curvature" : -0.3247341642611955 }, { - "time" : 0.3151289706110068, - "velocity" : 1.2605158824440272, - "acceleration" : 4.000000000000004, + "time" : 0.31447963957810243, + "velocity" : 1.2579185583124097, + "acceleration" : 3.999999999999992, "pose" : { "translation" : { - "x" : 7.428047492778063, - "y" : -3.1939619357255324 + "x" : 7.428818325054408, + "y" : -3.195966399958145 }, "rotation" : { - "radians" : 3.0784656278152482 + "radians" : 3.106029813407485 } }, - "curvature" : -0.6425731706470493 + "curvature" : -0.4213902848253676 }, { - "time" : 0.3658526486292684, - "velocity" : 1.463410594517074, - "acceleration" : 3.999999999999997, + "time" : 0.3645154779554353, + "velocity" : 1.4580619118217408, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.3592204221349675, - "y" : -3.1880118377084727 + "x" : 7.360957192140631, + "y" : -3.192528116363974 }, "rotation" : { - "radians" : 3.0315419813222593 + "radians" : 3.0752789658716972 } }, - "curvature" : -0.7042822762242509 + "curvature" : -0.47718431116456617 }, { - "time" : 0.4117606980978159, - "velocity" : 1.6470427923912636, - "acceleration" : 2.1723907907211393, + "time" : 0.409416754303078, + "velocity" : 1.6376670172123116, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.288476303159229, - "y" : -3.178375355932097 + "x" : 7.2916967297335304, + "y" : -3.1867497196945394 }, "rotation" : { - "radians" : 2.980846049930411 + "radians" : 3.041183989047963 } }, - "curvature" : -0.7073398126437707 + "curvature" : -0.498764188481344 }, { - "time" : 0.4555105966830825, - "velocity" : 1.7420846691728806, - "acceleration" : 1.9989588140063956, + "time" : 0.45085224980311017, + "velocity" : 1.8034089992124402, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.215626262967817, - "y" : -3.1646216243218532 + "x" : 7.22090281623241, + "y" : -3.178342718540307 }, "rotation" : { - "radians" : 2.9296129513061087 + "radians" : 3.005659202130603 } }, - "curvature" : -0.6695533478285349 + "curvature" : -0.49412155680179287 }, { - "time" : 0.4987555436671162, - "velocity" : 1.828529537107854, - "acceleration" : 2.813152489265579, + "time" : 0.48978986213967873, + "velocity" : 1.9591594485587145, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.140580355031915, - "y" : -3.146488708538989 + "x" : 7.148514718690194, + "y" : -3.16712114525658 }, "rotation" : { - "radians" : 2.8802105895971133 + "radians" : 2.970217325515413 } }, - "curvature" : -0.6077426894646522 + "curvature" : -0.4711532022444734 }, { - "time" : 0.5413750298351414, - "velocity" : 1.948424650712654, - "acceleration" : 3.6356664874107865, + "time" : 0.5268309899893706, + "velocity" : 2.107323959957482, + "acceleration" : 3.9999999999999885, "pose" : { "translation" : { - "x" : 7.063338340260088, - "y" : -3.1238689159508795 + "x" : 7.074538195505738, + "y" : -3.1529929030220956 }, "rotation" : { - "radians" : 2.834193414995866 + "radians" : 2.9359808911961505 } }, - "curvature" : -0.5352497494731134 + "curvature" : -0.4367169763361537 }, { - "time" : 0.5828078541546968, - "velocity" : 2.0990605815700403, - "acceleration" : 4.000000000000005, + "time" : 0.5623614610675747, + "velocity" : 2.249445844270298, + "acceleration" : 3.999999999999989, "pose" : { "translation" : { - "x" : 6.983980468081455, - "y" : -3.0967941056013544 + "x" : 6.9990385991161475, + "y" : -3.1359511128976223 }, "rotation" : { - "radians" : 2.7924479125830106 + "radians" : 2.903728300477056 } }, - "curvature" : -0.46118349636323475 + "curvature" : -0.39618861692344265 }, { - "time" : 0.6228085797994793, - "velocity" : 2.259063484149171, - "acceleration" : 3.999999999999995, + "time" : 0.596630045456207, + "velocity" : 2.3865201818248267, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.902658257528856, - "y" : -3.0654209981810254 + "x" : 6.922133978689089, + "y" : -3.116065460884556 }, "rotation" : { - "radians" : 2.755369864098415 + "radians" : 2.873957256024724 } }, - "curvature" : -0.3908757756828451 + "curvature" : -0.35341215031234335 }, { - "time" : 0.6614595623941697, - "velocity" : 2.413667414527932, - "acceleration" : 3.999999999999995, + "time" : 0.6297939149996428, + "velocity" : 2.5191756599985697, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 6.819585278322023, - "y" : -3.0300164859976135 + "x" : 6.84398818281511, + "y" : -3.0934735449835173 }, "rotation" : { - "radians" : 2.7230300588082508 + "radians" : 2.846951576467614 } }, - "curvature" : -0.32684828411643785 + "curvature" : -0.31088845012056077 }, { - "time" : 0.6988907922441839, - "velocity" : 2.563392333927989, - "acceleration" : 3.9999999999999907, + "time" : 0.6619472398665499, + "velocity" : 2.6477889594661983, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.7350279319507536, - "y" : -2.990942942946276 + "x" : 6.764803962199949, + "y" : -3.068372222252947 }, "rotation" : { - "radians" : 2.6953074205444363 + "radians" : 2.822842297620444 } }, - "curvature" : -0.26979273017272876 + "curvature" : -0.27006185244235026 }, { - "time" : 0.735158469971846, - "velocity" : 2.708463044838637, - "acceleration" : 1.279466838075468, + "time" : 0.6931403571893224, + "velocity" : 2.7725614287572884, + "acceleration" : -1.9238540319304083, "pose" : { "translation" : { - "x" : 6.649296232758076, - "y" : -2.9486435344799347 + "x" : 6.684816072356853, + "y" : -3.041008955867703 }, "rotation" : { - "radians" : 2.671985460234166 + "radians" : 2.8016585928334106 } }, - "curvature" : -0.2193389226412271 + "curvature" : -0.23161048905860385 }, { - "time" : 0.7708801601749081, - "velocity" : 2.75416776285346, - "acceleration" : -4.0, + "time" : 0.724392316024546, + "velocity" : 2.7124372217464203, + "acceleration" : -3.9999999999999947, "pose" : { "translation" : { - "x" : 6.562734589023421, - "y" : -2.9036275275796015 + "x" : 6.604284376298892, + "y" : -3.011673162177658 }, "rotation" : { - "radians" : 2.65281690834138 + "radians" : 2.7833674081938065 } }, - "curvature" : -0.17457123350751053 + "curvature" : -0.1956924881544235 }, { - "time" : 0.8078106139873205, - "velocity" : 2.606445947603811, + "time" : 0.7570834198223945, + "velocity" : 2.5816728065550265, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.4757125840457945, - "y" : -2.8564556007247077 + "x" : 6.523486947231277, + "y" : -2.980687557766295 }, "rotation" : { - "radians" : 2.6375642644084403 + "radians" : 2.7679026528198674 } }, - "curvature" : -0.13433724003134345 + "curvature" : -0.1621334305896103 }, { - "time" : 0.8472976500866943, - "velocity" : 2.4484978032063154, - "acceleration" : -4.0, + "time" : 0.7917065419550845, + "velocity" : 2.4431803180242664, + "acceleration" : -3.999999999999995, "pose" : { "translation" : { - "x" : 6.388615757226944, - "y" : -2.80772515386343 + "x" : 6.442713171243668, + "y" : -2.948399506509304 }, "rotation" : { - "radians" : 2.62602361109316 + "radians" : 2.7551856068980616 } }, - "curvature" : -0.09741150032488063 + "curvature" : -0.13055898031005425 }, { - "time" : 0.8895957780818, - "velocity" : 2.2793052912258926, - "acceleration" : -4.000000000000004, + "time" : 0.8284398713448611, + "velocity" : 2.29624700046516, + "acceleration" : -3.9999999999999947, "pose" : { "translation" : { - "x" : 6.30183638515453, - "y" : -2.7580556183830183 + "x" : 6.362256850002495, + "y" : -2.915172366633179 }, "rotation" : { - "radians" : 2.618037394000371 + "radians" : 2.745138312714669 } }, - "curvature" : -0.06256913374757356 + "curvature" : -0.10048447677557701 }, { - "time" : 0.9350786097662884, - "velocity" : 2.097373964487939, - "acceleration" : -3.999999999999998, + "time" : 0.867530257770865, + "velocity" : 2.1398854547611443, + "acceleration" : -4.000000000000008, "pose" : { "translation" : { - "x" : 6.215764262685298, - "y" : -2.7080737670801227 + "x" : 6.2824093034432735, + "y" : -2.8813768377738143 }, "rotation" : { - "radians" : 2.6135000159251436 + "radians" : 2.7376914272087047 } }, - "curvature" : -0.02860941649074867 + "curvature" : -0.07137524263218635 }, { - "time" : 0.9843259494632479, - "velocity" : 1.900384605700101, + "time" : 0.9093359695581078, + "velocity" : 1.9726626076121727, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.130777484028243, - "y" : -2.6583990241311213 + "x" : 6.203452472462914, + "y" : -2.847382308035101 }, "rotation" : { - "radians" : 2.612358417756541 + "radians" : 2.7327875500302903 } }, - "curvature" : 0.005641950266030671 + "curvature" : -0.04269098250129263 }, { - "time" : 1.038295625522969, - "velocity" : 1.6845059014612167, + "time" : 0.9544024614642719, + "velocity" : 1.7923966399875164, "acceleration" : -4.000000000000003, "pose" : { "translation" : { - "x" : 6.047233223827789, - "y" : -2.609628775062447 + "x" : 6.125652021612041, + "y" : -2.8135482010475243 }, "rotation" : { - "radians" : 2.6146083461531995 + "radians" : 2.7303805081764745 } }, - "curvature" : 0.04132896947061391 + "curvature" : -0.013927070967790611 }, { - "time" : 1.0987120643010575, - "velocity" : 1.4428401463488625, - "acceleration" : -4.000000000000001, + "time" : 1.0036098078372346, + "velocity" : 1.5955672544956654, + "acceleration" : -3.9999999999999973, "pose" : { "translation" : { - "x" : 5.9654585182469475, - "y" : -2.562323676720915 + "x" : 6.049250441787308, + "y" : -2.7802153230267592 }, "rotation" : { - "radians" : 2.620285612891302 + "radians" : 2.730430520921143 } }, - "curvature" : 0.07953607724401594 + "curvature" : 0.015334192163236617 }, { - "time" : 1.1691474210025192, - "velocity" : 1.1610987195430156, - "acceleration" : -4.000000000000001, + "time" : 1.058499003222123, + "velocity" : 1.3760104729561113, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.885741046050498, - "y" : -2.5169929672440503 + "x" : 5.974460152923712, + "y" : -2.7476972098322676 }, "rotation" : { - "radians" : 2.6294501603955758 + "radians" : 2.732894603484722 } }, - "curvature" : 0.12120182277895979 + "curvature" : 0.04534789601466625 }, { - "time" : 1.259422100888273, - "velocity" : 0.8, - "acceleration" : 0.0, + "time" : 1.1221487632123464, + "velocity" : 1.1214114329952183, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 5.8083199096881515, - "y" : -2.4740797760304147 + "x" : 5.9014566066869065, + "y" : -2.716271474025895 }, "rotation" : { - "radians" : 2.642159027386621 + "radians" : 2.7377110228114296 } }, - "curvature" : 0.1669290910240929 + "curvature" : 0.07609008232293428 }, { - "time" : 1.3656883669621598, + "time" : 1.2025016214611508, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.733376416377723, - "y" : -2.4339464337099344 + "x" : 5.830371389165521, + "y" : -2.6861711519304663 }, "rotation" : { - "radians" : 2.6584223838932473 + "radians" : 2.7447761809366122 } }, - "curvature" : 0.2166266434826088 + "curvature" : 0.10708399032696965 }, { - "time" : 1.4673170381313625, + "time" : 1.2959642165478331, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.661024859188302, - "y" : -2.396859782114227 + "x" : 5.76128532356347, + "y" : -2.657576050688384 }, "rotation" : { - "radians" : 2.6781351093523047 + "radians" : 2.7539121802444275 } }, - "curvature" : 0.2689053696942707 + "curvature" : 0.13715838990065068 }, { - "time" : 1.5642156155177847, + "time" : 1.3863197039855641, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.591303298123421, - "y" : -2.3629764842469285 + "x" : 5.694221572892275, + "y" : -2.6306040953202228 }, "rotation" : { - "radians" : 2.700976194613289 + "radians" : 2.764823929123116 } }, - "curvature" : 0.3201793855552225 + "curvature" : 0.1641479856882942 }, { - "time" : 1.6564699229560516, + "time" : 1.473604587960989, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.5241643412042265, - "y" : -2.332328334254022 + "x" : 5.629138742663372, + "y" : -2.605302675783328 }, "rotation" : { - "radians" : 2.726272217468977 + "radians" : 2.777046635977622 } }, - "curvature" : 0.36354779991332803 + "curvature" : 0.1845929194913137 }, { - "time" : 1.7443554806111186, + "time" : 1.5579775349142877, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.459465925552649, - "y" : -2.304807567394164 + "x" : 5.565923983580433, + "y" : -2.581639994030411 }, "rotation" : { - "radians" : 2.7528334527109286 + "radians" : 2.789888744275278 } }, - "curvature" : 0.38783779721239137 + "curvature" : 0.1935775556708476 }, { - "time" : 1.8283441053954617, + "time" : 1.6397283958798081, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.3969620984745745, - "y" : -2.2801521700090106 + "x" : 5.504386094231677, + "y" : -2.559496411068146 }, "rotation" : { - "radians" : 2.7787959191919596 + "radians" : 2.8023823664689296 } }, - "curvature" : 0.3776909643218038 + "curvature" : 0.18495835581671727 }, { - "time" : 1.9091062425604757, + "time" : 1.7192862234251227, "velocity" : 0.8, - "acceleration" : -2.5718750180081185, + "acceleration" : -2.5807096371230194, "pose" : { "translation" : { - "x" : 5.336293798543011, - "y" : -2.257931189493547 + "x" : 5.444248623782187, + "y" : -2.5386557940157672 }, "rotation" : { - "radians" : 2.8015387216773218 + "radians" : 2.8132624602013823 } }, - "curvature" : 0.31603589357887024 + "curvature" : 0.1523080406534184 }, { - "time" : 2.22016334852342, + "time" : 2.029278478459223, "velocity" : 0.0, - "acceleration" : -2.5718750180081185, + "acceleration" : -2.5807096371230194, "pose" : { "translation" : { - "x" : 5.218406677246094, - "y" : -2.2181358337402344 + "x" : 5.326601505279541, + "y" : -2.4994845390319824 }, "rotation" : { "radians" : 2.823947779781842 @@ -700,8 +700,8 @@ "x" : [ 7.626592636108398, -2.086981773376465, 0.0 ], "y" : [ -3.196845054626465, -0.026646852493286133, 0.0 ] }, { - "x" : [ 5.218406677246094, -1.8701248168945312, 0.0 ], - "y" : [ -2.2181358337402344, 0.6148552894592285, 0.0 ] + "x" : [ 5.326601505279541, -1.8701248168945312, 0.0 ], + "y" : [ -2.4994845390319824, 0.6148552894592285, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -709,7 +709,7 @@ "radians" : 2.9608083881863267 } }, { - "time" : 2.109999952837825, + "time" : 1.9199999570846558, "rotation" : { "radians" : -3.1066860685499065 } @@ -736,7 +736,7 @@ } ] }, { "type" : "script", - "script" : "VisionManager.shootBalls 3.5", + "script" : "VisionManager.shootBalls 4.5", "closed" : false, "valid" : true, "sendableScript" : { @@ -744,7 +744,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "3.5" ], + "args" : [ "4.5" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -777,8 +777,8 @@ "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.218406677246094, - "y" : -2.2181358337402344 + "x" : 5.326601505279541, + "y" : -2.4994845390319824 }, "rotation" : { "radians" : 2.9473819457269834 @@ -786,639 +786,709 @@ }, "curvature" : -0.0 }, { - "time" : 0.22577455195938007, - "velocity" : 0.9030982078375203, - "acceleration" : 4.0, + "time" : 0.22616825941529703, + "velocity" : 0.9046730376611881, + "acceleration" : 3.9999999999999982, "pose" : { "translation" : { - "x" : 5.118318357381213, - "y" : -2.198750699968514 + "x" : 5.226273182620389, + "y" : -2.479475304490734 }, "rotation" : { - "radians" : 2.9554310054193884 + "radians" : 2.940693415172918 } }, - "curvature" : 0.13027761823749012 + "curvature" : -0.08516764713670827 }, { - "time" : 0.3382371271442293, - "velocity" : 1.3529485085769173, - "acceleration" : 3.9999999999999973, + "time" : 0.28478553771106996, + "velocity" : 1.1391421508442798, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.993451572256163, - "y" : -2.176348690292798 + "x" : 5.167604320016238, + "y" : -2.467386164908845 }, "rotation" : { - "radians" : 2.972591803682675 + "radians" : 2.9363469447848343 } }, - "curvature" : 0.12784985955539324 + "curvature" : -0.057690329781864697 }, { - "time" : 0.3916828784133741, - "velocity" : 1.5667315136534963, - "acceleration" : 3.9999999999999973, + "time" : 0.34012838221283126, + "velocity" : 1.360513528851325, + "acceleration" : 3.9999999999999987, "pose" : { "translation" : { - "x" : 4.916478985341882, - "y" : -2.163593622500116 + "x" : 5.099909630283946, + "y" : -2.453181116929045 }, "rotation" : { - "radians" : 2.9819601021983635 + "radians" : 2.9335394561142465 } }, - "curvature" : 0.11259673461042294 + "curvature" : -0.024959128681885406 }, { - "time" : 0.4451005174091182, - "velocity" : 1.7804020696364724, + "time" : 0.39469640970120917, + "velocity" : 1.5787856388048367, "acceleration" : 3.999999999999998, "pose" : { "translation" : { - "x" : 4.82815027252218, - "y" : -2.1498081765639654 + "x" : 5.021453386801028, + "y" : -2.4365679640294218 }, "rotation" : { - "radians" : 2.9913800207169805 + "radians" : 2.9326521624329898 } }, - "curvature" : 0.09893287394574607 + "curvature" : 7.654668522091257E-4 }, { - "time" : 0.49879827939468, - "velocity" : 1.9951931175787196, - "acceleration" : 3.9999999999999982, + "time" : 0.4494207725602909, + "velocity" : 1.7976830902411636, + "acceleration" : 4.000000000000004, "pose" : { "translation" : { - "x" : 4.727849331012521, - "y" : -2.1351214277652844 + "x" : 4.931068547291034, + "y" : -2.4174357876372596 }, "rotation" : { - "radians" : 3.000843152646543 + "radians" : 2.933612583177434 } }, - "curvature" : 0.08854693535519663 + "curvature" : 0.018438020867265596 }, { - "time" : 0.5527581649942036, - "velocity" : 2.211032659976814, - "acceleration" : 3.9999999999999964, + "time" : 0.5045581168818597, + "velocity" : 2.0182324675274392, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 4.61541160941124, - "y" : -2.11974885687232 + "x" : 4.8281097953876895, + "y" : -2.3958376963394414 }, "rotation" : { - "radians" : 3.010451193020663 + "radians" : 2.9362264556688444 } }, - "curvature" : 0.08141460522576446 + "curvature" : 0.030240172077865078 }, { - "time" : 0.6067994538503118, - "velocity" : 2.4271978154012466, - "acceleration" : 4.0, + "time" : 0.5600430300070354, + "velocity" : 2.240172120028142, + "acceleration" : 3.999999999999993, "pose" : { "translation" : { - "x" : 4.491079470872819, - "y" : -2.1039811364393017 + "x" : 4.712406582199037, + "y" : -2.371973575092852 }, "rotation" : { - "radians" : 3.0203520346564443 + "radians" : 2.940317933492544 } }, - "curvature" : 0.0770754880726607 + "curvature" : 0.038432989629750024 }, { - "time" : 0.6337713815419612, - "velocity" : 2.5350855261678444, - "acceleration" : 3.999999999999981, + "time" : 0.587849840307584, + "velocity" : 2.3513993612303365, + "acceleration" : 3.9999999999999867, "pose" : { "translation" : { - "x" : 4.424629014592831, - "y" : -2.096057730950637 + "x" : 4.649840769147667, + "y" : -2.359290106139965 }, "rotation" : { - "radians" : 3.025464239893527 + "radians" : 2.9428792118280978 } }, - "curvature" : 0.07580950849707052 + "curvature" : 0.041716157528885855 }, { - "time" : 0.6606632003839968, - "velocity" : 2.6426528015359865, + "time" : 0.6156531468331837, + "velocity" : 2.462612587332735, + "acceleration" : 3.999999999999987, + "pose" : { + "translation" : { + "x" : 4.584216167871574, + "y" : -2.346172834434782 + }, + "rotation" : { + "radians" : 2.9457720291811604 + } + }, + "curvature" : 0.04467007920827488 + }, { + "time" : 0.643414376324417, + "velocity" : 2.5736575052976676, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.355457556281181, - "y" : -2.088172917105112 + "x" : 4.515624076932947, + "y" : -2.3326803444587343 }, "rotation" : { - "radians" : 3.030713652227927 + "radians" : 2.9489923575381187 } }, - "curvature" : 0.07508525249190778 + "curvature" : 0.04741776873328391 }, { - "time" : 0.687437732922965, - "velocity" : 2.749750931691859, + "time" : 0.671093384815894, + "velocity" : 2.6843735392635755, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.2836899587141986, - "y" : -2.0803791830604403 + "x" : 4.444176663154394, + "y" : -2.3188771596933293 }, "rotation" : { - "radians" : 3.036123218975336 + "radians" : 2.9525400109302637 } }, - "curvature" : 0.07486952079026583 + "curvature" : 0.05006038596145867 }, { - "time" : 0.7140574327285515, - "velocity" : 2.856229730914205, - "acceleration" : 4.0000000000000115, + "time" : 0.6986493044854305, + "velocity" : 2.7945972179417216, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.209468147422967, - "y" : -2.0727316138919605 + "x" : 4.370005494167825, + "y" : -2.304833203532975 }, "rotation" : { - "radians" : 3.0417167398697336 + "radians" : 2.956418464768416 } }, - "curvature" : 0.07513898148074699 + "curvature" : 0.05268264317257482 }, { - "time" : 0.7404848785000484, - "velocity" : 2.961939514000193, - "acceleration" : 1.4367050542224837, + "time" : 0.7260411741134952, + "velocity" : 2.9041646964539805, + "acceleration" : 3.5173178369045623, "pose" : { "translation" : { - "x" : 4.132949715792485, - "y" : -2.0652875411644693 + "x" : 4.293260070963328, + "y" : -2.2906232601978047 }, "rotation" : { - "radians" : 3.0475190269956305 + "radians" : 2.960634714311987 } }, - "curvature" : 0.07587984575547331 + "curvature" : 0.055357358915842876 }, { - "time" : 0.7669763882834565, + "time" : 0.7532878740109261, "velocity" : 3.0, - "acceleration" : -1.0982184022288134E-14, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 4.054306530160829, - "y" : -2.058106192504056 + "x" : 4.214106360438051, + "y" : -2.276326435646504 }, "rotation" : { - "radians" : 3.053556107860972 + "radians" : 2.965199184647632 } }, - "curvature" : 0.07708752042753088 + "curvature" : 0.05814923970169821 }, { - "time" : 0.7939345482116209, - "velocity" : 2.9999999999999996, - "acceleration" : 1.0755238227730279E-14, + "time" : 0.7808305395419614, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.9737233349183256, - "y" : -2.0512483411699343 + "x" : 4.1327253279450815, + "y" : -2.262025618489133 }, "rotation" : { - "radians" : 3.05985546264456 + "radians" : 2.970125697805022 } }, - "curvature" : 0.07876635846236638 + "curvature" : 0.061118028567780355 }, { - "time" : 0.8214615510510413, + "time" : 0.8090362176857757, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.891396357606709, - "y" : -2.044775955626278 + "x" : 4.049311469842324, + "y" : -2.247806940899955 }, "rotation" : { - "radians" : 3.0664462909649215 + "radians" : 2.975431499212657 } }, - "curvature" : 0.08092957341077114 + "curvature" : 0.06432117010498879 }, { - "time" : 0.8494883928631835, + "time" : 0.8378328544866487, "velocity" : 3.0, - "acceleration" : -1.0404470748216723E-14, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.8075319140182913, - "y" : -2.0387518491140533 + "x" : 3.9640713460413792, + "y" : -2.233759239530258 }, "rotation" : { - "radians" : 3.0733598063989405 + "radians" : 2.981137344223006 } }, - "curvature" : 0.08359935409570846 + "curvature" : 0.06781613205292471 }, { - "time" : 0.8779434176691419, - "velocity" : 2.9999999999999996, + "time" : 0.8671450353421751, + "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.7223450132951257, - "y" : -2.0332393292228517 + "x" : 3.877222112556427, + "y" : -2.219973516421182 }, "rotation" : { - "radians" : 3.080629558866709 + "radians" : 2.987267645068647 } }, - "curvature" : 0.08680720147794485 + "curvature" : 0.07166250783511754 }, { - "time" : 0.9067528176974184, - "velocity" : 2.9999999999999996, + "time" : 0.8968945284065684, + "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.636057963028172, - "y" : -2.028301847462725 + "x" : 3.7889900540531016, + "y" : -2.206542399916543 }, "rotation" : { - "radians" : 3.0882917861725874 + "radians" : 2.9938506788290518 } }, - "curvature" : 0.09059450379888723 + "curvature" : 0.07592400875189884 }, { - "time" : 0.9358411361147014, - "velocity" : 2.9999999999999996, - "acceleration" : 1.0107649040265085E-14, + "time" : 0.92700083074625, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.5488989743564616, - "y" : -2.024002648836017 + "x" : 3.699609116397373, + "y" : -2.1935596055756603 }, "rotation" : { - "radians" : 3.0963857966103348 + "radians" : 3.000918857463063 } }, - "curvature" : 0.09501336487557137 + "curvature" : 0.08067044151870519 }, { - "time" : 0.9651317725479596, + "time" : 0.9573817176378253, "velocity" : 3.0, - "acceleration" : -0.06809488522901795, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.461100767066263, - "y" : -2.0204044214092 + "x" : 3.6093194392044268, + "y" : -2.181119397086178 }, "rotation" : { - "radians" : 3.1049543846666157 + "radians" : 3.008509061471997 } }, - "curvature" : 0.10012770121562267 + "curvature" : 0.08597975704502848 }, { - "time" : 0.9945573185420075, - "velocity" : 2.997996270822734, - "acceleration" : -0.184591356598183, + "time" : 0.9879537954490873, + "velocity" : 3.0, + "acceleration" : -0.06708023937809733, "pose" : { "translation" : { - "x" : 3.3728991746902466, - "y" : -2.0175689458847046 + "x" : 3.5183658883875415, + "y" : -2.169316047176893 }, "rotation" : { - "radians" : 3.1140442814748748 + "radians" : 3.0166630391262212 } }, - "curvature" : 0.10601462452471887 + "curvature" : 0.0919402494188015 }, { - "time" : 1.0240672639708144, - "velocity" : 2.9925489899628923, - "acceleration" : -0.20849217138745274, + "time" : 1.0186435886693106, + "velocity" : 2.997941321324323, + "acceleration" : -0.22390997531894352, "pose" : { "translation" : { - "x" : 3.28453174960665, - "y" : -2.0155567451727556 + "x" : 3.42699658870697, + "y" : -2.1582432985305786 }, "rotation" : { - "radians" : 3.1237066406836274 + "radians" : 3.0254278732409468 } }, - "curvature" : 0.11276612545130336 + "curvature" : 0.09865297901559254 }, { - "time" : 1.0536051481517177, - "velocity" : 2.9863905723518247, - "acceleration" : -0.2602935739030425, + "time" : 1.0494023957488516, + "velocity" : 2.991054117590303, + "acceleration" : -0.24968095645801092, "pose" : { "translation" : { - "x" : 3.196236368138443, - "y" : -2.014426733963205 + "x" : 3.3354614563188187, + "y" : -2.147993824696811 }, "rotation" : { - "radians" : 3.13399755862567 + "radians" : 3.034856517018437 } }, - "curvature" : 0.12049107003628591 + "curvature" : 0.1062344889765406 }, { - "time" : 1.0831056442927933, - "velocity" : 2.9787117827793512, - "acceleration" : -0.3599966924344615, + "time" : 1.0801755500252042, + "velocity" : 2.9833706469973533, + "acceleration" : -0.2793532278338764, "pose" : { "translation" : { - "x" : 3.1082498356524937, - "y" : -2.014235868297366 + "x" : 3.2440107313239253, + "y" : -2.1386586910047924 }, "rotation" : { - "radians" : -3.1382066823601824 + "radians" : 3.045008399208256 } }, - "curvature" : 0.1293175095895187 + "curvature" : 0.11481987777652654 }, { - "time" : 1.112515242001862, - "velocity" : 2.9681244248782583, - "acceleration" : -0.41435957373242843, + "time" : 1.1108888389022582, + "velocity" : 2.974790790612154, + "acceleration" : -0.31347889545111546, "pose" : { "translation" : { - "x" : 3.020806491658732, - "y" : -2.0150387951398443 + "x" : 3.1528935103167397, + "y" : -2.1303268154761783 }, "rotation" : { - "radians" : -3.1264678126986505 + "radians" : 3.055950096387905 } }, - "curvature" : 0.13939528367796553 + "curvature" : 0.12456627894671345 }, { - "time" : 1.14178182090318, - "velocity" : 2.9559975377201018, - "acceleration" : -0.47782084790928264, + "time" : 1.1414701580581714, + "velocity" : 2.9652041924617203, + "acceleration" : -0.3526686678603589, "pose" : { "translation" : { - "x" : 2.9341368149093157, - "y" : -2.016887501950375 + "x" : 3.062356278934203, + "y" : -2.123084429737901 }, "rotation" : { - "radians" : -3.113896838891268 + "radians" : 3.0677560659844088 } }, - "curvature" : 0.150898858830179 + "curvature" : 0.13565677726404682 }, { - "time" : 1.1708492320949808, - "velocity" : 2.942108522657908, - "acceleration" : -0.5518862628311226, + "time" : 1.1718500837875183, + "velocity" : 2.954490144525055, + "acceleration" : -0.3975769717522214, "pose" : { "translation" : { - "x" : 2.8484660284977963, - "y" : -2.019830966255654 + "x" : 2.972641444404627, + "y" : -2.1170145399349947 }, "rotation" : { - "radians" : -3.1004122518647654 + "radians" : 3.080509426966478 } }, - "curvature" : 0.16403028405156617 + "curvature" : 0.14830475135070306 }, { - "time" : 1.199665689597395, - "velocity" : 2.9262051156188686, - "acceleration" : -0.6381658186444317, + "time" : 1.2019624393536652, + "velocity" : 2.94251816538674, + "acceleration" : -0.44887369505519986, "pose" : { "translation" : { - "x" : 2.764012704958283, - "y" : -2.023914805221171 + "x" : 2.8839858680965733, + "y" : -2.112196387643422 }, "rotation" : { - "radians" : -3.085924825413159 + "radians" : 3.0943027648494486 } }, - "curvature" : 0.179022046292927 + "curvature" : 0.16275856428249422 }, { - "time" : 1.2281844325226372, - "velocity" : 2.9080054286932713, - "acceleration" : -0.7382909496365782, + "time" : 1.231744849146722, + "velocity" : 2.9291496250552824, + "acceleration" : -0.5071956035960418, "pose" : { "translation" : { - "x" : 2.6809873713646084, - "y" : -2.029180925223045 + "x" : 2.7966193980677323, + "y" : -2.1087049107828983 }, "rotation" : { - "radians" : -3.0703372176489117 + "radians" : 3.109238922273074 } }, - "curvature" : 0.19613945786907577 + "curvature" : 0.1793064095821939 }, { - "time" : 1.256364374900713, - "velocity" : 2.8872004322742577, - "acceleration" : -2.163128503876278, + "time" : 1.2611392719425794, + "velocity" : 2.9142409030429803, + "acceleration" : -0.9399899449235506, "pose" : { "translation" : { - "x" : 2.5995911144294936, - "y" : -2.035667171419856 + "x" : 2.7107634016138036, + "y" : -2.1066102045297157 }, "rotation" : { - "radians" : -3.0535437643496612 + "radians" : 3.12543171391447 } }, - "curvature" : 0.215681980996833 + "curvature" : 0.19828093525498974 }, { - "time" : 1.2843497892040483, - "velocity" : 2.826664384901926, - "acceleration" : -3.9999999999999942, + "time" : 1.290145772258879, + "velocity" : 2.886975084408237, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.5200141856037135, - "y" : -2.04340697732448 + "x" : 2.6266292978173746, + "y" : -2.1059769822295697 }, "rotation" : { - "radians" : -3.035430580772711 + "radians" : -3.140178834940239 } }, - "curvature" : 0.2379825635975536 + "curvature" : 0.2200629785863679 }, { - "time" : 1.3125427756927424, - "velocity" : 2.7138924389471497, - "acceleration" : -4.000000000000006, + "time" : 1.3192095490289386, + "velocity" : 2.7707199773279982, + "acceleration" : -4.000000000000005, "pose" : { "translation" : { - "x" : 2.4424346061752615, - "y" : -2.052429014375921 + "x" : 2.5444170900968004, + "y" : -2.106864036310384 }, "rotation" : { - "radians" : -3.0158761347705023 + "radians" : -3.121085021922299 } }, - "curvature" : 0.2634035923974734 + "curvature" : 0.24508330253873933 }, { - "time" : 1.3411967497233397, - "velocity" : 2.5992765428247604, + "time" : 1.3487642900235868, + "velocity" : 2.6525010133494056, + "acceleration" : -4.000000000000005, + "pose" : { + "translation" : { + "x" : 2.4643138987550826, + "y" : -2.1093236991951345 + }, + "rotation" : { + "radians" : -3.1003235820288793 + } + }, + "curvature" : 0.27382057237627916 + }, { + "time" : 1.378824758299442, + "velocity" : 2.532259140245985, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.3670167723685154, - "y" : -2.0627568415111455 + "x" : 2.386492493528749, + "y" : -2.113401304214676 }, "rotation" : { - "radians" : -2.9947525137401847 + "radians" : -3.0777355237933675 } }, - "curvature" : 0.29232743281102963 + "curvature" : 0.3067928849363109 }, { - "time" : 1.3703306161997593, - "velocity" : 2.482741076919082, - "acceleration" : -4.000000000000006, + "time" : 1.409418937364567, + "velocity" : 2.409882423985485, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.2939100604434017, - "y" : -2.074408554736916 + "x" : 2.3111098261367333, + "y" : -2.1191346465205667 }, "rotation" : { - "radians" : -2.971927682130507 + "radians" : -3.053153686544722 } }, - "curvature" : 0.325138704477899 + "curvature" : 0.3445389130934825 }, { - "time" : 1.3999769283633923, - "velocity" : 2.36415582826455, + "time" : 1.440592604317041, + "velocity" : 2.2851877561755893, "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.2232474317945616, - "y" : -2.0873964367016242 + "x" : 2.238305562829254, + "y" : -2.126553443997892 }, "rotation" : { - "radians" : -2.947269113517704 + "radians" : -3.0264066985780818 } }, - "curvature" : 0.3621944652381181 + "curvature" : 0.38758316287414446 }, { - "time" : 1.4301864235012918, - "velocity" : 2.2433178477129516, - "acceleration" : -3.8425061187661265, + "time" : 1.472415714693242, + "velocity" : 2.157895314670785, + "acceleration" : -3.812563066154557, "pose" : { "translation" : { - "x" : 2.1551440380505156, - "y" : -2.1017266062671247 + "x" : 2.1682006169366934, + "y" : -2.135678798178092 }, "rotation" : { - "radians" : -2.9206492753984246 + "radians" : -2.997325314341709 } }, - "curvature" : 0.40377746810107285 + "curvature" : 0.43637811252079767 }, { - "time" : 1.4609990554339194, - "velocity" : 2.1249201209765416, - "acceleration" : -3.5248780323742457, + "time" : 1.504942660922755, + "velocity" : 2.0338842808213484, + "acceleration" : -3.4745285073926646, "pose" : { "translation" : { - "x" : 2.0896958261728287, - "y" : -2.1173986680805683 + "x" : 2.1008956814184785, + "y" : -2.1465226551517844 }, "rotation" : { - "radians" : -2.8919535310937237 + "radians" : -2.965751945842899 } }, - "curvature" : 0.45002691208795453 + "curvature" : 0.4912145518613118 }, { - "time" : 1.4923979532320715, - "velocity" : 2.0142428358870714, - "acceleration" : -3.197919951626316, + "time" : 1.5381580958310392, + "velocity" : 1.918476305347069, + "acceleration" : -3.1316450892116623, "pose" : { "translation" : { - "x" : 2.026978143555276, - "y" : -2.1344053621462358 + "x" : 2.0364697614119587, + "y" : -2.159087266481591 }, "rotation" : { - "radians" : -2.861091073527203 + "radians" : -2.9315543011342946 } }, - "curvature" : 0.5008412010105917 + "curvature" : 0.5520911992533699 }, { - "time" : 1.5243220106712716, - "velocity" : 1.9121522556653892, - "acceleration" : -2.6890710106994757, + "time" : 1.571997357302157, + "velocity" : 1.8125037483384936, + "acceleration" : -2.6083220889136767, "pose" : { "translation" : { - "x" : 1.967044343123007, - "y" : -2.152732213397371 + "x" : 1.974978706781286, + "y" : -2.173364650114962 }, "rotation" : { - "radians" : -2.8280094739823887 + "radians" : -2.8946440185387496 } }, - "curvature" : 0.555749093954951 + "curvature" : 0.6185372751982955 }, { - "time" : 1.5892933232552724, - "velocity" : 1.7374397824686587, - "acceleration" : -1.9497522202134734, + "time" : 1.6410415980225266, + "velocity" : 1.6324141301552804, + "acceleration" : -2.056186097930853, "pose" : { "translation" : { - "x" : 1.855623458766786, - "y" : -2.1932503092648403 + "x" : 1.8609000120313794, + "y" : -2.206971403483294 }, "rotation" : { - "radians" : -2.7552864354535944 + "radians" : -2.8127029122723353 } }, - "curvature" : 0.6731381127503112 + "curvature" : 0.762540672124892 }, { - "time" : 1.655072303925798, - "velocity" : 1.6091870688629224, - "acceleration" : -1.0749318224543505, + "time" : 1.6761338135036778, + "velocity" : 1.5602580045373433, + "acceleration" : -1.6675516226568237, "pose" : { "translation" : { - "x" : 1.7553671009011396, - "y" : -2.2386795374578696 + "x" : 1.8082950882143756, + "y" : -2.2262287892527253 }, "rotation" : { - "radians" : -2.674931532448897 + "radians" : -2.7679602634610805 } }, - "curvature" : 0.7847126172556117 + "curvature" : 0.8347008677720427 }, { - "time" : 1.720231550281118, - "velocity" : 1.539145321428446, - "acceleration" : -1.1921125316242216, + "time" : 1.7113375846273613, + "velocity" : 1.5015538988764054, + "acceleration" : -1.2481101573777, "pose" : { "translation" : { - "x" : 1.6657680121716112, - "y" : -2.288608611212112 + "x" : 1.7585875274754414, + "y" : -2.247053901220312 }, "rotation" : { - "radians" : -2.5902033764696597 + "radians" : -2.7211516082019864 } }, - "curvature" : 0.8577573547258329 + "curvature" : 0.9012428901840408 }, { - "time" : 1.784455858835107, - "velocity" : 1.4625827183663351, - "acceleration" : -4.000000000000001, + "time" : 1.7464375053486416, + "velocity" : 1.4577453313010234, + "acceleration" : -0.7779237166793617, "pose" : { "translation" : { - "x" : 1.5858331132455987, - "y" : -2.342477161522652 + "x" : 1.7116953915459305, + "y" : -2.2693795029500246 }, "rotation" : { - "radians" : -2.507140034918934 + "radians" : -2.672856659027319 } }, - "curvature" : 0.8472000723919605 + "curvature" : 0.9562256414450673 }, { - "time" : 1.853732644359764, - "velocity" : 1.1854755762677067, - "acceleration" : -3.9999999999999987, + "time" : 1.781173038934524, + "velocity" : 1.430723735913053, + "acceleration" : -0.22157809100517403, "pose" : { "translation" : { - "x" : 1.5140388659856399, - "y" : -2.3995645234426775 + "x" : 1.667504782177275, + "y" : -2.293124889867613 }, "rotation" : { - "radians" : -2.43458289692862 + "radians" : -2.623879203384706 } }, - "curvature" : 0.7096157054787944 + "curvature" : 0.9926865535315983 }, { - "time" : 1.9414753768133155, - "velocity" : 0.8345046464535002, - "acceleration" : -4.0, + "time" : 1.8152328314093547, + "velocity" : 1.4231768321164475, + "acceleration" : 0.4873333149229415, "pose" : { "translation" : { - "x" : 1.4482866366226972, - "y" : -2.4589785223821536 + "x" : 1.625868373689869, + "y" : -2.318195350173432 }, "rotation" : { - "radians" : -2.383232861630834 + "radians" : -2.5752533397836075 } }, - "curvature" : 0.422926581935277 + "curvature" : 1.003242618645744 }, { - "time" : 2.1501015384266906, + "time" : 1.8482473398234809, + "velocity" : 1.439265901942455, + "acceleration" : -3.6589651945051656, + "pose" : { + "translation" : { + "x" : 1.5866039455219436, + "y" : -2.3444816257552645 + }, + "rotation" : { + "radians" : -2.5282276575480873 + } + }, + "curvature" : 0.9809381622274879 + }, { + "time" : 1.9178314402693484, + "velocity" : 1.1846601003200747, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 1.5142788687799111, + "y" : -2.4001886242122055 + }, + "rotation" : { + "radians" : -2.44479080185293 + } + }, + "curvature" : 0.8173776396794599 + }, { + "time" : 2.0054115856723604, + "velocity" : 0.834339518708027, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 1.4483181266711824, + "y" : -2.4590604087766508 + }, + "rotation" : { + "radians" : -2.3859695614811485 + } + }, + "curvature" : 0.48392472113324264 + }, { + "time" : 2.2139964653493673, "velocity" : 0.0, - "acceleration" : -4.0, + "acceleration" : -3.9999999999999996, "pose" : { "translation" : { "x" : 1.3858580589294434, @@ -1431,8 +1501,8 @@ "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 5.218406677246094, -1.5258679389953613, 0.0 ], - "y" : [ -2.2181358337402344, 0.3001227378845215, 0.0 ] + "x" : [ 5.326601505279541, -1.5258679389953613, 0.0 ], + "y" : [ -2.4994845390319824, 0.3001227378845215, 0.0 ] }, { "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ], "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ] @@ -1443,7 +1513,7 @@ "radians" : -2.3736477827122884 } }, { - "time" : 2.059999953955412, + "time" : 2.1199999526143074, "rotation" : { "radians" : -2.3824463294692526 } diff --git a/src/main/deploy/autos/red/5ball.json b/src/main/deploy/autos/red/5ball.json index c0e07aab..5d82b260 100644 --- a/src/main/deploy/autos/red/5ball.json +++ b/src/main/deploy/autos/red/5ball.json @@ -212,7 +212,7 @@ "constraints" : [ ] }, { "type" : "script", - "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 6", + "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 5", "closed" : false, "valid" : true, "sendableScript" : { @@ -220,7 +220,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "6" ], + "args" : [ "5" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -250,7 +250,7 @@ "states" : [ { "time" : 0.0, "velocity" : 0.0, - "acceleration" : 4.0, + "acceleration" : 3.999999999999999, "pose" : { "translation" : { "x" : 7.626592636108398, @@ -262,433 +262,433 @@ }, "curvature" : 0.0 }, { - "time" : 0.18074788653672832, - "velocity" : 0.7229915461469133, - "acceleration" : 4.000000000000002, + "time" : 0.18074836042161999, + "velocity" : 0.7229934416864798, + "acceleration" : 3.9999999999999973, "pose" : { "translation" : { - "x" : 7.5612559258704835, - "y" : -3.197459243636949 + "x" : 7.561256032039637, + "y" : -3.197505262483041 }, "rotation" : { - "radians" : -3.138740759458168 + "radians" : -3.136668850289797 } }, - "curvature" : -0.29172803877514997 + "curvature" : -0.2308900234918559 }, { - "time" : 0.25626538315297653, - "velocity" : 1.0250615326119064, - "acceleration" : 3.999999999999997, + "time" : 0.2562629294282206, + "velocity" : 1.025051717712882, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 7.495254533242587, - "y" : -3.196840137823301 + "x" : 7.495255342415476, + "y" : -3.1971908724897276 }, "rotation" : { - "radians" : 3.1176073408097276 + "radians" : 3.1252538533846033 } }, - "curvature" : -0.5074451761358538 + "curvature" : -0.4025150795851139 }, { - "time" : 0.3151289706110068, - "velocity" : 1.2605158824440272, - "acceleration" : 4.000000000000004, + "time" : 0.3151027670199877, + "velocity" : 1.2604110680799503, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 7.428047492778063, - "y" : -3.1939619357255324 + "x" : 7.428050091650221, + "y" : -3.195088412615668 }, "rotation" : { - "radians" : 3.0784656278152482 + "radians" : 3.0941603378329714 } }, - "curvature" : -0.6425731706470493 + "curvature" : -0.5118276847264331 }, { - "time" : 0.3658526486292684, - "velocity" : 1.463410594517074, - "acceleration" : 3.999999999999997, + "time" : 0.365754563843708, + "velocity" : 1.4630182553748314, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 7.3592204221349675, - "y" : -3.1880118377084727 + "x" : 7.359226277680136, + "y" : -3.19054991419398 }, "rotation" : { - "radians" : 3.0315419813222593 + "radians" : 3.0567199098033253 } }, - "curvature" : -0.7042822762242509 + "curvature" : -0.5650059367626722 }, { - "time" : 0.4117606980978159, - "velocity" : 1.6470427923912636, - "acceleration" : 2.1723907907211393, + "time" : 0.41151249073481067, + "velocity" : 1.6460499629392418, + "acceleration" : 3.999999999999991, "pose" : { "translation" : { - "x" : 7.288476303159229, - "y" : -3.178375355932097 + "x" : 7.288487160873956, + "y" : -3.183081614639285 }, "rotation" : { - "radians" : 2.980846049930411 + "radians" : 3.0159966655848844 } }, - "curvature" : -0.7073398126437707 + "curvature" : -0.5735194202335906 }, { - "time" : 0.4555105966830825, - "velocity" : 1.7420846691728806, - "acceleration" : 1.9989588140063956, + "time" : 0.45404678811194626, + "velocity" : 1.8161871524477837, + "acceleration" : 3.999999999999997, "pose" : { "translation" : { - "x" : 7.215626262967817, - "y" : -3.1646216243218532 + "x" : 7.215644052943389, + "y" : -3.172332660165239 }, "rotation" : { - "radians" : 2.9296129513061087 + "radians" : 2.974462343798371 } }, - "curvature" : -0.6695533478285349 + "curvature" : -0.5505200761933615 }, { - "time" : 0.4987555436671162, - "velocity" : 1.828529537107854, - "acceleration" : 2.813152489265579, + "time" : 0.49431508345730446, + "velocity" : 1.9772603338292163, + "acceleration" : 3.4442482007392234, "pose" : { "translation" : { - "x" : 7.140580355031915, - "y" : -3.146488708538989 + "x" : 7.140607105853633, + "y" : -3.158083808502063 }, "rotation" : { - "radians" : 2.8802105895971133 + "radians" : 2.933950997371189 } }, - "curvature" : -0.6077426894646522 + "curvature" : -0.5081482982157391 }, { - "time" : 0.5413750298351414, - "velocity" : 1.948424650712654, - "acceleration" : 3.6356664874107865, + "time" : 0.533094323956189, + "velocity" : 2.110825663143533, + "acceleration" : 3.7197245506146066, "pose" : { "translation" : { - "x" : 7.063338340260088, - "y" : -3.1238689159508795 + "x" : 7.063376100733876, + "y" : -3.140236131614074 }, "rotation" : { - "radians" : 2.834193414995866 + "radians" : 2.8957084348464917 } }, - "curvature" : -0.5352497494731134 + "curvature" : -0.45605683856028184 }, { - "time" : 0.5828078541546968, - "velocity" : 2.0990605815700403, + "time" : 0.5707801304586655, + "velocity" : 2.2510064828005065, "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.983980468081455, - "y" : -3.0967941056013544 + "x" : 6.984031236787814, + "y" : -3.1187997184172147 }, "rotation" : { - "radians" : 2.7924479125830106 + "radians" : 2.860495559941412 } }, - "curvature" : -0.46118349636323475 + "curvature" : -0.40102385925601364 }, { - "time" : 0.6228085797994793, - "velocity" : 2.259063484149171, - "acceleration" : 3.999999999999995, + "time" : 0.6073691854310432, + "velocity" : 2.3973627026900175, + "acceleration" : 3.99999999999999, "pose" : { "translation" : { - "x" : 6.902658257528856, - "y" : -3.0654209981810254 + "x" : 6.902723920204153, + "y" : -3.093882377496584 }, "rotation" : { - "radians" : 2.755369864098415 + "radians" : 2.8287087580304324 } }, - "curvature" : -0.3908757756828451 + "curvature" : -0.3472627622064644 }, { - "time" : 0.6614595623941697, - "velocity" : 2.413667414527932, - "acceleration" : 3.999999999999995, + "time" : 0.6429036781342928, + "velocity" : 2.5395006735030154, + "acceleration" : 4.000000000000005, "pose" : { "translation" : { - "x" : 6.819585278322023, - "y" : -3.0300164859976135 + "x" : 6.81966755306712, + "y" : -3.065678339823968 }, "rotation" : { - "radians" : 2.7230300588082508 + "radians" : 2.8004923774748782 } }, - "curvature" : -0.32684828411643785 + "curvature" : -0.29702381463813854 }, { - "time" : 0.6988907922441839, - "velocity" : 2.563392333927989, - "acceleration" : 3.9999999999999907, + "time" : 0.6774510929226875, + "velocity" : 2.677690332656594, + "acceleration" : 4.000000000000004, "pose" : { "translation" : { - "x" : 6.7350279319507536, - "y" : -2.990942942946276 + "x" : 6.7351283222669736, + "y" : -3.034456961475371 }, "rotation" : { - "radians" : 2.6953074205444363 + "radians" : 2.775831102570153 } }, - "curvature" : -0.26979273017272876 + "curvature" : -0.2512121840579067 }, { - "time" : 0.735158469971846, - "velocity" : 2.708463044838637, - "acceleration" : 1.279466838075468, + "time" : 0.7110320540420592, + "velocity" : 2.8120141771340808, + "acceleration" : -3.2029286867045355, "pose" : { "translation" : { - "x" : 6.649296232758076, - "y" : -2.9486435344799347 + "x" : 6.64941598841051, + "y" : -3.0005514263485438 }, "rotation" : { - "radians" : 2.671985460234166 + "radians" : 2.7546193861322052 } }, - "curvature" : -0.2193389226412271 + "curvature" : -0.20989196042644145 }, { - "time" : 0.7708801601749081, - "velocity" : 2.75416776285346, + "time" : 0.7450512183156593, + "velocity" : 2.7030532199844526, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.562734589023421, - "y" : -2.9036275275796015 + "x" : 6.5628746747315745, + "y" : -2.964347448880517 }, "rotation" : { - "radians" : 2.65281690834138 + "radians" : 2.7367102690047114 } }, - "curvature" : -0.17457123350751053 + "curvature" : -0.17264507479778918 }, { - "time" : 0.8078106139873205, - "velocity" : 2.606445947603811, + "time" : 0.781148946516508, + "velocity" : 2.558662307181058, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.4757125840457945, - "y" : -2.8564556007247077 + "x" : 6.475873656001568, + "y" : -2.926271976765129 }, "rotation" : { - "radians" : 2.6375642644084403 + "radians" : 2.721947850294995 } }, - "curvature" : -0.13433724003134345 + "curvature" : -0.13880277069969946 }, { - "time" : 0.8472976500866943, - "velocity" : 2.4484978032063154, + "time" : 0.819677131050652, + "velocity" : 2.404549569044482, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.388615757226944, - "y" : -2.80772515386343 + "x" : 6.388798147439957, + "y" : -2.886781893670559 }, "rotation" : { - "radians" : 2.62602361109316 + "radians" : 2.7101877381984085 } }, - "curvature" : -0.09741150032488063 + "curvature" : -0.10758244075317105 }, { - "time" : 0.8895957780818, - "velocity" : 2.2793052912258926, + "time" : 0.8608963508797474, + "velocity" : 2.2396726897281005, "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.30183638515453, - "y" : -2.7580556183830183 + "x" : 6.3020400936247825, + "y" : -2.8463527219568547 }, "rotation" : { - "radians" : 2.618037394000371 + "radians" : 2.701309038995795 } }, - "curvature" : -0.06256913374757356 + "curvature" : -0.07816156796989873 }, { - "time" : 0.9350786097662884, - "velocity" : 2.097373964487939, - "acceleration" : -3.999999999999998, + "time" : 0.9051855357994181, + "velocity" : 2.0625159500494177, + "acceleration" : -4.000000000000004, "pose" : { "translation" : { - "x" : 6.215764262685298, - "y" : -2.7080737670801227 + "x" : 6.2159889574031695, + "y" : -2.805467325393465 }, "rotation" : { - "radians" : 2.6135000159251436 + "radians" : 2.695220423456694 } }, - "curvature" : -0.02860941649074867 + "curvature" : -0.049714314498870184 }, { - "time" : 0.9843259494632479, - "velocity" : 1.900384605700101, + "time" : 0.9531262186792658, + "velocity" : 1.8707532185300264, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.130777484028243, - "y" : -2.6583990241311213 + "x" : 6.131022508801834, + "y" : -2.76460461187677 }, "rotation" : { - "radians" : 2.612358417756541 + "radians" : 2.6918618037119115 } }, - "curvature" : 0.005641950266030671 + "curvature" : -0.021430491946737754 }, { - "time" : 1.038295625522969, - "velocity" : 1.6845059014612167, - "acceleration" : -4.000000000000003, + "time" : 1.0056683951523762, + "velocity" : 1.6605845126375849, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 6.047233223827789, - "y" : -2.609628775062447 + "x" : 6.047497613937594, + "y" : -2.72422823614761 }, "rotation" : { - "radians" : 2.6146083461531995 + "radians" : 2.6912022196748593 } }, - "curvature" : 0.04132896947061391 + "curvature" : 0.007466521997725307 }, { - "time" : 1.0987120643010575, - "velocity" : 1.4428401463488625, - "acceleration" : -4.000000000000001, + "time" : 1.0645039443355842, + "velocity" : 1.425242315904753, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 5.9654585182469475, - "y" : -2.562323676720915 + "x" : 5.965741023927876, + "y" : -2.684775302508818 }, "rotation" : { - "radians" : 2.620285612891302 + "radians" : 2.6932336344067656 } }, - "curvature" : 0.07953607724401594 + "curvature" : 0.03768607383081342 }, { - "time" : 1.1691474210025192, - "velocity" : 1.1610987195430156, - "acceleration" : -4.000000000000001, + "time" : 1.133097575805044, + "velocity" : 1.1508677900269133, + "acceleration" : -3.9999999999999987, "pose" : { "translation" : { - "x" : 5.885741046050498, - "y" : -2.5169929672440503 + "x" : 5.886040163801226, + "y" : -2.6466450675427495 }, "rotation" : { - "radians" : 2.6294501603955758 + "radians" : 2.697959401678686 } }, - "curvature" : 0.12120182277895979 + "curvature" : 0.06979988927808267 }, { - "time" : 1.259422100888273, + "time" : 1.2208145233117724, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.8083199096881515, - "y" : -2.4740797760304147 + "x" : 5.808633921407818, + "y" : -2.610187642828812 }, "rotation" : { - "radians" : 2.642159027386621 + "radians" : 2.705375145901104 } }, - "curvature" : 0.1669290910240929 + "curvature" : 0.10410688222029436 }, { - "time" : 1.3656883669621598, + "time" : 1.323926104530657, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.733376416377723, - "y" : -2.4339464337099344 + "x" : 5.733703436329961, + "y" : -2.5756926976609975 }, "rotation" : { - "radians" : 2.6584223838932473 + "radians" : 2.715438727807114 } }, - "curvature" : 0.2166266434826088 + "curvature" : 0.14039632008460795 }, { - "time" : 1.4673170381313625, + "time" : 1.422963534703475, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.661024859188302, - "y" : -2.396859782114227 + "x" : 5.66136288879261, + "y" : -2.5433781617654105 }, "rotation" : { - "radians" : 2.6781351093523047 + "radians" : 2.7280251266126854 } }, - "curvature" : 0.2689053696942707 + "curvature" : 0.17756966797704812 }, { - "time" : 1.5642156155177847, + "time" : 1.5178302228662353, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.591303298123421, - "y" : -2.3629764842469285 + "x" : 5.591650288573874, + "y" : -2.5133789280178007 }, "rotation" : { - "radians" : 2.700976194613289 + "radians" : 2.742862160518646 } }, - "curvature" : 0.3201793855552225 + "curvature" : 0.21310100879858962 }, { - "time" : 1.6564699229560516, + "time" : 1.6085811250451547, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.5241643412042265, - "y" : -2.332328334254022 + "x" : 5.524518263915525, + "y" : -2.4857355551610922 }, "rotation" : { - "radians" : 2.726272217468977 + "radians" : 2.7594454148032215 } }, - "curvature" : 0.36354779991332803 + "curvature" : 0.2423895877411266 }, { - "time" : 1.7443554806111186, + "time" : 1.6954358105818255, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.459465925552649, - "y" : -2.304807567394164 + "x" : 5.459824850433506, + "y" : -2.4603829705229145 }, "rotation" : { - "radians" : 2.7528334527109286 + "radians" : 2.7769377831485036 } }, - "curvature" : 0.38783779721239137 + "curvature" : 0.2582273725929391 }, { - "time" : 1.8283441053954617, + "time" : 1.7787893070277847, "velocity" : 0.8, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 5.3969620984745745, - "y" : -2.2801521700090106 + "x" : 5.3973242800284424, + "y" : -2.437139172733133 }, "rotation" : { - "radians" : 2.7787959191919596 + "radians" : 2.794073008855231 } }, - "curvature" : 0.3776909643218038 + "curvature" : 0.2508829468700981 }, { - "time" : 1.9091062425604757, + "time" : 1.8592209910556299, "velocity" : 0.8, - "acceleration" : -2.5718750180081185, + "acceleration" : -2.574202589173992, "pose" : { "translation" : { - "x" : 5.336293798543011, - "y" : -2.257931189493547 + "x" : 5.3366577697961475, + "y" : -2.4156939344413786 }, "rotation" : { - "radians" : 2.8015387216773218 + "radians" : 2.8091035777764453 } }, - "curvature" : 0.31603589357887024 + "curvature" : 0.20957902888612367 }, { - "time" : 2.22016334852342, + "time" : 2.1699968419402733, "velocity" : 0.0, - "acceleration" : -2.5718750180081185, + "acceleration" : -2.574202589173992, "pose" : { "translation" : { - "x" : 5.218406677246094, - "y" : -2.2181358337402344 + "x" : 5.218771457672119, + "y" : -2.376249313354492 }, "rotation" : { "radians" : 2.823947779781842 @@ -700,8 +700,8 @@ "x" : [ 7.626592636108398, -2.086981773376465, 0.0 ], "y" : [ -3.196845054626465, -0.026646852493286133, 0.0 ] }, { - "x" : [ 5.218406677246094, -1.8701248168945312, 0.0 ], - "y" : [ -2.2181358337402344, 0.6148552894592285, 0.0 ] + "x" : [ 5.218771457672119, -1.8701248168945312, 0.0 ], + "y" : [ -2.376249313354492, 0.6148552894592285, 0.0 ] } ], "rotations" : [ { "time" : 0.0, @@ -709,7 +709,7 @@ "radians" : 2.9608083881863267 } }, { - "time" : 2.109999952837825, + "time" : 2.059999953955412, "rotation" : { "radians" : -3.1066860685499065 } @@ -736,7 +736,7 @@ } ] }, { "type" : "script", - "script" : "VisionManager.shootBalls 3.5", + "script" : "VisionManager.shootBalls 4.5", "closed" : false, "valid" : true, "sendableScript" : { @@ -744,7 +744,7 @@ "delay" : 0.0, "commands" : [ { "methodName" : "frc.subsystem.VisionManager.shootBalls", - "args" : [ "3.5" ], + "args" : [ "4.5" ], "argTypes" : [ "double" ], "reflection" : true } ] @@ -777,8 +777,8 @@ "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.218406677246094, - "y" : -2.2181358337402344 + "x" : 5.218771457672119, + "y" : -2.376249313354492 }, "rotation" : { "radians" : 2.9473819457269834 @@ -786,639 +786,667 @@ }, "curvature" : -0.0 }, { - "time" : 0.22577455195938007, - "velocity" : 0.9030982078375203, + "time" : 0.2258499088780425, + "velocity" : 0.90339963551217, "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 5.118318357381213, - "y" : -2.198750699968514 + "x" : 5.11868232863435, + "y" : -2.356513444916345 }, "rotation" : { - "radians" : 2.9554310054193884 + "radians" : 2.9464389269674878 } }, - "curvature" : 0.13027761823749012 + "curvature" : -0.002138170805929468 }, { - "time" : 0.3382371271442293, - "velocity" : 1.3529485085769173, - "acceleration" : 3.9999999999999973, + "time" : 0.28397149126868465, + "velocity" : 1.1358859650747386, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.993451572256163, - "y" : -2.176348690292798 + "x" : 5.060542505813004, + "y" : -2.345028926573832 }, "rotation" : { - "radians" : 2.972591803682675 + "radians" : 2.9468943393418883 } }, - "curvature" : 0.12784985955539324 + "curvature" : 0.01712904542764913 }, { - "time" : 0.3916828784133741, - "velocity" : 1.5667315136534963, - "acceleration" : 3.9999999999999973, + "time" : 0.33858988144624014, + "velocity" : 1.3543595257849605, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.916478985341882, - "y" : -2.163593622500116 + "x" : 4.993810497137019, + "y" : -2.3319240934215486 }, "rotation" : { - "radians" : 2.9819601021983635 + "radians" : 2.9486470176545607 } }, - "curvature" : 0.11259673461042294 + "curvature" : 0.03310799771885568 }, { - "time" : 0.4451005174091182, - "velocity" : 1.7804020696364724, - "acceleration" : 3.999999999999998, + "time" : 0.3922352671630025, + "velocity" : 1.56894106865201, + "acceleration" : 3.999999999999995, "pose" : { "translation" : { - "x" : 4.82815027252218, - "y" : -2.1498081765639654 + "x" : 4.9168329080531805, + "y" : -2.317000843407186 }, "rotation" : { - "radians" : 2.9913800207169805 + "radians" : 2.9516944809829795 } }, - "curvature" : 0.09893287394574607 + "curvature" : 0.04352423972613848 }, { - "time" : 0.49879827939468, - "velocity" : 1.9951931175787196, - "acceleration" : 3.9999999999999982, + "time" : 0.445877887656658, + "velocity" : 1.7835115506266317, + "acceleration" : 3.9999999999999956, "pose" : { "translation" : { - "x" : 4.727849331012521, - "y" : -2.1351214277652844 + "x" : 4.828497262972633, + "y" : -2.3002106203348376 }, "rotation" : { - "radians" : 3.000843152646543 + "radians" : 2.9559185031716857 } }, - "curvature" : 0.08854693535519663 + "curvature" : 0.04975547748132501 }, { - "time" : 0.5527581649942036, - "velocity" : 2.211032659976814, - "acceleration" : 3.9999999999999964, + "time" : 0.499814290021222, + "velocity" : 1.9992571600848876, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 4.61541160941124, - "y" : -2.11974885687232 + "x" : 4.728187360616829, + "y" : -2.281639807416468 }, "rotation" : { - "radians" : 3.010451193020663 + "radians" : 2.961208234474397 } }, - "curvature" : 0.08141460522576446 + "curvature" : 0.053603892320815395 }, { - "time" : 0.6067994538503118, - "velocity" : 2.4271978154012466, - "acceleration" : 4.0, + "time" : 0.5540160403320278, + "velocity" : 2.2160641613281107, + "acceleration" : 4.0000000000000036, "pose" : { "translation" : { - "x" : 4.491079470872819, - "y" : -2.1039811364393017 + "x" : 4.615738629363477, + "y" : -2.2614951208233833 }, "rotation" : { - "radians" : 3.0203520346564443 + "radians" : 2.967499617187574 } }, - "curvature" : 0.0770754880726607 + "curvature" : 0.056398078240657186 }, { - "time" : 0.6337713815419612, - "velocity" : 2.5350855261678444, - "acceleration" : 3.999999999999981, + "time" : 0.6082934243307945, + "velocity" : 2.4331736973231775, + "acceleration" : 4.000000000000006, "pose" : { "translation" : { - "x" : 4.424629014592831, - "y" : -2.096057730950637 + "x" : 4.4913934825924855, + "y" : -2.240089003237699 }, "rotation" : { - "radians" : 3.025464239893527 + "radians" : 2.974780294083102 } }, - "curvature" : 0.07580950849707052 + "curvature" : 0.0589859321752288 }, { - "time" : 0.6606632003839968, - "velocity" : 2.6426528015359865, - "acceleration" : 4.0, + "time" : 0.6353787681419027, + "velocity" : 2.5415150725676106, + "acceleration" : 3.999999999999994, "pose" : { "translation" : { - "x" : 4.355457556281181, - "y" : -2.088172917105112 + "x" : 4.424935805760302, + "y" : -2.2290358611979 }, "rotation" : { - "radians" : 3.030713652227927 + "radians" : 2.9788008074550336 } }, - "curvature" : 0.07508525249190778 + "curvature" : 0.06037816383649394 }, { - "time" : 0.687437732922965, - "velocity" : 2.749750931691859, - "acceleration" : 4.0, + "time" : 0.6623797970275374, + "velocity" : 2.649519188110149, + "acceleration" : 3.999999999999982, "pose" : { "translation" : { - "x" : 4.2836899587141986, - "y" : -2.0803791830604403 + "x" : 4.355756674031909, + "y" : -2.217825017403811 }, "rotation" : { - "radians" : 3.036123218975336 + "radians" : 2.983085127613897 } }, - "curvature" : 0.07486952079026583 + "curvature" : 0.061904813840425225 }, { - "time" : 0.7140574327285515, - "velocity" : 2.856229730914205, - "acceleration" : 4.0000000000000115, + "time" : 0.6892585612750203, + "velocity" : 2.75703424510008, + "acceleration" : 4.000000000000018, "pose" : { "translation" : { - "x" : 4.209468147422967, - "y" : -2.0727316138919605 + "x" : 4.2839809727891565, + "y" : -2.206518758337751 }, "rotation" : { - "radians" : 3.0417167398697336 + "radians" : 2.987644244198006 } }, - "curvature" : 0.07513898148074699 + "curvature" : 0.06360865823678644 }, { - "time" : 0.7404848785000484, - "velocity" : 2.961939514000193, - "acceleration" : 1.4367050542224837, + "time" : 0.7159768045103969, + "velocity" : 2.863907218041587, + "acceleration" : 4.0, "pose" : { "translation" : { - "x" : 4.132949715792485, - "y" : -2.0652875411644693 + "x" : 4.209750653103896, + "y" : -2.1951832396798636 }, "rotation" : { - "radians" : 3.0475190269956305 + "radians" : 2.992491671784849 } }, - "curvature" : 0.07587984575547331 + "curvature" : 0.06552874485970991 }, { - "time" : 0.7669763882834565, - "velocity" : 3.0, - "acceleration" : -1.0982184022288134E-14, + "time" : 0.742496452777877, + "velocity" : 2.9699858111115076, + "acceleration" : 1.127748575111186, "pose" : { "translation" : { - "x" : 4.054306530160829, - "y" : -2.058106192504056 + "x" : 4.133223336592542, + "y" : -2.183888029856604 }, "rotation" : { - "radians" : 3.053556107860972 + "radians" : 2.997643463480912 } }, - "curvature" : 0.07708752042753088 + "curvature" : 0.06770258788043935 }, { - "time" : 0.7939345482116209, - "velocity" : 2.9999999999999996, - "acceleration" : 1.0755238227730279E-14, + "time" : 0.7691107084292603, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.9737233349183256, - "y" : -2.0512483411699343 + "x" : 4.054570920270635, + "y" : -2.172705653589219 }, "rotation" : { - "radians" : 3.05985546264456 + "radians" : 3.0031182727570744 } }, - "curvature" : 0.07876635846236638 + "curvature" : 0.07016799487252837 }, { - "time" : 0.8214615510510413, + "time" : 0.7962237823292616, "velocity" : 3.0, "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.891396357606709, - "y" : -2.044775955626278 + "x" : 3.9739781814073973, + "y" : -2.161711135442232 }, "rotation" : { - "radians" : 3.0664462909649215 + "radians" : 3.0089374623838587 } }, - "curvature" : 0.08092957341077114 + "curvature" : 0.0729646488132685 }, { - "time" : 0.8494883928631835, + "time" : 0.8239014360111758, "velocity" : 3.0, - "acceleration" : -1.0404470748216723E-14, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.8075319140182913, - "y" : -2.0387518491140533 + "x" : 3.8916413823802998, + "y" : -2.150981543371927 }, "rotation" : { - "radians" : 3.0733598063989405 + "radians" : 3.015125259707474 } }, - "curvature" : 0.08359935409570846 + "curvature" : 0.07613554688814887 }, { - "time" : 0.8779434176691419, - "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "time" : 0.8520731360071213, + "velocity" : 3.0, + "acceleration" : -1.035442254722702E-14, "pose" : { "translation" : { - "x" : 3.7223450132951257, - "y" : -2.0332393292228517 + "x" : 3.8077668755296186, + "y" : -2.14059553227483 }, "rotation" : { - "radians" : 3.080629558866709 + "radians" : 3.0217089580997 } }, - "curvature" : 0.08680720147794485 + "curvature" : 0.0797283808237485 }, { - "time" : 0.9067528176974184, + "time" : 0.8806656984492115, "velocity" : 2.9999999999999996, - "acceleration" : 0.0, + "acceleration" : 1.0230783524543899E-14, "pose" : { "translation" : { - "x" : 3.636057963028172, - "y" : -2.028301847462725 + "x" : 3.7225697080129976, + "y" : -2.130632887536194 }, "rotation" : { - "radians" : 3.0882917861725874 + "radians" : 3.02871916503608 } }, - "curvature" : 0.09059450379888723 + "curvature" : 0.08379693042086887 }, { - "time" : 0.9358411361147014, - "velocity" : 2.9999999999999996, - "acceleration" : 1.0107649040265085E-14, + "time" : 0.9096038020190037, + "velocity" : 3.0, + "acceleration" : 0.0, "pose" : { "translation" : { - "x" : 3.5488989743564616, - "y" : -2.024002648836017 + "x" : 3.636272226660009, + "y" : -2.1211740685784832 }, "rotation" : { - "radians" : 3.0963857966103348 + "radians" : 3.036190097790499 } }, - "curvature" : 0.09501336487557137 + "curvature" : 0.08840253262721617 }, { - "time" : 0.9651317725479596, + "time" : 0.9388105037985717, "velocity" : 3.0, - "acceleration" : -0.06809488522901795, + "acceleration" : -0.02866373936722982, "pose" : { "translation" : { - "x" : 3.461100767066263, - "y" : -2.0204044214092 + "x" : 3.549102682826714, + "y" : -2.1122997524098537 }, "rotation" : { - "radians" : 3.1049543846666157 + "radians" : 3.0441599280842286 } }, - "curvature" : 0.10012770121562267 + "curvature" : 0.09361568206062745 }, { - "time" : 0.9945573185420075, - "velocity" : 2.997996270822734, - "acceleration" : -0.184591356598183, + "time" : 0.9682118880878174, + "velocity" : 2.9991572463836973, + "acceleration" : -0.1982391394605512, "pose" : { "translation" : { - "x" : 3.3728991746902466, - "y" : -2.0175689458847046 + "x" : 3.4612938372502233, + "y" : -2.104090377172639 }, "rotation" : { - "radians" : 3.1140442814748748 + "radians" : 3.052671177092393 } }, - "curvature" : 0.10601462452471887 + "curvature" : 0.09951781431626906 }, { - "time" : 1.0240672639708144, - "velocity" : 2.9925489899628923, - "acceleration" : -0.20849217138745274, + "time" : 0.9977582138868922, + "velocity" : 2.9933000081830676, + "acceleration" : -0.22109236700729543, "pose" : { "translation" : { - "x" : 3.28453174960665, - "y" : -2.0155567451727556 + "x" : 3.3730815649032593, + "y" : -2.0966256856918335 }, "rotation" : { - "radians" : 3.1237066406836274 + "radians" : 3.06177116186092 } }, - "curvature" : 0.11276612545130336 + "curvature" : 0.10620331937809746 }, { - "time" : 1.0536051481517177, - "velocity" : 2.9863905723518247, - "acceleration" : -0.2602935739030425, + "time" : 1.0273992194582766, + "velocity" : 2.986746608100814, + "acceleration" : -0.24700563523218624, "pose" : { "translation" : { - "x" : 3.196236368138443, - "y" : -2.014426733963205 + "x" : 3.284703459848715, + "y" : -2.0899842690235744 }, "rotation" : { - "radians" : 3.13399755862567 + "radians" : 3.071512493242552 } }, - "curvature" : 0.12049107003628591 + "curvature" : 0.1137818273539462 }, { - "time" : 1.0831056442927933, - "velocity" : 2.9787117827793512, - "acceleration" : -0.3599966924344615, + "time" : 1.0570639837395306, + "velocity" : 2.9794192441555096, + "acceleration" : -0.276421450526931, "pose" : { "translation" : { - "x" : 3.1082498356524937, - "y" : -2.014235868297366 + "x" : 3.1963974400942163, + "y" : -2.0842431100036265 }, "rotation" : { - "radians" : -3.1382066823601824 + "radians" : 3.0819536236657514 } }, - "curvature" : 0.1293175095895187 + "curvature" : 0.12238080029737158 }, { - "time" : 1.112515242001862, - "velocity" : 2.9681244248782583, - "acceleration" : -0.41435957373242843, + "time" : 1.086682946461585, + "velocity" : 2.9712319275167762, + "acceleration" : -0.3098235530790123, "pose" : { "translation" : { - "x" : 3.020806491658732, - "y" : -2.0150387951398443 + "x" : 3.108400352446682, + "y" : -2.079477126795865 }, "rotation" : { - "radians" : -3.1264678126986505 + "radians" : 3.0931594400426694 } }, - "curvature" : 0.13939528367796553 + "curvature" : 0.132148448806923 }, { - "time" : 1.14178182090318, - "velocity" : 2.9559975377201018, - "acceleration" : -0.47782084790928264, + "time" : 1.1161884359145708, + "velocity" : 2.962090431939117, + "acceleration" : -0.3477272761374577, "pose" : { "translation" : { - "x" : 2.9341368149093157, - "y" : -2.016887501950375 + "x" : 3.0209465773668853, + "y" : -2.0757587164407596 }, "rotation" : { - "radians" : -3.113896838891268 + "radians" : 3.1052018923916562 } }, - "curvature" : 0.150898858830179 + "curvature" : 0.14325696560995094 }, { - "time" : 1.1708492320949808, - "velocity" : 2.942108522657908, - "acceleration" : -0.5518862628311226, + "time" : 1.1455151922201363, + "velocity" : 2.9518927188510355, + "acceleration" : -0.390660961922364, "pose" : { "translation" : { - "x" : 2.8484660284977963, - "y" : -2.019830966255654 + "x" : 2.9342666338240138, + "y" : -2.0731572984038564 }, "rotation" : { - "radians" : -3.1004122518647654 + "radians" : 3.118160641588388 } }, - "curvature" : 0.16403028405156617 + "curvature" : 0.15590602336612916 }, { - "time" : 1.199665689597395, - "velocity" : 2.9262051156188686, - "acceleration" : -0.6381658186444317, + "time" : 1.174600881740302, + "velocity" : 2.9405300754049124, + "acceleration" : -0.49735381964647707, "pose" : { "translation" : { - "x" : 2.764012704958283, - "y" : -2.023914805221171 + "x" : 2.848585784150231, + "y" : -2.071738858124263 }, "rotation" : { - "radians" : -3.085924825413159 + "radians" : 3.1321236991006915 } }, - "curvature" : 0.179022046292927 + "curvature" : 0.17032641010647295 }, { - "time" : 1.2281844325226372, - "velocity" : 2.9080054286932713, - "acceleration" : -0.7382909496365782, + "time" : 1.203394839648812, + "velocity" : 2.926209290456375, + "acceleration" : -0.6832682280406255, "pose" : { "translation" : { - "x" : 2.6809873713646084, - "y" : -2.029180925223045 + "x" : 2.7641226388952367, + "y" : -2.071565490563131 }, "rotation" : { - "radians" : -3.0703372176489117 + "radians" : -3.135997290868943 } }, - "curvature" : 0.19613945786907577 + "curvature" : 0.18678355815727837 }, { - "time" : 1.256364374900713, - "velocity" : 2.8872004322742577, - "acceleration" : -2.163128503876278, + "time" : 1.2318683790866636, + "velocity" : 2.9067542256186294, + "acceleration" : -1.2358898220357044, "pose" : { "translation" : { - "x" : 2.5995911144294936, - "y" : -2.035667171419856 + "x" : 2.6810877616808284, + "y" : -2.07269494375214 }, "rotation" : { - "radians" : -3.0535437643496612 + "radians" : -3.1197253477859697 } }, - "curvature" : 0.215681980996833 + "curvature" : 0.20558054021946048 }, { - "time" : 1.2843497892040483, - "velocity" : 2.826664384901926, - "acceleration" : -3.9999999999999942, + "time" : 1.2600559690912383, + "velocity" : 2.8719174700242602, + "acceleration" : -4.000000000000005, "pose" : { "translation" : { - "x" : 2.5200141856037135, - "y" : -2.04340697732448 + "x" : 2.5996822740554615, + "y" : -2.0751801623419794 }, "rotation" : { - "radians" : -3.035430580772711 + "radians" : -3.1021297315977163 } }, - "curvature" : 0.2379825635975536 + "curvature" : 0.2270598313832427 }, { - "time" : 1.3125427756927424, - "velocity" : 2.7138924389471497, - "acceleration" : -4.000000000000006, + "time" : 1.288358606349635, + "velocity" : 2.758706920990673, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.4424346061752615, - "y" : -2.052429014375921 + "x" : 2.5200964603488103, + "y" : -2.0790688311508347 }, "rotation" : { - "radians" : -3.0158761347705023 + "radians" : -3.0830847917195086 } }, - "curvature" : 0.2634035923974734 + "curvature" : 0.25160273261753874 }, { - "time" : 1.3411967497233397, - "velocity" : 2.5992765428247604, + "time" : 1.3171507941983114, + "velocity" : 2.6435381695959674, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.3670167723685154, - "y" : -2.0627568415111455 + "x" : 2.442508372526329, + "y" : -2.0844029187128683 }, "rotation" : { - "radians" : -2.9947525137401847 + "radians" : -3.0624558861984505 } }, - "curvature" : 0.29232743281102963 + "curvature" : 0.27962477885484877 }, { - "time" : 1.3703306161997593, - "velocity" : 2.482741076919082, + "time" : 1.3464486290742, + "velocity" : 2.526346830092413, "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.2939100604434017, - "y" : -2.074408554736916 + "x" : 2.3670824350438124, + "y" : -2.091218220826704 }, "rotation" : { - "radians" : -2.971927682130507 + "radians" : -3.0401007894747756 } }, - "curvature" : 0.325138704477899 + "curvature" : 0.3115646754019423 }, { - "time" : 1.3999769283633923, - "velocity" : 2.36415582826455, + "time" : 1.376280964585666, + "velocity" : 2.4070174880465487, "acceleration" : -4.000000000000006, "pose" : { "translation" : { - "x" : 2.2232474317945616, - "y" : -2.0873964367016242 + "x" : 2.293968049701957, + "y" : -2.0995439041039106 }, "rotation" : { - "radians" : -2.947269113517704 + "radians" : -3.015872245931216 } }, - "curvature" : 0.3621944652381181 + "curvature" : 0.34786330489418216 }, { - "time" : 1.4301864235012918, - "velocity" : 2.2433178477129516, - "acceleration" : -3.8425061187661265, + "time" : 1.4066937018304468, + "velocity" : 2.285366539067425, + "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 2.1551440380505156, - "y" : -2.1017266062671247 + "x" : 2.2232982005009205, + "y" : -2.1094020495174846 }, "rotation" : { - "radians" : -2.9206492753984246 + "radians" : -2.9896221228967343 } }, - "curvature" : 0.40377746810107285 + "curvature" : 0.38892817020202786 }, { - "time" : 1.4609990554339194, - "velocity" : 2.1249201209765416, - "acceleration" : -3.5248780323742457, + "time" : 1.4377557412227517, + "velocity" : 2.161118381498206, + "acceleration" : -3.6964624250106115, "pose" : { "translation" : { - "x" : 2.0896958261728287, - "y" : -2.1173986680805683 + "x" : 2.1551880584948853, + "y" : -2.1208071959503343 }, "rotation" : { - "radians" : -2.8919535310937237 + "radians" : -2.961207726869443 } }, - "curvature" : 0.45002691208795453 + "curvature" : 0.4350774646434125 }, { - "time" : 1.4923979532320715, - "velocity" : 2.0142428358870714, - "acceleration" : -3.197919951626316, + "time" : 1.469492309203223, + "velocity" : 2.043805350459599, + "acceleration" : -3.2130617613339636, "pose" : { "translation" : { - "x" : 2.026978143555276, - "y" : -2.1344053621462358 + "x" : 2.0897335866466165, + "y" : -2.133765883743763 }, "rotation" : { - "radians" : -2.861091073527203 + "radians" : -2.9305009432432754 } }, - "curvature" : 0.5008412010105917 + "curvature" : 0.4864572049110892 }, { - "time" : 1.5243220106712716, - "velocity" : 1.9121522556653892, - "acceleration" : -2.6890710106994757, + "time" : 1.5346845087245022, + "velocity" : 1.8343387870405228, + "acceleration" : -2.5376336019641084, "pose" : { "translation" : { - "x" : 1.967044343123007, - "y" : -2.152732213397371 + "x" : 1.9670710939447247, + "y" : -2.1643273133604453 }, "rotation" : { - "radians" : -2.8280094739823887 + "radians" : -2.8618508313251305 } }, - "curvature" : 0.555749093954951 + "curvature" : 0.603899409578728 }, { - "time" : 1.5892933232552724, - "velocity" : 1.7374397824686587, - "acceleration" : -1.9497522202134734, + "time" : 1.6017399968098842, + "velocity" : 1.6641765272789533, + "acceleration" : -1.8083036583159366, "pose" : { "translation" : { - "x" : 1.855623458766786, - "y" : -2.1932503092648403 + "x" : 1.8556412487423586, + "y" : -2.200961345108226 }, "rotation" : { - "radians" : -2.7552864354535944 + "radians" : -2.783526515109936 } }, - "curvature" : 0.6731381127503112 + "curvature" : 0.7337108127806126 }, { - "time" : 1.655072303925798, - "velocity" : 1.6091870688629224, - "acceleration" : -1.0749318224543505, + "time" : 1.6696662033549567, + "velocity" : 1.5413453194879747, + "acceleration" : -1.1909025235769102, "pose" : { "translation" : { - "x" : 1.7553671009011396, - "y" : -2.2386795374578696 + "x" : 1.7553779586158669, + "y" : -2.2433857961650574 }, "rotation" : { - "radians" : -2.674931532448897 + "radians" : -2.696866534237099 } }, - "curvature" : 0.7847126172556117 + "curvature" : 0.8553105081087353 }, { - "time" : 1.720231550281118, - "velocity" : 1.539145321428446, - "acceleration" : -1.1921125316242216, + "time" : 1.7035990274978792, + "velocity" : 1.500934633584077, + "acceleration" : -0.7132384921286533, "pose" : { "translation" : { - "x" : 1.6657680121716112, - "y" : -2.288608611212112 + "x" : 1.7092936806082193, + "y" : -2.266634670725722 }, "rotation" : { - "radians" : -2.5902033764696597 + "radians" : -2.651456117134893 } }, - "curvature" : 0.8577573547258329 + "curvature" : 0.9019867248099454 }, { - "time" : 1.784455858835107, - "velocity" : 1.4625827183663351, - "acceleration" : -4.000000000000001, + "time" : 1.7371443955145358, + "velocity" : 1.477008785881976, + "acceleration" : -0.14025816644194736, "pose" : { "translation" : { - "x" : 1.5858331132455987, - "y" : -2.342477161522652 + "x" : 1.66577386771678, + "y" : -2.291146687697619 }, "rotation" : { - "radians" : -2.507140034918934 + "radians" : -2.6055827914929948 } }, - "curvature" : 0.8472000723919605 + "curvature" : 0.931445708792421 }, { - "time" : 1.853732644359764, - "velocity" : 1.1854755762677067, - "acceleration" : -3.9999999999999987, + "time" : 1.7700077731011385, + "velocity" : 1.4723994287985898, + "acceleration" : -0.36599934740864887, "pose" : { "translation" : { - "x" : 1.5140388659856399, - "y" : -2.3995645234426775 + "x" : 1.6246788285368083, + "y" : -2.316835860229361 }, "rotation" : { - "radians" : -2.43458289692862 + "radians" : -2.5601885277763987 } }, - "curvature" : 0.7096157054787944 + "curvature" : 0.9372866321152672 }, { - "time" : 1.9414753768133155, - "velocity" : 0.8345046464535002, + "time" : 1.8021746246681412, + "velocity" : 1.460626382116876, "acceleration" : -4.0, "pose" : { "translation" : { - "x" : 1.4482866366226972, - "y" : -2.4589785223821536 + "x" : 1.5858357121177562, + "y" : -2.3436036384127874 }, "rotation" : { - "radians" : -2.383232861630834 + "radians" : -2.5164028420542666 } }, - "curvature" : 0.422926581935277 + "curvature" : 0.9134408086162918 }, { - "time" : 2.1501015384266906, + "time" : 1.871161022380072, + "velocity" : 1.1846807912691524, + "acceleration" : -3.9999999999999996, + "pose" : { + "translation" : { + "x" : 1.5140396751585286, + "y" : -2.399915258109104 + }, + "rotation" : { + "radians" : -2.438909851704161 + } + }, + "curvature" : 0.7583879709418248 + }, { + "time" : 1.9587433944421073, + "velocity" : 0.8343513030210117, + "acceleration" : -4.000000000000001, + "pose" : { + "translation" : { + "x" : 1.4482867427918507, + "y" : -2.4590245412282457 + }, + "rotation" : { + "radians" : -2.384362556563286 + } + }, + "curvature" : 0.4486116146017388 + }, { + "time" : 2.1673312201973602, "velocity" : 0.0, - "acceleration" : -4.0, + "acceleration" : -4.000000000000001, "pose" : { "translation" : { "x" : 1.3858580589294434, @@ -1431,8 +1459,8 @@ "curvature" : 0.0 } ], "pointList" : [ { - "x" : [ 5.218406677246094, -1.5258679389953613, 0.0 ], - "y" : [ -2.2181358337402344, 0.3001227378845215, 0.0 ] + "x" : [ 5.218771457672119, -1.5258679389953613, 0.0 ], + "y" : [ -2.376249313354492, 0.3001227378845215, 0.0 ] }, { "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ], "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ] @@ -1443,7 +1471,7 @@ "radians" : -2.3736477827122884 } }, { - "time" : 2.059999953955412, + "time" : 2.079999953508377, "rotation" : { "radians" : -2.3824463294692526 } diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 360e516d..c523d6cf 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -187,7 +187,7 @@ private void tryToShoot(Translation2d aimToPosition, double targetAngularSpeed, < getAllowedTurnError(aimToPosition.getNorm()) && Math.abs(robotTracker.getLatencyCompedChassisSpeeds().omegaRadiansPerSecond - targetAngularSpeed) < Math.toRadians(8) - && getAccel().getNorm() < 1 + && getAccel().getNorm() < 0.75 && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck) && Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) { //@formatter:on From 603ad81864bbbf835ad20679b5df2e32bb2904cc Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 14:44:29 -0700 Subject: [PATCH 094/108] increase speed of shot practice 31 --- src/main/deploy/shooter/shooterconfig.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index e1eb7984..14ea7943 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -45,11 +45,11 @@ "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2450.0, + "flywheelSpeed" : 2500.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2525.0, + "flywheelSpeed" : 2555.0, "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, From 88275e536884bb84f3ffef2cb64d3a669f92bcb3 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 16:05:31 -0700 Subject: [PATCH 095/108] log acceleration when we shoot --- src/main/java/frc/subsystem/VisionManager.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index c523d6cf..a19b83d0 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -197,8 +197,9 @@ && getAccel().getNorm() < 0.75 lastPrintTime = Timer.getFPGATimestamp(); checksPassedLastTime = true; System.out.println( - "Shooting at " + (150 - DriverStation.getMatchTime()) + " " - + Units.metersToInches(aimToPosition.getNorm())); + "Shooting at " + (150 - DriverStation.getMatchTime()) + "Distance: " + + Units.metersToInches(aimToPosition.getNorm()) + " " + + "Accel: " + getAccel().getNorm()); } } else { lastChecksFailedTime = Timer.getFPGATimestamp(); From bc305603aa05874db272f71df59a6779baea614f Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 16:07:40 -0700 Subject: [PATCH 096/108] bump up some shooter speeds a tiny bit qual 41 --- src/main/deploy/shooter/shooterconfig.json | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 14ea7943..631c5f0a 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -25,15 +25,15 @@ "distance" : 113.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 2020.0, "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2125.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2200.0, + "flywheelSpeed" : 2225.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, @@ -41,7 +41,7 @@ "distance" : 160.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2400.0, + "flywheelSpeed" : 2425.0, "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, From 419a3df7b22ecde7843dac8a73c2ab4e617e8d49 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 20:55:43 -0700 Subject: [PATCH 097/108] attempt to fix issue with autos not running --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/subsystem/Drive.java | 31 +++++++++++++++++--------- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 958c78b3..59af7146 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -374,7 +374,6 @@ public void autonomousInit() { climber.configBrake(); enabled.setBoolean(true); drive.configBrake(); - drive.resetAuto(); networkAutoLock.lock(); try { diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index f7c2c480..5065a27f 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -33,6 +33,7 @@ import frc.utility.wpimodified.HolonomicDriveController; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -52,16 +53,21 @@ public final class Drive extends AbstractSubsystem { final @NotNull NetworkTableEntry turnMaxAcceleration = SmartDashboard.getEntry("TurnMaxAcceleration"); public void resetAuto() { - ProfiledPIDController autoTurnPIDController - = new ProfiledPIDController(8, 0, 0.01, new TrapezoidProfile.Constraints(4, 4)); - autoTurnPIDController.enableContinuousInput(-Math.PI, Math.PI); - autoTurnPIDController.setTolerance(Math.toRadians(10)); - - swerveAutoController = new HolonomicDriveController( - new PIDController(3, 0, 0), - new PIDController(3, 0, 0), - autoTurnPIDController); - swerveAutoController.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune + swerveAutoControllerLock.lock(); + try { + ProfiledPIDController autoTurnPIDController + = new ProfiledPIDController(8, 0, 0.01, new TrapezoidProfile.Constraints(4, 4)); + autoTurnPIDController.enableContinuousInput(-Math.PI, Math.PI); + autoTurnPIDController.setTolerance(Math.toRadians(10)); + + swerveAutoController = new HolonomicDriveController( + new PIDController(3, 0, 0), + new PIDController(3, 0, 0), + autoTurnPIDController); + swerveAutoController.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune + } finally { + swerveAutoControllerLock.unlock(); + } } public enum DriveState { @@ -518,7 +524,8 @@ public double getSpeedSquared() { double autoStartTime; - private @NotNull HolonomicDriveController swerveAutoController; + private final ReentrantLock swerveAutoControllerLock = new ReentrantLock(); + private @Nullable HolonomicDriveController swerveAutoController; public void setAutoPath(Trajectory trajectory) { currentAutoTrajectoryLock.lock(); @@ -557,6 +564,8 @@ private void updateRamsete() { targetHeading = VisionManager.getInstance().getAngleToTarget(); } + if (swerveAutoController == null) resetAuto(); + if (swerveAutoController == null) return; ChassisSpeeds adjustedSpeeds = swerveAutoController.calculate( RobotTracker.getInstance().getLastEstimatedPoseMeters(), goal, From 7b2dc46fbc985b2b7732111f92fd4841e4527cf2 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 20:56:10 -0700 Subject: [PATCH 098/108] increase a shot a little bit last quals 4/2 --- src/main/deploy/shooter/shooterconfig.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 631c5f0a..28a2eb4e 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -45,7 +45,7 @@ "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2500.0, + "flywheelSpeed" : 2525.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, From 0d47082fb6abfb4692f685e6eddeb28d449a2bb8 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 21:19:17 -0700 Subject: [PATCH 099/108] don't try and get instances of singletons when the class is initted --- src/main/java/frc/robot/Robot.java | 57 ++++++++++++------- src/main/java/frc/subsystem/RobotTracker.java | 6 +- .../java/frc/subsystem/VisionManager.java | 35 +++++++++--- 3 files changed, 67 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 59af7146..9da019d0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -174,17 +174,6 @@ public class Robot extends TimedRobot { public static final SendableChooser sideChooser = new SendableChooser<>(); - //Subsystems - private final RobotTracker robotTracker = RobotTracker.getInstance(); - private final Drive drive = Drive.getInstance(); - private final BlinkinLED blinkinLED = BlinkinLED.getInstance(); - private final Limelight limelight = Limelight.getInstance(); - private final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME); - private final Hopper hopper = Hopper.getInstance(); - private final Intake intake = Intake.getInstance(); - private final Shooter shooter = Shooter.getInstance(); - private final Climber climber = Climber.getInstance(); - //Inputs private final static Controller xbox = new Controller(0); private final static Controller stick = new Controller(1); @@ -254,6 +243,10 @@ public static void setRumble(RumbleType type, double value) { */ @Override public void robotInit() { + final RobotTracker robotTracker = RobotTracker.getInstance(); + final Limelight limelight = Limelight.getInstance(); + final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME); + if (autoPath.getString(null) != null) { autoPathListener.accept(new EntryNotification(NetworkTableInstance.getDefault(), 1, 1, "", null, 12)); @@ -370,6 +363,9 @@ public void robotPeriodic() { @Override public void autonomousInit() { + final Drive drive = Drive.getInstance(); + final Climber climber = Climber.getInstance(); + startSubsystems(); climber.configBrake(); enabled.setBoolean(true); @@ -500,6 +496,10 @@ public void autonomousPeriodic() { */ @Override public void teleopInit() { + final Drive drive = Drive.getInstance(); + final Hopper hopper = Hopper.getInstance(); + final Climber climber = Climber.getInstance(); + startSubsystems(); climber.configBrake(); enabled.setBoolean(true); @@ -520,6 +520,13 @@ public void teleopInit() { */ @Override public void teleopPeriodic() { + final RobotTracker robotTracker = RobotTracker.getInstance(); + final Drive drive = Drive.getInstance(); + final Hopper hopper = Hopper.getInstance(); + final Intake intake = Intake.getInstance(); + final Shooter shooter = Shooter.getInstance(); + final Climber climber = Climber.getInstance(); + VisionManager visionManager = VisionManager.getInstance(); xbox.update(); stick.update(); @@ -689,9 +696,9 @@ private boolean isTryingToRunShooterFromButtonPanel() { private static final ControllerDriveInputs NO_MOTION_CONTROLLER_INPUTS = new ControllerDriveInputs(0, 0, 0); private void doNormalDriving() { - ControllerDriveInputs controllerDriveInputs = getControllerDriveInputs(); - + final Drive drive = Drive.getInstance(); + ControllerDriveInputs controllerDriveInputs = getControllerDriveInputs(); if (controllerDriveInputs.getX() == 0 && controllerDriveInputs.getY() == 0 && controllerDriveInputs.getRotation() == 0 && drive.getSpeedSquared() < 0.1) { if (xbox.getRawButton(XboxButtons.Y)) { @@ -734,7 +741,7 @@ private ControllerDriveInputs getControllerDriveInputs() { @Override public void disabledInit() { killAuto(); - drive.configCoast(); + Drive.getInstance().configCoast(); enabled.setBoolean(false); } @@ -750,7 +757,7 @@ public void disabledPeriodic() { */ @Override public void testInit() { - drive.configCoast(); + Drive.getInstance().configCoast(); } /** @@ -762,6 +769,12 @@ public void testPeriodic() { stick.update(); buttonPanel.update(); + final Drive drive = Drive.getInstance(); + final Hopper hopper = Hopper.getInstance(); + final Intake intake = Intake.getInstance(); + final Shooter shooter = Shooter.getInstance(); + final Climber climber = Climber.getInstance(); + // Climber Test Controls if (stick.getRisingEdge(9)) { climber.toggleClaw(); @@ -829,18 +842,20 @@ public void testPeriodic() { private void startSubsystems() { System.out.println("Starting Subsystems"); - robotTracker.start(); - drive.start(); - intake.start(); - hopper.start(); - shooter.start(); - climber.start(); + RobotTracker.getInstance().start(); + Drive.getInstance().start(); + Intake.getInstance().start(); + Hopper.getInstance().start(); + Shooter.getInstance().start(); + Climber.getInstance().start(); VisionManager.getInstance().start(); DashboardHandler.getInstance().start(); } public void killAuto() { System.out.println("Killing Auto"); + final Drive drive = Drive.getInstance(); + if (selectedAuto != null) { assert autoThread != null; System.out.println("2"); diff --git a/src/main/java/frc/subsystem/RobotTracker.java b/src/main/java/frc/subsystem/RobotTracker.java index 999096f6..866fe8d9 100644 --- a/src/main/java/frc/subsystem/RobotTracker.java +++ b/src/main/java/frc/subsystem/RobotTracker.java @@ -29,8 +29,6 @@ public final class RobotTracker extends AbstractSubsystem { private static @NotNull RobotTracker instance = new RobotTracker(); - private final Drive drive = Drive.getInstance(); - public static @NotNull RobotTracker getInstance() { return RobotTracker.instance; } @@ -142,6 +140,7 @@ private void kalmanFilterFailed() { */ @Override public void update() { + final Drive drive = Drive.getInstance(); double time = WPIUtilJNI.now() * 1.0e-6; // seconds Rotation2d rawGyroSensor = gyroSensor.getRotation2d(); @@ -364,6 +363,7 @@ public void resetPosition(Translation2d pose) { public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle) { + final Drive drive = Drive.getInstance(); lock.writeLock().lock(); try { @@ -372,7 +372,7 @@ public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle) { latestEstimatedPose = pose; latestChassisSpeeds = getRotatedSpeeds( - drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()), + Drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()), latestEstimatedPose.getRotation()); latencyCompensatedChassisSpeeds = latestChassisSpeeds; diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index a19b83d0..777ca2c5 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -39,11 +39,7 @@ public final class VisionManager extends AbstractSubsystem { private static final ReentrantReadWriteLock VISION_MANGER_INSTANCE_LOCK = new ReentrantReadWriteLock(); private static VisionManager instance; - private final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); private final @NotNull Limelight limelight = Limelight.getInstance(); - private final @NotNull Drive drive = Drive.getInstance(); - private final @NotNull Shooter shooter = Shooter.getInstance(); - private final @NotNull BlinkinLED blinkinLED = BlinkinLED.getInstance(); public final VisionLookUpTable visionLookUpTable = VisionLookUpTable.getInstance(); @@ -80,6 +76,10 @@ public void selfTest() { @Override public void logData() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); + final @NotNull Drive drive = Drive.getInstance(); + final @NotNull Shooter shooter = Shooter.getInstance(); + logData("Distance to Target", Units.metersToInches(getDistanceToTarget())); logData("Rotation Target", getAngleToTarget().getDegrees()); @@ -137,13 +137,13 @@ public void logData() { public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean useFieldRelative) { + final @NotNull Drive drive = Drive.getInstance(); + final @NotNull Shooter shooter = Shooter.getInstance(); double timeFromLastShoot = Timer.getFPGATimestamp() - shooter.getLastShotTime(); double shooterLookAheadTime = 0.15 - timeFromLastShoot; - boolean justShot = false; if (shooterLookAheadTime < 0) { shooterLookAheadTime = 0.15; - justShot = true; } double turnDelay = 0.0; @@ -169,6 +169,8 @@ public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean us public void autoTurnRobotToTarget(ControllerDriveInputs controllerDriveInputs, boolean fieldRelative) { + final @NotNull Drive drive = Drive.getInstance(); + Optional visionTranslation = getVisionTranslation(); Translation2d relativeGoalPos; if (visionTranslation.isPresent()) { @@ -182,6 +184,10 @@ public void autoTurnRobotToTarget(ControllerDriveInputs controllerDriveInputs, b } private void tryToShoot(Translation2d aimToPosition, double targetAngularSpeed, boolean doSpeedCheck) { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); + final @NotNull Drive drive = Drive.getInstance(); + final @NotNull Shooter shooter = Shooter.getInstance(); + //@formatter:off if (Math.abs((angleOf(aimToPosition).minus(robotTracker.getGyroAngle())).getRadians()) < getAllowedTurnError(aimToPosition.getNorm()) @@ -230,6 +236,8 @@ public void updateShooterState() { * @param distanceToTarget the distance to the target (in meters) */ public void updateShooterState(double distanceToTarget) { + final @NotNull Shooter shooter = Shooter.getInstance(); + logData("Shooter Distance to Target", Units.metersToInches(distanceToTarget)); shooter.set(visionLookUpTable.getShooterPreset(Units.metersToInches(distanceToTarget))); } @@ -239,8 +247,11 @@ public void updateShooterState(double distanceToTarget) { */ @Contract(pure = true) public Translation2d getRobotVel() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); + final @NotNull Drive drive = Drive.getInstance(); + Rotation2d rotation2d = robotTracker.getGyroAngle(); - ChassisSpeeds chassisSpeeds = drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()); + ChassisSpeeds chassisSpeeds = Drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()); return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond).rotateBy(rotation2d); } @@ -270,6 +281,7 @@ public Translation2d getRobotVel() { * @return current relative translation of the robot based on the robot tracker */ private Translation2d getRelativeGoalTranslation() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); return robotTracker.getLatencyCompedPoseMeters().getTranslation() .plus(robotPositionOffset) .minus(Constants.GOAL_POSITION); @@ -281,6 +293,7 @@ private Translation2d getRelativeGoalTranslation() { * You need to call {@link #forceVisionOn(Object)} before calling this method. */ public void forceUpdatePose() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); Optional visionTranslation = getVisionTranslation(); visionTranslation.ifPresent( translation2d -> { @@ -332,6 +345,7 @@ private double getAllowedTurnError(double distance) { @Contract(pure = true) public @NotNull Rotation2d getLatencyCompedLimelightRotation() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); return robotTracker.getGyroRotation(getLimelightTime()); } @@ -387,6 +401,9 @@ public boolean isVisionForcedOn() { @Override public void update() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); + final @NotNull BlinkinLED blinkinLED = BlinkinLED.getInstance(); + robotPositionOffset = new Translation2d(); Translation2d relativeGoalPos = getRelativeGoalTranslation(); @@ -451,6 +468,9 @@ public void update() { */ @SuppressWarnings({"unused", "BusyWait"}) public void shootBalls(double numBalls) throws InterruptedException { + final @NotNull Drive drive = Drive.getInstance(); + final @NotNull Shooter shooter = Shooter.getInstance(); + forceVisionOn(this); if (drive.driveState == DriveState.RAMSETE) { drive.setAutoAiming(true); @@ -573,6 +593,7 @@ private Translation2d getAdjustedTranslation(double predictAheadTime) { private Translation2d getAccel() { + final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); return robotTracker.getAcceleration(); } } From 628119b419477a1054edeef29dc1cbeec8a530e6 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 21:49:19 -0700 Subject: [PATCH 100/108] fix continuity in the tof model --- src/main/java/frc/subsystem/VisionManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 777ca2c5..4543306b 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -567,7 +567,7 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat double distance = Units.metersToInches(translation2d.getNorm()); double timeOfFlightFrames; - if (distance < 120) { + if (distance < 113) { timeOfFlightFrames = ((0.02 / 30) * (distance - 113)) + (22.0 / 30); } else { timeOfFlightFrames = ((0.071 / 30) * (distance - 113)) + (22.0 / 30); From 60e61544667da6868392ac224a0fac1459754a84 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 21:55:08 -0700 Subject: [PATCH 101/108] don't stop the climb twice --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9da019d0..0e8362c9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -651,7 +651,6 @@ public void teleopPeriodic() { if (buttonPanel.getRisingEdge(11)) { climber.stopClimb(); - climber.stopClimb(); } From c32426e3562206ee4114c80a6e5dd0ef7f91b2fa Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sat, 2 Apr 2022 21:59:20 -0700 Subject: [PATCH 102/108] fix formatting for the distance print on shoot --- src/main/java/frc/subsystem/VisionManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 4543306b..48b3a7cb 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -203,7 +203,7 @@ && getAccel().getNorm() < 0.75 lastPrintTime = Timer.getFPGATimestamp(); checksPassedLastTime = true; System.out.println( - "Shooting at " + (150 - DriverStation.getMatchTime()) + "Distance: " + "Shooting at " + (150 - DriverStation.getMatchTime()) + " Distance: " + Units.metersToInches(aimToPosition.getNorm()) + " " + "Accel: " + getAccel().getNorm()); } From c10fbf8537d37c668169677b0fd9edd20115e5fc Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 3 Apr 2022 10:49:46 -0700 Subject: [PATCH 103/108] adjust shooter qual 68 also reduce allowed shooter error --- src/main/deploy/shooter/shooterconfig.json | 8 ++++---- src/main/java/frc/subsystem/VisionManager.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 28a2eb4e..cfe847d7 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -29,19 +29,19 @@ "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2125.0, + "flywheelSpeed" : 2100.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2225.0, + "flywheelSpeed" : 2200.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2350.0, + "flywheelSpeed" : 2370.0, "distance" : 160.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2425.0, + "flywheelSpeed" : 2460.0, "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 48b3a7cb..4aedbcf1 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -340,7 +340,7 @@ private double getAllowedTurnError() { * @return The allowed turn error in radians */ private double getAllowedTurnError(double distance) { - return Math.tan((Constants.GOAL_RADIUS * 0.6) / distance); + return Math.tan((Constants.GOAL_RADIUS * 0.5) / distance); } @Contract(pure = true) From 8370edc7e8a23704ceaf238080af8809ad416a0c Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 3 Apr 2022 10:56:39 -0700 Subject: [PATCH 104/108] double the shot delay for close shots --- src/main/java/frc/subsystem/Shooter.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java index 6362fef2..177be052 100644 --- a/src/main/java/frc/subsystem/Shooter.java +++ b/src/main/java/frc/subsystem/Shooter.java @@ -627,7 +627,8 @@ public void update() { if ( ((feederWheelState == FeederWheelState.FORWARD) && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) - && (Timer.getFPGATimestamp() > SECOND_BALL_SHOOT_DELAY + lastShotTime) + && (Timer.getFPGATimestamp() > (VisionManager.getInstance().getDistanceToTarget() < 80 ? + 0.5 : SECOND_BALL_SHOOT_DELAY) + lastShotTime) || feederChecksDisabled)) ) { feederWheel.set(ControlMode.PercentOutput, FEEDER_WHEEL_SPEED); From f609285441d6f138eda524921bdd8012dfd17624 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 3 Apr 2022 12:34:29 -0700 Subject: [PATCH 105/108] shooter adjustments qual 75 --- src/main/deploy/shooter/shooterconfig.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index cfe847d7..98d0cf1c 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -37,15 +37,15 @@ "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2370.0, + "flywheelSpeed" : 2340.0, "distance" : 160.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2460.0, + "flywheelSpeed" : 2430.0, "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2525.0, + "flywheelSpeed" : 2500.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, From dbbb921b20d50f5a990edd77eec653ab6c36c3cb Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 3 Apr 2022 15:20:01 -0700 Subject: [PATCH 106/108] catch the random npe that occurs --- src/main/java/frc/subsystem/Drive.java | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 5065a27f..ea04327c 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -554,6 +554,7 @@ public void setAutoAiming(boolean autoAiming) { } + double nextAllowedPrintError = 0; private void updateRamsete() { currentAutoTrajectoryLock.lock(); try { @@ -566,15 +567,21 @@ private void updateRamsete() { if (swerveAutoController == null) resetAuto(); if (swerveAutoController == null) return; - ChassisSpeeds adjustedSpeeds = swerveAutoController.calculate( - RobotTracker.getInstance().getLastEstimatedPoseMeters(), - goal, - targetHeading); - - swerveDrive(adjustedSpeeds); - if (swerveAutoController.atReference() && (Timer.getFPGATimestamp() - autoStartTime) >= currentAutoTrajectory.getTotalTimeSeconds()) { - setDriveState(DriveState.DONE); - stopMovement(); + try { + ChassisSpeeds adjustedSpeeds = swerveAutoController.calculate( + RobotTracker.getInstance().getLastEstimatedPoseMeters(), + goal, + targetHeading); + swerveDrive(adjustedSpeeds); + if (swerveAutoController.atReference() && (Timer.getFPGATimestamp() - autoStartTime) >= currentAutoTrajectory.getTotalTimeSeconds()) { + setDriveState(DriveState.DONE); + stopMovement(); + } + } catch (NullPointerException exception) { + if (Timer.getFPGATimestamp() > nextAllowedPrintError) { + exception.printStackTrace(); + nextAllowedPrintError = Timer.getFPGATimestamp() + 2; + } } } finally { currentAutoTrajectoryLock.unlock(); From e2130bdda79d3dee5664c74dbc2d2ae1c699e4d7 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 3 Apr 2022 15:20:26 -0700 Subject: [PATCH 107/108] reduce fender shot quarter 1 --- src/main/deploy/shooter/shooterconfig.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 98d0cf1c..08eb560e 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -1,7 +1,7 @@ { "shooterConfigs" : [ { "hoodEjectAngle" : 70.0, - "flywheelSpeed" : 1950.0, + "flywheelSpeed" : 1850.0, "distance" : 45.0 }, { "hoodEjectAngle" : 62.0, From 616e110857db84f8ca8848a3dee6e7cf6f99d08b Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 4 Apr 2022 13:39:23 -0700 Subject: [PATCH 108/108] revert comp changes for the shooter --- src/main/deploy/shooter/shooterconfig.json | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 08eb560e..69ec53f0 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -1,7 +1,7 @@ { "shooterConfigs" : [ { "hoodEjectAngle" : 70.0, - "flywheelSpeed" : 1850.0, + "flywheelSpeed" : 1800.0, "distance" : 45.0 }, { "hoodEjectAngle" : 62.0, @@ -21,43 +21,43 @@ "distance" : 109.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 1950.0, + "flywheelSpeed" : 1900.0, "distance" : 113.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2020.0, + "flywheelSpeed" : 1900.0, "distance" : 124.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2000.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2200.0, + "flywheelSpeed" : 2100.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2340.0, + "flywheelSpeed" : 2200.0, "distance" : 160.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2430.0, + "flywheelSpeed" : 2300.0, "distance" : 173.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2500.0, + "flywheelSpeed" : 2400.0, "distance" : 181.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2555.0, + "flywheelSpeed" : 2400.0, "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2600.0, + "flywheelSpeed" : 2450.0, "distance" : 202.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2650.0, + "flywheelSpeed" : 2550.0, "distance" : 224.0 }, { "hoodEjectAngle" : 51.0, @@ -65,11 +65,11 @@ "distance" : 241.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2750.0, + "flywheelSpeed" : 3000.0, "distance" : 265.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3000.0, + "flywheelSpeed" : 3200.0, "distance" : 300.0 } ] } \ No newline at end of file