mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge branch 'master' into add-storage
This commit is contained in:
ryan123rudder
@@ -1,21 +1,24 @@
|
||||
MIT License
|
||||
Copyright (c) 2009-2018 FIRST
|
||||
All rights reserved.
|
||||
|
||||
Copyright (c) 2019 HFocus
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the FIRST nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
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:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
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.
|
||||
THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
|
||||
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -7,6 +7,7 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
@@ -28,13 +29,19 @@ public final class Constants {
|
||||
|
||||
/* PID Constants Drive*/
|
||||
public static final int DRIVE_TIMEOUT_MS = 30;
|
||||
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.1, 0, 1.0);
|
||||
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.3);
|
||||
public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final int DRIVE_CRUISE_VELOCITY = 20000;
|
||||
public static final int DRIVE_ACCELERATION = 7000;
|
||||
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05);
|
||||
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.5);
|
||||
//public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
//public static final int DRIVE_CRUISE_VELOCITY = 20000;
|
||||
//public static final int DRIVE_ACCELERATION = 7000;
|
||||
|
||||
/* Trajectory Constants */
|
||||
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
|
||||
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3;
|
||||
public static final double TRACK_WIDTH_METERS = 0.648;
|
||||
public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
|
||||
|
||||
/* Remote Sensors */
|
||||
public final static int REMOTE_0 = 0;
|
||||
public final static int REMOTE_1 = 1;
|
||||
@@ -50,36 +57,44 @@ public final class Constants {
|
||||
public final static int SLOT_MOTION_MAGIC = 3;
|
||||
|
||||
/* Drive Train Characteristics */
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048*2;
|
||||
public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5;
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 6;
|
||||
public static final double TICKS_PER_GYRO_REV = 8192;
|
||||
|
||||
/* Ratio Calculation */
|
||||
public static final double TICK_TIME_TO_SECONDS = 0.1;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
|
||||
public static final double WHEEL_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO;
|
||||
public static final double WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
|
||||
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
|
||||
public static final double INCHES_PER_METER = 39.370;
|
||||
public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
|
||||
}
|
||||
|
||||
public static final class IntakeConstants {
|
||||
public static final int INTAKE_SPARK_ID = 9;
|
||||
public static final int EXTENDER_SPARK_ID = 10;
|
||||
public static final int INTAKE_SPARK_ID = -9;
|
||||
public static final int EXTENDER_SPARK_ID = -10;
|
||||
}
|
||||
|
||||
public static final class ShooterConstants {
|
||||
public static final int SHOOTER_FALCON_ID = 8;
|
||||
/* Motor IDs */
|
||||
public static final int SHOOTER_FALCON_ID = -1;
|
||||
public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
|
||||
public static final int SHOOTER_ROTATE_ID = 10;
|
||||
|
||||
/* PID Constants Shooter */
|
||||
public static final int SHOOTER_SLOT_IDX = 0;
|
||||
public static final int SHOOTER_PID_LOOP_IDX = 1;
|
||||
public static final int SHOOTER_TIMEOUT_MS = 30;
|
||||
public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
|
||||
|
||||
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
|
||||
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final double SHOOTER_TURRET_MIN = -1.0;
|
||||
public static final double ENCODER_TICKS_PER_REV = 2048;
|
||||
public static final double NEO_UNITS_PER_REV = 42;
|
||||
public static final double DEGREES_PER_ROT = 360;
|
||||
}
|
||||
|
||||
public static final class ClimberConstants {
|
||||
@@ -91,7 +106,7 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class StorageConstants {
|
||||
public static final int STORAGE_CAN_ID = 10;
|
||||
public static final int STORAGE_CAN_ID = -1;
|
||||
|
||||
/* Ball Indexes */
|
||||
public static final int BEAM_SENSOR_DIO_0 = 0;
|
||||
@@ -116,6 +131,17 @@ public final class Constants {
|
||||
public static final int LED_SPARK_ID = 0;
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
public static final double FOV = 29.8; //Field of view of limelight
|
||||
public static final double TARGET_HEIGHT = 82.75;
|
||||
public static final double LIME_ANGLE = 18.7366;
|
||||
public static final double TURN_P_VALUE = 0.65;
|
||||
public static final double X_ANGLE_ERROR = 1.3;
|
||||
public static final double MOTOR_DEAD_ZONE = 0.3;
|
||||
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
|
||||
public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
|
||||
@@ -7,9 +7,11 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
@@ -34,6 +36,7 @@ public class Robot extends TimedRobot {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
SmartDashboard.putString("Auto?", "NAH");
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -61,6 +64,7 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
//m_robotContainer.setDriveGearState(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -75,6 +79,10 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
m_robotContainer.resetOdometry();
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
@@ -85,6 +93,7 @@ public class Robot extends TimedRobot {
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
System.err.println("Auto Start");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -98,6 +107,9 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
@@ -105,6 +117,8 @@ public class Robot extends TimedRobot {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
|
||||
SmartDashboard.putString("Auto?", "NAH");
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -7,19 +7,34 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.controller.RamseteController;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.trajectory.Trajectory;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RamseteCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.DriveStraightAtVelocityPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveWithJoystickAuxPID;
|
||||
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
|
||||
import frc4388.robot.commands.DriveWithJoystickDriveStraight;
|
||||
import frc4388.robot.commands.RunClimberWithTriggers;
|
||||
import frc4388.robot.commands.RunExtenderOutIn;
|
||||
import frc4388.robot.commands.RunIntakeWithTriggers;
|
||||
@@ -33,6 +48,8 @@ import frc4388.robot.commands.RunLevelerWithJoystick;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.commands.TrackTarget;
|
||||
import frc4388.robot.subsystems.Camera;
|
||||
import frc4388.robot.subsystems.Leveler;
|
||||
import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
@@ -56,6 +73,10 @@ public class RobotContainer {
|
||||
private final Leveler m_robotLeveler = new Leveler();
|
||||
private final Storage m_robotStorage = new Storage();
|
||||
|
||||
/* Cameras */
|
||||
private final Camera m_robotCameraFront = new Camera("front",0,160,120,40);
|
||||
private final Camera m_robotCameraBack = new Camera("back",1,160,120,40);
|
||||
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
@@ -76,9 +97,10 @@ public class RobotContainer {
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
// runs the drum shooter in idle mode
|
||||
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter));
|
||||
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
|
||||
// drives the leveler with an axis input from the driver controller
|
||||
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -91,16 +113,19 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
//new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
// .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
// .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
|
||||
.whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
.whileHeld(new TrackTarget(m_robotShooter));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(new RunExtenderOutIn(m_robotIntake));
|
||||
@@ -116,27 +141,33 @@ public class RobotContainer {
|
||||
|
||||
// resets the yaw of the pigeon
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
|
||||
.whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
|
||||
|
||||
// turn 45 degrees
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
|
||||
|
||||
|
||||
// sets solenoids into high gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
|
||||
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
|
||||
|
||||
// sets solenoids into low gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
|
||||
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
|
||||
|
||||
// interrupts any running command
|
||||
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
|
||||
|
||||
// safety for climber and leveler
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
|
||||
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
|
||||
|
||||
/* Storage Neo PID Test */
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
|
||||
.whileHeld(new TrackTarget(m_robotShooter));
|
||||
|
||||
//Prepares storage for intaking
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
|
||||
@@ -146,7 +177,7 @@ public class RobotContainer {
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake()));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
* @param mode NeutralMode to set motors to
|
||||
@@ -155,13 +186,59 @@ public class RobotContainer {
|
||||
m_robotDrive.setDriveTrainNeutralMode(mode);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the gear of the drivetrain
|
||||
* @param state the gearing of the gearbox (true is high, false is low)
|
||||
*/
|
||||
public void setDriveGearState(boolean state) {
|
||||
m_robotDrive.setShiftState(state);
|
||||
}
|
||||
|
||||
public void configDriveTrainSensors(FeedbackDevice type) {
|
||||
m_robotDrive.configMotorSensor(type);
|
||||
}
|
||||
|
||||
public void resetOdometry() {
|
||||
m_robotDrive.resetGyroAngles();
|
||||
m_robotDrive.setOdometry(new Pose2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
|
||||
// Create config for trajectory
|
||||
/*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND,
|
||||
DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
.setKinematics(DriveConstants.kDriveKinematics);
|
||||
|
||||
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
new Pose2d(0, 0, new Rotation2d(0)),
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(
|
||||
new Translation2d(10, 0)
|
||||
),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(20, 20, new Rotation2d(0)),
|
||||
// Pass config
|
||||
config);
|
||||
// 10 = 20, 20 = 35, 30 = 53.5
|
||||
// (0,10) = (8,22)
|
||||
RamseteCommand ramseteCommand = new RamseteCommand(
|
||||
exampleTrajectory,
|
||||
m_robotDrive::getPose,
|
||||
new RamseteController(),
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_robotDrive::tankDriveVelocity,
|
||||
m_robotDrive);
|
||||
|
||||
// Run path following command, then stop at the end.
|
||||
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
|
||||
return new InstantCommand();
|
||||
}
|
||||
|
||||
|
||||
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
|
||||
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -47,7 +47,7 @@ public class DriveStraightToPositionPID extends CommandBase {
|
||||
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro);
|
||||
m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -38,7 +38,7 @@ public class DriveWithJoystick extends CommandBase {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double moveInput = m_controller.getLeftYAxis();
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveOutput = 0;
|
||||
double steerOutput = 0;
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpiutil.math.MathUtil;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class DriveWithJoystickDriveStraight extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_targetGyro, m_currentGyro;
|
||||
double m_stopPos;
|
||||
long m_currTime, m_deltaTime;
|
||||
long m_deadTimeSteer, m_deadTimeMove;
|
||||
long m_deadTimeout = 100;
|
||||
IHandController m_controller;
|
||||
|
||||
/**
|
||||
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
|
||||
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
|
||||
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
|
||||
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
|
||||
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
|
||||
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
*/
|
||||
public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_controller = controller;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_currTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveOutput = 0;
|
||||
m_deltaTime = System.currentTimeMillis() - m_currTime;
|
||||
m_currTime = System.currentTimeMillis();
|
||||
|
||||
/* If steer stick is being used */
|
||||
if (steerInput != 0) {
|
||||
m_deadTimeSteer = m_currTime;
|
||||
}
|
||||
|
||||
/* Curves the moveInput to be slightly more gradual at first */
|
||||
if (moveInput >= 0) {
|
||||
moveOutput = -Math.cos(1.571*moveInput)+1;
|
||||
} else {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
}
|
||||
|
||||
/* If steer stick has not been used for less than 1 sec */
|
||||
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
|
||||
runDriveWithInput(moveOutput, steerInput);
|
||||
resetGyroTarget();
|
||||
}
|
||||
/* If steer stick has not been used for 1 sec */
|
||||
else {
|
||||
runDriveStraight(moveOutput);
|
||||
}
|
||||
}
|
||||
|
||||
private void runDriveWithInput(double move, double steer) {
|
||||
double cosMultiplier = .45;
|
||||
double steerOutput = 0;
|
||||
double deadzone = .2;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steer > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
|
||||
} else {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
|
||||
}
|
||||
m_drive.driveWithInput(move, steerOutput);
|
||||
System.out.println("Driving With Input");
|
||||
}
|
||||
|
||||
private void runDriveStraight(double move) {
|
||||
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
|
||||
System.out.println("Driving Straight with Target: " + m_targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* set target angle to current angle (prevents buildup of gyro error).
|
||||
*/
|
||||
private void resetGyroTarget() {
|
||||
//m_targetGyro = m_currentGyro;
|
||||
m_targetGyro = m_currentGyro
|
||||
+ m_drive.getTurnRate();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,147 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpiutil.math.MathUtil;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_targetGyro, m_currentGyro;
|
||||
double m_stopPos;
|
||||
long m_currTime, m_deltaTime;
|
||||
long m_deadTimeSteer, m_deadTimeMove;
|
||||
long m_deadTimeout = 100;
|
||||
IHandController m_controller;
|
||||
|
||||
/**
|
||||
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
|
||||
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
|
||||
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
|
||||
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
|
||||
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
|
||||
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
*/
|
||||
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_controller = controller;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_currTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveOutput = 0;
|
||||
m_deltaTime = System.currentTimeMillis() - m_currTime;
|
||||
m_currTime = System.currentTimeMillis();
|
||||
|
||||
/* If move stick is being used */
|
||||
if (moveInput != 0) {
|
||||
m_deadTimeMove = m_currTime;
|
||||
m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
|
||||
+ (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
|
||||
}
|
||||
/* If steer stick is being used */
|
||||
if (steerInput != 0) {
|
||||
m_deadTimeSteer = m_currTime;
|
||||
}
|
||||
|
||||
/* If move stick has been pressed within 1 sec */
|
||||
if (m_currTime - m_deadTimeMove < m_deadTimeout) {
|
||||
/* Curves the moveInput to be slightly more gradual at first */
|
||||
if (moveInput >= 0) {
|
||||
moveOutput = -Math.cos(1.571*moveInput)+1;
|
||||
} else {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
}
|
||||
|
||||
/* If steer stick has not been used for less than 1 sec */
|
||||
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
|
||||
runDriveWithInput(moveOutput, steerInput);
|
||||
resetGyroTarget();
|
||||
}
|
||||
/* If steer stick has not been used for 1 sec */
|
||||
else {
|
||||
runDriveStraight(moveOutput);
|
||||
}
|
||||
}
|
||||
/* If the move stick has not been used for 1 sec */
|
||||
else {
|
||||
runStoppedTurn(steerInput);
|
||||
}
|
||||
}
|
||||
|
||||
private void runDriveWithInput(double move, double steer) {
|
||||
double cosMultiplier = .45;
|
||||
double steerOutput = 0;
|
||||
double deadzone = .2;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steer > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
|
||||
} else {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
|
||||
}
|
||||
m_drive.driveWithInput(move, steerOutput);
|
||||
System.out.println("Driving With Input");
|
||||
}
|
||||
|
||||
private void runDriveStraight(double move) {
|
||||
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
|
||||
System.out.println("Driving Straight with Target: " + m_targetGyro);
|
||||
}
|
||||
|
||||
private void runStoppedTurn(double steer) {
|
||||
updateGyroTarget(steer);
|
||||
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
|
||||
System.out.println("Turning with Target: " + m_targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* If AuxPID is enabled, then update using the steer input
|
||||
*/
|
||||
private void updateGyroTarget(double steerInput) {
|
||||
m_targetGyro -= 5 * steerInput * m_deltaTime;
|
||||
m_targetGyro = MathUtil.clamp( m_targetGyro,
|
||||
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
|
||||
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
|
||||
}
|
||||
|
||||
/**
|
||||
* set target angle to current angle (prevents buildup of gyro error).
|
||||
*/
|
||||
private void resetGyroTarget() {
|
||||
m_targetGyro = m_currentGyro;
|
||||
m_targetGyro = m_currentGyro
|
||||
+ m_drive.getTurnRate();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
public class PlaySongDrive extends CommandBase {
|
||||
private Drive m_drive;
|
||||
|
||||
/**
|
||||
* Creates a new PlaySongDrive.
|
||||
*/
|
||||
public PlaySongDrive(Drive subsystem) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_drive.m_rightFrontMotor.set(0);
|
||||
m_drive.m_leftFrontMotor.set(0);
|
||||
m_drive.m_rightBackMotor.set(0);
|
||||
m_drive.m_leftBackMotor.set(0);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_drive.playSong();
|
||||
//System.err.println("Playing " + m_drive.m_orchestra.isPlaying());
|
||||
//m_drive.m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class TrackTarget extends CommandBase {
|
||||
//Setup
|
||||
NetworkTableEntry xEntry;
|
||||
Shooter m_shooter;
|
||||
IHandController m_driverController;
|
||||
//Aiming
|
||||
double turnAmount = 0;
|
||||
double xAngle = 0;
|
||||
double yAngle = 0;
|
||||
double target = 0;
|
||||
public double distance;
|
||||
|
||||
/**
|
||||
* Uses the Limelight to track the target
|
||||
*/
|
||||
public TrackTarget(Shooter shooterSubsystem) {
|
||||
m_shooter = shooterSubsystem;
|
||||
addRequirements(m_shooter);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
//Vision Processing Mode
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
|
||||
xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
|
||||
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
|
||||
|
||||
if (target == 1.0){ //If target in view
|
||||
//Aiming Left/Right
|
||||
turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
|
||||
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone
|
||||
//Deadzones
|
||||
else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;}
|
||||
else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
|
||||
m_shooter.runShooterWithInput(turnAmount/5);
|
||||
|
||||
//Finding Distance
|
||||
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
|
||||
SmartDashboard.putNumber("Distance to Target", distance);
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
//Drive Camera Mode
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.cscore.MjpegServer;
|
||||
import edu.wpi.cscore.UsbCamera;
|
||||
import edu.wpi.cscore.VideoSource;
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
|
||||
|
||||
public class Camera extends SubsystemBase {
|
||||
CameraServer camServ = CameraServer.getInstance();
|
||||
/**
|
||||
* Creates a new Camera.
|
||||
* Makes a Camera and sends the stream to a CameraServer, to be viewed in Shuffle Board.
|
||||
* @param name Name of the Camera in Shuffle Board.
|
||||
* @param id USB Id of the Camera.
|
||||
* @param width Resolution width.
|
||||
* @param height Resolution height.
|
||||
* @param brightness Percent brightness of the stream.
|
||||
*/
|
||||
public Camera(String name, int id, int width, int height, int brightness) {
|
||||
try{
|
||||
UsbCamera cam = new UsbCamera(name, id);
|
||||
cam.setResolution(width, height);
|
||||
cam.setBrightness(brightness);
|
||||
cam.setFPS(10);
|
||||
VideoSource camera = cam;
|
||||
camServ.startAutomaticCapture(camera);
|
||||
}
|
||||
catch(Exception e){
|
||||
System.err.println("Camera broken, pls nerf");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
}
|
||||
}
|
||||
@@ -10,6 +10,7 @@ package frc4388.robot.subsystems;
|
||||
import com.revrobotics.CANDigitalInput;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@@ -17,18 +18,23 @@ import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
public class Climber extends SubsystemBase {
|
||||
CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
|
||||
CANDigitalInput m_forwardLimit, m_reverseLimit;
|
||||
CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
|
||||
|
||||
public boolean climberSafety = false;
|
||||
|
||||
/**
|
||||
* Creates a new Climber.
|
||||
*/
|
||||
public Climber() {
|
||||
m_climberMotor.restoreFactoryDefaults();
|
||||
|
||||
m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
|
||||
m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
|
||||
|
||||
m_forwardLimit.enableLimitSwitch(false);
|
||||
m_reverseLimit.enableLimitSwitch(false);
|
||||
m_climberMotor.setIdleMode(IdleMode.kBrake);
|
||||
m_climberMotor.setInverted(false);
|
||||
|
||||
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
|
||||
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
|
||||
m_climberForwardLimit.enableLimitSwitch(false);
|
||||
m_climberReverseLimit.enableLimitSwitch(false);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -41,6 +47,23 @@ public class Climber extends SubsystemBase {
|
||||
* @param input the voltage to run motor at
|
||||
*/
|
||||
public void runClimber(double input) {
|
||||
m_climberMotor.set(input);
|
||||
if(climberSafety){
|
||||
m_climberMotor.set(input);
|
||||
}
|
||||
else{
|
||||
m_climberMotor.set(0);
|
||||
}
|
||||
}
|
||||
|
||||
/* Safety Button for Climber */
|
||||
public void setSafetyPressed()
|
||||
{
|
||||
climberSafety = true;
|
||||
}
|
||||
|
||||
/* Safety Button for Climber set back to false */
|
||||
public void setSafetyNotPressed()
|
||||
{
|
||||
climberSafety = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,6 +7,16 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.FilenameFilter;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.nio.file.Paths;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.stream.Collectors;
|
||||
import java.util.stream.Stream;
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.DemandType;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
@@ -18,15 +28,25 @@ import com.ctre.phoenix.motorcontrol.SensorTerm;
|
||||
import com.ctre.phoenix.motorcontrol.StatusFrame;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.music.Orchestra;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
|
||||
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpiutil.math.MathUtil;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Gains;
|
||||
|
||||
@@ -42,6 +62,11 @@ public class Drive extends SubsystemBase {
|
||||
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
|
||||
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
|
||||
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
|
||||
public Orchestra m_orchestra = new Orchestra();
|
||||
|
||||
public double m_rightFrontMotorPos;
|
||||
|
||||
public double m_rightFrontMotorVel;
|
||||
|
||||
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
|
||||
|
||||
@@ -49,9 +74,19 @@ public class Drive extends SubsystemBase {
|
||||
public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
|
||||
public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
|
||||
public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
|
||||
public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
|
||||
//public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
|
||||
|
||||
public final DifferentialDriveOdometry m_odometry;
|
||||
|
||||
public DoubleSolenoid m_speedShift;
|
||||
public DoubleSolenoid m_coolFalcon;
|
||||
|
||||
public DoubleSolenoid speedShift;
|
||||
SendableChooser<String> m_songChooser = new SendableChooser<String>();
|
||||
|
||||
public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
|
||||
public long m_lastTime, m_deltaTime; //in milliseconds
|
||||
|
||||
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
@@ -65,31 +100,40 @@ public class Drive extends SubsystemBase {
|
||||
m_pigeon.configFactoryDefault();
|
||||
resetGyroYaw();
|
||||
|
||||
speedShift = new DoubleSolenoid(7,0,1);
|
||||
m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()),
|
||||
new Pose2d(0, 0, new Rotation2d()) );
|
||||
|
||||
m_speedShift = new DoubleSolenoid(7,0,1);
|
||||
m_coolFalcon = new DoubleSolenoid(7,3,2);
|
||||
|
||||
coolFalcon(false);
|
||||
|
||||
/* set back motors as followers */
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(false);
|
||||
m_rightFrontMotor.setInverted(true);
|
||||
//m_driveTrain.setRightSideInverted(false);
|
||||
m_leftBackMotor.setInverted(InvertType.FollowMaster);
|
||||
m_rightBackMotor.setInverted(InvertType.FollowMaster);
|
||||
|
||||
setDriveTrainNeutralMode(NeutralMode.Coast);
|
||||
|
||||
/* deadbands */
|
||||
m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); //Ensures motors run at the same speed
|
||||
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
|
||||
//m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
//m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(false);
|
||||
m_rightFrontMotor.setInverted(true);
|
||||
m_driveTrain.setRightSideInverted(false);
|
||||
m_leftBackMotor.setInverted(InvertType.FollowMaster);
|
||||
m_rightBackMotor.setInverted(InvertType.FollowMaster);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
/* PID for Front Motor Control in Teleop */
|
||||
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
//m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
@@ -97,47 +141,65 @@ public class Drive extends SubsystemBase {
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
//m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* PID for Back Motor control in Auto */
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sensors for WPI_TalonFXs */
|
||||
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
resetEncoders();
|
||||
|
||||
/* Configure the left Talon's selected sensor as local QuadEncoder */
|
||||
m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout
|
||||
m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the left back Talon's selected sensor as local QuadEncoder */
|
||||
m_leftBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the right back Talon's selected sensor as local QuadEncoder */
|
||||
m_rightBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
|
||||
RemoteSensorSource.TalonSRX_SelectedSensor,
|
||||
DriveConstants.REMOTE_0, // Source number [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
|
||||
RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(),
|
||||
RemoteSensorSource.Pigeon_Yaw,
|
||||
DriveConstants.REMOTE_1,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
|
||||
DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sum signal to be used for Distance */
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
@@ -148,130 +210,170 @@ public class Drive extends SubsystemBase {
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference,
|
||||
DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient
|
||||
DriveConstants.PID_PRIMARY, // PID Slot of Source
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
configMotorSensor(FeedbackDevice.SensorDifference);
|
||||
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
/*
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient
|
||||
DriveConstants.PID_PRIMARY, // PID Slot of Source
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
*/
|
||||
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
|
||||
DriveConstants.PID_TURN,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1,
|
||||
DriveConstants.PID_TURN,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
m_leftFrontMotor.configSelectedFeedbackCoefficient( 1,
|
||||
DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Set status frame periods to ensure we don't have stale data */
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Set status frame periods to ensure we don't have stale data */
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Smart Dashboard Initial Values */
|
||||
|
||||
/* Set up Chooser */
|
||||
m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
|
||||
//setDriveTrainGains("Distance PID", m_gainsDistance);
|
||||
m_chooser.addOption("Velocity PID", m_gainsVelocity);
|
||||
//setDriveTrainGains("Velocity PID", m_gainsVelocity);
|
||||
m_chooser.addOption("Turning PID", m_gainsTurning);
|
||||
//setDriveTrainGains("Turning PID", m_gainsTurning);
|
||||
m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
|
||||
//setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
|
||||
Shuffleboard.getTab("PID").add(m_chooser);
|
||||
|
||||
/* Gyro */
|
||||
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
|
||||
/* Sensor Values */
|
||||
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
|
||||
|
||||
/* PID */
|
||||
Gains gains = m_chooser.getSelected();
|
||||
Shuffleboard.getTab("PID").add("P Value Drive", gains.m_kP);
|
||||
Shuffleboard.getTab("PID").add("I Value Drive", gains.m_kI);
|
||||
Shuffleboard.getTab("PID").add("D Value Drive", gains.m_kD);
|
||||
Shuffleboard.getTab("PID").add("F Value Drive", gains.m_kF);
|
||||
|
||||
|
||||
/**
|
||||
* Max out the peak output (for all modes).
|
||||
* However you can limit the output of a given PID object with configClosedLoopPeakOutput().
|
||||
*/
|
||||
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* 1ms per loop. PID loop can be slowed down if need be.
|
||||
* For example,
|
||||
* - if sensor updates are too slow
|
||||
* - sensor deltas are very small per update, so derivative error never gets large enough to be useful.
|
||||
* - sensor movement is very slow causing the derivative error to be near zero.
|
||||
*/
|
||||
* Max out the peak output (for all modes). However you can limit the output of
|
||||
* a given PID object with configClosedLoopPeakOutput().
|
||||
*/
|
||||
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* 1ms per loop. PID loop can be slowed down if need be. For example, - if
|
||||
* sensor updates are too slow - sensor deltas are very small per update, so
|
||||
* derivative error never gets large enough to be useful. - sensor movement is
|
||||
* very slow causing the derivative error to be near zero.
|
||||
*/
|
||||
int closedLoopTimeMs = 1;
|
||||
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
/**
|
||||
* configAuxPIDPolarity(boolean invert, int timeoutMs)
|
||||
* false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
|
||||
* true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
|
||||
*/
|
||||
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
|
||||
* output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
|
||||
* local output is PID0 - PID1, and other side Talon is PID0 + PID1
|
||||
*/
|
||||
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
|
||||
m_orchestra.addInstrument(m_leftBackMotor);
|
||||
m_orchestra.addInstrument(m_rightFrontMotor);
|
||||
m_orchestra.addInstrument(m_rightBackMotor);
|
||||
m_orchestra.addInstrument(m_leftFrontMotor);
|
||||
|
||||
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
|
||||
System.err.println(songsDir.getPath());
|
||||
String[] songsStrings = songsDir.list();
|
||||
for (String songString : songsStrings){
|
||||
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
|
||||
}
|
||||
Shuffleboard.getTab("Songs").add(m_songChooser);
|
||||
}
|
||||
|
||||
String currentSong = "";
|
||||
@Override
|
||||
public void periodic() {
|
||||
m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
|
||||
SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis());
|
||||
|
||||
if (m_currentTimeSec % 30 == 0) {
|
||||
coolFalcon(true);
|
||||
SmartDashboard.putBoolean("Solenoid", true);
|
||||
} else if ((m_currentTimeSec - 1) % 30 == 0) {
|
||||
coolFalcon(false);
|
||||
SmartDashboard.putBoolean("Solenoid", false);
|
||||
}
|
||||
|
||||
m_deltaTime = System.currentTimeMillis() - m_lastTime;
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
m_lastAngleYaw = m_currentAngleYaw;
|
||||
m_currentAngleYaw = getGyroYaw();
|
||||
|
||||
m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
|
||||
m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
|
||||
|
||||
try {
|
||||
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
|
||||
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
|
||||
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
|
||||
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
|
||||
SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
|
||||
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
|
||||
SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
|
||||
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
|
||||
|
||||
SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
|
||||
|
||||
//SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
|
||||
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
|
||||
SmartDashboard.putNumber("Odometry Heading", getHeading());
|
||||
|
||||
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
|
||||
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
|
||||
|
||||
if (currentSong != m_songChooser.getSelected()){
|
||||
currentSong = m_songChooser.getSelected();
|
||||
selectSong(currentSong);
|
||||
System.err.println(currentSong);
|
||||
}
|
||||
} catch (Exception e) {
|
||||
System.err.println("Error in the Drive Subsystem");
|
||||
//e.printStackTrace(System.err);
|
||||
// e.printStackTrace(System.err);
|
||||
}
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
inchesToMeters(getDistanceInches(m_leftBackMotor)),
|
||||
-inchesToMeters(getDistanceInches(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
*
|
||||
* @param mode NeutralMode to set motors to
|
||||
*/
|
||||
public void setDriveTrainNeutralMode(NeutralMode mode) {
|
||||
@@ -282,120 +384,142 @@ public class Drive extends SubsystemBase {
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes the drive train gains kP, kI, kD, and kF
|
||||
* @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or "Turning PID"
|
||||
* @param gains A gains object which is the gains that are set for the slot
|
||||
* Runs percent output control on the moving and steering of the drive train,
|
||||
* using the Differential Drive class to manage the two inputs
|
||||
*/
|
||||
public void setDriveTrainGains(String slot, Gains gains){
|
||||
/* Distance */
|
||||
if (slot.equals("Distance PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Velocity */
|
||||
if (slot.equals("Velocity PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
/* Turning */
|
||||
if (slot.equals("Turning PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Motion Magic */
|
||||
if (slot.equals("Motion Magic PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
* Runs percent output control on the drive train while using an AUX PID for rotation
|
||||
* @param targetPos The position to drive to in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void driveWithInput(double move, double steer){
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
}
|
||||
|
||||
public void driveWithInputAux(double move, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a position PID while driving straight
|
||||
* @param targetPos The position to drive to in units
|
||||
* Runs position PID.
|
||||
* Position is absolute and displacement should be handled on the command side.
|
||||
* @param targetPos The position to drive to in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
|
||||
public void runDrivePositionPID(double targetPos, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
//m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs velocity PID while driving straight
|
||||
* @param targetVel The velocity to drive at in units
|
||||
* Runs velocity PID
|
||||
*
|
||||
* @param targetVel The velocity to drive at in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
|
||||
public void runDriveVelocityPID(double targetVel, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
//m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs motion magic PID while driving straight (has not been tested)
|
||||
* Runs motion magic PID while driving straight
|
||||
* @param targetPos The position to drive to in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runMotionMagicPID(double targetPos, double targetGyro){
|
||||
public void runMotionMagicPID(double targetPos, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a Turning PID to rotate a to a target angle
|
||||
*
|
||||
* @param targetAngle target angle in degrees
|
||||
*/
|
||||
public void runTurningPID(double targetAngle) {
|
||||
double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
runDriveVelocityPID(0, targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* Controls the left and right sides of the drive with velocity targets.
|
||||
*
|
||||
* @param leftSpeed the commanded left speed
|
||||
* @param rightSpeed the commanded right speed
|
||||
*/
|
||||
public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
|
||||
//DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
|
||||
//ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
|
||||
//double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
|
||||
//double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
|
||||
//double angleVelDeg = Math.toDegrees(angleVelRad);
|
||||
|
||||
//m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
|
||||
//m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle,
|
||||
// m_currentAngleYaw-(360),
|
||||
// m_currentAngleYaw+(360));
|
||||
//double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
|
||||
//SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
|
||||
//SmartDashboard.putNumber("Move Vel Right", moveVelRight);
|
||||
|
||||
//runDriveVelocityPID(moveVel*2, targetGyro);
|
||||
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
|
||||
System.err.println(moveVelLeft);
|
||||
|
||||
m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight);
|
||||
m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
|
||||
m_leftFrontMotor.follow(m_leftBackMotor);
|
||||
m_rightFrontMotor.follow(m_rightBackMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a Turning PID to rotate a to a target angle
|
||||
* @param targetAngle target angle in degrees
|
||||
* Selects the feedback device for the motors.
|
||||
* @param feedbackDevice The feedback device to set it to, usually SensorDifference or
|
||||
*/
|
||||
public void runTurningPID(double targetAngle){
|
||||
double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
runDriveStraightVelocityPID(0, targetGyro);
|
||||
public void configMotorSensor(FeedbackDevice type) {
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -404,17 +528,17 @@ public class Drive extends SubsystemBase {
|
||||
public double getGyroYaw() {
|
||||
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[0];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current pitch of the pigeon
|
||||
*/
|
||||
public double getGyroPitch() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[1];
|
||||
}
|
||||
@@ -424,7 +548,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public double getGyroRoll() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[2];
|
||||
}
|
||||
@@ -435,18 +559,170 @@ public class Drive extends SubsystemBase {
|
||||
public void resetGyroYaw() {
|
||||
m_pigeon.setYaw(0);
|
||||
m_pigeon.setAccumZAngle(0);
|
||||
resetGyroAngles();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add docs here
|
||||
*/
|
||||
public void resetGyroAngles() {
|
||||
m_lastAngleYaw = 0;
|
||||
m_currentAngleYaw = 0;
|
||||
m_kinematicsTargetAngle = 0;
|
||||
}
|
||||
//lol
|
||||
//sko
|
||||
//ridge
|
||||
/**
|
||||
//brayden=bad coder
|
||||
* Returns the heading of the robot
|
||||
* @return The robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Math.IEEEremainder(getGyroYaw(), 360);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
|
||||
double turnRate = 1000 * deltaYaw / m_deltaTime;
|
||||
return turnRate;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns current wheel speeds of robot.
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
|
||||
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)),
|
||||
-inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the encoders for both motors.
|
||||
*/
|
||||
public void resetEncoders() {
|
||||
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void setOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the encoder value (position) of a motor
|
||||
* @param falcon The motor to get the position of
|
||||
* @return The position of the motor in inches
|
||||
*/
|
||||
public double getDistanceInches(WPI_TalonFX falcon) {
|
||||
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the encoder value (velocity) of a motor
|
||||
* @param falcon The motor to get the velocity of
|
||||
* @return The velocity of the motor in inches per second
|
||||
*/
|
||||
public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
|
||||
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()/DriveConstants.TICK_TIME_TO_SECONDS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in ticks to inches.
|
||||
* @param ticks The value in ticks to convert
|
||||
* @return The converted value in inches
|
||||
*/
|
||||
public double ticksToInches(double ticks) {
|
||||
return ticks * DriveConstants.INCHES_PER_TICK;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in inches to ticks.
|
||||
* @param inches The value in inches to convert
|
||||
* @return The converted value in ticks
|
||||
*/
|
||||
public double inchesToTicks(double inches) {
|
||||
return inches * DriveConstants.TICKS_PER_INCH;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in inches to meters.
|
||||
* @param inches The value in inches to convert
|
||||
* @return The converted value in meters
|
||||
*/
|
||||
public double inchesToMeters(double inches) {
|
||||
return inches * DriveConstants.METERS_PER_INCH;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in meters to inches.
|
||||
* @param meters The value in meters to convert
|
||||
* @return The converted value in inches
|
||||
*/
|
||||
public double metersToInches(double meters) {
|
||||
return meters * DriveConstants.INCHES_PER_METER;
|
||||
}
|
||||
|
||||
/*
|
||||
* Plays Music!
|
||||
*/
|
||||
public void playSong() {
|
||||
m_orchestra.play();
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects a song to play!
|
||||
* @param song The name of the song to be played
|
||||
*/
|
||||
public void selectSong(String song) {
|
||||
SmartDashboard.putString("Selected Song", song);
|
||||
m_orchestra.loadMusic(song);
|
||||
}
|
||||
/**
|
||||
* Set to high or low gear based on boolean state, true = high, false = low
|
||||
* @param state Chooses between high or low gear
|
||||
*/
|
||||
public void setShiftState(boolean state) {
|
||||
if (state == true) {
|
||||
speedShift.set(DoubleSolenoid.Value.kForward);
|
||||
m_speedShift.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
speedShift.set(DoubleSolenoid.Value.kReverse);
|
||||
m_speedShift.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set to open or close solenoid that cools the falcon, true = open, false = close
|
||||
* @param state Chooses between open and close
|
||||
*/
|
||||
public void coolFalcon(boolean state) {
|
||||
if (state == true) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -17,16 +17,20 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
import edu.wpi.first.wpilibj.Talon;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.LevelerConstants;
|
||||
import frc4388.robot.subsystems.*;
|
||||
|
||||
public class Leveler extends SubsystemBase {
|
||||
CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
private final Climber m_robotClimber = new Climber();
|
||||
|
||||
/**
|
||||
* Creates a new Leveler.
|
||||
*/
|
||||
public Leveler() {
|
||||
m_levelerMotor.restoreFactoryDefaults();
|
||||
m_levelerMotor.setIdleMode(IdleMode.kCoast);
|
||||
|
||||
m_levelerMotor.setIdleMode(IdleMode.kBrake);
|
||||
m_levelerMotor.setInverted(false);
|
||||
}
|
||||
|
||||
@@ -40,6 +44,11 @@ public class Leveler extends SubsystemBase {
|
||||
* @param input the percent output to run motor at
|
||||
*/
|
||||
public void runLeveler(double input) {
|
||||
m_levelerMotor.set(input);
|
||||
if(m_robotClimber.climberSafety){
|
||||
m_levelerMotor.set(input);
|
||||
}
|
||||
else{
|
||||
m_levelerMotor.set(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -11,28 +11,56 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANPIDController;
|
||||
import com.revrobotics.ControlType;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
|
||||
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
|
||||
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
|
||||
|
||||
public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
|
||||
|
||||
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static Shooter m_shooter;
|
||||
public static IHandController m_controller;
|
||||
|
||||
|
||||
// Configure PID Controllers
|
||||
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
|
||||
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
|
||||
|
||||
CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
|
||||
CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
|
||||
|
||||
double velP;
|
||||
/**
|
||||
double input;
|
||||
|
||||
/*
|
||||
* Creates a new Shooter subsystem.
|
||||
*/
|
||||
public Shooter() {
|
||||
//Testing purposes reseting gyros
|
||||
resetGyroAngleAdj();
|
||||
resetGyroShooterRotate();
|
||||
|
||||
m_shooterFalcon.configFactoryDefault();
|
||||
|
||||
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
|
||||
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
m_shooterFalcon.setInverted(true);
|
||||
m_shooterFalcon.setInverted(false);
|
||||
|
||||
setShooterGains();
|
||||
|
||||
@@ -64,10 +92,10 @@ public class Shooter extends SubsystemBase {
|
||||
*/
|
||||
public void setShooterGains() {
|
||||
m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
|
||||
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
}
|
||||
/**
|
||||
* Runs drum shooter velocity PID.
|
||||
@@ -87,4 +115,55 @@ public class Shooter extends SubsystemBase {
|
||||
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void runShooterWithInput(double input) {
|
||||
m_shooterRotateMotor.set(input);
|
||||
}
|
||||
|
||||
/* Angle Adjustment PID Control */
|
||||
public void runAngleAdjustPID(double targetAngle)
|
||||
{
|
||||
// Set PID Coefficients
|
||||
m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
|
||||
m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
|
||||
m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
|
||||
m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
|
||||
m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
|
||||
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
|
||||
|
||||
// Convert input angle in degrees to rotations of the motor
|
||||
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
|
||||
|
||||
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
/* Rotate Shooter PID Control */
|
||||
public void runshooterRotatePID(double targetAngle)
|
||||
{
|
||||
// Set PID Coefficients
|
||||
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
|
||||
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
|
||||
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
|
||||
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
|
||||
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
|
||||
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
|
||||
|
||||
// Convert input angle in degrees to rotations of the motor
|
||||
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
|
||||
|
||||
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
/* For Testing Purposes, reseting gyro for angle adjuster */
|
||||
public void resetGyroAngleAdj()
|
||||
{
|
||||
m_angleEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
/* For Testing Purposes, reseting gyro for shooter rotation */
|
||||
public void resetGyroShooterRotate()
|
||||
{
|
||||
m_shooterRotateEncoder.setPosition(0);
|
||||
}
|
||||
}
|
||||
@@ -17,7 +17,6 @@ import com.revrobotics.ControlType;
|
||||
import com.revrobotics.SparkMax;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@@ -61,6 +60,10 @@ public class Storage extends SubsystemBase {
|
||||
* @param input the voltage to run motor at
|
||||
*/
|
||||
|
||||
public void runStorage(final double input) {
|
||||
m_storageMotor.set(input);
|
||||
final boolean beam_on = m_beamSensors[0].get();
|
||||
}
|
||||
|
||||
public void resetEncoder()
|
||||
{
|
||||
|
||||
+15
-15
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"fileName": "Phoenix.json",
|
||||
"name": "CTRE-Phoenix",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
|
||||
"mavenUrls": [
|
||||
"http://devsite.ctr-electronics.com/maven/release/"
|
||||
@@ -11,19 +11,19 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "api-java",
|
||||
"version": "5.17.4"
|
||||
"version": "5.18.1"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "wpiapi-java",
|
||||
"version": "5.17.4"
|
||||
"version": "5.18.1"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "cci",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -35,7 +35,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "diagnostics",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -47,7 +47,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "canutils",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -58,7 +58,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "platform-stub",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -69,7 +69,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "core",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
@@ -83,7 +83,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "wpiapi-cpp",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_Phoenix_WPI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
@@ -97,7 +97,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "api-cpp",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_Phoenix",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
@@ -111,7 +111,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "cci",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_PhoenixCCI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
@@ -125,7 +125,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "diagnostics",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_PhoenixDiagnostics",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
@@ -139,7 +139,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "canutils",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_PhoenixCanutils",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
@@ -152,7 +152,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "platform-stub",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_PhoenixPlatform",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
@@ -165,7 +165,7 @@
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "core",
|
||||
"version": "5.17.4",
|
||||
"version": "5.18.1",
|
||||
"libName": "CTRE_PhoenixCore",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
{
|
||||
"fileName": "WPILibOldCommands.json",
|
||||
"name": "WPILib-Old-Commands",
|
||||
"version": "2020.0.0",
|
||||
"uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
|
||||
"mavenUrls": [],
|
||||
"jsonUrl": "",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "edu.wpi.first.wpilibOldCommands",
|
||||
"artifactId": "wpilibOldCommands-java",
|
||||
"version": "wpilib"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "edu.wpi.first.wpilibOldCommands",
|
||||
"artifactId": "wpilibOldCommands-cpp",
|
||||
"version": "wpilib",
|
||||
"libName": "wpilibOldCommands",
|
||||
"headerClassifier": "headers",
|
||||
"sourcesClassifier": "sources",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"linuxaarch64bionic",
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxx86-64",
|
||||
"osxx86-64"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user