├── .classpath ├── .gitignore ├── .project ├── LICENSE ├── README.md ├── build.properties ├── build.xml ├── build ├── com │ └── kauailabs │ │ └── navx │ │ └── frc │ │ ├── AHRS$BoardAxis.class │ │ ├── AHRS$BoardCapabilities.class │ │ ├── AHRS$BoardYawAxis.class │ │ ├── AHRS$IOCompleteNotification.class │ │ ├── AHRS$IOThread.class │ │ ├── AHRS$SerialDataType.class │ │ ├── AHRS.class │ │ ├── AHRSProtocol$AHRSPosUpdate.class │ │ ├── AHRSProtocol$AHRSUpdate.class │ │ ├── AHRSProtocol$AHRS_DATA_ACTION.class │ │ ├── AHRSProtocol$AHRS_DATA_TYPE.class │ │ ├── AHRSProtocol$AHRS_TUNING_VAR_ID.class │ │ ├── AHRSProtocol$BoardID.class │ │ ├── AHRSProtocol$DataSetResponse.class │ │ ├── AHRSProtocol$IntegrationControl.class │ │ ├── AHRSProtocol$MagCalData.class │ │ ├── AHRSProtocol$TuningVar.class │ │ ├── AHRSProtocol.class │ │ ├── ContinuousAngleTracker.class │ │ ├── IBoardCapabilities.class │ │ ├── IIOCompleteNotification$BoardState.class │ │ ├── IIOCompleteNotification.class │ │ ├── IIOProvider.class │ │ ├── IMUProtocol$GyroUpdate.class │ │ ├── IMUProtocol$QuaternionUpdate.class │ │ ├── IMUProtocol$StreamCommand.class │ │ ├── IMUProtocol$StreamResponse.class │ │ ├── IMUProtocol$YPRUpdate.class │ │ ├── IMUProtocol.class │ │ ├── IMURegisters.class │ │ ├── IRegisterIO.class │ │ ├── InertialDataIntegrator.class │ │ ├── OffsetTracker.class │ │ ├── RegisterIO.class │ │ ├── RegisterIO_I2C.class │ │ ├── RegisterIO_SPI.class │ │ └── SerialIO.class ├── jars │ ├── CTRLib.jar │ ├── NetworkTables.jar │ ├── WPILib.jar │ ├── cscore.jar │ └── opencv.jar └── org │ └── camsrobotics │ ├── frc2016 │ ├── Constants.class │ ├── DriverInput.class │ ├── HardwareAdapter.class │ ├── Robot$AUTO_MODES.class │ ├── Robot$TEST_COMMANDS.class │ ├── Robot.class │ ├── Vision.class │ ├── auto │ │ ├── AutoExecutor$1.class │ │ ├── AutoExecutor.class │ │ ├── AutoMode.class │ │ ├── actions │ │ │ ├── Action.class │ │ │ ├── TimeoutAction.class │ │ │ ├── WaitForDriveAction.class │ │ │ ├── WaitForIntakeAction.class │ │ │ ├── WaitForShooterRPMAction.class │ │ │ ├── WaitForUltrasonicBlindAction.class │ │ │ ├── WaitForUltrasonicSeeAction.class │ │ │ ├── WaitForVisionAlignX.class │ │ │ └── WaitForVisionTargetAction.class │ │ └── modes │ │ │ ├── DoNothingAuto.class │ │ │ ├── LowBarAuto.class │ │ │ ├── LowBarNoShootAuto.class │ │ │ └── RockWallAuto.class │ ├── subsystems │ │ ├── Drive$DriveController.class │ │ ├── Drive$DriveSensorSignal.class │ │ ├── Drive$DriveSignal.class │ │ ├── Drive.class │ │ ├── Intake$IntakeStates.class │ │ ├── Intake.class │ │ ├── Shooter.class │ │ └── controllers │ │ │ ├── DriveRotationController.class │ │ │ ├── DriveStraightController.class │ │ │ ├── DriveStraightDistanceController.class │ │ │ └── VisionTargetingController.class │ └── teleop │ │ ├── Commands$DriveCommands.class │ │ ├── Commands$DriveShiftCommands.class │ │ ├── Commands$FlywheelCommands.class │ │ ├── Commands$IntakeCommands.class │ │ ├── Commands$RollerCommands.class │ │ ├── Commands$ShooterCommands.class │ │ ├── Commands.class │ │ ├── TeleopManager$1.class │ │ └── TeleopManager.class │ └── lib │ ├── DecreaseOffBatterShooterAngle.class │ ├── Gearbox.class │ ├── Logger.class │ ├── Loopable.class │ ├── Looper$Task.class │ ├── Looper.class │ ├── MultiLooper.class │ ├── NerdyButton.class │ ├── NerdyJoystick.class │ ├── NerdyMath.class │ ├── NerdyPID.class │ └── Subsystem.class ├── dist └── FRCUserProgram.jar ├── src ├── com │ └── kauailabs │ │ └── navx │ │ └── frc │ │ ├── AHRS.java │ │ ├── AHRSProtocol.java │ │ ├── ContinuousAngleTracker.java │ │ ├── IBoardCapabilities.java │ │ ├── IIOCompleteNotification.java │ │ ├── IIOProvider.java │ │ ├── IMUProtocol.java │ │ ├── IMURegisters.java │ │ ├── IRegisterIO.java │ │ ├── InertialDataIntegrator.java │ │ ├── OffsetTracker.java │ │ ├── RegisterIO.java │ │ ├── RegisterIO_I2C.java │ │ ├── RegisterIO_SPI.java │ │ └── SerialIO.java └── org │ └── camsrobotics │ ├── frc2016 │ ├── Constants.java │ ├── DriverInput.java │ ├── HardwareAdapter.java │ ├── Robot.java │ ├── Vision.java │ ├── auto │ │ ├── AutoExecutor.java │ │ ├── AutoMode.java │ │ ├── actions │ │ │ ├── Action.java │ │ │ ├── TimeoutAction.java │ │ │ ├── WaitForDriveAction.java │ │ │ ├── WaitForIntakeAction.java │ │ │ ├── WaitForShooterRPMAction.java │ │ │ ├── WaitForUltrasonicBlindAction.java │ │ │ ├── WaitForUltrasonicSeeAction.java │ │ │ ├── WaitForVisionAlignX.java │ │ │ └── WaitForVisionTargetAction.java │ │ └── modes │ │ │ ├── DoNothingAuto.java │ │ │ ├── LowBarAuto.java │ │ │ ├── LowBarNoShootAuto.java │ │ │ └── RockWallAuto.java │ ├── subsystems │ │ ├── Drive.java │ │ ├── Intake.java │ │ ├── Shooter.java │ │ └── controllers │ │ │ ├── DriveRotationController.java │ │ │ ├── DriveStraightController.java │ │ │ ├── DriveStraightDistanceController.java │ │ │ └── VisionTargetingController.java │ └── teleop │ │ ├── Commands.java │ │ └── TeleopManager.java │ └── lib │ ├── DecreaseOffBatterShooterAngle.java │ ├── Gearbox.java │ ├── Logger.java │ ├── Loopable.java │ ├── Looper.java │ ├── MultiLooper.java │ ├── NerdyButton.java │ ├── NerdyJoystick.java │ ├── NerdyMath.java │ ├── NerdyPID.java │ └── Subsystem.java └── sysProps.xml /.classpath: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /bin/ 2 | .gitignore 3 | -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | Stronghold 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.jdt.core.javabuilder 10 | 11 | 12 | 13 | 14 | 15 | org.eclipse.jdt.core.javanature 16 | edu.wpi.first.wpilib.plugins.core.nature.FRCProjectNature 17 | 18 | 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | Copyright (c) 2016 FRC Team 687 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 5 | 6 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | 8 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Stronghold 2 | The official code of Team 687: The Nerd Herd 3 | 4 | ### Navigation Guide 5 | - ```com.kauailabs``` The NavX MXP Libraries 6 | - ```org.camsrobotics.frc2016``` The magic package with the main class 7 | - ```org.camsrobotics.frc2016.auto``` Autonomous mode 8 | - ```org.camsrobotics.frc2016.subsystems``` The various subsystems on the robot and controllers for them 9 | - ```org.camsrobotics.frc2016.teleop``` Various Teleop controllers 10 | - ```org.camsrobotics.lib``` Various utility packages 11 | -------------------------------------------------------------------------------- /build.properties: -------------------------------------------------------------------------------- 1 | # Project specific information 2 | package=org.camsrobotics.frc2016 3 | robot.class=${package}.Robot 4 | simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world 5 | #Uncomment and point at user libraries to include them in the build. Do not put libraries in the \wpilib\java folder, this folder is completely overwritten on plugin update. 6 | #userLibs=${user.home}/wpilib/user/lib -------------------------------------------------------------------------------- /build.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS$BoardAxis.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS$BoardAxis.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS$BoardCapabilities.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS$BoardCapabilities.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS$BoardYawAxis.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS$BoardYawAxis.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS$IOCompleteNotification.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS$IOCompleteNotification.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS$IOThread.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS$IOThread.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS$SerialDataType.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS$SerialDataType.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRS.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRS.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$AHRSPosUpdate.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$AHRSPosUpdate.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$AHRSUpdate.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$AHRSUpdate.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$AHRS_DATA_ACTION.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$AHRS_DATA_ACTION.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$AHRS_DATA_TYPE.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$AHRS_DATA_TYPE.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$AHRS_TUNING_VAR_ID.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$AHRS_TUNING_VAR_ID.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$BoardID.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$BoardID.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$DataSetResponse.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$DataSetResponse.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$IntegrationControl.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$IntegrationControl.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$MagCalData.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$MagCalData.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol$TuningVar.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol$TuningVar.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/AHRSProtocol.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/AHRSProtocol.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/ContinuousAngleTracker.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/ContinuousAngleTracker.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IBoardCapabilities.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IBoardCapabilities.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IIOCompleteNotification$BoardState.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IIOCompleteNotification$BoardState.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IIOCompleteNotification.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IIOCompleteNotification.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IIOProvider.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IIOProvider.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMUProtocol$GyroUpdate.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMUProtocol$GyroUpdate.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMUProtocol$QuaternionUpdate.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMUProtocol$QuaternionUpdate.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMUProtocol$StreamCommand.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMUProtocol$StreamCommand.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMUProtocol$StreamResponse.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMUProtocol$StreamResponse.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMUProtocol$YPRUpdate.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMUProtocol$YPRUpdate.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMUProtocol.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMUProtocol.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IMURegisters.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IMURegisters.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/IRegisterIO.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/IRegisterIO.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/InertialDataIntegrator.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/InertialDataIntegrator.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/OffsetTracker.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/OffsetTracker.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/RegisterIO.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/RegisterIO.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/RegisterIO_I2C.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/RegisterIO_I2C.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/RegisterIO_SPI.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/RegisterIO_SPI.class -------------------------------------------------------------------------------- /build/com/kauailabs/navx/frc/SerialIO.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/com/kauailabs/navx/frc/SerialIO.class -------------------------------------------------------------------------------- /build/jars/CTRLib.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/jars/CTRLib.jar -------------------------------------------------------------------------------- /build/jars/NetworkTables.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/jars/NetworkTables.jar -------------------------------------------------------------------------------- /build/jars/WPILib.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/jars/WPILib.jar -------------------------------------------------------------------------------- /build/jars/cscore.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/jars/cscore.jar -------------------------------------------------------------------------------- /build/jars/opencv.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/jars/opencv.jar -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/Constants.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/Constants.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/DriverInput.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/DriverInput.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/HardwareAdapter.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/HardwareAdapter.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/Robot$AUTO_MODES.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/Robot$AUTO_MODES.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/Robot$TEST_COMMANDS.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/Robot$TEST_COMMANDS.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/Robot.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/Robot.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/Vision.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/Vision.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/AutoExecutor$1.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/AutoExecutor$1.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/AutoExecutor.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/AutoExecutor.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/AutoMode.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/AutoMode.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/Action.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/Action.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/TimeoutAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/TimeoutAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForDriveAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForDriveAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForIntakeAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForIntakeAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForShooterRPMAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForShooterRPMAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForUltrasonicBlindAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForUltrasonicBlindAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForUltrasonicSeeAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForUltrasonicSeeAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForVisionAlignX.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForVisionAlignX.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/actions/WaitForVisionTargetAction.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/actions/WaitForVisionTargetAction.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/modes/DoNothingAuto.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/modes/DoNothingAuto.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/modes/LowBarAuto.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/modes/LowBarAuto.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/modes/LowBarNoShootAuto.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/modes/LowBarNoShootAuto.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/auto/modes/RockWallAuto.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/auto/modes/RockWallAuto.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Drive$DriveController.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Drive$DriveController.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Drive$DriveSensorSignal.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Drive$DriveSensorSignal.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Drive$DriveSignal.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Drive$DriveSignal.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Drive.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Drive.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Intake$IntakeStates.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Intake$IntakeStates.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Intake.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Intake.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/Shooter.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/Shooter.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/controllers/DriveRotationController.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/controllers/DriveRotationController.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/controllers/DriveStraightController.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/controllers/DriveStraightController.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/controllers/DriveStraightDistanceController.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/controllers/DriveStraightDistanceController.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/subsystems/controllers/VisionTargetingController.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/subsystems/controllers/VisionTargetingController.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands$DriveCommands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands$DriveCommands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands$DriveShiftCommands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands$DriveShiftCommands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands$FlywheelCommands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands$FlywheelCommands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands$IntakeCommands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands$IntakeCommands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands$RollerCommands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands$RollerCommands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands$ShooterCommands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands$ShooterCommands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/Commands.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/Commands.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/TeleopManager$1.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/TeleopManager$1.class -------------------------------------------------------------------------------- /build/org/camsrobotics/frc2016/teleop/TeleopManager.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/frc2016/teleop/TeleopManager.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/DecreaseOffBatterShooterAngle.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/DecreaseOffBatterShooterAngle.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/Gearbox.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/Gearbox.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/Logger.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/Logger.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/Loopable.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/Loopable.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/Looper$Task.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/Looper$Task.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/Looper.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/Looper.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/MultiLooper.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/MultiLooper.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/NerdyButton.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/NerdyButton.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/NerdyJoystick.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/NerdyJoystick.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/NerdyMath.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/NerdyMath.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/NerdyPID.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/NerdyPID.class -------------------------------------------------------------------------------- /build/org/camsrobotics/lib/Subsystem.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/build/org/camsrobotics/lib/Subsystem.class -------------------------------------------------------------------------------- /dist/FRCUserProgram.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nerdherd/Stronghold2016/ad53aa2fd4a79898a8ae02ea31337cc8e6317a42/dist/FRCUserProgram.jar -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/ContinuousAngleTracker.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | class ContinuousAngleTracker { 13 | 14 | private float last_angle; 15 | private double last_rate; 16 | private int zero_crossing_count; 17 | 18 | public ContinuousAngleTracker() { 19 | last_angle = 0.0f; 20 | zero_crossing_count = 0; 21 | last_rate = 0; 22 | } 23 | 24 | public void nextAngle( float newAngle ) { 25 | 26 | int angle_last_direction; 27 | float adjusted_last_angle = ( last_angle < 0.0f ) ? last_angle + 360.0f : last_angle; 28 | float adjusted_curr_angle = ( newAngle < 0.0f ) ? newAngle + 360.0f : newAngle; 29 | float delta_angle = adjusted_curr_angle - adjusted_last_angle; 30 | this.last_rate = delta_angle; 31 | 32 | angle_last_direction = 0; 33 | if ( adjusted_curr_angle < adjusted_last_angle ) { 34 | if ( delta_angle < -180.0f ) { 35 | angle_last_direction = -1; 36 | } else { 37 | angle_last_direction = 1; 38 | } 39 | } else if ( adjusted_curr_angle > adjusted_last_angle ) { 40 | if ( delta_angle > 180.0f ) { 41 | angle_last_direction = -1; 42 | } else { 43 | angle_last_direction = 1; 44 | } 45 | } 46 | 47 | if ( angle_last_direction < 0 ) { 48 | if ( ( adjusted_curr_angle < 0.0f ) && ( adjusted_last_angle >= 0.0f ) ) { 49 | zero_crossing_count--; 50 | } 51 | } else if ( angle_last_direction > 0 ) { 52 | if ( ( adjusted_curr_angle >= 0.0f ) && ( adjusted_last_angle < 0.0f ) ) { 53 | zero_crossing_count++; 54 | } 55 | } 56 | last_angle = newAngle; 57 | 58 | } 59 | 60 | public double getAngle() { 61 | double accumulated_angle = (double)zero_crossing_count * 360.0f; 62 | double curr_angle = (double)last_angle; 63 | if ( curr_angle < 0.0f ) { 64 | curr_angle += 360.0f; 65 | } 66 | accumulated_angle += curr_angle; 67 | return accumulated_angle; 68 | } 69 | 70 | public double getRate() { 71 | return last_rate; 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/IBoardCapabilities.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | interface IBoardCapabilities { 13 | public boolean isOmniMountSupported(); 14 | public boolean isBoardYawResetSupported(); 15 | public boolean isDisplacementSupported(); 16 | } 17 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/IIOCompleteNotification.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | interface IIOCompleteNotification { 13 | class BoardState { 14 | public byte op_status; 15 | public short sensor_status; 16 | public byte cal_status; 17 | public byte selftest_status; 18 | public short capability_flags; 19 | public byte update_rate_hz; 20 | public short accel_fsr_g; 21 | public short gyro_fsr_dps; 22 | } 23 | void setYawPitchRoll(IMUProtocol.YPRUpdate yprupdate); 24 | void setAHRSData(AHRSProtocol.AHRSUpdate ahrs_update); 25 | void setAHRSPosData(AHRSProtocol.AHRSPosUpdate ahrs_update); 26 | void setRawData(IMUProtocol.GyroUpdate raw_data_update); 27 | void setBoardID(AHRSProtocol.BoardID board_id); 28 | void setBoardState( BoardState board_state); 29 | } 30 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/IIOProvider.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | interface IIOProvider { 13 | public boolean isConnected(); 14 | public double getByteCount(); 15 | public double getUpdateCount(); 16 | public void setUpdateRateHz(byte update_rate); 17 | public void zeroYaw(); 18 | public void zeroDisplacement(); 19 | public void run(); 20 | public void stop(); 21 | } 22 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/IMUProtocol.java: -------------------------------------------------------------------------------- 1 | package com.kauailabs.navx.frc; 2 | 3 | /* ============================================ 4 | Nav6 source code is placed under the MIT license 5 | Copyright (c) 2013 Kauai Labs 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 20 | THE SOFTWARE. 21 | =============================================== 22 | */ 23 | 24 | public class IMUProtocol { 25 | 26 | public final static byte PACKET_START_CHAR = '!'; 27 | final static int PROTOCOL_FLOAT_LENGTH = 7; 28 | final static int CHECKSUM_LENGTH = 2; 29 | final static int TERMINATOR_LENGTH = 2; 30 | 31 | // Yaw/Pitch/Roll (YPR) Update Packet - e.g., 32 | // !y[yaw][pitch][roll][compass_heading] 33 | public final static byte MSGID_YPR_UPDATE = 'y'; 34 | final static int YPR_UPDATE_YAW_VALUE_INDEX = 2; 35 | final static int YPR_UPDATE_PITCH_VALUE_INDEX = 9; 36 | final static int YPR_UPDATE_ROLL_VALUE_INDEX = 16; 37 | final static int YPR_UPDATE_COMPASS_VALUE_INDEX = 23; 38 | final static int YPR_UPDATE_CHECKSUM_INDEX = 30; 39 | final static int YPR_UPDATE_TERMINATOR_INDEX = 32; 40 | final static int YPR_UPDATE_MESSAGE_LENGTH = 34; 41 | 42 | // Quaternion Data Update Packet - e.g., 43 | // !r[q1][q2][q3][q4][accelx][accely][accelz][magx][magy][magz] 44 | public final static byte MSGID_QUATERNION_UPDATE = 'q'; 45 | final static int QUATERNION_UPDATE_MESSAGE_LENGTH = 53; 46 | final static int QUATERNION_UPDATE_QUAT1_VALUE_INDEX = 2; 47 | final static int QUATERNION_UPDATE_QUAT2_VALUE_INDEX = 6; 48 | final static int QUATERNION_UPDATE_QUAT3_VALUE_INDEX = 10; 49 | final static int QUATERNION_UPDATE_QUAT4_VALUE_INDEX = 14; 50 | final static int QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX = 18; 51 | final static int QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX = 22; 52 | final static int QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX = 26; 53 | final static int QUATERNION_UPDATE_MAG_X_VALUE_INDEX = 30; 54 | final static int QUATERNION_UPDATE_MAG_Y_VALUE_INDEX = 34; 55 | final static int QUATERNION_UPDATE_MAG_Z_VALUE_INDEX = 38; 56 | final static int QUATERNION_UPDATE_TEMP_VALUE_INDEX = 42; 57 | final static int QUATERNION_UPDATE_CHECKSUM_INDEX = 49; 58 | final static int QUATERNION_UPDATE_TERMINATOR_INDEX = 51; 59 | 60 | // Gyro/Raw Data Update packet - e.g., 61 | // !g[gx][gy][gz][accelx][accely][accelz][magx][magy][magz][temp_c][cr][lf] 62 | 63 | public final static byte MSGID_GYRO_UPDATE = 'g'; 64 | final static int GYRO_UPDATE_GYRO_X_VALUE_INDEX = 2; 65 | final static int GYRO_UPDATE_GYRO_Y_VALUE_INDEX = 6; 66 | final static int GYRO_UPDATE_GYRO_Z_VALUE_INDEX = 10; 67 | final static int GYRO_UPDATE_ACCEL_X_VALUE_INDEX = 14; 68 | final static int GYRO_UPDATE_ACCEL_Y_VALUE_INDEX = 18; 69 | final static int GYRO_UPDATE_ACCEL_Z_VALUE_INDEX = 22; 70 | final static int GYRO_UPDATE_MAG_X_VALUE_INDEX = 26; 71 | final static int GYRO_UPDATE_MAG_Y_VALUE_INDEX = 30; 72 | final static int GYRO_UPDATE_MAG_Z_VALUE_INDEX = 34; 73 | final static int GYRO_UPDATE_TEMP_VALUE_INDEX = 38; 74 | final static int GYRO_UPDATE_CHECKSUM_INDEX = 42; 75 | final static int GYRO_UPDATE_TERMINATOR_INDEX = 44; 76 | final static int GYRO_UPDATE_MESSAGE_LENGTH = 46; 77 | 78 | // EnableStream Command Packet - e.g., !S[stream type][checksum][cr][lf] 79 | public final static byte MSGID_STREAM_CMD = 'S'; 80 | public final static int STREAM_CMD_STREAM_TYPE_YPR = MSGID_YPR_UPDATE; 81 | public final static int STREAM_CMD_STREAM_TYPE_QUATERNION = MSGID_QUATERNION_UPDATE; 82 | public final static int STREAM_CMD_STREAM_TYPE_GYRO = MSGID_GYRO_UPDATE; 83 | final static int STREAM_CMD_STREAM_TYPE_INDEX = 2; 84 | final static int STREAM_CMD_UPDATE_RATE_HZ_INDEX = 3; 85 | final static int STREAM_CMD_CHECKSUM_INDEX = 5; 86 | final static int STREAM_CMD_TERMINATOR_INDEX = 7; 87 | final static int STREAM_CMD_MESSAGE_LENGTH = 9; 88 | 89 | // EnableStream Response Packet - e.g., !s[stream type][gyro full scale 90 | // range][accel full scale range][update rate 91 | // hz][yaw_offset_degrees][flags][checksum][cr][lf] 92 | public final static byte MSG_ID_STREAM_RESPONSE = 's'; 93 | final static int STREAM_RESPONSE_MESSAGE_LENGTH = 46; 94 | final static int STREAM_RESPONSE_STREAM_TYPE_INDEX = 2; 95 | final static int STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE = 3; 96 | final static int STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE = 7; 97 | final static int STREAM_RESPONSE_UPDATE_RATE_HZ = 11; 98 | final static int STREAM_RESPONSE_YAW_OFFSET_DEGREES = 15; 99 | final static int STREAM_RESPONSE_QUAT1_OFFSET = 22; 100 | final static int STREAM_RESPONSE_QUAT2_OFFSET = 26; 101 | final static int STREAM_RESPONSE_QUAT3_OFFSET = 30; 102 | final static int STREAM_RESPONSE_QUAT4_OFFSET = 34; 103 | final static int STREAM_RESPONSE_FLAGS = 38; 104 | final static int STREAM_RESPONSE_CHECKSUM_INDEX = 42; 105 | final static int STREAM_RESPONSE_TERMINATOR_INDEX = 44; 106 | 107 | public final static byte STREAM_MSG_TERMINATION_CHAR = (byte) '\n'; 108 | 109 | public final static short NAV6_FLAG_MASK_CALIBRATION_STATE = 0x03; 110 | 111 | public final static short NAV6_CALIBRATION_STATE_WAIT = 0x00; 112 | public final static short NAV6_CALIBRATION_STATE_ACCUMULATE = 0x01; 113 | public final static short NAV6_CALIBRATION_STATE_COMPLETE = 0x02; 114 | 115 | public final static int IMU_PROTOCOL_MAX_MESSAGE_LENGTH = QUATERNION_UPDATE_MESSAGE_LENGTH; 116 | 117 | static public class YPRUpdate { 118 | 119 | public float yaw; 120 | public float pitch; 121 | public float roll; 122 | public float compass_heading; 123 | } 124 | 125 | static public class StreamCommand { 126 | 127 | public byte stream_type; 128 | } 129 | 130 | static public class StreamResponse { 131 | 132 | public byte stream_type; 133 | public short gyro_fsr_dps; 134 | public short accel_fsr_g; 135 | public short update_rate_hz; 136 | public float yaw_offset_degrees; 137 | public short q1_offset; 138 | public short q2_offset; 139 | public short q3_offset; 140 | public short q4_offset; 141 | public short flags; 142 | } 143 | 144 | static public class QuaternionUpdate { 145 | 146 | public short q1; 147 | public short q2; 148 | public short q3; 149 | public short q4; 150 | public short accel_x; 151 | public short accel_y; 152 | public short accel_z; 153 | public short mag_x; 154 | public short mag_y; 155 | public short mag_z; 156 | public float temp_c; 157 | } 158 | 159 | static public class GyroUpdate { 160 | 161 | public short gyro_x; 162 | public short gyro_y; 163 | public short gyro_z; 164 | public short accel_x; 165 | public short accel_y; 166 | public short accel_z; 167 | public short mag_x; 168 | public short mag_y; 169 | public short mag_z; 170 | public float temp_c; 171 | } 172 | 173 | public static int encodeStreamCommand(byte[] protocol_buffer, byte stream_type, byte update_rate_hz) { 174 | // Header 175 | protocol_buffer[0] = PACKET_START_CHAR; 176 | protocol_buffer[1] = MSGID_STREAM_CMD; 177 | 178 | // Data 179 | protocol_buffer[STREAM_CMD_STREAM_TYPE_INDEX] = stream_type; 180 | byteToHex(update_rate_hz, protocol_buffer, STREAM_CMD_UPDATE_RATE_HZ_INDEX); 181 | 182 | // Footer 183 | encodeTermination(protocol_buffer, STREAM_CMD_MESSAGE_LENGTH, STREAM_CMD_MESSAGE_LENGTH - 4); 184 | 185 | return STREAM_CMD_MESSAGE_LENGTH; 186 | } 187 | 188 | public static int decodeStreamResponse(byte[] buffer, int offset, int length, StreamResponse r) { 189 | 190 | if (length < STREAM_RESPONSE_MESSAGE_LENGTH) { 191 | return 0; 192 | } 193 | if ((buffer[offset + 0] == PACKET_START_CHAR) && (buffer[offset + 1] == MSG_ID_STREAM_RESPONSE)) { 194 | if (!verifyChecksum(buffer, offset, STREAM_RESPONSE_CHECKSUM_INDEX)) { 195 | return 0; 196 | } 197 | 198 | r.stream_type = buffer[offset + 2]; 199 | r.gyro_fsr_dps = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE); 200 | r.accel_fsr_g = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE); 201 | r.update_rate_hz = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_UPDATE_RATE_HZ); 202 | r.yaw_offset_degrees = decodeProtocolFloat(buffer, offset + STREAM_RESPONSE_YAW_OFFSET_DEGREES); 203 | r.q1_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT1_OFFSET); 204 | r.q2_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT2_OFFSET); 205 | r.q3_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT3_OFFSET); 206 | r.q4_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT4_OFFSET); 207 | r.flags = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_FLAGS); 208 | 209 | return STREAM_RESPONSE_MESSAGE_LENGTH; 210 | } 211 | return 0; 212 | } 213 | 214 | public static int decodeStreamCommand(byte[] buffer, int offset, int length, StreamCommand c) { 215 | if (length < STREAM_CMD_MESSAGE_LENGTH) { 216 | return 0; 217 | } 218 | if ((buffer[offset + 0] == '!') && (buffer[offset + 1] == MSGID_STREAM_CMD)) { 219 | if (!verifyChecksum(buffer, offset, STREAM_CMD_CHECKSUM_INDEX)) { 220 | return 0; 221 | } 222 | 223 | c.stream_type = buffer[offset + STREAM_CMD_STREAM_TYPE_INDEX]; 224 | return STREAM_CMD_MESSAGE_LENGTH; 225 | } 226 | return 0; 227 | } 228 | 229 | public static int decodeYPRUpdate(byte[] buffer, int offset, int length, YPRUpdate u) { 230 | if (length < YPR_UPDATE_MESSAGE_LENGTH) { 231 | return 0; 232 | } 233 | if ((buffer[offset + 0] == '!') && (buffer[offset + 1] == 'y')) { 234 | if (!verifyChecksum(buffer, offset, YPR_UPDATE_CHECKSUM_INDEX)) { 235 | return 0; 236 | } 237 | 238 | u.yaw = decodeProtocolFloat(buffer, offset + YPR_UPDATE_YAW_VALUE_INDEX); 239 | u.pitch = decodeProtocolFloat(buffer, offset + YPR_UPDATE_PITCH_VALUE_INDEX); 240 | u.roll = decodeProtocolFloat(buffer, offset + YPR_UPDATE_ROLL_VALUE_INDEX); 241 | u.compass_heading = decodeProtocolFloat(buffer, offset + YPR_UPDATE_COMPASS_VALUE_INDEX); 242 | return YPR_UPDATE_MESSAGE_LENGTH; 243 | } 244 | return 0; 245 | } 246 | 247 | public static int decodeQuaternionUpdate(byte[] buffer, int offset, int length, QuaternionUpdate u) { 248 | if (length < QUATERNION_UPDATE_MESSAGE_LENGTH) { 249 | return 0; 250 | } 251 | if ((buffer[offset + 0] == PACKET_START_CHAR) && (buffer[offset + 1] == MSGID_QUATERNION_UPDATE)) { 252 | if (!verifyChecksum(buffer, offset, QUATERNION_UPDATE_CHECKSUM_INDEX)) { 253 | return 0; 254 | } 255 | 256 | u.q1 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT1_VALUE_INDEX); 257 | u.q2 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT2_VALUE_INDEX); 258 | u.q3 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT3_VALUE_INDEX); 259 | u.q4 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT4_VALUE_INDEX); 260 | u.accel_x = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX); 261 | u.accel_y = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX); 262 | u.accel_z = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX); 263 | u.mag_x = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_MAG_X_VALUE_INDEX); 264 | u.mag_y = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_MAG_Y_VALUE_INDEX); 265 | u.mag_z = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_MAG_Z_VALUE_INDEX); 266 | u.temp_c = decodeProtocolFloat(buffer, offset + QUATERNION_UPDATE_TEMP_VALUE_INDEX); 267 | return QUATERNION_UPDATE_MESSAGE_LENGTH; 268 | } 269 | return 0; 270 | } 271 | 272 | public static int decodeGyroUpdate(byte[] buffer, int offset, int length, GyroUpdate u) { 273 | if (length < GYRO_UPDATE_MESSAGE_LENGTH) { 274 | return 0; 275 | } 276 | if ((buffer[offset + 0] == PACKET_START_CHAR) && (buffer[offset + 1] == MSGID_GYRO_UPDATE)) { 277 | if (!verifyChecksum(buffer, offset, GYRO_UPDATE_CHECKSUM_INDEX)) { 278 | return 0; 279 | } 280 | 281 | u.gyro_x = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_GYRO_X_VALUE_INDEX); 282 | u.gyro_y = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_GYRO_Y_VALUE_INDEX); 283 | u.gyro_z = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_GYRO_Z_VALUE_INDEX); 284 | u.accel_x = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_ACCEL_X_VALUE_INDEX); 285 | u.accel_y = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_ACCEL_Y_VALUE_INDEX); 286 | u.accel_z = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_ACCEL_Z_VALUE_INDEX); 287 | u.mag_x = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_MAG_X_VALUE_INDEX); 288 | u.mag_y = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_MAG_Y_VALUE_INDEX); 289 | u.mag_z = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_MAG_Z_VALUE_INDEX); 290 | u.temp_c = decodeProtocolUnsignedHundredthsFloat(buffer, offset + GYRO_UPDATE_TEMP_VALUE_INDEX); 291 | return GYRO_UPDATE_MESSAGE_LENGTH; 292 | } 293 | return 0; 294 | } 295 | 296 | public static void encodeTermination(byte[] buffer, int total_length, int content_length) { 297 | if ((total_length >= (CHECKSUM_LENGTH + TERMINATOR_LENGTH)) 298 | && (total_length >= content_length + (CHECKSUM_LENGTH + TERMINATOR_LENGTH))) { 299 | // Checksum 300 | byte checksum = 0; 301 | for (int i = 0; i < content_length; i++) { 302 | checksum += buffer[i]; 303 | } 304 | // convert checksum to two ascii bytes 305 | 306 | byteToHex(checksum, buffer, content_length); 307 | // Message Terminator 308 | buffer[content_length + CHECKSUM_LENGTH + 0] = '\r'; 309 | buffer[content_length + CHECKSUM_LENGTH + 1] = '\n'; 310 | } 311 | } 312 | 313 | final protected static byte[] hexArray = new byte[] { (byte) '0', (byte) '1', (byte) '2', (byte) '3', (byte) '4', 314 | (byte) '5', (byte) '6', (byte) '7', (byte) '8', (byte) '9', (byte) 'A', (byte) 'B', (byte) 'C', (byte) 'D', 315 | (byte) 'E', (byte) 'F' }; 316 | 317 | public static void byteToHex(byte thebyte, byte[] dest, int offset) { 318 | int v = thebyte & 0xFF; 319 | dest[offset + 0] = hexArray[v >> 4]; 320 | dest[offset + 1] = hexArray[v & 0x0F]; 321 | } 322 | 323 | public static short decodeProtocolUint16(byte[] uint16_string, int offset) { 324 | short decoded_uint16 = 0; 325 | int shift_left = 12; 326 | for (int i = offset + 0; i < offset + 4; i++) { 327 | byte digit = (byte) (uint16_string[i] <= '9' ? uint16_string[i] - '0' : ((uint16_string[i] - 'A') + 10)); 328 | decoded_uint16 += (((short) digit) << shift_left); 329 | shift_left -= 4; 330 | } 331 | return decoded_uint16; 332 | } 333 | 334 | /* 0 to 655.35 */ 335 | public static float decodeProtocolUnsignedHundredthsFloat(byte[] uint8_unsigned_hundredths_float, int offset) { 336 | float unsigned_float = (float) decodeProtocolUint16(uint8_unsigned_hundredths_float, offset); 337 | unsigned_float /= 100; 338 | return unsigned_float; 339 | } 340 | 341 | public static boolean verifyChecksum(byte[] buffer, int offset, int content_length) { 342 | // Calculate Checksum 343 | byte checksum = 0; 344 | for (int i = 0; i < content_length; i++) { 345 | checksum += buffer[offset + i]; 346 | } 347 | 348 | // Decode Checksum 349 | byte decoded_checksum = decodeUint8(buffer, offset + content_length); 350 | 351 | return (checksum == decoded_checksum); 352 | } 353 | 354 | public static byte decodeUint8(byte[] checksum, int offset) { 355 | byte first_digit = (byte) (checksum[0 + offset] <= '9' ? checksum[0 + offset] - '0' 356 | : ((checksum[0 + offset] - 'A') + 10)); 357 | byte second_digit = (byte) (checksum[1 + offset] <= '9' ? checksum[1 + offset] - '0' 358 | : ((checksum[1 + offset] - 'A') + 10)); 359 | byte decoded_checksum = (byte) ((first_digit * 16) + second_digit); 360 | return decoded_checksum; 361 | } 362 | 363 | public static float decodeProtocolFloat(byte[] buffer, int offset) { 364 | String float_string = new String(buffer, offset, PROTOCOL_FLOAT_LENGTH); 365 | return Float.parseFloat(float_string); 366 | } 367 | } 368 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/IMURegisters.java: -------------------------------------------------------------------------------- 1 | package com.kauailabs.navx.frc; 2 | 3 | /* ============================================ 4 | NavX-MXP source code is placed under the MIT license 5 | Copyright (c) 2015 Kauai Labs 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 20 | THE SOFTWARE. 21 | =============================================== 22 | */ 23 | 24 | public class IMURegisters { 25 | 26 | /**********************************************/ 27 | /* Device Identification Registers */ 28 | /**********************************************/ 29 | 30 | public static final byte NAVX_REG_WHOAMI = 0x00; /* IMU_MODEL_XXX */ 31 | public static final byte NAVX_REG_HW_REV = 0x01; 32 | public static final byte NAVX_REG_FW_VER_MAJOR = 0x02; 33 | public static final byte NAVX_REG_FW_VER_MINOR = 0x03; 34 | 35 | /**********************************************/ 36 | /* Status and Control Registers */ 37 | /**********************************************/ 38 | 39 | /* Read-write */ 40 | public static final byte NAVX_REG_UPDATE_RATE_HZ = 0x04; /* Range: 4 - 50 [unsigned byte] */ 41 | /* Read-only */ 42 | /* Accelerometer Full-Scale Range: in units of G [unsigned byte] */ 43 | public static final byte NAVX_REG_ACCEL_FSR_G = 0x05; 44 | /* Gyro Full-Scale Range (Degrees/Sec): Range: 250, 500, 1000 or 2000 [unsigned short] */ 45 | public static final byte NAVX_REG_GYRO_FSR_DPS_L = 0x06; /* Lower 8-bits of Gyro Full-Scale Range */ 46 | public static final byte NAVX_REG_GYRO_FSR_DPS_H = 0x07; /* Upper 8-bits of Gyro Full-Scale Range */ 47 | public static final byte NAVX_REG_OP_STATUS = 0x08; /* NAVX_OP_STATUS_XXX */ 48 | public static final byte NAVX_REG_CAL_STATUS = 0x09; /* NAVX_CAL_STATUS_XXX */ 49 | public static final byte NAVX_REG_SELFTEST_STATUS = 0x0A; /* NAVX_SELFTEST_STATUS_XXX */ 50 | public static final byte NAVX_REG_CAPABILITY_FLAGS_L = 0x0B; 51 | public static final byte NAVX_REG_CAPABILITY_FLAGS_H = 0x0C; 52 | 53 | /**********************************************/ 54 | /* Processed Data Registers */ 55 | /**********************************************/ 56 | 57 | public static final byte NAVX_REG_SENSOR_STATUS_L = 0x10; /* NAVX_SENSOR_STATUS_XXX */ 58 | public static final byte NAVX_REG_SENSOR_STATUS_H = 0x11; 59 | /* Timestamp: [unsigned long] */ 60 | public static final byte NAVX_REG_TIMESTAMP_L_L = 0x12; 61 | public static final byte NAVX_REG_TIMESTAMP_L_H = 0x13; 62 | public static final byte NAVX_REG_TIMESTAMP_H_L = 0x14; 63 | public static final byte NAVX_REG_TIMESTAMP_H_H = 0x15; 64 | 65 | /* Yaw, Pitch, Roll: Range: -180.00 to 180.00 [signed hundredths] */ 66 | /* Compass Heading: Range: 0.00 to 360.00 [unsigned hundredths] */ 67 | /* Altitude in Meters: In units of meters [16:16] */ 68 | 69 | public static final byte NAVX_REG_YAW_L = 0x16; /* Lower 8 bits of Yaw */ 70 | public static final byte NAVX_REG_YAW_H = 0x17; /* Upper 8 bits of Yaw */ 71 | public static final byte NAVX_REG_PITCH_L = 0x18; /* Lower 8 bits of Pitch */ 72 | public static final byte NAVX_REG_PITCH_H = 0x19; /* Upper 8 bits of Pitch */ 73 | public static final byte NAVX_REG_ROLL_L = 0x1A; /* Lower 8 bits of Roll */ 74 | public static final byte NAVX_REG_ROLL_H = 0x1B; /* Upper 8 bits of Roll */ 75 | public static final byte NAVX_REG_HEADING_L = 0x1C; /* Lower 8 bits of Heading */ 76 | public static final byte NAVX_REG_HEADING_H = 0x1D; /* Upper 8 bits of Heading */ 77 | public static final byte NAVX_REG_FUSED_HEADING_L = 0x1E; /* Upper 8 bits of Fused Heading */ 78 | public static final byte NAVX_REG_FUSED_HEADING_H = 0x1F; /* Upper 8 bits of Fused Heading */ 79 | public static final byte NAVX_REG_ALTITUDE_I_L = 0x20; 80 | public static final byte NAVX_REG_ALTITUDE_I_H = 0x21; 81 | public static final byte NAVX_REG_ALTITUDE_D_L = 0x22; 82 | public static final byte NAVX_REG_ALTITUDE_D_H = 0x23; 83 | 84 | /* World-frame Linear Acceleration: In units of +/- G * 1000 [signed thousandths] */ 85 | 86 | public static final byte NAVX_REG_LINEAR_ACC_X_L = 0x24; /* Lower 8 bits of Linear Acceleration X */ 87 | public static final byte NAVX_REG_LINEAR_ACC_X_H = 0x25; /* Upper 8 bits of Linear Acceleration X */ 88 | public static final byte NAVX_REG_LINEAR_ACC_Y_L = 0x26; /* Lower 8 bits of Linear Acceleration Y */ 89 | public static final byte NAVX_REG_LINEAR_ACC_Y_H = 0x27; /* Upper 8 bits of Linear Acceleration Y */ 90 | public static final byte NAVX_REG_LINEAR_ACC_Z_L = 0x28; /* Lower 8 bits of Linear Acceleration Z */ 91 | public static final byte NAVX_REG_LINEAR_ACC_Z_H = 0x29; /* Upper 8 bits of Linear Acceleration Z */ 92 | 93 | /* Quaternion: Range -1 to 1 [signed short ratio] */ 94 | 95 | public static final byte NAVX_REG_QUAT_W_L = 0x2A; /* Lower 8 bits of Quaternion W */ 96 | public static final byte NAVX_REG_QUAT_W_H = 0x2B; /* Upper 8 bits of Quaternion W */ 97 | public static final byte NAVX_REG_QUAT_X_L = 0x2C; /* Lower 8 bits of Quaternion X */ 98 | public static final byte NAVX_REG_QUAT_X_H = 0x2D; /* Upper 8 bits of Quaternion X */ 99 | public static final byte NAVX_REG_QUAT_Y_L = 0x2E; /* Lower 8 bits of Quaternion Y */ 100 | public static final byte NAVX_REG_QUAT_Y_H = 0x2F; /* Upper 8 bits of Quaternion Y */ 101 | public static final byte NAVX_REG_QUAT_Z_L = 0x30; /* Lower 8 bits of Quaternion Z */ 102 | public static final byte NAVX_REG_QUAT_Z_H = 0x31; /* Upper 8 bits of Quaternion Z */ 103 | 104 | /**********************************************/ 105 | /* Raw Data Registers */ 106 | /**********************************************/ 107 | 108 | /* Sensor Die Temperature: Range +/- 150, In units of Centigrade * 100 [signed hundredths float */ 109 | 110 | public static final byte NAVX_REG_MPU_TEMP_C_L = 0x32; /* Lower 8 bits of Temperature */ 111 | public static final byte NAVX_REG_MPU_TEMP_C_H = 0x33; /* Upper 8 bits of Temperature */ 112 | 113 | /* Raw, Calibrated Angular Rotation, in device units. Value in DPS = units / GYRO_FSR_DPS [signed short] */ 114 | 115 | public static final byte NAVX_REG_GYRO_X_L = 0x34; 116 | public static final byte NAVX_REG_GYRO_X_H = 0x35; 117 | public static final byte NAVX_REG_GYRO_Y_L = 0x36; 118 | public static final byte NAVX_REG_GYRO_Y_H = 0x37; 119 | public static final byte NAVX_REG_GYRO_Z_L = 0x38; 120 | public static final byte NAVX_REG_GYRO_Z_H = 0x39; 121 | 122 | /* Raw, Calibrated, Acceleration Data, in device units. Value in G = units / ACCEL_FSR_G [signed short] */ 123 | 124 | public static final byte NAVX_REG_ACC_X_L = 0x3A; 125 | public static final byte NAVX_REG_ACC_X_H = 0x3B; 126 | public static final byte NAVX_REG_ACC_Y_L = 0x3C; 127 | public static final byte NAVX_REG_ACC_Y_H = 0x3D; 128 | public static final byte NAVX_REG_ACC_Z_L = 0x3E; 129 | public static final byte NAVX_REG_ACC_Z_H = 0x3F; 130 | 131 | /* Raw, Calibrated, Un-tilt corrected Magnetometer Data, in device units. 1 unit = 0.15 uTesla [signed short] */ 132 | 133 | public static final byte NAVX_REG_MAG_X_L = 0x40; 134 | public static final byte NAVX_REG_MAG_X_H = 0x41; 135 | public static final byte NAVX_REG_MAG_Y_L = 0x42; 136 | public static final byte NAVX_REG_MAG_Y_H = 0x43; 137 | public static final byte NAVX_REG_MAG_Z_L = 0x44; 138 | public static final byte NAVX_REG_MAG_Z_H = 0x45; 139 | 140 | /* Calibrated Pressure in millibars Valid Range: 10.00 Max: 1200.00 [16:16 float] */ 141 | 142 | public static final byte NAVX_REG_PRESSURE_IL = 0x46; 143 | public static final byte NAVX_REG_PRESSURE_IH = 0x47; 144 | public static final byte NAVX_REG_PRESSURE_DL = 0x48; 145 | public static final byte NAVX_REG_PRESSURE_DH = 0x49; 146 | 147 | /* Pressure Sensor Die Temperature: Range +/- 150.00C [signed hundredths] */ 148 | 149 | public static final byte NAVX_REG_PRESSURE_TEMP_L = 0x4A; 150 | public static final byte NAVX_REG_PRESSURE_TEMP_H = 0x4B; 151 | 152 | /**********************************************/ 153 | /* Calibration Registers */ 154 | /**********************************************/ 155 | 156 | /* Yaw Offset: Range -180.00 to 180.00 [signed hundredths] */ 157 | 158 | public static final byte NAVX_REG_YAW_OFFSET_L = 0x4C; /* Lower 8 bits of Yaw Offset */ 159 | public static final byte NAVX_REG_YAW_OFFSET_H = 0x4D; /* Upper 8 bits of Yaw Offset */ 160 | 161 | /* Quaternion Offset: Range: -1 to 1 [signed short ratio] */ 162 | 163 | public static final byte NAVX_REG_QUAT_OFFSET_W_L = 0x4E; /* Lower 8 bits of Quaternion W */ 164 | public static final byte NAVX_REG_QUAT_OFFSET_W_H = 0x4F; /* Upper 8 bits of Quaternion W */ 165 | public static final byte NAVX_REG_QUAT_OFFSET_X_L = 0x50; /* Lower 8 bits of Quaternion X */ 166 | public static final byte NAVX_REG_QUAT_OFFSET_X_H = 0x51; /* Upper 8 bits of Quaternion X */ 167 | public static final byte NAVX_REG_QUAT_OFFSET_Y_L = 0x52; /* Lower 8 bits of Quaternion Y */ 168 | public static final byte NAVX_REG_QUAT_OFFSET_Y_H = 0x53; /* Upper 8 bits of Quaternion Y */ 169 | public static final byte NAVX_REG_QUAT_OFFSET_Z_L = 0x54; /* Lower 8 bits of Quaternion Z */ 170 | public static final byte NAVX_REG_QUAT_OFFSET_Z_H = 0x55; /* Upper 8 bits of Quaternion Z */ 171 | 172 | /**********************************************/ 173 | /* Integrated Data Registers */ 174 | /**********************************************/ 175 | 176 | /* Integration Control (Write-Only) */ 177 | public static final byte NAVX_REG_INTEGRATION_CTL = 0x56; 178 | public static final byte NAVX_REG_PAD_UNUSED = 0x57; 179 | 180 | /* Velocity: Range -32768.9999 - 32767.9999 in units of Meters/Sec */ 181 | 182 | public static final byte NAVX_REG_VEL_X_I_L = 0x58; 183 | public static final byte NAVX_REG_VEL_X_I_H = 0x59; 184 | public static final byte NAVX_REG_VEL_X_D_L = 0x5A; 185 | public static final byte NAVX_REG_VEL_X_D_H = 0x5B; 186 | public static final byte NAVX_REG_VEL_Y_I_L = 0x5C; 187 | public static final byte NAVX_REG_VEL_Y_I_H = 0x5D; 188 | public static final byte NAVX_REG_VEL_Y_D_L = 0x5E; 189 | public static final byte NAVX_REG_VEL_Y_D_H = 0x5F; 190 | public static final byte NAVX_REG_VEL_Z_I_L = 0x60; 191 | public static final byte NAVX_REG_VEL_Z_I_H = 0x61; 192 | public static final byte NAVX_REG_VEL_Z_D_L = 0x62; 193 | public static final byte NAVX_REG_VEL_Z_D_H = 0x63; 194 | 195 | /* Displacement: Range -32768.9999 - 32767.9999 in units of Meters */ 196 | 197 | public static final byte NAVX_REG_DISP_X_I_L = 0x64; 198 | public static final byte NAVX_REG_DISP_X_I_H = 0x65; 199 | public static final byte NAVX_REG_DISP_X_D_L = 0x66; 200 | public static final byte NAVX_REG_DISP_X_D_H = 0x67; 201 | public static final byte NAVX_REG_DISP_Y_I_L = 0x68; 202 | public static final byte NAVX_REG_DISP_Y_I_H = 0x69; 203 | public static final byte NAVX_REG_DISP_Y_D_L = 0x6A; 204 | public static final byte NAVX_REG_DISP_Y_D_H = 0x6B; 205 | public static final byte NAVX_REG_DISP_Z_I_L = 0x6C; 206 | public static final byte NAVX_REG_DISP_Z_I_H = 0x6D; 207 | public static final byte NAVX_REG_DISP_Z_D_L = 0x6E; 208 | public static final byte NAVX_REG_DISP_Z_D_H = 0x6F; 209 | 210 | public static final byte NAVX_REG_LAST = NAVX_REG_DISP_Z_D_H; 211 | } 212 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/IRegisterIO.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | interface IRegisterIO { 13 | boolean init(); 14 | boolean write(byte address, byte value ); 15 | boolean read(byte first_address, byte[] buffer); 16 | boolean shutdown(); 17 | } 18 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/InertialDataIntegrator.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | class InertialDataIntegrator { 13 | 14 | private float last_velocity[] = new float[2]; 15 | private float displacement[] = new float[2]; 16 | 17 | public InertialDataIntegrator() { 18 | resetDisplacement(); 19 | } 20 | 21 | public void updateDisplacement( float accel_x_g, float accel_y_g, 22 | int update_rate_hz, boolean is_moving ) { 23 | if ( is_moving ) { 24 | float accel_g[] = new float[2]; 25 | float accel_m_s2[] = new float[2]; 26 | float curr_velocity_m_s[] = new float[2]; 27 | float sample_time = (1.0f / update_rate_hz); 28 | accel_g[0] = accel_x_g; 29 | accel_g[1] = accel_y_g; 30 | for ( int i = 0; i < 2; i++ ) { 31 | accel_m_s2[i] = accel_g[i] * 9.80665f; 32 | curr_velocity_m_s[i] = last_velocity[i] + (accel_m_s2[i] * sample_time); 33 | displacement[i] += last_velocity[i] + (0.5f * accel_m_s2[i] * sample_time * sample_time); 34 | last_velocity[i] = curr_velocity_m_s[i]; 35 | } 36 | } else { 37 | last_velocity[0] = 0.0f; 38 | last_velocity[1] = 0.0f; 39 | } 40 | } 41 | 42 | public void resetDisplacement() { 43 | for ( int i = 0; i < 2; i++ ) { 44 | last_velocity[i] = 0.0f; 45 | displacement[i] = 0.0f; 46 | } 47 | } 48 | 49 | public float getVelocityX() { 50 | return last_velocity[0]; 51 | } 52 | 53 | public float getVelocityY() { 54 | return last_velocity[1]; 55 | } 56 | 57 | public float getVelocityZ() { 58 | return 0; 59 | } 60 | 61 | public float getDisplacementX() { 62 | return displacement[0]; 63 | } 64 | 65 | public float getDisplacementY() { 66 | return displacement[1]; 67 | } 68 | 69 | public float getDisplacementZ() { 70 | return 0; 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/OffsetTracker.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | import java.util.Arrays; 13 | 14 | class OffsetTracker { 15 | float value_history[]; 16 | int next_value_history_index; 17 | int history_len; 18 | double value_offset; 19 | 20 | public OffsetTracker(int history_length) { 21 | history_len = history_length; 22 | value_history = new float[history_len]; 23 | Arrays.fill(value_history,0); 24 | next_value_history_index = 0; 25 | value_offset = 0; 26 | } 27 | 28 | public void updateHistory(float curr_value) { 29 | if (next_value_history_index >= history_len) { 30 | next_value_history_index = 0; 31 | } 32 | value_history[next_value_history_index] = curr_value; 33 | next_value_history_index++; 34 | } 35 | 36 | public double getAverageFromHistory() { 37 | double value_history_sum = 0.0; 38 | for (int i = 0; i < history_len; i++) { 39 | value_history_sum += value_history[i]; 40 | } 41 | double value_history_avg = value_history_sum / history_len; 42 | return value_history_avg; 43 | } 44 | 45 | public void setOffset() { 46 | value_offset = getAverageFromHistory(); 47 | } 48 | 49 | public double getOffset() { 50 | return value_offset; 51 | } 52 | 53 | public double applyOffset( double value ) { 54 | float offseted_value = (float) (value - value_offset); 55 | if (offseted_value < -180) { 56 | offseted_value += 360; 57 | } 58 | if (offseted_value > 180) { 59 | offseted_value -= 360; 60 | } 61 | return offseted_value; 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/RegisterIO.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | import edu.wpi.first.wpilibj.Timer; 13 | 14 | class RegisterIO implements IIOProvider { 15 | IRegisterIO io_provider; 16 | byte update_rate_hz; 17 | boolean stop; 18 | IMUProtocol.GyroUpdate raw_data_update; 19 | AHRSProtocol.AHRSUpdate ahrs_update; 20 | AHRSProtocol.AHRSPosUpdate ahrspos_update; 21 | IIOCompleteNotification notify_sink; 22 | IIOCompleteNotification.BoardState board_state; 23 | AHRSProtocol.BoardID board_id; 24 | IBoardCapabilities board_capabilities; 25 | double last_update_time; 26 | int byte_count; 27 | int update_count; 28 | 29 | public RegisterIO( IRegisterIO io_provider, byte update_rate_hz, IIOCompleteNotification notify_sink, IBoardCapabilities board_capabilities ) { 30 | this.io_provider = io_provider; 31 | this.update_rate_hz = update_rate_hz; 32 | this.board_capabilities = board_capabilities; 33 | this.notify_sink = notify_sink; 34 | raw_data_update = new IMUProtocol.GyroUpdate(); 35 | ahrs_update = new AHRSProtocol.AHRSUpdate(); 36 | ahrspos_update = new AHRSProtocol.AHRSPosUpdate(); 37 | board_state = new IIOCompleteNotification.BoardState(); 38 | board_id = new AHRSProtocol.BoardID(); 39 | } 40 | 41 | private final double IO_TIMEOUT_SECONDS = 1.0; 42 | 43 | public void stop() { 44 | stop = true; 45 | } 46 | 47 | public void run() { 48 | 49 | io_provider.init(); 50 | 51 | /* Initial Device Configuration */ 52 | setUpdateRateHz(this.update_rate_hz); 53 | getConfiguration(); 54 | 55 | /* IO Loop */ 56 | while (!stop) { 57 | if ( board_state.update_rate_hz != this.update_rate_hz ) { 58 | setUpdateRateHz(this.update_rate_hz); 59 | } 60 | getCurrentData(); 61 | Timer.delay(1.0/this.update_rate_hz); 62 | } 63 | } 64 | 65 | private boolean getConfiguration() { 66 | boolean success = false; 67 | int retry_count = 0; 68 | while ( retry_count < 3 && !success ) { 69 | byte config[] = new byte[IMURegisters.NAVX_REG_SENSOR_STATUS_H+1]; 70 | if ( io_provider.read(IMURegisters.NAVX_REG_WHOAMI,config) ) { 71 | board_id.hw_rev = config[IMURegisters.NAVX_REG_HW_REV]; 72 | board_id.fw_ver_major = config[IMURegisters.NAVX_REG_FW_VER_MAJOR]; 73 | board_id.fw_ver_minor = config[IMURegisters.NAVX_REG_FW_VER_MINOR]; 74 | board_id.type = config[IMURegisters.NAVX_REG_WHOAMI]; 75 | notify_sink.setBoardID(board_id); 76 | 77 | board_state.cal_status = config[IMURegisters.NAVX_REG_CAL_STATUS]; 78 | board_state.op_status = config[IMURegisters.NAVX_REG_OP_STATUS]; 79 | board_state.selftest_status = config[IMURegisters.NAVX_REG_SELFTEST_STATUS]; 80 | board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_SENSOR_STATUS_L); 81 | board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_GYRO_FSR_DPS_L); 82 | board_state.accel_fsr_g = (short)config[IMURegisters.NAVX_REG_ACCEL_FSR_G]; 83 | board_state.update_rate_hz = config[IMURegisters.NAVX_REG_UPDATE_RATE_HZ]; 84 | board_state.capability_flags = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L); 85 | notify_sink.setBoardState(board_state); 86 | success = true; 87 | } else { 88 | success = false; 89 | Timer.delay(0.05); 90 | } 91 | retry_count++; 92 | } 93 | return success; 94 | } 95 | 96 | 97 | private void getCurrentData() { 98 | byte first_address = IMURegisters.NAVX_REG_UPDATE_RATE_HZ; 99 | boolean displacement_registers = board_capabilities.isDisplacementSupported(); 100 | byte curr_data[]; 101 | /* If firmware supports displacement data, acquire it - otherwise implement */ 102 | /* similar (but potentially less accurate) calculations on this processor. */ 103 | if ( displacement_registers ) { 104 | curr_data = new byte[IMURegisters.NAVX_REG_LAST + 1 - first_address]; 105 | } else { 106 | curr_data= new byte[IMURegisters.NAVX_REG_QUAT_OFFSET_Z_H + 1 - first_address]; 107 | } 108 | long timestamp_low, timestamp_high; 109 | long sensor_timestamp; 110 | if ( io_provider.read(first_address,curr_data) ) { 111 | timestamp_low = (long)AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_L_L-first_address); 112 | timestamp_high = (long)AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_H_L-first_address); 113 | sensor_timestamp = (timestamp_high << 16) + timestamp_low; 114 | ahrspos_update.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS - first_address]; 115 | ahrspos_update.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]; 116 | ahrspos_update.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS]; 117 | ahrspos_update.sensor_status = curr_data[IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address]; 118 | ahrspos_update.yaw = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_YAW_L-first_address); 119 | ahrspos_update.pitch = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_PITCH_L-first_address); 120 | ahrspos_update.roll = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_ROLL_L-first_address); 121 | ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_HEADING_L-first_address); 122 | ahrspos_update.mpu_temp = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address); 123 | ahrspos_update.linear_accel_x = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L-first_address); 124 | ahrspos_update.linear_accel_y = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L-first_address); 125 | ahrspos_update.linear_accel_z = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L-first_address); 126 | ahrspos_update.altitude = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_ALTITUDE_D_L - first_address); 127 | ahrspos_update.barometric_pressure = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_PRESSURE_DL - first_address); 128 | ahrspos_update.fused_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L-first_address); 129 | ahrspos_update.quat_w = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_W_L-first_address); 130 | ahrspos_update.quat_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_X_L-first_address); 131 | ahrspos_update.quat_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Y_L-first_address); 132 | ahrspos_update.quat_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Z_L-first_address); 133 | if ( displacement_registers ) { 134 | ahrspos_update.vel_x = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_X_I_L-first_address); 135 | ahrspos_update.vel_y = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Y_I_L-first_address); 136 | ahrspos_update.vel_z = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Z_I_L-first_address); 137 | ahrspos_update.disp_x = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_X_I_L-first_address); 138 | ahrspos_update.disp_y = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Y_I_L-first_address); 139 | ahrspos_update.disp_z = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Z_I_L-first_address); 140 | notify_sink.setAHRSPosData(ahrspos_update); 141 | } else { 142 | ahrs_update.op_status = ahrspos_update.op_status; 143 | ahrs_update.selftest_status = ahrspos_update.selftest_status; 144 | ahrs_update.cal_status = ahrspos_update.cal_status; 145 | ahrs_update.sensor_status = ahrspos_update.sensor_status; 146 | ahrs_update.yaw = ahrspos_update.yaw; 147 | ahrs_update.pitch = ahrspos_update.pitch; 148 | ahrs_update.roll = ahrspos_update.roll; 149 | ahrs_update.compass_heading = ahrspos_update.compass_heading; 150 | ahrs_update.mpu_temp = ahrspos_update.mpu_temp; 151 | ahrs_update.linear_accel_x = ahrspos_update.linear_accel_x; 152 | ahrs_update.linear_accel_y = ahrspos_update.linear_accel_y; 153 | ahrs_update.linear_accel_z = ahrspos_update.linear_accel_z; 154 | ahrs_update.altitude = ahrspos_update.altitude; 155 | ahrs_update.barometric_pressure = ahrspos_update.barometric_pressure; 156 | ahrs_update.fused_heading = ahrspos_update.fused_heading; 157 | notify_sink.setAHRSData( ahrs_update ); 158 | } 159 | 160 | board_state.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS-first_address]; 161 | board_state.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS-first_address]; 162 | board_state.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS-first_address]; 163 | board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_SENSOR_STATUS_L-first_address); 164 | board_state.update_rate_hz = curr_data[IMURegisters.NAVX_REG_UPDATE_RATE_HZ-first_address]; 165 | board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_GYRO_FSR_DPS_L); 166 | board_state.accel_fsr_g = (short)curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G]; 167 | board_state.capability_flags= AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L-first_address); 168 | notify_sink.setBoardState(board_state); 169 | 170 | raw_data_update.gyro_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_X_L-first_address); 171 | raw_data_update.gyro_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Y_L-first_address); 172 | raw_data_update.gyro_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Z_L-first_address); 173 | raw_data_update.accel_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_X_L-first_address); 174 | raw_data_update.accel_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Y_L-first_address); 175 | raw_data_update.accel_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Z_L-first_address); 176 | raw_data_update.mag_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_X_L-first_address); 177 | raw_data_update.mag_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Y_L-first_address); 178 | raw_data_update.mag_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Z_L-first_address); 179 | raw_data_update.temp_c = ahrspos_update.mpu_temp; 180 | notify_sink.setRawData(raw_data_update); 181 | 182 | this.last_update_time = Timer.getFPGATimestamp(); 183 | byte_count += curr_data.length; 184 | update_count++; 185 | } 186 | } 187 | 188 | @Override 189 | public boolean isConnected() { 190 | double time_since_last_update = Timer.getFPGATimestamp() - this.last_update_time; 191 | return time_since_last_update <= IO_TIMEOUT_SECONDS; 192 | } 193 | 194 | @Override 195 | public double getByteCount() { 196 | return byte_count; 197 | } 198 | 199 | @Override 200 | public double getUpdateCount() { 201 | return update_count; 202 | } 203 | 204 | @Override 205 | public void setUpdateRateHz(byte update_rate) { 206 | io_provider.write(IMURegisters.NAVX_REG_UPDATE_RATE_HZ, update_rate); 207 | } 208 | 209 | @Override 210 | public void zeroYaw() { 211 | io_provider.write( IMURegisters.NAVX_REG_INTEGRATION_CTL, 212 | AHRSProtocol.NAVX_INTEGRATION_CTL_RESET_YAW ); 213 | } 214 | 215 | @Override 216 | public void zeroDisplacement() { 217 | io_provider.write( IMURegisters.NAVX_REG_INTEGRATION_CTL, 218 | (byte)(AHRSProtocol.NAVX_INTEGRATION_CTL_RESET_DISP_X | 219 | AHRSProtocol.NAVX_INTEGRATION_CTL_RESET_DISP_Y | 220 | AHRSProtocol.NAVX_INTEGRATION_CTL_RESET_DISP_Z ) ); 221 | } 222 | 223 | } 224 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/RegisterIO_I2C.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | import edu.wpi.first.wpilibj.I2C; 13 | 14 | class RegisterIO_I2C implements IRegisterIO{ 15 | 16 | I2C port; 17 | 18 | public RegisterIO_I2C( I2C i2c_port ) { 19 | port = i2c_port; 20 | } 21 | 22 | @Override 23 | public boolean init() { 24 | return true; 25 | } 26 | 27 | @Override 28 | public boolean write(byte address, byte value ) { 29 | return port.write(address | 0x80, value); 30 | } 31 | 32 | final static int MAX_WPILIB_I2C_READ_BYTES = 127; 33 | 34 | @Override 35 | public boolean read(byte first_address, byte[] buffer) { 36 | int len = buffer.length; 37 | int buffer_offset = 0; 38 | while ( len > 0 ) { 39 | int read_len = (len > MAX_WPILIB_I2C_READ_BYTES) ? MAX_WPILIB_I2C_READ_BYTES : len; 40 | byte[] read_buffer = new byte[read_len]; 41 | if (!port.write(first_address + buffer_offset, read_len) && 42 | !port.readOnly(read_buffer, read_len) ) { 43 | System.arraycopy(read_buffer, 0, buffer, buffer_offset, read_len); 44 | buffer_offset += read_len; 45 | len -= read_len; 46 | } else { 47 | break; 48 | } 49 | } 50 | return (len == 0); 51 | } 52 | 53 | @Override 54 | public boolean shutdown() { 55 | return true; 56 | } 57 | 58 | } 59 | -------------------------------------------------------------------------------- /src/com/kauailabs/navx/frc/RegisterIO_SPI.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) Kauai Labs 2015. All Rights Reserved. */ 3 | /* */ 4 | /* Created in support of Team 2465 (Kauaibots). Go Purple Wave! */ 5 | /* */ 6 | /* Open Source Software - may be modified and shared by FRC teams. Any */ 7 | /* modifications to this code must be accompanied by the \License.txt file */ 8 | /* in the root directory of the project. */ 9 | /*----------------------------------------------------------------------------*/ 10 | package com.kauailabs.navx.frc; 11 | 12 | import edu.wpi.first.wpilibj.SPI; 13 | import edu.wpi.first.wpilibj.Timer; 14 | 15 | class RegisterIO_SPI implements IRegisterIO{ 16 | 17 | SPI port; 18 | int bitrate; 19 | 20 | static final int DEFAULT_SPI_BITRATE_HZ = 500000; 21 | 22 | public RegisterIO_SPI( SPI spi_port ) { 23 | port = spi_port; 24 | bitrate = DEFAULT_SPI_BITRATE_HZ; 25 | } 26 | 27 | public RegisterIO_SPI( SPI spi_port, int bitrate ) { 28 | port = spi_port; 29 | this.bitrate = bitrate; 30 | } 31 | 32 | @Override 33 | public boolean init() { 34 | port.setClockRate(bitrate); 35 | port.setMSBFirst(); 36 | port.setSampleDataOnFalling(); 37 | port.setClockActiveLow(); 38 | port.setChipSelectActiveLow(); 39 | return true; 40 | } 41 | 42 | @Override 43 | public boolean write(byte address, byte value ) { 44 | byte[] cmd = new byte[3]; 45 | cmd[0] = (byte) (address | (byte)0x80); 46 | cmd[1] = value; 47 | cmd[2] = AHRSProtocol.getCRC(cmd, 2); 48 | if ( port.write(cmd, cmd.length) != cmd.length) { 49 | return false; // WRITE ERROR 50 | } 51 | return true; 52 | } 53 | 54 | @Override 55 | public boolean read(byte first_address, byte[] buffer) { 56 | byte[] cmd = new byte[3]; 57 | cmd[0] = first_address; 58 | cmd[1] = (byte)buffer.length; 59 | cmd[2] = AHRSProtocol.getCRC(cmd, 2); 60 | if ( port.write(cmd, cmd.length) != cmd.length ) { 61 | return false; // WRITE ERROR 62 | } 63 | // delay 200 us /* TODO: What is min. granularity of delay()? */ 64 | Timer.delay(0.001); 65 | byte[] received_data = new byte[buffer.length+1]; 66 | if ( port.read(true, received_data, received_data.length) != received_data.length ) { 67 | return false; // READ ERROR 68 | } 69 | byte crc = AHRSProtocol.getCRC(received_data, received_data.length - 1); 70 | if ( crc != received_data[received_data.length-1] ) { 71 | return false; // CRC ERROR 72 | } 73 | System.arraycopy(received_data, 0, buffer, 0, received_data.length - 1); 74 | return true; 75 | } 76 | 77 | @Override 78 | public boolean shutdown() { 79 | return true; 80 | } 81 | 82 | } 83 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/Constants.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016; 2 | 3 | /** 4 | * Assorted Constants 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public class Constants { 10 | 11 | /* 12 | * Camera Constants 13 | */ 14 | public final static int kCameraFrameHeight = 240; 15 | public final static int kCameraFrameWidth = 320; 16 | public static int kCameraAim = 157; 17 | public static int kCameraDefault = 123; 18 | public final static double kCameraFOVAngle = 21.2505055*2; 19 | public static double kCameraLiftP = 0.000055; 20 | public static double kCameraLiftD = 0.00000833; 21 | public static double kCameraLiftAlpha = 0.1; 22 | public final static double kCameraActualWidth = 16; 23 | public final static double kCameraActualHeight = 12; 24 | public static double kCameraDistortion = 0; 25 | public static double kCameraVerticalAim = 120; 26 | public static double kGoalHeight = 77.25; 27 | 28 | /* 29 | * Drive Constants 30 | */ 31 | public final static double kDriveRotationP = 0.04444444; 32 | public final static double kDriveRotationI = 0.00044444; 33 | public final static double kDriveRotationD = 0; 34 | public final static double kDriveVisionP = 0.007; 35 | public final static double kDriveVisionI = 0.0005; 36 | public final static double kDriveVisionD = 0.15; 37 | public final static double kDriveTranslationP = 0; 38 | public final static double kDriveTranslationI = 0; 39 | public final static double kDriveTranslationD = 0; 40 | public final static double kDriveMaxAccel = 1.5; 41 | 42 | /* 43 | * Shooter Constants 44 | */ 45 | public final static double kFlywheelF = 0; 46 | public final static double kFlywheelP = 0.08204; 47 | public final static double kFlywheelI = 0.0005; 48 | public final static double kFlywheelD = 0.375; 49 | public final static double kLiftF = 0; 50 | public final static double kLiftP = 3; 51 | public final static double kLiftI = 0.015625; 52 | public final static double kLiftD = 0; 53 | public final static double kLiftAlpha = 0.05; 54 | public final static double kShootTime = 0.5; 55 | 56 | public static int kHighGoalRPM = 4000; 57 | public static int kLowGoalRPM = 1000; //THIS ONE 58 | public final static int kShortRangeRPM = 2000; 59 | public final static int kMediumRangeRPM = 3000; 60 | public final static int kLongRangeRPM = 4000; 61 | 62 | public static double kOuterWorksAngle = 0.819; // Long Range 63 | public static double kBatterAngle = 0.819; // Batter THIS ONE 64 | public static double kOffBatterAngle = 0.819; // Off Batter 65 | 66 | public final static double kMinHeight = 0.652; 67 | public final static double kMaxHeight = 0.823; 68 | 69 | /* 70 | * Intake Constants 71 | */ 72 | public final static double kIntakeSpeed = 0.8; 73 | public final static double kIntakeF = 0; 74 | public final static double kIntakeP = 4; 75 | public final static double kIntakeI = 0; 76 | public final static double kIntakeD = 16; 77 | public final static double kIntakeAlpha = 0.025; 78 | 79 | public final static double kIntakeBallPickup = 0.473; 80 | public final static double kIntakeResting = 0.787; 81 | public final static double kIntakeTucked = 0.575; 82 | public final static double kIntakeGround = 0.421; 83 | 84 | } 85 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/DriverInput.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016; 2 | 3 | import org.camsrobotics.frc2016.teleop.Commands; 4 | import org.camsrobotics.lib.NerdyButton; 5 | import org.camsrobotics.lib.NerdyJoystick; 6 | 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | 9 | /** 10 | * Driver Interface 11 | * 12 | * @author Wesley 13 | * 14 | */ 15 | public class DriverInput { 16 | 17 | private Commands m_commands; 18 | 19 | private NerdyJoystick m_driverLeftStick; 20 | private NerdyJoystick m_driverRightStick; 21 | private NerdyJoystick m_buttonBox; 22 | 23 | private NerdyButton m_snapToVisionTarget; 24 | private NerdyButton m_shiftUp; 25 | private NerdyButton m_shiftDown; 26 | 27 | private NerdyButton m_shooterOuterWorks; 28 | private NerdyButton m_shooterBatter; 29 | private NerdyButton m_shooterLowGoal; 30 | private NerdyButton m_shooterVerticalAlign; 31 | private NerdyButton m_shootFire; 32 | private NerdyButton m_shootReady; 33 | 34 | private NerdyButton m_intake; 35 | private NerdyButton m_outtake; 36 | private NerdyButton m_ballPickup; 37 | private NerdyButton m_tuckedInAll; 38 | private NerdyButton m_groundIntake; 39 | private NerdyButton m_tuckedIntake; 40 | private NerdyButton m_stop; 41 | 42 | private NerdyButton m_compress; 43 | 44 | private NerdyButton m_reset; 45 | 46 | public DriverInput(NerdyJoystick leftStick, NerdyJoystick rightStick, NerdyJoystick buttonBox) { 47 | m_commands = new Commands(); 48 | 49 | m_driverLeftStick = leftStick; 50 | m_driverRightStick = rightStick; 51 | m_buttonBox = buttonBox; 52 | 53 | m_snapToVisionTarget = m_driverLeftStick.getButton(1); 54 | m_shiftUp = m_driverRightStick.getButton(4); 55 | m_shiftDown = m_driverRightStick.getButton(3); 56 | 57 | 58 | m_shooterOuterWorks = m_buttonBox.getButton(12); 59 | m_shooterBatter = m_buttonBox.getButton(10); 60 | m_shooterLowGoal = m_buttonBox.getButton(8); 61 | m_shooterVerticalAlign = m_driverRightStick.getButton(7); 62 | m_shootFire = m_buttonBox.getButton(1); 63 | m_shootReady = m_buttonBox.getButton(2); 64 | 65 | m_intake = m_buttonBox.getButton(3); 66 | m_outtake = m_buttonBox.getButton(5); 67 | m_ballPickup = m_buttonBox.getButton(9); 68 | m_tuckedInAll = m_buttonBox.getButton(6); 69 | m_groundIntake = m_buttonBox.getButton(7); 70 | m_tuckedIntake = m_buttonBox.getButton(11); 71 | 72 | m_stop = m_buttonBox.getButton(4); 73 | 74 | m_compress = m_driverLeftStick.getButton(9); 75 | 76 | m_reset = m_buttonBox.getButton(8); 77 | } 78 | 79 | public Commands update() { 80 | // Refresh buttons 81 | // m_snapToVisionTarget.update(); 82 | m_shiftUp.update(); 83 | m_shiftDown.update(); 84 | // m_shooterOuterWorks.update(); 85 | // m_shooterBatter.update(); 86 | m_shooterLowGoal.update(); 87 | // m_shooterVerticalAlign.update(); 88 | // m_shoot.update(); 89 | m_shootReady.update(); 90 | 91 | m_intake.update(); 92 | m_outtake.update(); 93 | 94 | m_ballPickup.update(); 95 | m_tuckedInAll.update(); 96 | m_tuckedIntake.update(); 97 | // m_groundIntake.update(); 98 | m_stop.update(); 99 | 100 | m_compress.update(); 101 | 102 | // m_reset.update(); 103 | 104 | /* Do the magic */ 105 | 106 | // Drive 107 | if(m_snapToVisionTarget.get()) { 108 | m_commands.driveCommand = Commands.DriveCommands.VISION; 109 | } else { 110 | m_commands.driveCommand = Commands.DriveCommands.TANK_DRIVE; 111 | } 112 | 113 | if(m_shiftUp.wasPressed()) { 114 | m_commands.shiftCommand = Commands.DriveShiftCommands.UP; 115 | } else if(m_shiftDown.wasPressed()) { 116 | m_commands.shiftCommand = Commands.DriveShiftCommands.DOWN; 117 | } else { 118 | m_commands.shiftCommand = Commands.DriveShiftCommands.IDLE; 119 | } 120 | 121 | // Shooter 122 | // if(m_shooterOuterWorks.get()) { 123 | // m_commands.shooterCommand = Commands.ShooterCommands.OUTER_WORKS; 124 | // } else if(m_shooterBatter.get()) { 125 | // m_commands.shooterCommand = Commands.ShooterCommands.BATTER; 126 | // } else if(m_shooterVerticalAlign.get()) { 127 | // m_commands.shooterCommand = Commands.ShooterCommands.VERTICAL_ALIGN; 128 | // } 129 | 130 | if(m_shootReady.get()) { 131 | m_commands.flywheelCommand = Commands.FlywheelCommands.HIGH; 132 | m_commands.shooterCommand = Commands.ShooterCommands.BATTER; 133 | m_commands.intakeCommand = Commands.IntakeCommands.TUCKED; 134 | } else if(m_shooterLowGoal.get()) { 135 | m_commands.flywheelCommand = Commands.FlywheelCommands.LOW; 136 | m_commands.shooterCommand = Commands.ShooterCommands.BATTER; 137 | m_commands.intakeCommand = Commands.IntakeCommands.TUCKED; 138 | } else { 139 | m_commands.flywheelCommand = Commands.FlywheelCommands.IDLE; 140 | m_commands.shooterCommand = Commands.ShooterCommands.IDLE; 141 | } 142 | 143 | m_commands.shooterFire = m_shootFire.get(); 144 | 145 | // Intake 146 | if(m_intake.get()) { 147 | HardwareAdapter.kIntakeRollers.set(Constants.kIntakeSpeed); 148 | m_commands.shooterCommand = Commands.ShooterCommands.IDLE; 149 | } else if(m_outtake.get()) { 150 | HardwareAdapter.kIntakeRollers.set(-Constants.kIntakeSpeed); 151 | } else { 152 | HardwareAdapter.kIntakeRollers.set(0); 153 | } 154 | 155 | if(m_ballPickup.get()) { 156 | m_commands.intakeCommand = Commands.IntakeCommands.BALL_PICKUP; 157 | } else if(m_tuckedInAll.get()) { 158 | m_commands.intakeCommand = Commands.IntakeCommands.TUCKED_IN_ALL; 159 | } else if(m_groundIntake.get()) { 160 | m_commands.intakeCommand = Commands.IntakeCommands.GROUND; 161 | } else if(m_tuckedIntake.get()) { 162 | m_commands.intakeCommand = Commands.IntakeCommands.TUCKED; 163 | } else if(m_stop.get()) { 164 | m_commands.intakeCommand = Commands.IntakeCommands.MANUAL; 165 | } 166 | 167 | m_commands.compress = m_compress.get(); 168 | 169 | m_commands.reset = m_reset.get(); 170 | SmartDashboard.putBoolean("reset", m_reset.get()); 171 | 172 | return m_commands; 173 | } 174 | 175 | } 176 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/HardwareAdapter.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016; 2 | 3 | import org.camsrobotics.frc2016.subsystems.Drive; 4 | import org.camsrobotics.frc2016.subsystems.Intake; 5 | import org.camsrobotics.frc2016.subsystems.Shooter; 6 | import org.camsrobotics.lib.Gearbox; 7 | import org.camsrobotics.lib.NerdyJoystick; 8 | 9 | import com.ctre.CANTalon; 10 | import com.kauailabs.navx.frc.AHRS; 11 | 12 | import edu.wpi.first.wpilibj.AnalogInput; 13 | import edu.wpi.first.wpilibj.BuiltInAccelerometer; 14 | import edu.wpi.first.wpilibj.Compressor; 15 | import edu.wpi.first.wpilibj.DoubleSolenoid; 16 | import edu.wpi.first.wpilibj.Encoder; 17 | import edu.wpi.first.wpilibj.SerialPort; 18 | import edu.wpi.first.wpilibj.Solenoid; 19 | import edu.wpi.first.wpilibj.Ultrasonic; 20 | import edu.wpi.first.wpilibj.VictorSP; 21 | 22 | /** 23 | * Hardware Components 24 | * 25 | * @author Wesley 26 | * 27 | */ 28 | public class HardwareAdapter { 29 | // Driver Input 30 | public static final NerdyJoystick kDriveLeftStick = new NerdyJoystick(0); 31 | public static final NerdyJoystick kDriveRightStick = new NerdyJoystick(1); 32 | public static final NerdyJoystick kButtonBox = new NerdyJoystick(2); 33 | public static final DriverInput kDriverInput = new DriverInput(kDriveLeftStick, kDriveRightStick, kButtonBox); 34 | 35 | // Motors 36 | public static final VictorSP kDriveLeft1 = new VictorSP(3); 37 | public static final VictorSP kDriveLeft2 = new VictorSP(4); 38 | public static final VictorSP kDriveLeft3 = new VictorSP(5); 39 | public static final VictorSP kDriveRight1 = new VictorSP(0); 40 | public static final VictorSP kDriveRight2 = new VictorSP(1); 41 | public static final VictorSP kDriveRight3 = new VictorSP(2); 42 | 43 | public static final CANTalon kShooterLeft = new CANTalon(1); 44 | public static final CANTalon kShooterRight = new CANTalon(2); 45 | public static final CANTalon kShooterLift = new CANTalon(3); 46 | 47 | public static final CANTalon kIntakeRollers = new CANTalon(4); 48 | public static final CANTalon kIntakeArtic = new CANTalon(5); 49 | 50 | // Pneumatics 51 | public static final Compressor kCompressor = new Compressor(); 52 | public static final DoubleSolenoid kShifter = new DoubleSolenoid(3,4); 53 | public static final DoubleSolenoid kShooterPunch = new DoubleSolenoid(1,6); 54 | public static final DoubleSolenoid kCompress = new DoubleSolenoid(2,5); 55 | public static final Solenoid kPhotonCannon = new Solenoid(7); 56 | 57 | // Sensors 58 | public static final AHRS kNavX = new AHRS(SerialPort.Port.kMXP); 59 | public static final Encoder kDriveLeftEncoder = new Encoder(0,1); 60 | public static final Encoder kDriveRightEncoder = new Encoder(2,3); 61 | public static final Ultrasonic kUltrasonic = new Ultrasonic(4,5); 62 | public static final BuiltInAccelerometer kAccelerometer = new BuiltInAccelerometer(); 63 | public static final AnalogInput kCompressLeft = new AnalogInput(1); 64 | public static final AnalogInput kCompressRight = new AnalogInput(2); 65 | 66 | // Gearboxes 67 | public static final Gearbox kDriveLeftGearbox = new Gearbox(kDriveLeft1, kDriveLeft2, kDriveLeft3, kDriveLeftEncoder, kShifter); 68 | public static final Gearbox kDriveRightGearbox = new Gearbox(kDriveRight1, kDriveRight2, kDriveRight3, kDriveRightEncoder); 69 | 70 | // Subsystems 71 | public static final Drive kDrive = new Drive("Drivebase", kDriveLeftGearbox, kDriveRightGearbox, kNavX); 72 | public static final Shooter kShooter = new Shooter("Shooter", kShooterLeft, kShooterRight, kShooterPunch, kShooterLift); 73 | public static final Intake kIntake = new Intake("Intakes", kIntakeRollers, kIntakeArtic); 74 | 75 | } 76 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/Robot.java: -------------------------------------------------------------------------------- 1 | 2 | package org.camsrobotics.frc2016; 3 | 4 | import org.camsrobotics.frc2016.auto.AutoExecutor; 5 | import org.camsrobotics.frc2016.auto.modes.*; 6 | import org.camsrobotics.frc2016.subsystems.Drive; 7 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 8 | import org.camsrobotics.frc2016.subsystems.Intake; 9 | import org.camsrobotics.frc2016.subsystems.Shooter; 10 | import org.camsrobotics.frc2016.teleop.Commands; 11 | import org.camsrobotics.frc2016.teleop.TeleopManager; 12 | import org.camsrobotics.lib.MultiLooper; 13 | import org.camsrobotics.lib.NerdyMath; 14 | 15 | import com.kauailabs.navx.frc.AHRS; 16 | 17 | import edu.wpi.first.wpilibj.Compressor; 18 | import edu.wpi.first.wpilibj.IterativeRobot; 19 | import edu.wpi.first.wpilibj.PowerDistributionPanel; 20 | import edu.wpi.first.wpilibj.Timer; 21 | import edu.wpi.first.wpilibj.livewindow.LiveWindow; 22 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 23 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 24 | 25 | /** 26 | * This is where the magic happens! 27 | * 28 | * @author Wesley 29 | * 30 | */ 31 | public class Robot extends IterativeRobot { 32 | 33 | MultiLooper controllers = new MultiLooper("Controllers", 1/200.0); 34 | MultiLooper slowControllers = new MultiLooper("SlowControllers", 1/100.0); 35 | 36 | AutoExecutor auto = new AutoExecutor(new RockWallAuto()); 37 | 38 | Compressor compressor = HardwareAdapter.kCompressor; 39 | Drive drive = HardwareAdapter.kDrive; 40 | Shooter shooter = HardwareAdapter.kShooter; 41 | Intake intake = HardwareAdapter.kIntake; 42 | 43 | DriverInput driverInput = HardwareAdapter.kDriverInput; 44 | TeleopManager teleop = new TeleopManager(); 45 | PowerDistributionPanel pdp = new PowerDistributionPanel(); 46 | 47 | SendableChooser testCommands; 48 | 49 | enum TEST_COMMANDS { 50 | DISABLED, DRIVE_LEFT, DRIVE_RIGHT, SHOOTER_RPM 51 | } 52 | 53 | enum AUTO_MODES { 54 | LOW_BAR, CAT_D, RAMPARTS 55 | } 56 | 57 | AUTO_MODES autoMode = AUTO_MODES.RAMPARTS; 58 | double driveP = 0.044444; 59 | double driveI = 0; 60 | 61 | public void robotInit() { 62 | compressor.start(); 63 | 64 | System.out.println("NerdyBot Mantis Initialization"); 65 | 66 | controllers.addLoopable(shooter); 67 | controllers.addLoopable(intake); 68 | 69 | slowControllers.addLoopable(drive); 70 | 71 | testCommands = new SendableChooser(); 72 | testCommands.addDefault("Disabled", TEST_COMMANDS.DISABLED); 73 | testCommands.addObject("Drive Left", TEST_COMMANDS.DRIVE_LEFT); 74 | testCommands.addObject("Drive Right", TEST_COMMANDS.DRIVE_RIGHT); 75 | testCommands.addObject("Shooter RPM", TEST_COMMANDS.SHOOTER_RPM); 76 | 77 | SmartDashboard.putNumber("OuterWorks", Constants.kOuterWorksAngle); 78 | SmartDashboard.putNumber("OffBatter", Constants.kOffBatterAngle); 79 | SmartDashboard.putNumber("Batter", Constants.kBatterAngle); 80 | 81 | SmartDashboard.putNumber("Vision P", Constants.kDriveVisionP); 82 | SmartDashboard.putNumber("Vision I", Constants.kDriveVisionI); 83 | SmartDashboard.putNumber("Vision D", Constants.kDriveVisionD); 84 | 85 | SmartDashboard.putNumber("Camera Aim", Constants.kCameraAim); 86 | 87 | SmartDashboard.putNumber("driveP", driveP); 88 | SmartDashboard.putNumber("driveI", driveI); 89 | 90 | SmartDashboard.putNumber("Default Vision", Constants.kCameraDefault); 91 | 92 | SmartDashboard.putNumber("Time", 3); 93 | SmartDashboard.putNumber("Time2", 10); 94 | 95 | SmartDashboard.putNumber("Forward Priority", 1); 96 | 97 | SmartDashboard.putNumber("Ramparts Straight Power", 1); 98 | 99 | SmartDashboard.putNumber("Camera Lift P", Constants.kCameraLiftP); 100 | SmartDashboard.putNumber("Camera Lift D", Constants.kCameraLiftD); 101 | SmartDashboard.putNumber("Camera Lift Alpha", Constants.kCameraLiftAlpha); 102 | 103 | SmartDashboard.putNumber("ShooterPositionVision", 120); 104 | 105 | SmartDashboard.putNumber("Intakes Tucked", Constants.kIntakeTucked); 106 | SmartDashboard.putNumber("Intakes Ground", Constants.kIntakeGround); 107 | SmartDashboard.putNumber("Intakes Resting", Constants.kIntakeResting); 108 | 109 | SmartDashboard.putNumber("Distortion", Constants.kCameraDistortion); 110 | 111 | SmartDashboard.putNumber("High Goal RPM", Constants.kHighGoalRPM); 112 | SmartDashboard.putNumber("Low Goal RPM", Constants.kLowGoalRPM); 113 | 114 | SmartDashboard.putNumber("Auto Mode", 0); // Use this to select auto mode 115 | 116 | } 117 | 118 | Timer autoTimer = new Timer(); 119 | double lastTime; 120 | AHRS nav = HardwareAdapter.kNavX; 121 | 122 | public void autonomousInit() { 123 | System.out.println("NerdyBot Mantis Autonomous Start"); 124 | integration = 0; 125 | drive.zero(); 126 | drive.shiftDown(); 127 | 128 | // auto.start(); 129 | 130 | nav.zeroYaw(); 131 | 132 | autoTimer.start(); 133 | 134 | controllers.start(); 135 | slowControllers.start(); 136 | 137 | lastTime = Timer.getFPGATimestamp(); 138 | 139 | drive.shiftUp(); 140 | 141 | double aMode = SmartDashboard.getNumber("Auto Mode"); 142 | if(aMode == 0) { 143 | autoMode = AUTO_MODES.RAMPARTS; 144 | } else if(aMode == 1) { 145 | autoMode = AUTO_MODES.CAT_D; 146 | } else if(aMode == 2) { 147 | autoMode = AUTO_MODES.LOW_BAR; 148 | } 149 | 150 | } 151 | 152 | double integration = 0; 153 | double lastError = 0; 154 | 155 | double m_oscilateCount = 0; 156 | 157 | int state = 0; 158 | 159 | public void autonomousPeriodic() { 160 | Constants.kCameraDefault = (int) SmartDashboard.getNumber("Default Vision"); 161 | driveP = SmartDashboard.getNumber("driveP"); 162 | driveI = SmartDashboard.getNumber("driveI"); 163 | 164 | SmartDashboard.putNumber("Yaw", nav.getYaw()); 165 | 166 | if(autoMode == AUTO_MODES.LOW_BAR) { 167 | intake.manualDrive(-0.25); 168 | if(autoTimer.get() < 4) { 169 | drive.driveOpenLoop(new DriveSignal(-1,-1)); 170 | } else { 171 | drive.driveOpenLoop(DriveSignal.kStop); 172 | } 173 | } else if(autoMode == AUTO_MODES.CAT_D) { 174 | if(autoTimer.get() < 4) { 175 | drive.driveOpenLoop(new DriveSignal(-1,-1)); 176 | } else { 177 | drive.driveOpenLoop(DriveSignal.kStop); 178 | } 179 | } else if(autoMode == AUTO_MODES.RAMPARTS) { 180 | if(autoTimer.get() < 5) { 181 | // intake.setIntakeHeight (Constants.kIntakeBallPickup); 182 | double straightPower = SmartDashboard.getNumber("Ramparts Straight Power"); 183 | double time = Timer.getFPGATimestamp(); 184 | double error = nav.getYaw(); 185 | double p = error * driveP; 186 | integration += (error + lastError) * (time - lastTime)/2; 187 | lastTime = time; 188 | lastError = error; 189 | double i = integration * driveI; 190 | SmartDashboard.putNumber("P", p); 191 | SmartDashboard.putNumber("I",i); 192 | double pow = p + i; 193 | SmartDashboard.putNumber("Pow", pow); 194 | double leftPow = pow - straightPower; 195 | double rightPow = -pow - straightPower; 196 | SmartDashboard.putNumber("Left Power", leftPow); 197 | SmartDashboard.putNumber("Right Power", rightPow); 198 | SmartDashboard.putNumber("Yaw", error); 199 | 200 | double[] unnormalized = {leftPow, rightPow}; 201 | double[] normalized = NerdyMath.normalize(unnormalized, true); 202 | 203 | double leftNormalized = normalized[0]; 204 | double rightNormalized = normalized[1]; 205 | SmartDashboard.putNumber("Left Normalized", leftNormalized); 206 | SmartDashboard.putNumber("Right Normalized", rightNormalized); 207 | 208 | drive.driveOpenLoop(new DriveSignal(leftNormalized, rightNormalized)); 209 | } else { 210 | drive.driveOpenLoop(DriveSignal.kStop); 211 | } 212 | // } else if(state == 1) { 213 | // shooter.setShooterAngle(Constants.kOffBatterAngle); 214 | // if(autoTimer.get() < SmartDashboard.getNumber("Time2")) { 215 | // drive.setController(new VisionTargetingController(3)); 216 | // } else { 217 | // drive.driveOpenLoop(DriveSignal.kStop); 218 | // state++; 219 | // } 220 | // } else if(state == 2) { 221 | // shooter.setDesiredRPM(Constants.kManualRPM); 222 | // 223 | // if(Math.abs(shooter.getSpeed() - Constants.kManualRPM) < 5) { 224 | // m_oscilateCount++; 225 | // 226 | // if(m_oscilateCount == 5) { 227 | // shooter.shoot(); 228 | // } 229 | // } 230 | // } 231 | } 232 | 233 | } 234 | 235 | public void teleopInit() { 236 | System.out.println("NerdyBot Mantis Teleoperated Start"); 237 | 238 | drive.zero(); 239 | 240 | controllers.start(); 241 | slowControllers.start(); 242 | } 243 | 244 | public void teleopPeriodic() { 245 | Constants.kCameraAim = (int) SmartDashboard.getNumber("Camera Aim"); 246 | 247 | Commands c = driverInput.update(); 248 | teleop.update(c); 249 | 250 | SmartDashboard.putData("PDP", pdp); 251 | 252 | drive.reportState(); 253 | shooter.reportState(); 254 | intake.reportState(); 255 | 256 | Constants.kBatterAngle = SmartDashboard.getNumber("Batter"); 257 | Constants.kOffBatterAngle = SmartDashboard.getNumber("OffBatter"); 258 | Constants.kOuterWorksAngle = SmartDashboard.getNumber("OuterWorks"); 259 | 260 | Constants.kCameraLiftP = SmartDashboard.getNumber("Camera Lift P"); 261 | Constants.kCameraLiftD = SmartDashboard.getNumber("Camera Lift D"); 262 | Constants.kCameraLiftAlpha = SmartDashboard.getNumber("Camera Lift Alpha"); 263 | Constants.kCameraDistortion = SmartDashboard.getNumber("Distortion"); 264 | 265 | Constants.kHighGoalRPM = (int) SmartDashboard.getNumber("High Goal RPM"); 266 | Constants.kLowGoalRPM = (int) SmartDashboard.getNumber("Low Goal RPM"); 267 | 268 | 269 | drive.setPID(SmartDashboard.getNumber("Vision P", Constants.kDriveVisionP), 270 | SmartDashboard.getNumber("Vision I", Constants.kDriveVisionI), 271 | SmartDashboard.getNumber("Vision D", Constants.kDriveVisionD)); 272 | } 273 | 274 | public void disabledInit() { 275 | System.out.println("NerdyBot Mantis Disabled...enable me!"); 276 | 277 | auto.stop(); 278 | 279 | controllers.stop(); 280 | slowControllers.stop(); 281 | 282 | drive.stop(); 283 | 284 | shooter.stop(); 285 | 286 | intake.stop(); 287 | } 288 | 289 | public void disabledPeriodic() { 290 | 291 | } 292 | 293 | public void allPeriodic() { 294 | SmartDashboard.putNumber("PDP 0", pdp.getCurrent(0)); 295 | SmartDashboard.putNumber("PDP 1", pdp.getCurrent(1)); 296 | SmartDashboard.putNumber("PDP 2", pdp.getCurrent(2)); 297 | SmartDashboard.putNumber("PDP 3", pdp.getCurrent(3)); 298 | SmartDashboard.putNumber("PDP 4", pdp.getCurrent(4)); 299 | SmartDashboard.putNumber("PDP 5", pdp.getCurrent(5)); 300 | SmartDashboard.putNumber("PDP 6", pdp.getCurrent(6)); 301 | SmartDashboard.putNumber("PDP 7", pdp.getCurrent(7)); 302 | SmartDashboard.putNumber("PDP 8", pdp.getCurrent(8)); 303 | SmartDashboard.putNumber("PDP 9", pdp.getCurrent(9)); 304 | SmartDashboard.putNumber("PDP 10", pdp.getCurrent(10)); 305 | SmartDashboard.putNumber("PDP 11", pdp.getCurrent(11)); 306 | SmartDashboard.putNumber("PDP 12", pdp.getCurrent(12)); 307 | SmartDashboard.putNumber("PDP 13", pdp.getCurrent(13)); 308 | SmartDashboard.putNumber("PDP 14", pdp.getCurrent(14)); 309 | SmartDashboard.putNumber("PDP 15", pdp.getCurrent(15)); 310 | 311 | } 312 | 313 | public void testPeriodic() { 314 | LiveWindow.setEnabled(false); 315 | 316 | // // Get the Command 317 | // SmartDashboard.putData("Test Mode Commands", testCommands); 318 | // if(testCommands.getSelected() == TEST_COMMANDS.DRIVE_LEFT) { 319 | // HardwareAdapter.kDriveLeft1.set(1); 320 | // HardwareAdapter.kDriveLeft2.set(1); 321 | // HardwareAdapter.kDriveLeft3.set(1); 322 | // 323 | // HardwareAdapter.kDriveRight1.set(0); 324 | // HardwareAdapter.kDriveRight2.set(0); 325 | // HardwareAdapter.kDriveRight3.set(0); 326 | // } else if(testCommands.getSelected() == TEST_COMMANDS.DRIVE_RIGHT) { 327 | // HardwareAdapter.kDriveLeft1.set(0); 328 | // HardwareAdapter.kDriveLeft2.set(0); 329 | // HardwareAdapter.kDriveLeft3.set(0); 330 | // 331 | // HardwareAdapter.kDriveRight1.set(1); 332 | // HardwareAdapter.kDriveRight2.set(1); 333 | // HardwareAdapter.kDriveRight3.set(1); 334 | // } else { 335 | // HardwareAdapter.kDriveLeft1.set(0); 336 | // HardwareAdapter.kDriveLeft2.set(0); 337 | // HardwareAdapter.kDriveLeft3.set(0); 338 | // 339 | // HardwareAdapter.kDriveRight1.set(0); 340 | // HardwareAdapter.kDriveRight2.set(0); 341 | // HardwareAdapter.kDriveRight3.set(0); 342 | // } 343 | 344 | // PDP 345 | SmartDashboard.putData("PDP", pdp); 346 | 347 | // Drive Data 348 | // SmartDashboard.putData("Drive Left 1", HardwareAdapter.kDriveLeft1); 349 | // SmartDashboard.putData("Drive Left 2", HardwareAdapter.kDriveLeft2); 350 | // SmartDashboard.putData("Drive Left 3", HardwareAdapter.kDriveLeft3); 351 | // SmartDashboard.putData("Drive Right 1", HardwareAdapter.kDriveRight1); 352 | // SmartDashboard.putData("Drive Right 2", HardwareAdapter.kDriveRight2); 353 | // SmartDashboard.putData("Drive Right 3", HardwareAdapter.kDriveRight3); 354 | 355 | SmartDashboard.putData("Drive Left Encoder", HardwareAdapter.kDriveLeftEncoder); 356 | SmartDashboard.putData("Drive Right Encoder", HardwareAdapter.kDriveRightEncoder); 357 | 358 | // Shooter Data 359 | // SmartDashboard.putData("Shooter Lift", HardwareAdapter.kShooterLift); 360 | // SmartDashboard.putData("Shooter Left", HardwareAdapter.kShooterLeft); 361 | // SmartDashboard.putData("Shooter Right", HardwareAdapter.kShooterRight); 362 | 363 | SmartDashboard.putNumber("Shooter Lift Position", HardwareAdapter.kShooterLift.getPosition()); 364 | SmartDashboard.putNumber("Shooter Left RPM", HardwareAdapter.kShooterLeft.getSpeed()); 365 | SmartDashboard.putNumber("Shooter Right RPM", HardwareAdapter.kShooterRight.getSpeed()); 366 | 367 | // Intake Data 368 | // SmartDashboard.putData("Intake Artic", HardwareAdapter.kIntakeArtic); 369 | // SmartDashboard.putData("Intake Rollers", HardwareAdapter.kIntakeRollers); 370 | 371 | SmartDashboard.putNumber("Intake Position", HardwareAdapter.kIntakeArtic.getPosition()); 372 | SmartDashboard.putNumber("Intake RPM", HardwareAdapter.kIntakeRollers.getSpeed()); 373 | 374 | // shooter.setOuterWorksAngle(SmartDashboard.getNumber("OuterWorks")); 375 | // shooter.setOffBatterAngle(SmartDashboard.getNumber("OffBatter")); 376 | // shooter.setBatterAngle(SmartDashboard.getNumber("Batter")); 377 | 378 | Commands c = driverInput.update(); 379 | teleop.update(c); 380 | } 381 | 382 | } 383 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/Vision.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016; 2 | 3 | import org.camsrobotics.frc2016.subsystems.Shooter; 4 | 5 | import com.ctre.CANTalon; 6 | import edu.wpi.first.wpilibj.networktables.NetworkTable; 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | 9 | /** 10 | * NetworkTables Vision Adapter 11 | * 12 | * @author Mike 13 | * 14 | */ 15 | public class Vision { 16 | private NetworkTable table; 17 | 18 | private double[] m_defaultVal = new double[0]; 19 | 20 | private int m_max; 21 | private int m_maxKey = 0; 22 | 23 | private double m_apparentWidth = 0; 24 | private double m_actualWidth = Constants.kCameraActualWidth; 25 | private double m_apparentHeight = 0; 26 | private double m_actualHeightPX = 0; 27 | private double m_actualHeight = Constants.kCameraActualHeight; 28 | private double m_viewAngle = 0; 29 | private double m_theta = 0; 30 | private double m_distanceToGoal = 0; 31 | private double m_distance = 0; 32 | 33 | private CANTalon m_lifter = HardwareAdapter.kShooterLift; 34 | 35 | // private CANTalon m_lifter = HardwareAdapter.kShooterLift; 36 | 37 | private static Vision m_instance = null; 38 | 39 | /** 40 | * Constructs a network table given a key. (For instance 'GRIP/myContourReport') 41 | * @param key 42 | */ 43 | private Vision() { 44 | table = NetworkTable.getTable("GRIP/Courtyard"); 45 | } 46 | /** 47 | * Get one instance of vision object. (Since we will never need more than one.) 48 | * @Vision Instance 49 | */ 50 | public static Vision getInstance() { 51 | if (m_instance == null) { 52 | m_instance = new Vision(); 53 | } 54 | return m_instance; 55 | } 56 | /** 57 | * Returns max value in a given array. 58 | * @param array 59 | * @throws Exception 60 | */ 61 | private int getMax(double[] array) throws Exception { 62 | if (array != m_defaultVal) { 63 | m_max = 0; 64 | for (int i = 0; i < array.length; i++) { 65 | m_max = array[i] > m_max ? i : m_max; 66 | } 67 | return m_max; 68 | } else { 69 | throw new Exception("There is nothing in the array."); 70 | } 71 | } 72 | 73 | /** 74 | * Returns width of largest contour. Only works with contour reports. 75 | * @throws Exception 76 | */ 77 | public double getWidth() throws Exception { 78 | double[] m_areas = table.getNumberArray("area", m_defaultVal); 79 | double[] m_widths = table.getNumberArray("width", m_defaultVal); 80 | m_maxKey = getMax(m_areas); 81 | return m_widths[m_maxKey]; 82 | } 83 | /** 84 | * Returns height of largest contour, or the length of the longest line. 85 | * @throws Exception 86 | */ 87 | public double getHeight() throws Exception { 88 | double[] m_areas = table.getNumberArray("area", m_defaultVal); 89 | double[] m_heights = table.getNumberArray("height", m_defaultVal); 90 | m_maxKey = getMax(m_areas); 91 | return m_heights[m_maxKey]; 92 | } 93 | 94 | /** 95 | * Returns largest measured area. Only works for contour reports. 96 | * @throws Exception 97 | */ 98 | public double getArea() throws Exception { 99 | double[] m_areas = table.getNumberArray("area", m_defaultVal); 100 | m_maxKey = getMax(m_areas); 101 | return m_areas[m_maxKey]; 102 | } 103 | /** 104 | * Returns centerX of the largest contour or longest line. 105 | * @throws Exception 106 | */ 107 | public double getCenterX() throws Exception { 108 | double[] m_areas = table.getNumberArray("area", m_defaultVal); 109 | double[] m_centerxs = table.getNumberArray("centerX", m_defaultVal); 110 | m_maxKey = getMax(m_areas); 111 | return m_centerxs[m_maxKey]; 112 | } 113 | /** 114 | * Returns centerY of the largest contour. 115 | * @throws Exception 116 | */ 117 | public double getCenterY() throws Exception { 118 | double[] m_areas = table.getNumberArray("area", m_defaultVal); 119 | double[] m_centerys = table.getNumberArray("centerY", m_defaultVal); 120 | m_maxKey = getMax(m_areas); 121 | return m_centerys[m_maxKey]; 122 | } 123 | 124 | /** 125 | * After vertical alignment, gets shooter angle and uses it to calculate distance. 126 | * @return Distance in inches 127 | * @throws Exception 128 | */ 129 | public double getDistance() throws Exception { 130 | if(Math.abs(getCenterY()-Constants.kCameraFrameHeight/2) < 3) { 131 | m_theta = m_lifter.getPosition()/360+0.595; //Convert Encoder Reading to Angle 132 | m_distance = Math.tan(m_theta)/Constants.kGoalHeight; 133 | } else { 134 | throw new Exception("Target is not centered!"); 135 | } 136 | return m_distance; 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/AutoExecutor.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto; 2 | 3 | /** 4 | * Autonomous mode magic 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public class AutoExecutor { 10 | private AutoMode m_auto; 11 | private Thread m_thread = null; 12 | 13 | /** 14 | * Constructs the AutoExecutor with the given mode 15 | * 16 | * @param mode The mode to be executed 17 | */ 18 | public AutoExecutor(AutoMode mode) { 19 | m_auto = mode; 20 | } 21 | 22 | /** 23 | * Let's go! 24 | */ 25 | public void start() { 26 | if(m_thread == null) { 27 | m_thread = new Thread(new Runnable() { 28 | @Override 29 | public void run() { 30 | m_auto.run(); 31 | } 32 | }); 33 | 34 | m_thread.start(); 35 | } 36 | } 37 | 38 | /** 39 | * Stops the Autonomous mode 40 | */ 41 | public void stop() { 42 | if(m_auto != null) { 43 | m_auto.stop(); 44 | } 45 | m_thread = null; 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/AutoMode.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto; 2 | 3 | import org.camsrobotics.frc2016.HardwareAdapter; 4 | import org.camsrobotics.frc2016.auto.actions.Action; 5 | import org.camsrobotics.frc2016.subsystems.Drive; 6 | import org.camsrobotics.frc2016.subsystems.Intake; 7 | import org.camsrobotics.frc2016.subsystems.Shooter; 8 | 9 | /** 10 | * Base class for Autonomous modes 11 | * 12 | * @author Wesley 13 | * 14 | */ 15 | public abstract class AutoMode { 16 | protected Drive drive = HardwareAdapter.kDrive; 17 | protected Shooter shooter = HardwareAdapter.kShooter; 18 | protected Intake intake = HardwareAdapter.kIntake; 19 | 20 | private double m_period = 1/50.0; 21 | private boolean m_enabled = false; 22 | 23 | public abstract void routine(); 24 | 25 | public void run() { 26 | m_enabled = true; 27 | routine(); 28 | } 29 | 30 | public void stop() { 31 | m_enabled = false; 32 | } 33 | 34 | public boolean isEnabled() { 35 | return m_enabled; 36 | } 37 | 38 | public void runAction(Action a) { 39 | if(m_enabled) { 40 | a.start(); 41 | while(isEnabled() && !a.isFinished()) { 42 | a.update(); 43 | try { 44 | Thread.sleep((long) (m_period * 1000)); 45 | } catch (InterruptedException e) { 46 | e.printStackTrace(); 47 | } 48 | } 49 | } 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/Action.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | import org.camsrobotics.frc2016.HardwareAdapter; 4 | import org.camsrobotics.frc2016.subsystems.Drive; 5 | import org.camsrobotics.frc2016.subsystems.Intake; 6 | import org.camsrobotics.frc2016.subsystems.Shooter; 7 | 8 | /** 9 | * It does a thing 10 | * 11 | * @author Wesley 12 | * 13 | */ 14 | public abstract class Action { 15 | protected Drive drive = HardwareAdapter.kDrive; 16 | protected Shooter shooter = HardwareAdapter.kShooter; 17 | protected Intake intake = HardwareAdapter.kIntake; 18 | 19 | public abstract boolean isFinished(); 20 | 21 | public abstract void update(); 22 | 23 | public abstract void disable(); 24 | 25 | public abstract void start(); 26 | } 27 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/TimeoutAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | 5 | /** 6 | * Delays for a certain amount of time 7 | * 8 | * @author Wesley 9 | * 10 | */ 11 | public class TimeoutAction extends Action { 12 | protected double m_timeout; 13 | protected double m_startTime; 14 | 15 | public TimeoutAction(double timeout) { 16 | m_timeout = timeout; 17 | } 18 | 19 | @Override 20 | public boolean isFinished() { 21 | return m_timeout < Timer.getFPGATimestamp() - m_startTime; 22 | } 23 | 24 | @Override 25 | public void update() { 26 | // Nothing. Just wait 27 | } 28 | 29 | @Override 30 | public void start() { 31 | m_startTime = Timer.getFPGATimestamp(); 32 | } 33 | 34 | @Override 35 | public void disable() { 36 | 37 | } 38 | 39 | } 40 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForDriveAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | /** 4 | * Delay until the Drive is on target 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public class WaitForDriveAction extends TimeoutAction { 10 | public WaitForDriveAction(double timeout) { 11 | super(timeout); 12 | } 13 | 14 | @Override 15 | public boolean isFinished() { 16 | return super.isFinished() || drive.isOnTarget(); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForIntakeAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | /** 4 | * Delay until the Intake is on target 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public class WaitForIntakeAction extends TimeoutAction { 10 | private double m_tolerance; 11 | public WaitForIntakeAction(double timeout, double tolerance) { 12 | super(timeout); 13 | m_tolerance = tolerance; 14 | } 15 | 16 | @Override 17 | public boolean isFinished() { 18 | return super.isFinished() || intake.isOnTarget(m_tolerance); 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForShooterRPMAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | import org.camsrobotics.frc2016.HardwareAdapter; 4 | import org.camsrobotics.frc2016.subsystems.Shooter; 5 | 6 | /** 7 | * Wait for the shooter to get up to the right RPM 8 | * 9 | * @author Wesley 10 | * 11 | */ 12 | public class WaitForShooterRPMAction extends TimeoutAction { 13 | private Shooter m_shooter; 14 | 15 | public WaitForShooterRPMAction(double timeout) { 16 | super(timeout); 17 | 18 | m_shooter = HardwareAdapter.kShooter; 19 | } 20 | 21 | @Override 22 | public boolean isFinished() { 23 | return m_shooter.flywheelOnTarget(25); 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForUltrasonicBlindAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | import org.camsrobotics.frc2016.HardwareAdapter; 4 | 5 | import edu.wpi.first.wpilibj.Ultrasonic; 6 | 7 | /** 8 | * Waits for the ultrasonic to be more than the specified range. 9 | * 10 | * @author Wesley 11 | * 12 | */ 13 | public class WaitForUltrasonicBlindAction extends TimeoutAction { 14 | private double m_range; 15 | private Ultrasonic m_ultrasonic; 16 | 17 | /** 18 | * Default constructor 19 | * 20 | * @param range Max range before triggering, in inches 21 | * @param timeout 22 | */ 23 | public WaitForUltrasonicBlindAction(double range, double timeout) { 24 | super(timeout); 25 | m_range = range; 26 | m_ultrasonic = HardwareAdapter.kUltrasonic; 27 | } 28 | 29 | @Override 30 | public boolean isFinished() { 31 | return super.isFinished() || m_ultrasonic.getRangeInches() > m_range; 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForUltrasonicSeeAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | import org.camsrobotics.frc2016.HardwareAdapter; 4 | 5 | import edu.wpi.first.wpilibj.Ultrasonic; 6 | 7 | /** 8 | * Waits for the ultrasonic to be more less the specified range. 9 | * 10 | * @author Wesley 11 | * 12 | */ 13 | public class WaitForUltrasonicSeeAction extends TimeoutAction { 14 | private double m_range; 15 | private Ultrasonic m_ultrasonic; 16 | 17 | /** 18 | * Default constructor 19 | * 20 | * @param range Max range before triggering, in inches 21 | * @param timeout 22 | */ 23 | public WaitForUltrasonicSeeAction(double range, double timeout) { 24 | super(timeout); 25 | m_range = range; 26 | m_ultrasonic = HardwareAdapter.kUltrasonic; 27 | } 28 | 29 | @Override 30 | public boolean isFinished() { 31 | return super.isFinished() || m_ultrasonic.getRangeInches() < m_range; 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForVisionAlignX.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | */ 4 | package org.camsrobotics.frc2016.auto.actions; 5 | 6 | /** 7 | * @author Michael 8 | * 9 | */ 10 | public class WaitForVisionAlignX extends TimeoutAction { 11 | 12 | public WaitForVisionAlignX(double timeout) { 13 | super(timeout); 14 | } 15 | 16 | @Override 17 | public boolean isFinished() { 18 | return super.isFinished(); 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/actions/WaitForVisionTargetAction.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.actions; 2 | 3 | import edu.wpi.first.wpilibj.networktables.NetworkTable; 4 | 5 | /** 6 | * Waits for the vision target 7 | * 8 | * @author Michael 9 | * 10 | */ 11 | public class WaitForVisionTargetAction extends TimeoutAction { 12 | private NetworkTable m_table; 13 | private double m_contour; 14 | 15 | public WaitForVisionTargetAction(double timeout) { 16 | super(timeout); 17 | m_table = NetworkTable.getTable("GRIP/myContourReport"); 18 | m_contour = m_table.getNumber("centerx",0); 19 | } 20 | 21 | @Override 22 | public boolean isFinished() { 23 | return super.isFinished() || m_contour != 0; 24 | } 25 | } -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/modes/DoNothingAuto.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.modes; 2 | 3 | import org.camsrobotics.frc2016.auto.AutoMode; 4 | 5 | /** 6 | * Does nothing 7 | * 8 | * @author Wesley 9 | * 10 | */ 11 | public class DoNothingAuto extends AutoMode { 12 | 13 | @Override 14 | public void routine() { 15 | 16 | } 17 | 18 | } 19 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/modes/LowBarAuto.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.modes; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.auto.AutoMode; 5 | import org.camsrobotics.frc2016.auto.actions.*; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 7 | import org.camsrobotics.frc2016.subsystems.controllers.DriveRotationController; 8 | import org.camsrobotics.frc2016.subsystems.controllers.DriveStraightController; 9 | 10 | /** 11 | * Lowbar and shoot 12 | * 13 | * @author Wesley 14 | * 15 | */ 16 | public class LowBarAuto extends AutoMode { 17 | 18 | @Override 19 | public void routine() { 20 | // Pass the Low Bar 21 | drive.setController(new DriveStraightController(3, .5)); 22 | //runAction(new WaitForUltrasonicSeeAction(45, 15)); 23 | //runAction(new WaitForUltrasonicBlindAction(45, 15)); 24 | 25 | // Turn until vision sees 26 | drive.setController(new DriveRotationController(45, 0)); // Tolerance really does not matter 27 | runAction(new WaitForVisionTargetAction(15)); 28 | 29 | // Stop 30 | drive.driveOpenLoop(DriveSignal.kStop); 31 | 32 | //Line up with target 33 | //runAction(new WaitForVisionAlignX(15)); 34 | //runAction(new WaitForVisionAlignY()); 35 | // Spin Wheels 36 | shooter.setDesiredRPM(Constants.kLongRangeRPM); 37 | runAction(new WaitForShooterRPMAction(15)); 38 | 39 | // Shoot 40 | shooter.shoot(); 41 | 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/modes/LowBarNoShootAuto.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.modes; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.auto.AutoMode; 5 | import org.camsrobotics.frc2016.auto.actions.*; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 7 | import org.camsrobotics.frc2016.subsystems.controllers.DriveStraightController; 8 | 9 | public class LowBarNoShootAuto extends AutoMode { 10 | 11 | @Override 12 | public void routine() { 13 | // Lower Intake 14 | intake.setIntakeHeight(Constants.kIntakeBallPickup); 15 | runAction(new WaitForIntakeAction(15, .025)); 16 | 17 | // Pass the Low Bar 18 | drive.setController(new DriveStraightController(3, .25)); 19 | runAction(new TimeoutAction(5)); 20 | 21 | // Stop 22 | drive.driveOpenLoop(DriveSignal.kStop); 23 | } 24 | 25 | } 26 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/auto/modes/RockWallAuto.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.auto.modes; 2 | 3 | import org.camsrobotics.frc2016.auto.AutoMode; 4 | import org.camsrobotics.frc2016.auto.actions.*; 5 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 6 | 7 | public class RockWallAuto extends AutoMode { 8 | 9 | @Override 10 | public void routine() { 11 | // Pass the Low Bar 12 | drive.driveOpenLoop(new DriveSignal(1,1)); 13 | runAction(new TimeoutAction(5)); 14 | 15 | // Stop 16 | drive.driveOpenLoop(DriveSignal.kStop); 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/Drive.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.Vision; 5 | import org.camsrobotics.lib.Gearbox; 6 | import org.camsrobotics.lib.Subsystem; 7 | 8 | import com.kauailabs.navx.frc.AHRS; 9 | 10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 11 | 12 | /** 13 | * Drivebase Interface 14 | * 15 | * @author Wesley 16 | * 17 | */ 18 | public class Drive extends Subsystem { 19 | 20 | /** 21 | * Send a DriveSignal to the drivebase to control it 22 | * 23 | * @author Wesley 24 | * 25 | */ 26 | public static class DriveSignal { 27 | public double leftSpeed; 28 | public double rightSpeed; 29 | 30 | public DriveSignal(double leftSpeed, double rightSpeed) { 31 | this.leftSpeed = leftSpeed; 32 | this.rightSpeed = rightSpeed; 33 | } 34 | 35 | public static DriveSignal kStop = new DriveSignal(0, 0); 36 | } 37 | 38 | public static class DriveSensorSignal { 39 | public double yaw; 40 | public double leftEncoderDistance; 41 | public double rightEncoderDistance; 42 | public double vision; 43 | 44 | public DriveSensorSignal(double yaw, double leftEncoderDistance, double rightEncoderDistance, double vision) { 45 | this.yaw = yaw; 46 | this.leftEncoderDistance = leftEncoderDistance; 47 | this.rightEncoderDistance = rightEncoderDistance; 48 | this.vision = vision; 49 | } 50 | } 51 | 52 | /** 53 | * Controller Interface 54 | * 55 | * @author Wesley 56 | * 57 | */ 58 | public interface DriveController { 59 | public DriveSignal get(DriveSensorSignal sig); 60 | 61 | public boolean isOnTarget(); 62 | 63 | public void setPID(double p, double i, double d); 64 | } 65 | 66 | private Gearbox m_leftGearbox; 67 | private Gearbox m_rightGearbox; 68 | 69 | private AHRS m_nav; 70 | 71 | private DriveController m_controller = null; 72 | private DriveSignal m_signal = null; 73 | 74 | private Vision m_table = Vision.getInstance(); 75 | 76 | public Drive(String name, Gearbox leftGearbox, Gearbox rightGearbox, AHRS nav) { 77 | super(name); 78 | m_leftGearbox = leftGearbox; 79 | m_rightGearbox = rightGearbox; 80 | m_rightGearbox.setReversed(); 81 | m_nav = nav; 82 | } 83 | 84 | public void setController(DriveController controller) { 85 | m_controller = controller; 86 | } 87 | 88 | public void setEncoderDistancePerPulse(double distancePerPulse) { 89 | m_leftGearbox.setEncoderDistancePerPulse(distancePerPulse); 90 | m_rightGearbox.setEncoderDistancePerPulse(distancePerPulse); 91 | } 92 | 93 | public int getLeftEncoderRaw() { 94 | return m_leftGearbox.getEncoderRaw(); 95 | } 96 | 97 | public int getRightEncoderRaw() { 98 | return m_rightGearbox.getEncoderRaw(); 99 | } 100 | 101 | public double getLeftEncoderDistance() { 102 | return m_leftGearbox.getEncoderDistance(); 103 | } 104 | 105 | public double getRightEncoderDistance() { 106 | return m_rightGearbox.getEncoderDistance(); 107 | } 108 | 109 | public void shiftDown() { 110 | m_leftGearbox.shiftUp(); 111 | m_rightGearbox.shiftUp(); 112 | } 113 | 114 | public void shiftUp() { 115 | m_leftGearbox.shiftDown(); 116 | m_rightGearbox.shiftDown(); 117 | } 118 | 119 | public void driveOpenLoop(DriveSignal signal) { 120 | m_controller = null; 121 | m_signal = signal; 122 | } 123 | 124 | public boolean isOnTarget() { 125 | return m_controller != null && m_controller.isOnTarget(); 126 | } 127 | 128 | public double getYaw() { 129 | return m_nav.getYaw(); 130 | } 131 | 132 | public double getVision() { 133 | try { 134 | return m_table.getCenterX(); 135 | } catch (Exception e) { 136 | e.printStackTrace(); 137 | return Constants.kCameraDefault; 138 | } 139 | } 140 | 141 | public void setPID(double p, double i, double d) { 142 | if(m_controller != null) { 143 | m_controller.setPID(p, i, d); 144 | } 145 | } 146 | 147 | public void stop() { 148 | m_leftGearbox.setSpeed(0); 149 | m_rightGearbox.setSpeed(0); 150 | m_signal = null; 151 | } 152 | 153 | public void zero() { 154 | m_leftGearbox.resetEncoder(); 155 | m_rightGearbox.resetEncoder(); 156 | } 157 | 158 | /** 159 | * Let's drive the bot! 160 | */ 161 | @Override 162 | public void update() { 163 | if(m_controller != null) { 164 | m_signal = m_controller.get(new DriveSensorSignal(getYaw(), getLeftEncoderDistance(), getRightEncoderDistance(), getVision())); 165 | } 166 | 167 | if(m_signal != null) { 168 | m_leftGearbox.setSpeed(m_signal.leftSpeed); 169 | m_rightGearbox.setSpeed(m_signal.rightSpeed); 170 | } else { 171 | m_leftGearbox.setSpeed(0); 172 | m_rightGearbox.setSpeed(0); 173 | } 174 | } 175 | 176 | @Override 177 | public void reportState() { 178 | SmartDashboard.putNumber("LeftSpeed", m_signal.leftSpeed); 179 | SmartDashboard.putNumber("RightSpeed", m_signal.rightSpeed); 180 | 181 | SmartDashboard.putNumber("Yaw", getYaw()); 182 | SmartDashboard.putNumber("LeftEncoder", getLeftEncoderDistance()); 183 | SmartDashboard.putNumber("RightEncoder", getRightEncoderDistance()); 184 | SmartDashboard.putNumber("VisionCenterX", getVision()); 185 | 186 | try { 187 | SmartDashboard.putNumber("Vision Distance", m_table.getDistance()); 188 | } catch (Exception e) { 189 | } 190 | } 191 | } 192 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/Intake.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.lib.Subsystem; 5 | 6 | import com.ctre.CANTalon; 7 | import com.ctre.CANTalon.FeedbackDevice; 8 | import com.ctre.CANTalon.TalonControlMode; 9 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 10 | 11 | 12 | /** 13 | * Intake Interface 14 | * 15 | * @author Jamari 16 | * @author Wesley 17 | * 18 | */ 19 | public class Intake extends Subsystem { 20 | private CANTalon m_intake; 21 | private CANTalon m_angleAdjust; 22 | 23 | private boolean m_manual = false; 24 | private double m_manualPow = 0; 25 | 26 | private double m_desiredAngle = 0; 27 | private double m_actualAngle = Constants.kIntakeResting; 28 | private double m_intakeAlpha = Constants.kIntakeAlpha; 29 | private IntakeStates m_rollerState; 30 | 31 | public Intake(String name, CANTalon rollers, CANTalon angleAdjust) { 32 | super(name); 33 | m_intake = rollers; 34 | m_angleAdjust = angleAdjust; 35 | 36 | m_angleAdjust.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Absolute); 37 | m_intake.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative); 38 | 39 | m_angleAdjust.changeControlMode(TalonControlMode.Position); 40 | 41 | m_angleAdjust.reverseSensor(false); 42 | m_angleAdjust.reverseOutput(false); 43 | m_angleAdjust.enableBrakeMode(true); 44 | 45 | m_angleAdjust.setF(Constants.kIntakeF); 46 | m_angleAdjust.setP(Constants.kIntakeP); 47 | m_angleAdjust.setI(Constants.kIntakeI); 48 | m_angleAdjust.setD(Constants.kIntakeD); 49 | } 50 | 51 | private enum IntakeStates { 52 | INTAKE, OUTTAKE, IDLE 53 | } 54 | 55 | public void intake() { // turn on intake rollers to intake 56 | m_rollerState = IntakeStates.INTAKE; 57 | } 58 | 59 | public void outtake() { // turn on intake rollers to outake 60 | m_rollerState = IntakeStates.OUTTAKE; 61 | } 62 | 63 | public void idle() { // cease all intake motors 64 | m_rollerState = IntakeStates.IDLE; 65 | } 66 | 67 | public void setIntakeHeight(double angle) { 68 | m_manual = false; 69 | m_desiredAngle = angle; 70 | } 71 | 72 | public double getHeight() { 73 | return m_angleAdjust.getPosition(); 74 | } 75 | 76 | public void manualDrive(double pow) { 77 | m_manual = true; 78 | m_manualPow = pow; 79 | 80 | } 81 | 82 | public boolean isOnTarget(double tolerance) { 83 | return Math.abs(m_desiredAngle - getHeight()) < tolerance; 84 | } 85 | 86 | public void stop() { 87 | idle(); 88 | manualDrive(0); 89 | } 90 | 91 | @Override 92 | public void update() { 93 | // switch(m_rollerState) { 94 | // case INTAKE: 95 | // m_intake.set(-Constants.kIntakeSpeed); 96 | // break; 97 | // case OUTTAKE: 98 | // m_intake.set(Constants.kIntakeSpeed); 99 | // break; 100 | // case IDLE: 101 | // m_intake.set(0); 102 | // break; 103 | // } 104 | 105 | 106 | if(m_manual) { 107 | m_angleAdjust.changeControlMode(TalonControlMode.PercentVbus); 108 | m_angleAdjust.set(m_manualPow); 109 | } else { 110 | m_angleAdjust.changeControlMode(TalonControlMode.Position); 111 | m_actualAngle = m_actualAngle*(1-m_intakeAlpha)+m_desiredAngle*m_intakeAlpha; 112 | m_angleAdjust.set(m_actualAngle); 113 | } 114 | } 115 | 116 | @Override 117 | public void reportState() { 118 | SmartDashboard.putNumber("Intake Pos", m_angleAdjust.getPosition()); 119 | SmartDashboard.putNumber("Intake RPM", m_intake.getSpeed()); 120 | } 121 | 122 | } -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/Shooter.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.HardwareAdapter; 5 | import org.camsrobotics.frc2016.Vision; 6 | 7 | import org.camsrobotics.lib.Subsystem; 8 | 9 | import com.ctre.CANTalon; 10 | 11 | import edu.wpi.first.wpilibj.AnalogInput; 12 | 13 | import com.ctre.CANTalon.FeedbackDevice; 14 | import com.ctre.CANTalon.TalonControlMode; 15 | import edu.wpi.first.wpilibj.DoubleSolenoid; 16 | import edu.wpi.first.wpilibj.Timer; 17 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 18 | 19 | /** 20 | * Shooter Interface 21 | * 22 | * @author Wesley 23 | * 24 | */ 25 | public class Shooter extends Subsystem { 26 | 27 | private CANTalon m_shooterLeft; 28 | private CANTalon m_shooterRight; 29 | private DoubleSolenoid m_shooterPunch; 30 | 31 | private CANTalon m_lifter; 32 | 33 | AnalogInput m_leftCompress = HardwareAdapter.kCompressLeft; 34 | AnalogInput m_rightCompress = HardwareAdapter.kCompressRight; 35 | 36 | private double m_flywheelF = Constants.kFlywheelF; 37 | private double m_flywheelP = Constants.kFlywheelP; 38 | private double m_flywheelI = Constants.kFlywheelI; 39 | private double m_flywheelD = Constants.kFlywheelD; 40 | 41 | private double m_lifterF = Constants.kLiftF; 42 | private double m_lifterP = Constants.kLiftP; 43 | private double m_lifterI = Constants.kLiftI; 44 | private double m_lifterD = Constants.kLiftD; 45 | private double m_lifterAlpha = Constants.kLiftAlpha; 46 | 47 | private double m_shootTime = Constants.kShootTime; 48 | 49 | private int m_desiredRPM = 0; 50 | 51 | private double m_desiredAngle = 0.0; 52 | private double m_actualAngle = Constants.kMinHeight; 53 | 54 | private Vision m_table = Vision.getInstance(); 55 | private double m_kCameraLiftAlpha = Constants.kCameraLiftAlpha; 56 | private double m_kCameraLiftP = Constants.kCameraLiftP; 57 | private double m_kCameraLiftD = Constants.kCameraLiftD; 58 | private double m_lastCameraError = 0; 59 | private double m_currentCenterY = 0; 60 | private double m_cameraError = 0; 61 | private double m_cameraLastError = 0; 62 | 63 | private boolean m_shooting = false; 64 | private boolean m_setLift = false; 65 | private boolean m_cameraLift = false; 66 | private boolean m_manualLift = true; 67 | private Timer m_shootTimer; 68 | 69 | private DoubleSolenoid m_compress = HardwareAdapter.kCompress; 70 | 71 | private AnalogInput m_compressLeft = HardwareAdapter.kCompressLeft; 72 | private AnalogInput m_compressRight = HardwareAdapter.kCompressRight; 73 | 74 | private double m_compression = 0; 75 | 76 | public Shooter(String name, CANTalon shooterLeft, CANTalon shooterRight, DoubleSolenoid shooterPunch, CANTalon lifter) { 77 | super(name); 78 | 79 | m_shooterLeft = shooterLeft; 80 | m_shooterRight = shooterRight; 81 | m_shooterPunch = shooterPunch; 82 | 83 | m_lifter = lifter; 84 | 85 | m_shooterLeft.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative); 86 | m_shooterRight.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative); 87 | 88 | m_shooterLeft.changeControlMode(TalonControlMode.Speed); 89 | m_shooterRight.changeControlMode(TalonControlMode.Speed); 90 | 91 | m_shooterLeft.enableBrakeMode(false); 92 | m_shooterRight.enableBrakeMode(false); 93 | 94 | m_shooterLeft.setF(m_flywheelF); 95 | m_shooterLeft.setP(m_flywheelP); 96 | m_shooterLeft.setI(m_flywheelI); 97 | m_shooterLeft.setD(m_flywheelD); 98 | 99 | m_shooterRight.setF(m_flywheelF); 100 | m_shooterRight.setP(m_flywheelP); 101 | m_shooterRight.setI(m_flywheelI); 102 | m_shooterRight.setD(m_flywheelD); 103 | 104 | m_shooterLeft.reverseSensor(true); 105 | m_shooterLeft.reverseOutput(true); 106 | m_shooterRight.reverseSensor(false); 107 | m_shooterRight.reverseOutput(false); 108 | 109 | m_shootTimer = new Timer(); 110 | 111 | m_lifter.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Absolute); 112 | m_lifter.changeControlMode(TalonControlMode.Position); 113 | 114 | m_lifter.reverseSensor(false); 115 | m_lifter.reverseOutput(false); 116 | 117 | m_lifter.setF(m_lifterF); 118 | m_lifter.setP(m_lifterP); 119 | m_lifter.setI(m_lifterI); 120 | m_lifter.setD(m_lifterD); 121 | 122 | } 123 | 124 | public void setDesiredRPM(int rpm) { 125 | m_desiredRPM = rpm; 126 | } 127 | 128 | public boolean flywheelOnTarget(double threshold) { 129 | return threshold > Math.abs(m_shooterLeft.getSpeed() - m_desiredRPM) && 130 | threshold > Math.abs(m_shooterRight.getSpeed() - m_desiredRPM); 131 | } 132 | 133 | public void setShooterAngle(double angle) { 134 | m_manualLift = false; 135 | m_cameraLift = false; 136 | m_setLift = true; 137 | m_desiredAngle = angle; 138 | } 139 | 140 | public void verticalAlign() { 141 | m_manualLift = false; 142 | m_cameraLift = true; 143 | m_setLift = false; 144 | } 145 | 146 | public void setManualShooterAngle(double pow) { 147 | m_manualLift = true; 148 | m_cameraLift = false; 149 | m_setLift = false; 150 | m_desiredAngle = pow; 151 | } 152 | 153 | public double getShooterAngle() { 154 | return m_lifter.getPosition(); 155 | } 156 | 157 | public double getSpeed() { 158 | return m_shooterLeft.getSpeed(); 159 | } 160 | 161 | 162 | public boolean liftOnTarget(double threshold) { 163 | return threshold > Math.abs(m_lifter.getPosition() - m_desiredAngle); 164 | } 165 | 166 | public void shoot() { 167 | if(!m_shooting) { 168 | m_shooting = true; 169 | m_shootTimer.reset(); 170 | m_shootTimer.start(); 171 | } 172 | } 173 | 174 | public void compress(boolean in) { 175 | if(in) { 176 | m_compress.set(DoubleSolenoid.Value.kForward); 177 | m_compression = m_compressLeft.getVoltage() + m_compressRight.getVoltage(); 178 | } else { 179 | m_compress.set(DoubleSolenoid.Value.kReverse); 180 | } 181 | } 182 | 183 | public void stop() { 184 | m_shooting = false; 185 | m_shooterLeft.set(0); 186 | m_shooterRight.set(0); 187 | setManualShooterAngle(0); 188 | setDesiredRPM(0); 189 | } 190 | 191 | @Override 192 | public void update() { 193 | if(m_desiredRPM != 0) { 194 | m_shooterLeft.changeControlMode(TalonControlMode.Speed); 195 | m_shooterRight.changeControlMode(TalonControlMode.Speed); 196 | 197 | m_shooterLeft.set(m_desiredRPM); 198 | m_shooterRight.set(m_desiredRPM); 199 | } else { 200 | m_shooterLeft.changeControlMode(TalonControlMode.PercentVbus); 201 | m_shooterRight.changeControlMode(TalonControlMode.PercentVbus); 202 | 203 | m_shooterLeft.set(m_desiredRPM); 204 | m_shooterRight.set(m_desiredRPM); 205 | } 206 | 207 | if(m_manualLift) { 208 | m_lifter.changeControlMode(TalonControlMode.PercentVbus); 209 | m_actualAngle = Constants.kMinHeight; 210 | m_lifter.set(m_desiredAngle); 211 | } else if (m_setLift) { 212 | m_lifter.changeControlMode(TalonControlMode.Position); 213 | m_actualAngle = m_actualAngle*(1-m_lifterAlpha) + m_desiredAngle*m_lifterAlpha; 214 | m_lifter.set(m_actualAngle); 215 | } else if (m_cameraLift) { 216 | try { 217 | m_currentCenterY = m_table.getCenterY(); 218 | } catch (Exception e) { 219 | m_currentCenterY = 110; //If target is not found, keep moving up. 220 | } 221 | m_cameraError = m_currentCenterY-Constants.kCameraVerticalAim; 222 | m_actualAngle += m_kCameraLiftP*m_cameraError + m_kCameraLiftD*(m_cameraError-m_cameraLastError); 223 | m_cameraLastError = m_cameraError; 224 | m_lifter.set(m_actualAngle); 225 | } 226 | 227 | 228 | if(m_shooting) { 229 | if(m_shootTimer.get() < m_shootTime) { 230 | if(m_shooterPunch.get() != DoubleSolenoid.Value.kForward) 231 | m_shooterPunch.set(DoubleSolenoid.Value.kForward); 232 | } else { 233 | m_shooterPunch.set(DoubleSolenoid.Value.kReverse); 234 | m_shootTimer.stop(); 235 | m_shooting = false; 236 | } 237 | } else { 238 | if(m_shooterPunch.get() != DoubleSolenoid.Value.kReverse) 239 | m_shooterPunch.set(DoubleSolenoid.Value.kReverse); 240 | } 241 | 242 | } 243 | 244 | @Override 245 | public void reportState() { 246 | SmartDashboard.putNumber("Left Compress", m_leftCompress.getVoltage()); 247 | SmartDashboard.putNumber("Right Compress", m_rightCompress.getVoltage()); 248 | 249 | SmartDashboard.putNumber("Desired Angle", m_actualAngle); 250 | SmartDashboard.putNumber("Shooter Position", m_lifter.getPosition()); 251 | SmartDashboard.putNumber("Error", m_lifter.getError()); 252 | SmartDashboard.putNumber("Shooter Left RPM", m_shooterLeft.getSpeed()); 253 | SmartDashboard.putNumber("Shooter Right RPM", m_shooterRight.getSpeed()); 254 | 255 | SmartDashboard.putNumber("Compression", m_compression); 256 | } 257 | } 258 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/controllers/DriveRotationController.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems.controllers; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.subsystems.Drive.DriveController; 5 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSensorSignal; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 7 | import org.camsrobotics.lib.NerdyPID; 8 | 9 | /** 10 | * PID Drive Rotation Controller 11 | * 12 | * @author Wesley 13 | * 14 | */ 15 | public class DriveRotationController implements DriveController { 16 | private final double m_kP = Constants.kDriveRotationP; 17 | private final double m_kI = Constants.kDriveRotationI; 18 | private final double m_kD = Constants.kDriveRotationD; 19 | 20 | private double m_tolerance; 21 | private double m_power = 0; 22 | 23 | private NerdyPID m_pidController; 24 | 25 | public DriveRotationController(double desired, double tolerance) { 26 | m_tolerance = tolerance; 27 | 28 | m_pidController = new NerdyPID(m_kP, m_kI, m_kD); 29 | m_pidController.setDesired((desired + 360) %360); 30 | } 31 | 32 | @Override 33 | public DriveSignal get(DriveSensorSignal sig) { 34 | m_power = m_pidController.calculate((sig.yaw + 360) % 360); 35 | return new DriveSignal(m_power, -m_power); 36 | } 37 | 38 | @Override 39 | public boolean isOnTarget() { 40 | return m_pidController.onTarget(m_tolerance); 41 | } 42 | 43 | @Override 44 | public void setPID(double p, double i, double d) { 45 | // TODO Auto-generated method stub 46 | 47 | } 48 | 49 | } 50 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/controllers/DriveStraightController.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems.controllers; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.subsystems.Drive.DriveController; 5 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSensorSignal; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 7 | import org.camsrobotics.lib.NerdyMath; 8 | import org.camsrobotics.lib.NerdyPID; 9 | 10 | /** 11 | * PID Drive Straight Controller 12 | * 13 | * @author Mike 14 | * 15 | */ 16 | public class DriveStraightController implements DriveController { 17 | private final double m_kP = Constants.kDriveRotationP; 18 | private final double m_kI = Constants.kDriveRotationI; 19 | private final double m_kD = Constants.kDriveRotationD; 20 | 21 | private double m_tolerance; 22 | private double m_rotPower = 0; 23 | private double m_straitPower = 0; 24 | 25 | private NerdyPID m_pidController; 26 | 27 | public DriveStraightController(double tolerance, double power) { 28 | m_tolerance = tolerance; 29 | 30 | m_straitPower = power; 31 | 32 | m_pidController = new NerdyPID(m_kP, m_kI, m_kD); 33 | m_pidController.setDesired(0); 34 | } 35 | 36 | @Override 37 | public DriveSignal get(DriveSensorSignal sig) { 38 | m_rotPower = m_pidController.calculate((sig.yaw + 360) % 360); 39 | double[] pow = {m_rotPower + m_straitPower, -m_rotPower + m_straitPower}; 40 | pow = NerdyMath.normalize(pow, false); 41 | return new DriveSignal(pow[0], pow[1]); 42 | } 43 | 44 | @Override 45 | public boolean isOnTarget() { 46 | return m_pidController.onTarget(m_tolerance); 47 | } 48 | 49 | @Override 50 | public void setPID(double p, double i, double d) { 51 | // TODO Auto-generated method stub 52 | 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/controllers/DriveStraightDistanceController.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems.controllers; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.subsystems.Drive.DriveController; 5 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSensorSignal; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 7 | import org.camsrobotics.lib.NerdyMath; 8 | import org.camsrobotics.lib.NerdyPID; 9 | 10 | /** 11 | * PID Drive Straight to distance Controller 12 | * 13 | * @author Wesley 14 | * 15 | */ 16 | public class DriveStraightDistanceController implements DriveController { 17 | private final double m_kRotP = Constants.kDriveRotationP; 18 | private final double m_kRotI = Constants.kDriveRotationI; 19 | private final double m_kRotD = Constants.kDriveRotationD; 20 | 21 | private final double m_kDistP = Constants.kDriveTranslationP; 22 | private final double m_kDistI = Constants.kDriveTranslationI; 23 | private final double m_kDistD = Constants.kDriveTranslationD; 24 | 25 | private double m_tolerance; 26 | private double m_rotPower = 0; 27 | private double m_straightPower = 0; 28 | 29 | private NerdyPID m_pidRotController; 30 | private NerdyPID m_pidDistController; 31 | 32 | public DriveStraightDistanceController(double tolerance, double distance) { 33 | m_tolerance = tolerance; 34 | 35 | m_pidDistController = new NerdyPID(m_kDistP, m_kDistI, m_kDistD); 36 | m_pidDistController.setDesired(distance); 37 | 38 | m_pidRotController = new NerdyPID(m_kRotP, m_kRotI, m_kRotD); 39 | m_pidRotController.setDesired(0); 40 | } 41 | 42 | @Override 43 | public DriveSignal get(DriveSensorSignal sig) { 44 | m_rotPower = m_pidRotController.calculate((sig.yaw + 360) % 360); 45 | m_straightPower = m_pidDistController.calculate((sig.leftEncoderDistance + sig.rightEncoderDistance)/2); 46 | double[] pow = {m_rotPower + m_straightPower, -m_rotPower + m_straightPower}; 47 | pow = NerdyMath.normalize(pow, false); 48 | return new DriveSignal(pow[0], pow[1]); 49 | } 50 | 51 | @Override 52 | public boolean isOnTarget() { 53 | return m_pidDistController.onTarget(m_tolerance) && m_pidRotController.onTarget(m_tolerance); 54 | } 55 | 56 | @Override 57 | public void setPID(double p, double i, double d) { 58 | // TODO Auto-generated method stub 59 | 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/subsystems/controllers/VisionTargetingController.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.subsystems.controllers; 2 | 3 | 4 | import org.camsrobotics.frc2016.Constants; 5 | import org.camsrobotics.frc2016.subsystems.Drive.DriveController; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSensorSignal; 7 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 8 | import org.camsrobotics.lib.NerdyPID; 9 | 10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 11 | 12 | /** 13 | * Snaps to vision target 14 | * 15 | * @author Michael 16 | * 17 | */ 18 | public class VisionTargetingController implements DriveController { 19 | 20 | private double m_tolerance; 21 | private double m_pwr; 22 | private NerdyPID m_pid; 23 | 24 | 25 | private double kP = Constants.kDriveVisionP; 26 | private double kI = Constants.kDriveVisionI; 27 | private double kD = Constants.kDriveVisionD; 28 | 29 | public VisionTargetingController(double tolerance) { 30 | m_tolerance = tolerance; 31 | m_pid = new NerdyPID(kP,kI,kD); 32 | m_pid.setDesired(Constants.kCameraAim); 33 | } 34 | 35 | public DriveSignal get(DriveSensorSignal sig) { 36 | m_pwr = m_pid.calculate(sig.vision); 37 | SmartDashboard.putNumber("Vision Signal", sig.vision); 38 | return new DriveSignal(m_pwr, -m_pwr); 39 | } 40 | 41 | public void setPID(double p, double i, double d) { 42 | m_pid.setPID(p, i, d); 43 | } 44 | 45 | public boolean isOnTarget() { 46 | return m_pid.onTarget(m_tolerance); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/teleop/Commands.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.teleop; 2 | 3 | /** 4 | * Teleoperated commands 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public class Commands { 10 | 11 | public static enum DriveCommands { 12 | TANK_DRIVE, VISION 13 | } 14 | 15 | public static enum DriveShiftCommands { 16 | DOWN, UP, IDLE 17 | } 18 | 19 | public static enum ShooterCommands { 20 | IDLE, MANUAL, OFF_BATTER, BATTER, OUTER_WORKS, RESTING, VERTICAL_ALIGN 21 | } 22 | 23 | public static enum FlywheelCommands { 24 | IDLE, HIGH, LOW 25 | } 26 | 27 | public static enum IntakeCommands { 28 | IDLE, MANUAL, BALL_PICKUP, TUCKED_IN_ALL, GROUND, TUCKED 29 | } 30 | 31 | public static enum RollerCommands { 32 | IDLE, INTAKE, OUTTAKE 33 | } 34 | 35 | public DriveCommands driveCommand = DriveCommands.TANK_DRIVE; 36 | public DriveShiftCommands shiftCommand = DriveShiftCommands.DOWN; 37 | public ShooterCommands shooterCommand = ShooterCommands.IDLE; 38 | public FlywheelCommands flywheelCommand = FlywheelCommands.IDLE; 39 | public IntakeCommands intakeCommand = IntakeCommands.IDLE; 40 | public RollerCommands rollerCommand = RollerCommands.IDLE; 41 | 42 | public boolean compress = false; 43 | public boolean shooterFire = false; 44 | 45 | public boolean reset = false; 46 | } 47 | -------------------------------------------------------------------------------- /src/org/camsrobotics/frc2016/teleop/TeleopManager.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.frc2016.teleop; 2 | 3 | import org.camsrobotics.frc2016.Constants; 4 | import org.camsrobotics.frc2016.HardwareAdapter; 5 | import org.camsrobotics.frc2016.subsystems.Drive; 6 | import org.camsrobotics.frc2016.subsystems.Drive.DriveSignal; 7 | import org.camsrobotics.frc2016.subsystems.Intake; 8 | import org.camsrobotics.frc2016.subsystems.Shooter; 9 | import org.camsrobotics.frc2016.subsystems.controllers.VisionTargetingController; 10 | import org.camsrobotics.lib.NerdyJoystick; 11 | 12 | import edu.wpi.first.wpilibj.BuiltInAccelerometer; 13 | import edu.wpi.first.wpilibj.DoubleSolenoid; 14 | import edu.wpi.first.wpilibj.Solenoid; 15 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 16 | 17 | /** 18 | * Executes or outsources the various teleoperated commands. 19 | * 20 | * @author Wesley 21 | * 22 | */ 23 | public class TeleopManager { 24 | private Drive m_drive = HardwareAdapter.kDrive; 25 | private Shooter m_shooter = HardwareAdapter.kShooter; 26 | private Intake m_intake = HardwareAdapter.kIntake; 27 | 28 | private NerdyJoystick m_driverLeftStick = HardwareAdapter.kDriveLeftStick; 29 | private NerdyJoystick m_driverRightStick = HardwareAdapter.kDriveRightStick; 30 | private NerdyJoystick m_buttonBox = HardwareAdapter.kButtonBox; 31 | 32 | private DoubleSolenoid m_compress = HardwareAdapter.kCompress; 33 | private Solenoid m_photonCannon = HardwareAdapter.kPhotonCannon; 34 | 35 | private BuiltInAccelerometer m_accelerometer = HardwareAdapter.kAccelerometer; 36 | 37 | private int m_oscilateCount = 0; 38 | private int m_compressCount = 0; 39 | 40 | boolean rolling = false; 41 | 42 | public void update(Commands c) { 43 | // Drive 44 | switch(c.driveCommand) { 45 | case TANK_DRIVE: 46 | m_drive.driveOpenLoop(new DriveSignal(m_driverLeftStick.getY(), m_driverRightStick.getY())); 47 | break; 48 | case VISION: 49 | m_drive.setController(new VisionTargetingController(3)); 50 | break; 51 | } 52 | 53 | switch(c.shiftCommand) { 54 | case UP: 55 | m_drive.shiftUp(); 56 | break; 57 | case DOWN: 58 | m_drive.shiftDown(); 59 | break; 60 | case IDLE: 61 | if(Math.abs(m_accelerometer.getY()) > Constants.kDriveMaxAccel) { 62 | m_drive.shiftUp(); 63 | } 64 | break; 65 | } 66 | 67 | // Shooter 68 | if(c.intakeCommand != Commands.IntakeCommands.TUCKED_IN_ALL) { 69 | switch(c.shooterCommand) { 70 | case IDLE: 71 | case MANUAL: 72 | m_shooter.setManualShooterAngle(0); 73 | SmartDashboard.putNumber("ShooterAngleSet", 0); 74 | break; 75 | case OFF_BATTER: 76 | m_shooter.setShooterAngle(Constants.kOffBatterAngle); 77 | SmartDashboard.putNumber("ShooterAngleSet", Constants.kOffBatterAngle); 78 | break; 79 | case BATTER: 80 | m_shooter.setShooterAngle(Constants.kBatterAngle); 81 | SmartDashboard.putNumber("ShooterAngleSet", Constants.kBatterAngle); 82 | break; 83 | case OUTER_WORKS: 84 | m_shooter.setShooterAngle(Constants.kOuterWorksAngle); 85 | SmartDashboard.putNumber("ShooterAngleSet", Constants.kOuterWorksAngle); 86 | break; 87 | case VERTICAL_ALIGN: 88 | m_shooter.verticalAlign(); 89 | break; 90 | case RESTING: 91 | m_shooter.setShooterAngle(Constants.kMinHeight); 92 | SmartDashboard.putNumber("ShooterAngleSet", Constants.kMinHeight); 93 | break; 94 | } 95 | } else { 96 | m_shooter.setManualShooterAngle(0); 97 | } 98 | 99 | switch(c.flywheelCommand) { 100 | case IDLE: 101 | m_shooter.setDesiredRPM(0); 102 | m_oscilateCount = 0; 103 | m_compressCount = 0; 104 | break; 105 | case HIGH: 106 | m_shooter.setDesiredRPM(Constants.kHighGoalRPM); 107 | m_shooter.compress(true); 108 | m_photonCannon.set(true); 109 | 110 | if(Math.abs(m_shooter.getSpeed() - Constants.kHighGoalRPM) < 5) { 111 | m_oscilateCount++; 112 | 113 | if(m_oscilateCount > 5 && c.shooterFire) { 114 | m_shooter.compress(false); 115 | m_compressCount++; 116 | 117 | if(m_compressCount > 2) { 118 | m_shooter.shoot(); 119 | m_photonCannon.set(false); 120 | } 121 | } 122 | } 123 | break; 124 | case LOW: 125 | m_shooter.setDesiredRPM(Constants.kLowGoalRPM); 126 | m_shooter.compress(true); 127 | m_photonCannon.set(true); 128 | 129 | if(Math.abs(m_shooter.getSpeed() - Constants.kLowGoalRPM) < 5) { 130 | m_oscilateCount++; 131 | 132 | if(m_oscilateCount > 5 && c.shooterFire) { 133 | m_shooter.compress(false); 134 | m_compressCount++; 135 | 136 | if(m_compressCount > 2) { 137 | m_shooter.shoot(); 138 | m_photonCannon.set(false); 139 | } 140 | } 141 | } 142 | break; 143 | } 144 | 145 | // Intake 146 | switch(c.intakeCommand) { 147 | case IDLE: 148 | m_intake.manualDrive(0); 149 | rolling = false; 150 | break; 151 | case MANUAL: 152 | m_intake.manualDrive(m_buttonBox.getY()); 153 | rolling = false; 154 | break; 155 | case BALL_PICKUP: 156 | m_intake.setIntakeHeight(Constants.kIntakeBallPickup); 157 | rolling = false; 158 | break; 159 | case TUCKED_IN_ALL: 160 | m_intake.manualDrive(0.25); 161 | rolling = false; 162 | break; 163 | case TUCKED: 164 | m_intake.setIntakeHeight(Constants.kIntakeTucked); 165 | rolling = false; 166 | break; 167 | case GROUND: 168 | m_intake.manualDrive(-0.25); 169 | HardwareAdapter.kIntakeRollers.set(0.25); 170 | rolling = true; 171 | break; 172 | } 173 | 174 | } 175 | } 176 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/DecreaseOffBatterShooterAngle.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | 5 | /** 6 | * 7 | */ 8 | public class DecreaseOffBatterShooterAngle extends Command { 9 | 10 | public DecreaseOffBatterShooterAngle() { 11 | // Use requires() here to declare subsystem dependencies 12 | // eg. requires(chassis); 13 | } 14 | 15 | // Called just before this Command runs the first time 16 | protected void initialize() { 17 | } 18 | 19 | // Called repeatedly when this Command is scheduled to run 20 | protected void execute() { 21 | } 22 | 23 | // Make this return true when this Command no longer needs to run execute() 24 | protected boolean isFinished() { 25 | return false; 26 | } 27 | 28 | // Called once after isFinished returns true 29 | protected void end() { 30 | } 31 | 32 | // Called when another command which requires one or more of the same 33 | // subsystems is scheduled to run 34 | protected void interrupted() { 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/Gearbox.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import edu.wpi.first.wpilibj.DoubleSolenoid; 4 | import edu.wpi.first.wpilibj.Encoder; 5 | import edu.wpi.first.wpilibj.SpeedController; 6 | 7 | /** 8 | * Gearbox Interface 9 | * 10 | * @author Wesley 11 | * 12 | */ 13 | public class Gearbox { 14 | private SpeedController m_motor1; 15 | private SpeedController m_motor2; 16 | private SpeedController m_motor3; 17 | private Encoder m_encoder; 18 | private DoubleSolenoid m_shifter; 19 | private boolean m_reversed; 20 | 21 | public Gearbox(SpeedController motor1, SpeedController motor2, SpeedController motor3, Encoder encoder, DoubleSolenoid shifter) { 22 | m_motor1 = motor1; 23 | m_motor2 = motor2; 24 | m_motor3 = motor3; 25 | 26 | m_encoder = encoder; 27 | m_shifter = shifter; 28 | } 29 | 30 | public Gearbox(SpeedController motor1, SpeedController motor2, SpeedController motor3, Encoder encoder) { 31 | this(motor1, motor2, motor3, encoder, null); 32 | } 33 | 34 | public Gearbox(SpeedController motor1, SpeedController motor2, SpeedController motor3, DoubleSolenoid shifter) { 35 | this(motor1, motor2, motor3, null, shifter); 36 | } 37 | 38 | public Gearbox(SpeedController motor1, SpeedController motor2, SpeedController motor3) { 39 | this(motor1, motor2, motor3, null, null); 40 | } 41 | 42 | /** 43 | * Sets whether to inverse the motor speed 44 | * 45 | * @param reversed 46 | */ 47 | public void setReversed(boolean reversed) { 48 | m_reversed = reversed; 49 | } 50 | 51 | /** 52 | * Sets the reversed value to true 53 | */ 54 | public void setReversed() { 55 | setReversed(true); 56 | } 57 | 58 | /** 59 | * Shift to high gear 60 | */ 61 | public void shiftUp() { 62 | if(m_shifter != null) { 63 | m_shifter.set(DoubleSolenoid.Value.kReverse); 64 | } 65 | } 66 | 67 | /** 68 | * Shift to low gear 69 | */ 70 | public void shiftDown() { 71 | if(m_shifter != null) { 72 | m_shifter.set(DoubleSolenoid.Value.kForward); 73 | } 74 | } 75 | 76 | /** 77 | * Sets the Encoder distance per pulse 78 | * 79 | * @param distancePerPulse 80 | */ 81 | public void setEncoderDistancePerPulse(double distancePerPulse) { 82 | if(m_encoder != null) { 83 | m_encoder.setDistancePerPulse(distancePerPulse); 84 | } 85 | } 86 | 87 | /** 88 | * Gets the encoder distance 89 | * 90 | * @return 91 | */ 92 | public double getEncoderDistance() { 93 | if(m_encoder != null) { 94 | return m_encoder.getDistance(); 95 | } else { 96 | return 0; 97 | } 98 | } 99 | 100 | /** 101 | * Gets the raw value of the encoder 102 | * 103 | * @return 104 | */ 105 | public int getEncoderRaw() { 106 | if(m_encoder != null) { 107 | return m_encoder.getRaw(); 108 | } else { 109 | return 0; 110 | } 111 | } 112 | 113 | /** 114 | * Rezeros the encoder reading 115 | */ 116 | public void resetEncoder() { 117 | if(m_encoder != null) { 118 | m_encoder.reset(); 119 | } 120 | } 121 | 122 | /** 123 | * Sets the motor speed 124 | * 125 | * @param speed 126 | */ 127 | public void setSpeed(double speed) { 128 | m_motor1.set(speed * (m_reversed ? -1 : 1)); 129 | m_motor2.set(speed * (m_reversed ? -1 : 1)); 130 | m_motor3.set(speed * (m_reversed ? -1 : 1)); 131 | } 132 | } 133 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/Logger.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import java.io.File; 4 | import java.io.FileWriter; 5 | import java.io.IOException; 6 | 7 | /** 8 | * Logger Class 9 | * 10 | * @author Michael 11 | * 12 | */ 13 | public class Logger { 14 | private File m_csv; 15 | private FileWriter m_fw; 16 | private String m_filename; 17 | 18 | /** 19 | * Constructs file logger. 20 | * @param filename 21 | */ 22 | public Logger(String filename) { 23 | m_filename = filename; 24 | } 25 | 26 | /** 27 | * Run to create file to write to. 28 | */ 29 | public void init() { 30 | m_csv = new File(m_filename); 31 | try { 32 | m_csv.createNewFile(); 33 | m_fw = new FileWriter(m_csv); 34 | } catch (IOException e) { 35 | e.printStackTrace(); 36 | return; 37 | } 38 | } 39 | 40 | /** 41 | * Pass a string to write a message to the file. 42 | * @param message 43 | */ 44 | //TODO make it so you cant do this if init has not been run 45 | public void write(String message) { 46 | try { 47 | m_fw.write(message); 48 | } catch (IOException e) { 49 | e.printStackTrace(); 50 | } 51 | } 52 | 53 | /** 54 | * Close the file. 55 | */ 56 | public void close() { 57 | try { 58 | m_fw.flush(); 59 | } catch (IOException e) { 60 | e.printStackTrace(); 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/Loopable.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | /** 4 | * Make a Loopable and give it to the Looper 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public interface Loopable { 10 | public void update(); 11 | } 12 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/Looper.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import java.util.Timer; 4 | import java.util.TimerTask; 5 | 6 | 7 | /** 8 | * A separate timed thread that gets called periodically 9 | * 10 | * @author Wesley 11 | * 12 | */ 13 | public class Looper { 14 | 15 | private double period = 1.0 / 100.0; 16 | protected Loopable loopable; 17 | private Timer looperUpdater; 18 | protected String m_name; 19 | 20 | /** 21 | * Default constructor 22 | * 23 | * @param name 24 | * @param loopable 25 | * @param period 26 | */ 27 | public Looper(String name, Loopable loopable, double period) { 28 | this.period = period; 29 | this.loopable = loopable; 30 | this.m_name = name; 31 | } 32 | 33 | private class Task extends TimerTask { 34 | 35 | private Looper looper; 36 | 37 | public Task(Looper looper) { 38 | if (looper == null) { 39 | throw new NullPointerException("Given Looper was null"); 40 | } 41 | this.looper = looper; 42 | } 43 | 44 | public void run() { 45 | looper.update(); 46 | } 47 | } 48 | 49 | /** 50 | * Start the thread 51 | */ 52 | public void start() { 53 | if (looperUpdater == null) { 54 | looperUpdater = new Timer("Looper - " + this.m_name); 55 | looperUpdater.scheduleAtFixedRate(new Task(this), 0L, (long) (this.period * 1000)); 56 | } 57 | } 58 | 59 | /** 60 | * Stop the thread 61 | */ 62 | public void stop() { 63 | if (looperUpdater != null) { 64 | looperUpdater.cancel(); 65 | looperUpdater = null; 66 | } 67 | } 68 | 69 | private void update() { 70 | loopable.update(); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/MultiLooper.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import java.util.Vector; 4 | 5 | /** 6 | * Loops a bunch of stuff 7 | * 8 | * @author Wesley 9 | * 10 | */ 11 | public class MultiLooper implements Loopable { 12 | private Looper m_looper; 13 | private Vector m_loopables = new Vector(); 14 | 15 | public MultiLooper(String name, double period) { 16 | m_looper = new Looper(name, this, period); 17 | } 18 | 19 | @Override 20 | public void update() { 21 | for(int i = 0; i < m_loopables.size(); i++) { 22 | m_loopables.get(i).update(); 23 | } 24 | } 25 | 26 | public void addLoopable(Loopable loopable) { 27 | m_loopables.add(loopable); 28 | } 29 | 30 | public void start() { 31 | m_looper.start(); 32 | } 33 | 34 | public void stop() { 35 | m_looper.stop(); 36 | } 37 | 38 | } 39 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/NerdyButton.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import edu.wpi.first.wpilibj.Joystick; 4 | 5 | /** 6 | * Button monitoring system 7 | * 8 | * @author Wesley 9 | * 10 | */ 11 | public class NerdyButton { 12 | private int m_buttonPort; 13 | private boolean m_current = false; 14 | private boolean m_last = false; 15 | private Joystick m_joy; 16 | 17 | /** 18 | * Default constructor 19 | * 20 | * @param joy The joystick the button is located on 21 | * @param buttonPort The port of the button 22 | */ 23 | public NerdyButton(Joystick joy, int buttonPort) { 24 | m_buttonPort = buttonPort; 25 | m_joy = joy; 26 | } 27 | 28 | /** 29 | * Was the button just pressed? 30 | * 31 | * @return True for pressed, false for not pressed 32 | */ 33 | public boolean wasPressed() { 34 | return m_current && !m_last; 35 | } 36 | 37 | /** 38 | * Was the button just released? 39 | * 40 | * @return True for released, false for not released 41 | */ 42 | public boolean wasReleased() { 43 | return !m_current && m_last; 44 | } 45 | 46 | /** 47 | * Was the button held? 48 | * 49 | * @return True for held, else false 50 | */ 51 | public boolean wasHeld() { 52 | return m_current && m_last; 53 | } 54 | 55 | /** 56 | * Is the button pressed? 57 | * 58 | * @return True for pressed, false for not pressed 59 | */ 60 | public boolean get() { 61 | return m_current; 62 | } 63 | 64 | /** 65 | * Updated the values. Run every time getting a value 66 | */ 67 | public void update() { 68 | m_last = m_current; 69 | m_current = m_joy.getRawButton(m_buttonPort); 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/NerdyJoystick.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import edu.wpi.first.wpilibj.Joystick; 4 | 5 | /** 6 | * An improved Joystick class 7 | * 8 | * @author Wesley 9 | * 10 | */ 11 | public class NerdyJoystick extends Joystick{ 12 | 13 | /** 14 | * Default constructor 15 | * 16 | * @param port The port on driverstation 17 | */ 18 | public NerdyJoystick(int port) { 19 | super(port); 20 | } 21 | 22 | /** 23 | * Gets the real Y position (up is positive) 24 | * 25 | * @return +1 for all the way up, -1 for all the way down 26 | */ 27 | public double getTrueY() { 28 | return -getY(); 29 | } 30 | 31 | /** 32 | * Gets the angle of the joystick in radians 33 | * @return 0 is forward, pi/2 is right, pi is backwards, 3pi/2 is left 34 | */ 35 | public double getAngleRad() { 36 | return (Math.atan2(getTrueY(), getX()) + 3*Math.PI/2) % (2*Math.PI); 37 | } 38 | 39 | /** 40 | * Gets the angle of the joystick in degrees 41 | * @return 0 is forward, 90 is right, 180 is backwards, 270 is left 42 | */ 43 | public double getAngleDeg() { 44 | return NerdyMath.radsToDeg(getAngleRad()); 45 | } 46 | 47 | /** 48 | * Gets a Button class with a specified port 49 | * 50 | * @param port The number that corresponds to the joystick button number 51 | * @return The instance of this button. 52 | */ 53 | public NerdyButton getButton(int port) { 54 | return new NerdyButton(this, port); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/NerdyMath.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | /** 4 | * Math class with added functionality 5 | * 6 | * @author Wesley 7 | * 8 | */ 9 | public class NerdyMath { 10 | public static double radsToDeg(double deg) { 11 | // Native algorithm 12 | return deg * 360 / (2 * Math.PI); 13 | } 14 | 15 | public static double degToRads(double rads) { 16 | // Native algorithm 17 | return rads * (2 * Math.PI) / 360; 18 | } 19 | 20 | /** 21 | * Thresholds the value between a upper and lower limit 22 | * 23 | * @param val The value 24 | * @param min The lower limit 25 | * @param max The upper limit 26 | * @return The thresholded value 27 | */ 28 | public static double threshold(double val, double min, double max) { 29 | return Math.min(max, Math.max(min, val)); 30 | } 31 | 32 | /** 33 | * Normalizes the array to -1 or 1 34 | * 35 | * @param values The values 36 | * @param scaleUp True makes the values scale no matter what 37 | * @return The normalized values 38 | */ 39 | public static double[] normalize(double[] values, boolean scaleUp){ 40 | double[] normalizedValues = new double[values.length]; 41 | double max = Math.max(Math.abs(values[0]), Math.abs(values[1])); 42 | for(int i = 2; i < values.length; i++){ 43 | max = Math.max(Math.abs(values[i]), max); 44 | } 45 | if(max < 1 && scaleUp == false) { 46 | for(int i = 0; i < values.length; i++){ 47 | normalizedValues[i] = values[i]; 48 | } 49 | } else { 50 | for(int i = 0; i < values.length; i++){ 51 | normalizedValues[i] = values[i] / max; 52 | } 53 | } 54 | 55 | return normalizedValues; 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/NerdyPID.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | import edu.wpi.first.wpilibj.util.BoundaryException; 4 | 5 | /** 6 | * PID Controller 7 | * 8 | * @author Wesley 9 | * 10 | */ 11 | public class NerdyPID { 12 | // Constants 13 | private double m_kP; 14 | private double m_kI; 15 | private double m_kD; 16 | 17 | private double m_minimumOutput = -1; 18 | private double m_maximumOutput = 1; 19 | 20 | private double m_error = 0; 21 | private double m_lastError = 0; 22 | private double m_totalError = 0; 23 | private double m_desired = 0; 24 | private double m_result = 0; 25 | private double m_lastInput = Double.NaN; 26 | 27 | /** 28 | * Default Constructor. Need to call the setPID method after this. 29 | * Default PID values are all 0. 30 | */ 31 | public NerdyPID() { 32 | setPID(0, 0, 0); 33 | } 34 | 35 | /** 36 | * Constructs a PID controller with the set PID Values 37 | * 38 | * @param p 39 | * @param i 40 | * @param d 41 | */ 42 | public NerdyPID(double p, double i, double d) { 43 | setPID(p, i, d); 44 | } 45 | 46 | /** 47 | * The magical calculation 48 | * 49 | * @param input The sensor input 50 | * @return The magical output 51 | */ 52 | public double calculate(double input) { 53 | m_lastInput = input; 54 | m_lastError = m_error; 55 | m_error = m_desired - input; 56 | m_totalError += m_error; 57 | 58 | m_result = NerdyMath.threshold((m_kP * m_error) + (m_kI * m_totalError) - (m_kD * (m_error - m_lastError)), m_minimumOutput, m_maximumOutput); 59 | 60 | return m_result; 61 | } 62 | 63 | /** 64 | * Sets the PID Values 65 | * 66 | * @param p 67 | * @param i 68 | * @param d 69 | */ 70 | public void setPID(double p, double i, double d) { 71 | m_kP = p; 72 | m_kI = i; 73 | m_kD = d; 74 | } 75 | 76 | /** 77 | * Sets the target value 78 | * 79 | * @param desired 80 | */ 81 | public void setDesired(double desired) { 82 | m_desired = desired; 83 | } 84 | 85 | public void setOutputRange(double minimum, double maximum) { 86 | if(minimum > maximum) { 87 | throw new BoundaryException("Lower bound is greater than upper bound"); 88 | } 89 | } 90 | 91 | /** 92 | * Gets the output of the controller 93 | * 94 | * @return The last calculated output. 95 | */ 96 | public double get() { 97 | return m_result; 98 | } 99 | 100 | /** 101 | * Return true if the error is within the tolerance 102 | * 103 | * @return true if the error is less than the tolerance 104 | */ 105 | public boolean onTarget(double tolerance) { 106 | return m_lastInput != Double.NaN && Math.abs(m_lastInput - m_desired) < tolerance; 107 | } 108 | } 109 | -------------------------------------------------------------------------------- /src/org/camsrobotics/lib/Subsystem.java: -------------------------------------------------------------------------------- 1 | package org.camsrobotics.lib; 2 | 3 | public abstract class Subsystem implements Loopable { 4 | private final String m_name; 5 | 6 | public Subsystem(String name) { 7 | m_name = name; 8 | } 9 | 10 | public String getName() { 11 | return m_name; 12 | } 13 | 14 | public abstract void reportState(); 15 | } 16 | -------------------------------------------------------------------------------- /sysProps.xml: -------------------------------------------------------------------------------- 1 | <?xml version='1.0' encoding='utf-16le'?><NISysAPI_Results hr='0' version='00010001'><PropertyBag><Property tag='1000000' type='6'>//localhost/nisyscfg/system</Property><Property tag='1001000' type='1'>1</Property><Property tag='1002000' type='3'>0</Property><Property tag='1004000' type='6'>National Instruments</Property><Property tag='1005000' type='3'>30450</Property><Property tag='1006000' type='6'>roboRIO</Property><Property tag='1007000' type='6'>0306AD98</Property><Property tag='1008000' type='1'>1</Property><Property tag='1009000' type='1'>0</Property><Property tag='100D000' type='3'>0</Property><Property tag='101C000' type='2'>1</Property><Property tag='101D000' type='6'>system</Property><Property tag='101E000' type='6'>nisyscfg</Property><Property tag='101F000' type='6'>roboRIO-687-FRC</Property><Property tag='1020000' type='1'>0</Property><Property tag='1022000' type='5'>{00000000-0000-0000-0000-000000000000}</Property><Property tag='1024000' type='2'>1</Property><Property tag='1028000' type='1'>0</Property><Property tag='102A000' type='2'>1000</Property><Property tag='102F000' type='6'>3.0.0f0</Property><Property tag='1033000' type='6'>00:80:2F:19:21:7B</Property><Property tag='1037000' type='3'>983040</Property><Property tag='1038000' type='1'>1</Property><Property tag='1039000' type='2'>1</Property><Property tag='103A000' type='3'>4</Property><Property tag='103C000' type='6'>cRIO</Property><Property tag='103D000' type='6' /><Property tag='104A000' type='1'>1</Property><Property tag='104B000' type='6'>*.cfg</Property><Property tag='104C000' type='2'>0</Property><Property tag='104E000' type='6'>Linux-ARMv7-A</Property><Property tag='104F000' type='6'>3.14.40-rt37-ni-3.2.0d0</Property><Property tag='1050000' type='6'>NI Linux Real-Time ARMv7-A 3.14.40-rt37-ni-3.2.0d0</Property><Property tag='1051000' type='7'>122FD000 CB3A4821 D3B085AA 0</Property><Property tag='1052000' type='1'>0</Property><Property tag='1053000' type='6'>Running</Property><Property tag='1054000' type='1'>0</Property><Property tag='1058000' type='2'>2</Property><Property tag='5105000' type='1'>0</Property><Property tag='D11D000' type='6'>en</Property><Property tag='D11E000' type='6'>en</Property><Property tag='D120000' type='6'>CUT0</Property><Property tag='D122000' type='4'>395580.000000</Property><Property tag='D123000' type='4'>237980.000000</Property><Property tag='D129000' type='1'>1</Property><Property tag='D12B000' type='2'>-25200</Property><Property tag='D14E000' type='6'>UTC</Property><Property tag='D159000' type='6' /><Property tag='D15A000' type='6'>{28F4E7BB-E931-4AF9-96ED-CA15CF87C877}</Property><Property tag='D15B000' type='6'>roboRIO Image</Property><Property tag='D15C000' type='6'>FRC_roboRIO_2016_v19</Property><Property tag='D15D000' type='1'>1</Property><Property tag='D15E000' type='1'>0</Property><Property tag='D15F000' type='1'>1</Property><Property tag='13000000' type='1'>1</Property><Property tag='14000000' type='1'>1</Property><Property tag='14002000' type='1'>0</Property></PropertyBag></NISysAPI_Results> 2 | --------------------------------------------------------------------------------