From 1a766c8ea3ff8e3fadfb1c212944258a1b73af86 Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Fri, 1 Feb 2019 19:27:47 -0800 Subject: [PATCH 01/10] First Draft of robot code --- .classpath | 8 +- .../com/spartronics4915/frc2019/Robot.java | 1 + .../frc2019/subsystems/LED.java | 107 ++++++++++++------ 3 files changed, 77 insertions(+), 39 deletions(-) diff --git a/.classpath b/.classpath index 86166cc..47a8ba9 100644 --- a/.classpath +++ b/.classpath @@ -12,8 +12,12 @@ - + - + + + + + diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index c2fb4e3..f22d965 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/src/main/java/com/spartronics4915/frc2019/Robot.java @@ -330,6 +330,7 @@ public void teleopPeriodic() if (mControlBoard.getReverseDirection()) { mSuperstructure.reverseDrivingDirection(); + mLED.setDriveState(); } else if (mControlBoard.getDriveToSelectedTarget()) { diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java index 675d6aa..1280db5 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java @@ -3,9 +3,14 @@ import com.spartronics4915.lib.util.ILoop; import com.spartronics4915.lib.util.ILooper; +import edu.wpi.first.wpilibj.SerialPort; + + public class LED extends Subsystem { + private SerialPort mBling; + private static LED mInstance = null; public static LED getInstance() @@ -17,25 +22,35 @@ public static LED getInstance() return mInstance; } - public enum WantedState + // private enum SubsytemLEDState + // { + // CLIMBING, + // } + + private final byte[] kForwards = "2".getBytes(); + private final byte[] kBackwards = "3".getBytes(); + private final byte[] kOff = "4".getBytes(); + private final byte[] kAutonomous = "5".getBytes(); + + private enum LEDState { - CLOSED, INTAKE, + DISABLING, UPDATING, } - private enum SystemState + public enum DriveLEDState { - CLOSING, INTAKING, + FORWARDS, BACKWARDS, AUTONOMOUS, DISABLED } - private WantedState mWantedState = WantedState.CLOSED; - private SystemState mSystemState = SystemState.CLOSING; + private LEDState mLEDState = LEDState.DISABLING; + private DriveLEDState mDriveState = DriveLEDState.DISABLED; - private LED() + public LED() { boolean success = true; try { - // Instantiate your hardware here + mBling = new SerialPort(9600, SerialPort.Port.kUSB); } catch (Exception e) { @@ -54,8 +69,7 @@ public void onStart(double timestamp) { synchronized (LED.this) { - mWantedState = WantedState.CLOSED; - mSystemState = SystemState.CLOSING; + mDriveState = DriveLEDState.DISABLED; } } @@ -64,18 +78,18 @@ public void onLoop(double timestamp) { synchronized (LED.this) { - SystemState newState = defaultStateTransfer(); - switch (mSystemState) - { - case INTAKING: - break; - case CLOSING: - stop(); - break; - default: - logError("Unhandled system state!"); - } - mSystemState = newState; + // SystemState newState = defaultStateTransfer(); + // switch (mSystemState) + // { + // case INTAKING: + // break; + // case CLOSING: + // stop(); + // break; + // default: + // logError("Unhandled system state!"); + // } + // mSystemState = newState; } } @@ -89,27 +103,46 @@ public void onStop(double timestamp) } }; - private SystemState defaultStateTransfer() + public synchronized void setDriveState(DriveLEDState driveState) { - SystemState newState = mSystemState; - switch (mWantedState) + mDriveState = driveState; + mLEDState = LEDState.UPDATING; + } + + public synchronized void setDriveState() + { + if (mDriveState == DriveLEDState.FORWARDS) + { + mDriveState = DriveLEDState.BACKWARDS; + mLEDState = LEDState.UPDATING; + } else if (mDriveState == DriveLEDState.BACKWARDS) { - case CLOSED: - newState = SystemState.CLOSING; - break; - case INTAKE: - newState = SystemState.INTAKING; - break; - default: - newState = SystemState.CLOSING; - break; + mDriveState = DriveLEDState.FORWARDS; + mLEDState = LEDState.UPDATING; } - return newState; } - public synchronized void setWantedState(WantedState wantedState) + public synchronized void updateBling() { - mWantedState = wantedState; + if (mLEDState == LEDState.UPDATING) + { + switch (mDriveState) + { + case FORWARDS: + mBling.write(kForwards, kForwards.length); + break; + case BACKWARDS: + mBling.write(kBackwards, kBackwards.length); + break; + case AUTONOMOUS: + mBling.write(kAutonomous, kAutonomous.length); + break; + default: + mBling.write(kOff, kOff.length); + break; + } + mLEDState = LEDState.DISABLING; + } } @Override From 89b29265fff9c5293881653c16ba54f23b91a46e Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Sat, 2 Feb 2019 13:28:46 -0800 Subject: [PATCH 02/10] Fixed mistakes and made DriveLEDState more concise. --- .../frc2019/subsystems/LED.java | 96 ++++--------------- 1 file changed, 21 insertions(+), 75 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java index 1280db5..c014264 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java @@ -1,6 +1,5 @@ package com.spartronics4915.frc2019.subsystems; -import com.spartronics4915.lib.util.ILoop; import com.spartronics4915.lib.util.ILooper; import edu.wpi.first.wpilibj.SerialPort; @@ -9,7 +8,7 @@ public class LED extends Subsystem { - private SerialPort mBling; + private SerialPort mSerialPort; private static LED mInstance = null; @@ -22,24 +21,25 @@ public static LED getInstance() return mInstance; } - // private enum SubsytemLEDState - // { - // CLIMBING, - // } - - private final byte[] kForwards = "2".getBytes(); - private final byte[] kBackwards = "3".getBytes(); - private final byte[] kOff = "4".getBytes(); - private final byte[] kAutonomous = "5".getBytes(); - private enum LEDState { - DISABLING, UPDATING, + DISABLING, UPDATING, OFF, } public enum DriveLEDState { - FORWARDS, BACKWARDS, AUTONOMOUS, DISABLED + OFF("0".getBytes()), + FORWARDS("1".getBytes()), + BACKWARDS("2".getBytes()), + AUTONOMOUS("3".getBytes()), + DISABLED("4".getBytes()); // Add the remainder of your states + + public final byte[] serialSignal; + + private DriveLEDState(byte[] serialSignal) + { + this.serialSignal = serialSignal; + } } private LEDState mLEDState = LEDState.DISABLING; @@ -50,7 +50,7 @@ public LED() boolean success = true; try { - mBling = new SerialPort(9600, SerialPort.Port.kUSB); + mSerialPort = new SerialPort(9600, SerialPort.Port.kUSB); } catch (Exception e) { @@ -61,48 +61,6 @@ public LED() logInitialized(success); } - private final ILoop mLoop = new ILoop() - { - - @Override - public void onStart(double timestamp) - { - synchronized (LED.this) - { - mDriveState = DriveLEDState.DISABLED; - } - } - - @Override - public void onLoop(double timestamp) - { - synchronized (LED.this) - { - // SystemState newState = defaultStateTransfer(); - // switch (mSystemState) - // { - // case INTAKING: - // break; - // case CLOSING: - // stop(); - // break; - // default: - // logError("Unhandled system state!"); - // } - // mSystemState = newState; - } - } - - @Override - public void onStop(double timestamp) - { - synchronized (LED.this) - { - stop(); - } - } - }; - public synchronized void setDriveState(DriveLEDState driveState) { mDriveState = driveState; @@ -126,21 +84,7 @@ public synchronized void updateBling() { if (mLEDState == LEDState.UPDATING) { - switch (mDriveState) - { - case FORWARDS: - mBling.write(kForwards, kForwards.length); - break; - case BACKWARDS: - mBling.write(kBackwards, kBackwards.length); - break; - case AUTONOMOUS: - mBling.write(kAutonomous, kAutonomous.length); - break; - default: - mBling.write(kOff, kOff.length); - break; - } + mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length); mLEDState = LEDState.DISABLING; } } @@ -148,13 +92,13 @@ public synchronized void updateBling() @Override public void registerEnabledLoops(ILooper enabledLooper) { - enabledLooper.register(mLoop); + } @Override public boolean checkSystem(String variant) { - return false; + return true; } @Override @@ -166,6 +110,8 @@ public void outputTelemetry() @Override public void stop() { - // Stop your hardware here + mDriveState = DriveLEDState.OFF; + mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length); + mLEDState = LEDState.OFF; } } From 1412bc124fdd5311522c0b398d0e655d98c31263 Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Sat, 2 Feb 2019 13:35:37 -0800 Subject: [PATCH 03/10] Combined setDriveState and updateSerial --- .../com/spartronics4915/frc2019/Robot.java | 2 +- .../frc2019/subsystems/LED.java | 28 +++---------------- 2 files changed, 5 insertions(+), 25 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index f22d965..60980b2 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/src/main/java/com/spartronics4915/frc2019/Robot.java @@ -330,7 +330,7 @@ public void teleopPeriodic() if (mControlBoard.getReverseDirection()) { mSuperstructure.reverseDrivingDirection(); - mLED.setDriveState(); + mLED.setDriveState(mSuperstructure.isDrivingReversed() ? LED.DriveLEDState.BACKWARDS : LED.DriveLEDState.FORWARDS); } else if (mControlBoard.getDriveToSelectedTarget()) { diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java index c014264..fb64625 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java @@ -23,7 +23,7 @@ public static LED getInstance() private enum LEDState { - DISABLING, UPDATING, OFF, + ON, OFF, } public enum DriveLEDState @@ -42,7 +42,7 @@ private DriveLEDState(byte[] serialSignal) } } - private LEDState mLEDState = LEDState.DISABLING; + private LEDState mLEDState = LEDState.ON; private DriveLEDState mDriveState = DriveLEDState.DISABLED; public LED() @@ -63,29 +63,9 @@ public LED() public synchronized void setDriveState(DriveLEDState driveState) { - mDriveState = driveState; - mLEDState = LEDState.UPDATING; - } - - public synchronized void setDriveState() - { - if (mDriveState == DriveLEDState.FORWARDS) - { - mDriveState = DriveLEDState.BACKWARDS; - mLEDState = LEDState.UPDATING; - } else if (mDriveState == DriveLEDState.BACKWARDS) - { - mDriveState = DriveLEDState.FORWARDS; - mLEDState = LEDState.UPDATING; - } - } - - public synchronized void updateBling() - { - if (mLEDState == LEDState.UPDATING) + if (mLEDState == LEDState.ON) { - mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length); - mLEDState = LEDState.DISABLING; + mSerialPort.write(driveState.serialSignal, driveState.serialSignal.length); } } From 1389228048553fb4b55e39c6a0fd5ceb99ab344d Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Tue, 5 Feb 2019 15:27:59 -0800 Subject: [PATCH 04/10] First commit of 2/5/29. Just making sure everything is ok. --- .../com/spartronics4915/frc2019/Robot.java | 2 ++ .../frc2019/subsystems/LED.java | 18 ++++++------------ 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index 9ef1377..9b9b654 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/src/main/java/com/spartronics4915/frc2019/Robot.java @@ -99,6 +99,8 @@ public void robotInit() mLED = LED.getInstance(); mSuperstructure = Superstructure.getInstance(); + mLED.setDriveState(LED.DriveLEDState.BACKWARDS); + mSubsystemManager = new SubsystemManager( Arrays.asList( RobotStateEstimator.getInstance(), diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java index fb64625..85caa70 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java @@ -21,11 +21,6 @@ public static LED getInstance() return mInstance; } - private enum LEDState - { - ON, OFF, - } - public enum DriveLEDState { OFF("0".getBytes()), @@ -42,7 +37,6 @@ private DriveLEDState(byte[] serialSignal) } } - private LEDState mLEDState = LEDState.ON; private DriveLEDState mDriveState = DriveLEDState.DISABLED; public LED() @@ -63,10 +57,10 @@ public LED() public synchronized void setDriveState(DriveLEDState driveState) { - if (mLEDState == LEDState.ON) - { - mSerialPort.write(driveState.serialSignal, driveState.serialSignal.length); - } + mDriveState = driveState; + dashboardPutNumber("Byte yeet", mDriveState.serialSignal.length); + dashboardPutNumber("Bytes sent", mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length)); + mSerialPort.flush(); } @Override @@ -84,7 +78,7 @@ public boolean checkSystem(String variant) @Override public void outputTelemetry() { - + dashboardPutState(mDriveState.toString()); } @Override @@ -92,6 +86,6 @@ public void stop() { mDriveState = DriveLEDState.OFF; mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length); - mLEDState = LEDState.OFF; + mSerialPort.close(); } } From a369a17127f348edef4aaae5a606f3b7bd8ea56f Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Wed, 6 Feb 2019 20:27:07 -0800 Subject: [PATCH 05/10] IT WORKS!!!!1!!! --- .../com/spartronics4915/frc2019/Robot.java | 14 ++++++-- .../frc2019/subsystems/LED.java | 36 ++++++++++++------- 2 files changed, 34 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index 9b9b654..5f97132 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/src/main/java/com/spartronics4915/frc2019/Robot.java @@ -98,8 +98,7 @@ public void robotInit() mClimber = Climber.getInstance(); mLED = LED.getInstance(); mSuperstructure = Superstructure.getInstance(); - - mLED.setDriveState(LED.DriveLEDState.BACKWARDS); + mLED.setDriveState(LED.DriveLEDState.FORWARDS); mSubsystemManager = new SubsystemManager( Arrays.asList( @@ -109,7 +108,7 @@ public void robotInit() mCargoChute, mCargoIntake, mClimber, - mLED, + // mLED, mSuperstructure)); mSubsystemManager.registerEnabledLoops(mEnabledLooper); mSubsystemManager.registerDisabledLoops(mDisabledLooper); @@ -203,6 +202,7 @@ public void teleopInit() { Logger.logTeleopInit(); Logger.setVerbosity(SmartDashboard.getString(kRobotLogVerbosity, "NOTICE")); + mLED.setDriveState(LED.DriveLEDState.BACKWARDS); mDisabledLooper.stop(); if (mAutoModeExecutor != null) @@ -329,6 +329,14 @@ public void teleopPeriodic() // command.scale(Constants.kDriveLeftKv).getRight() + Math.copySign(Constants.kDriveLeftVIntercept, command.getRight()) // )); + if(mControlBoard.getTestButtonOne()) + { + mLED.setDriveState(mSuperstructure.isDrivingReversed() ? LED.DriveLEDState.BACKWARDS : LED.DriveLEDState.FORWARDS); + } + else { + mLED.setDriveState(mSuperstructure.isDrivingReversed() ? LED.DriveLEDState.BACKWARDS : LED.DriveLEDState.BACKWARDS); + } + if (mControlBoard.getReverseDirection()) { mSuperstructure.reverseDrivingDirection(); diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java index 85caa70..afc684e 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java @@ -1,5 +1,9 @@ package com.spartronics4915.frc2019.subsystems; +import java.io.FileOutputStream; +import java.io.IOException; +import java.util.Arrays; + import com.spartronics4915.lib.util.ILooper; import edu.wpi.first.wpilibj.SerialPort; @@ -8,7 +12,8 @@ public class LED extends Subsystem { - private SerialPort mSerialPort; + // private SerialPort mSerialPort; + private FileOutputStream mSerialPort; private static LED mInstance = null; @@ -44,7 +49,8 @@ public LED() boolean success = true; try { - mSerialPort = new SerialPort(9600, SerialPort.Port.kUSB); + // mSerialPort = new SerialPort(9600, SerialPort.Port.kUSB); + mSerialPort = new FileOutputStream("/dev/ttyACM0"); } catch (Exception e) { @@ -57,17 +63,25 @@ public LED() public synchronized void setDriveState(DriveLEDState driveState) { - mDriveState = driveState; - dashboardPutNumber("Byte yeet", mDriveState.serialSignal.length); + /** mDriveState = driveState; + dashboardPutString("Byte value", Arrays.toString(mDriveState.serialSignal)); dashboardPutNumber("Bytes sent", mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length)); + // dashboardPutNumber("Bytes sent", mSerialPort.write(new byte[] {'2'}, 1)); mSerialPort.flush(); - } + dashboardPutNumber("Bytes in response", mSerialPort.getBytesReceived());*/ + try + { + mSerialPort.write(driveState.serialSignal); + } + catch(IOException e) + { + logException("We didn't think this would work anyways, but we did hope", e); + } + } @Override public void registerEnabledLoops(ILooper enabledLooper) - { - - } + {} @Override public boolean checkSystem(String variant) @@ -83,9 +97,5 @@ public void outputTelemetry() @Override public void stop() - { - mDriveState = DriveLEDState.OFF; - mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length); - mSerialPort.close(); - } + {} } From 9904e5162c1ecad0dd231d0046e148210d281eb0 Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Sat, 16 Feb 2019 13:55:08 -0800 Subject: [PATCH 06/10] YEET --- .../com/spartronics4915/frc2019/Robot.java | 7 +++-- .../frc2019/subsystems/LED.java | 26 +++++++------------ 2 files changed, 13 insertions(+), 20 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index dba984a..d57d5a4 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/src/main/java/com/spartronics4915/frc2019/Robot.java @@ -102,11 +102,10 @@ public void robotInit() mClimber = Climber.getInstance(); mLED = LED.getInstance(); mSuperstructure = Superstructure.getInstance(); -<<<<<<< HEAD - mLED.setDriveState(LED.DriveLEDState.FORWARDS); -======= mRobotStateEstimator = RobotStateEstimator.getInstance(); ->>>>>>> e5baee087f9c7036cfc43028538c59d5de658bb7 + + mLED.setDriveState(LED.DriveLEDState.FORWARDS); + mSubsystemManager = new SubsystemManager( Arrays.asList( diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java index afc684e..4ee076e 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java @@ -50,7 +50,7 @@ public LED() try { // mSerialPort = new SerialPort(9600, SerialPort.Port.kUSB); - mSerialPort = new FileOutputStream("/dev/ttyACM0"); + //mSerialPort = new FileOutputStream("/dev/ttyACM0"); } catch (Exception e) { @@ -63,21 +63,15 @@ public LED() public synchronized void setDriveState(DriveLEDState driveState) { - /** mDriveState = driveState; - dashboardPutString("Byte value", Arrays.toString(mDriveState.serialSignal)); - dashboardPutNumber("Bytes sent", mSerialPort.write(mDriveState.serialSignal, mDriveState.serialSignal.length)); - // dashboardPutNumber("Bytes sent", mSerialPort.write(new byte[] {'2'}, 1)); - mSerialPort.flush(); - dashboardPutNumber("Bytes in response", mSerialPort.getBytesReceived());*/ - try - { - mSerialPort.write(driveState.serialSignal); - } - catch(IOException e) - { - logException("We didn't think this would work anyways, but we did hope", e); - } - } + // try + // { + // mSerialPort.write(driveState.serialSignal); + // } + // catch(IOException e) + // { + // logException("We didn't think this would work anyways, but we did hope", e); + // } + } @Override public void registerEnabledLoops(ILooper enabledLooper) From 9b80f9c1c83ac19d9b8c192ead12bfe74207cf6f Mon Sep 17 00:00:00 2001 From: Declan Freeman-Gleason Date: Sun, 17 Feb 2019 18:06:00 -0800 Subject: [PATCH 07/10] Begin working on adding a camera offset --- .../frc2019/VisionUpdateManager.java | 14 ++++++++------ .../frc2019/subsystems/Superstructure.java | 2 +- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java index 82a94d5..4657ce6 100644 --- a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java +++ b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java @@ -16,18 +16,20 @@ public class VisionUpdateManager { - public static VisionUpdateManager reverseVisionManager = new VisionUpdateManager("Reverse"); + public static VisionUpdateManager reverseVisionManager = new VisionUpdateManager("Reverse", new Pose2d(-10, 0, Rotation2d.fromDegrees(180))); private static final int kRawUpdateNumDoubles = 4; // 2 for x y, 1 for rotation, and 1 for processing time - private final String kNetworkTablesKey; + private final String mNetworkTablesKey; + private final Pose2d mCameraOffset; private VisionUpdate mLatestVisionUpdate = null; - private VisionUpdateManager(String coprocessorID) + private VisionUpdateManager(String coprocessorID, Pose2d cameraOffset) { - kNetworkTablesKey = "/SmartDashboard/Vision/" + coprocessorID + "/solvePNP"; + mNetworkTablesKey = "/SmartDashboard/Vision/" + coprocessorID + "/solvePNP"; + mCameraOffset = cameraOffset; - NetworkTableInstance.getDefault().addEntryListener(kNetworkTablesKey, (e) -> visionKeyChangedCallback(e), + NetworkTableInstance.getDefault().addEntryListener(mNetworkTablesKey, (e) -> visionKeyChangedCallback(e), EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); } @@ -82,7 +84,7 @@ public static VisionUpdate fromRawUpdate(String vu) public Pose2d getFieldPosition(RobotStateMap stateMap) { - return stateMap.getFieldToVehicle(this.frameCapturedTime).transformBy(targetRobotRelativePosition); + return stateMap.getFieldToVehicle(this.frameCapturedTime).transformBy(mCameraOffset).transformBy(targetRobotRelativePosition); } public Pose2d getCorrectedRobotPose(ScorableLandmark landmark, RobotStateMap stateMap, double timeToGetAt) diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java b/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java index 7818101..f956633 100755 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java @@ -343,7 +343,7 @@ private void makeAndDrivePath(Pose2d goalPose, boolean reversed) double startTime = Timer.getFPGATimestamp(); TrajectoryIterator> t = new TrajectoryIterator<>((new TimedView<>((mTrajectoryGenerator.generateTrajectory(reversed, waypoints))))); - // TODO: Maybe plug in our current velocity as the start veloicty of the path? + // TODO: Maybe plug in our current velocity as the start velocity of the path? Logger.notice("Path generated; took " + (Timer.getFPGATimestamp() - startTime) + " seconds."); mDrive.setTrajectory(t); From ccc6f757479f880c60431b7398abb8dd810b86e7 Mon Sep 17 00:00:00 2001 From: Declan Freeman-Gleason Date: Mon, 18 Feb 2019 07:20:47 -0800 Subject: [PATCH 08/10] Add camera offset to VisionCameraManager --- .../frc2019/VisionUpdateManager.java | 21 ++++++++++++------- .../frc2019/subsystems/Superstructure.java | 7 ++++--- 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java index 4657ce6..0b9408f 100644 --- a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java +++ b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java @@ -16,6 +16,7 @@ public class VisionUpdateManager { + public static VisionUpdateManager reverseVisionManager = new VisionUpdateManager("Reverse", new Pose2d(-10, 0, Rotation2d.fromDegrees(180))); private static final int kRawUpdateNumDoubles = 4; // 2 for x y, 1 for rotation, and 1 for processing time @@ -38,7 +39,7 @@ private void visionKeyChangedCallback(EntryNotification entryNotification) try { String rawVisionUpdate = entryNotification.value.getString(); - mLatestVisionUpdate = VisionUpdate.fromRawUpdate(rawVisionUpdate); + mLatestVisionUpdate = VisionUpdate.fromRawUpdate(rawVisionUpdate, mCameraOffset); } catch (Exception e) { @@ -49,7 +50,7 @@ private void visionKeyChangedCallback(EntryNotification entryNotification) } /** - * @return the latest vision update _or_ null if there has not been an update yet + * @return either empty or contains the latest vision update */ public Optional getLatestVisionUpdate() { @@ -62,13 +63,17 @@ public static class VisionUpdate public final double frameCapturedTime; // Time in seconds where the epoch the boot of the RoboRIO (getFPGATimestamp's epoch) public final Pose2d targetRobotRelativePosition; // The target's robot-relative position at frameCapturedTime (x and y in inches) - private VisionUpdate(double capturedTime, Pose2d targetRelativePosition) + private final Pose2d mCameraOffset; + + private VisionUpdate(double capturedTime, Pose2d targetRelativePosition, Pose2d cameraOffset) { this.frameCapturedTime = capturedTime; this.targetRobotRelativePosition = targetRelativePosition; + + mCameraOffset = cameraOffset; } - public static VisionUpdate fromRawUpdate(String vu) + public static VisionUpdate fromRawUpdate(String vu, Pose2d cameraOffset) { Double[] rawVisionUpdate = Arrays.stream(vu.split(",")).map(Double::parseDouble).toArray(Double[]::new); @@ -79,7 +84,7 @@ public static VisionUpdate fromRawUpdate(String vu) double frameCapTime = 0;//Timer.getFPGATimestamp() - rawVisionUpdate[3]; TODO Pose2d targetRelativePosition = new Pose2d(rawVisionUpdate[0], rawVisionUpdate[1], Rotation2d.fromDegrees(rawVisionUpdate[2])); - return new VisionUpdate(frameCapTime, targetRelativePosition); + return new VisionUpdate(frameCapTime, targetRelativePosition, cameraOffset); } public Pose2d getFieldPosition(RobotStateMap stateMap) @@ -89,7 +94,8 @@ public Pose2d getFieldPosition(RobotStateMap stateMap) public Pose2d getCorrectedRobotPose(ScorableLandmark landmark, RobotStateMap stateMap, double timeToGetAt) { - Pose2d robotPoseRelativeToLastVisionUpdate = stateMap.get(this.frameCapturedTime).pose.inverse().transformBy(stateMap.get(timeToGetAt).pose); + Pose2d robotPoseRelativeToLastVisionUpdate = + stateMap.get(this.frameCapturedTime).pose.transformBy(mCameraOffset).inverse().transformBy(stateMap.get(timeToGetAt).pose); return this.targetRobotRelativePosition.inverse().transformBy(landmark.fieldPose).transformBy(robotPoseRelativeToLastVisionUpdate); } @@ -109,7 +115,8 @@ public Pose2d getCorrectedRobotPoseForClosestTarget(RobotStateMap stateMap, doub } } - if (closestTargetPose == null) throw new RuntimeException("No vision targets are close! Is Constants.kVisionTargetLocations empty?"); + if (closestTargetPose == null) + throw new RuntimeException("No vision targets are close! Is Constants.kVisionTargetLocations empty?"); return getCorrectedRobotPose(closestTargetPose, stateMap, timeToGetAt); } diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java b/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java index f956633..a10d1f9 100755 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java @@ -261,10 +261,11 @@ else if (mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_BAY newState = SystemState.EJECTING_PANEL; break; case EJECTING_PANEL: - mCargoChute.setWantedState(CargoChute.WantedState.LOWER); - - if (mCargoChute.atTarget()) + if (mStateChanged) + { + mCargoChute.setWantedState(CargoChute.WantedState.LOWER); mPanelHandler.setWantedState(PanelHandler.WantedState.EJECT); + } if (mWantedState == WantedState.ALIGN_AND_EJECT_PANEL && mStateChangedTimer.hasPeriodPassed(kPanelHandlingDuration) && mCargoChute.atTarget() && mPanelHandler.atTarget()) From ea559534aa18bcedb9e6199cb12d0e6c4a1901fe Mon Sep 17 00:00:00 2001 From: dbadb Date: Mon, 18 Feb 2019 12:12:01 -0800 Subject: [PATCH 09/10] Migrate back to number representation for PNP results. --- .../frc2019/VisionUpdateManager.java | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java index b19a6d6..987f9ba 100644 --- a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java +++ b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java @@ -38,7 +38,7 @@ private void visionKeyChangedCallback(EntryNotification entryNotification) { try { - String rawVisionUpdate = entryNotification.value.getString(); + double[] rawVisionUpdate = entryNotification.value.getDoubleArray(); mLatestVisionUpdate = VisionUpdate.fromRawUpdate(rawVisionUpdate, mCameraOffset); } catch (Exception e) @@ -62,9 +62,15 @@ public static class VisionUpdate public final double frameCapturedTime; // Time in seconds where the epoch the boot of the RoboRIO (getFPGATimestamp's epoch) public final Pose2d[] targetRobotRelativePositions; // The target's robot-relative position at frameCapturedTime (x and y in inches) - private final Pose2d mCameraOffset; + private VisionUpdate() + { + frameCapturedTime = 0; + mCameraOffset = null; + targetRobotRelativePositions = null; + } + private VisionUpdate(double capturedTime, Pose2d cameraOffset, Pose2d... targetRelativePositions) { @@ -74,30 +80,24 @@ private VisionUpdate(double capturedTime, Pose2d cameraOffset, mCameraOffset = cameraOffset; } - public static VisionUpdate fromRawUpdate(String vu, Pose2d cameraOffset) + public static VisionUpdate fromRawUpdate(double[] values, Pose2d cameraOffset) { - String[] fields = vu.split(";"); // expect 2 or 3 fields - int ntargets = fields.length - 1; + // a target is 3 numbers, we also expect one time, so + // the valid lengths are 1, 4, 7 => 0, 1, 2 targets + int len = values.length; + int ntargets = (len == 7) ? 2 : (len == 4) ? 1 : 0; if (ntargets <= 0) - throw new RuntimeException("A vision update must have at least one target"); + { + Logger.warning("A vision update must have at least one target"); + return new VisionUpdate(); + } // last field is timestamp - double frameCapTime = Double.parseDouble(fields[ntargets]); - + double frameCapTime = values[len - 1]; Pose2d[] targets = new Pose2d[ntargets]; - for (int i = 0; i < ntargets; i++) + for (int i = 0, j=0; i < ntargets; i++, j+=3) { - Double[] targetNumbers = Arrays.stream(fields[i].split(",")).map(Double::parseDouble).toArray(Double[]::new); - if (targetNumbers.length < kRawUpdateNumDoubles) - { - targets[i] = new Pose2d(targetNumbers[0], targetNumbers[1], Rotation2d.fromDegrees(targetNumbers[2])); - } - else - { - throw new RuntimeException("A vision update must have at least " + - kRawUpdateNumDoubles + " doubles in the array. This one has " + - targetNumbers.length + "."); - } + targets[i] = new Pose2d(values[j+0], values[j+1], Rotation2d.fromDegrees(values[j+2])); } return new VisionUpdate(frameCapTime, cameraOffset, targets); } From b7207f6018f96226326c634df22cb9675f6027b5 Mon Sep 17 00:00:00 2001 From: Camden Greenhalgh Date: Mon, 18 Feb 2019 13:15:41 -0800 Subject: [PATCH 10/10] First commit of 2/18/2019 --- .../com/spartronics4915/frc2019/Robot.java | 5 - .../frc2019/subsystems/LED.java | 95 ------------------- 2 files changed, 100 deletions(-) delete mode 100644 src/main/java/com/spartronics4915/frc2019/subsystems/LED.java diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index be0d2db..f7ad6e4 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/src/main/java/com/spartronics4915/frc2019/Robot.java @@ -35,7 +35,6 @@ public class Robot extends TimedRobot private CargoChute mCargoChute = null; private CargoIntake mCargoIntake = null; private Climber mClimber = null; - private LED mLED = null; private RobotStateEstimator mRobotStateEstimator = null; private Superstructure mSuperstructure = null; private AutoModeExecutor mAutoModeExecutor; @@ -100,12 +99,9 @@ public void robotInit() mCargoChute = CargoChute.getInstance(); mCargoIntake = CargoIntake.getInstance(); mClimber = Climber.getInstance(); - mLED = LED.getInstance(); mSuperstructure = Superstructure.getInstance(); mRobotStateEstimator = RobotStateEstimator.getInstance(); - mLED.setDriveState(LED.DriveLEDState.FORWARDS); - mSubsystemManager = new SubsystemManager( Arrays.asList( @@ -209,7 +205,6 @@ public void teleopInit() { Logger.logTeleopInit(); Logger.setVerbosity(SmartDashboard.getString(kRobotLogVerbosity, "NOTICE")); - mLED.setDriveState(LED.DriveLEDState.BACKWARDS); mDisabledLooper.stop(); if (mAutoModeExecutor != null) diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java b/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java deleted file mode 100644 index 4ee076e..0000000 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/LED.java +++ /dev/null @@ -1,95 +0,0 @@ -package com.spartronics4915.frc2019.subsystems; - -import java.io.FileOutputStream; -import java.io.IOException; -import java.util.Arrays; - -import com.spartronics4915.lib.util.ILooper; - -import edu.wpi.first.wpilibj.SerialPort; - - -public class LED extends Subsystem -{ - - // private SerialPort mSerialPort; - private FileOutputStream mSerialPort; - - private static LED mInstance = null; - - public static LED getInstance() - { - if (mInstance == null) - { - mInstance = new LED(); - } - return mInstance; - } - - public enum DriveLEDState - { - OFF("0".getBytes()), - FORWARDS("1".getBytes()), - BACKWARDS("2".getBytes()), - AUTONOMOUS("3".getBytes()), - DISABLED("4".getBytes()); // Add the remainder of your states - - public final byte[] serialSignal; - - private DriveLEDState(byte[] serialSignal) - { - this.serialSignal = serialSignal; - } - } - - private DriveLEDState mDriveState = DriveLEDState.DISABLED; - - public LED() - { - boolean success = true; - try - { - // mSerialPort = new SerialPort(9600, SerialPort.Port.kUSB); - //mSerialPort = new FileOutputStream("/dev/ttyACM0"); - } - catch (Exception e) - { - success = false; - logException("Couldn't instantiate hardware", e); - } - - logInitialized(success); - } - - public synchronized void setDriveState(DriveLEDState driveState) - { - // try - // { - // mSerialPort.write(driveState.serialSignal); - // } - // catch(IOException e) - // { - // logException("We didn't think this would work anyways, but we did hope", e); - // } - } - - @Override - public void registerEnabledLoops(ILooper enabledLooper) - {} - - @Override - public boolean checkSystem(String variant) - { - return true; - } - - @Override - public void outputTelemetry() - { - dashboardPutState(mDriveState.toString()); - } - - @Override - public void stop() - {} -}