mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge branch 'master' into GalacticSearch
This commit is contained in:
Kyra
@@ -36,7 +36,7 @@ public final class Constants {
|
||||
public static final boolean isRightMotorInverted = true;
|
||||
public static final boolean isLeftMotorInverted = false;
|
||||
public static final boolean isRightArcadeInverted = false;
|
||||
public static final boolean isAuxPIDInverted = true;
|
||||
public static final boolean isAuxPIDInverted = false;
|
||||
|
||||
/* Drive Configuration */
|
||||
public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
|
||||
@@ -112,7 +112,8 @@ public final class Constants {
|
||||
|
||||
public static final class ShooterConstants {
|
||||
/* Motor IDs */
|
||||
public static final int SHOOTER_FALCON_ID = 8;
|
||||
public static final int SHOOTER_FALCON_BALLER_ID = 8;
|
||||
public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15;
|
||||
public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
|
||||
public static final int SHOOTER_ROTATE_ID = 9;
|
||||
|
||||
@@ -120,7 +121,7 @@ public final class Constants {
|
||||
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 DRUM_SHOOTER_GAINS = new Gains(1.5, 0.0, 100, 0.055, 0, 1.0);
|
||||
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); //Ff was 0.055
|
||||
//public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
|
||||
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
|
||||
@@ -156,7 +157,7 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class LevelerConstants {
|
||||
public static final int LEVELER_CAN_ID = 15;
|
||||
public static final int LEVELER_CAN_ID = 30;
|
||||
}
|
||||
|
||||
public static final class IntakeConstants {;
|
||||
@@ -171,7 +172,7 @@ public final class Constants {
|
||||
public static final int STORAGE_CAN_ID = 11;
|
||||
public static final double STORAGE_PARTIAL_BALL = 2;
|
||||
public static final double STORAGE_FULL_BALL = 7;
|
||||
public static final double STORAGE_SPEED = 0.5;
|
||||
public static final double STORAGE_SPEED = 0.75;
|
||||
public static final double STORAGE_TIMEOUT = 3000;
|
||||
|
||||
/* Storage Characteristics */
|
||||
@@ -211,10 +212,10 @@ public final class Constants {
|
||||
|
||||
public static final class VisionConstants {
|
||||
public static final double FOV = 29.8; //Field of view of limelight
|
||||
public static final double TARGET_HEIGHT = 71.5;
|
||||
public static final double TARGET_HEIGHT = 67.5;
|
||||
public static final double LIME_ANGLE = 24.7;
|
||||
public static final double TURN_P_VALUE = 0.8;
|
||||
public static final double X_ANGLE_ERROR = 1.3;
|
||||
public static final double X_ANGLE_ERROR = 0.5;
|
||||
public static final double MOTOR_DEAD_ZONE = 0.2;
|
||||
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
|
||||
public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
|
||||
|
||||
@@ -70,6 +70,7 @@ public class Robot extends TimedRobot {
|
||||
/* Builds Autos */
|
||||
m_robotContainer.buildAutos();
|
||||
SmartDashboard.putString("Is Auto Start?", "NAH");
|
||||
m_robotContainer.m_robotLime.limeOff();
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -107,7 +107,7 @@ public class RobotContainer {
|
||||
/* 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);
|
||||
private final LimeLight m_robotLime = new LimeLight();
|
||||
public final LimeLight m_robotLime = new LimeLight();
|
||||
|
||||
/* Controllers */
|
||||
|
||||
@@ -184,7 +184,7 @@ public class RobotContainer {
|
||||
// runs the hood with joystick
|
||||
m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
|
||||
// moves the drum not
|
||||
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0), m_robotShooter));
|
||||
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(12000), m_robotShooter));
|
||||
// drives climber with input from triggers on the opperator controller
|
||||
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
|
||||
// drives the leveler with an axis input from the driver controller
|
||||
@@ -192,9 +192,9 @@ 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 storage not
|
||||
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
|
||||
m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
|
||||
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
|
||||
m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -205,14 +205,20 @@ public class RobotContainer {
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Test Buttons */
|
||||
|
||||
// A driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
|
||||
.whenReleased(new InterruptSubystem(m_robotShooter))
|
||||
.whenReleased(new InterruptSubystem(m_robotShooterHood));
|
||||
|
||||
// B driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(new TurnDegrees(m_robotDrive, 45));
|
||||
.whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
|
||||
|
||||
// Y driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new Wait(m_robotDrive, 0, 0));
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
|
||||
|
||||
// X driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
@@ -250,17 +256,19 @@ public class RobotContainer {
|
||||
//.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
|
||||
//.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
|
||||
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
|
||||
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
|
||||
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage))
|
||||
.whenReleased(new InterruptSubystem(m_robotStorage));
|
||||
|
||||
// extends or retracts the extender
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
|
||||
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
|
||||
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
|
||||
|
||||
//.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
|
||||
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
|
||||
//.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
|
||||
|
||||
// safety for climber and leveler
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
|
||||
@@ -271,10 +279,10 @@ public class RobotContainer {
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whileHeld(new TrackTarget(m_robotShooterAim))
|
||||
.whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
|
||||
//.whenPressed(new StoragePrep(m_robotStorage))
|
||||
//.whenReleased(new InterruptSubystem(m_robotStorage))
|
||||
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
|
||||
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
|
||||
|
||||
//.whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID()));
|
||||
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000)));
|
||||
//.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
|
||||
//.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
|
||||
|
||||
@@ -287,6 +295,7 @@ public class RobotContainer {
|
||||
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
|
||||
|
||||
|
||||
|
||||
//Run drum
|
||||
new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false)
|
||||
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
|
||||
@@ -339,6 +348,51 @@ public class RobotContainer {
|
||||
|
||||
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
|
||||
|
||||
String[] sixBallAutoMiddle0Paths = new String[]{
|
||||
"SixBallMid0"
|
||||
};
|
||||
|
||||
m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
|
||||
|
||||
String[] sixBallAutoMiddle1Paths = new String[]{
|
||||
"SixBallMid1"
|
||||
};
|
||||
|
||||
m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
|
||||
|
||||
String[] slalom = new String[]{
|
||||
"Slalom"
|
||||
};
|
||||
|
||||
m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
|
||||
|
||||
String[] barrel = new String[]{
|
||||
"BarrelStart"
|
||||
};
|
||||
|
||||
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
|
||||
|
||||
String[] barrelStart = new String[]{
|
||||
"Barrel"
|
||||
};
|
||||
|
||||
m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
|
||||
|
||||
String[] bounce = new String[]{
|
||||
"Bounce1",
|
||||
"Bounce2",
|
||||
"Bounce3",
|
||||
"Bounce4"
|
||||
};
|
||||
|
||||
m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
|
||||
|
||||
String[] barrelMany = new String[]{
|
||||
"BarrelManyWaypoints"
|
||||
};
|
||||
|
||||
m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
|
||||
|
||||
String[] eightBallAutoMiddlePaths = new String[]{
|
||||
"EightBallMidComplete"
|
||||
};
|
||||
@@ -349,14 +403,14 @@ public class RobotContainer {
|
||||
"DriveOffLineForward"
|
||||
};
|
||||
|
||||
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
|
||||
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
|
||||
|
||||
String[] driveOffLineBackwardPaths = new String[]{
|
||||
"DriveOffLineBackward"
|
||||
};
|
||||
|
||||
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
|
||||
|
||||
|
||||
String[] fiveBallAutoMiddlePaths = new String[]{
|
||||
"FiveBallMidComplete"
|
||||
};
|
||||
@@ -367,17 +421,16 @@ public class RobotContainer {
|
||||
"SixBallMidComplete",
|
||||
"TenBallMidComplete"
|
||||
};
|
||||
|
||||
m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
|
||||
m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
|
||||
|
||||
|
||||
String[] galacticSearchPaths = new String[]{
|
||||
"GSC_ARED",
|
||||
"GSC_ABLUE",
|
||||
"GSC_BRED",
|
||||
"GSC_BBLUE"
|
||||
};
|
||||
idenPath();
|
||||
|
||||
m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
|
||||
|
||||
}
|
||||
@@ -402,8 +455,8 @@ public class RobotContainer {
|
||||
//return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
//return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
@@ -417,7 +470,7 @@ public class RobotContainer {
|
||||
System.err.println("ERROR");
|
||||
}
|
||||
|
||||
return new InstantCommand();
|
||||
return new InstantCommand();
|
||||
}
|
||||
|
||||
TrajectoryConfig getTrajectoryConfig() {
|
||||
@@ -451,7 +504,7 @@ public class RobotContainer {
|
||||
String path = paths[0];
|
||||
String trajectoryJSON = "paths/" + path + ".wpilib.json";
|
||||
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
|
||||
|
||||
|
||||
SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString());
|
||||
|
||||
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
|
||||
@@ -499,7 +552,7 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
*
|
||||
*/
|
||||
public void shiftClimberRachet(boolean state) {
|
||||
m_robotClimber.shiftServo(state);
|
||||
|
||||
@@ -7,8 +7,10 @@
|
||||
|
||||
package frc4388.robot.commands.auto;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RamseteCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
@@ -23,7 +25,10 @@ public class BarrelStart extends SequentialCommandGroup {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
addCommands(
|
||||
paths[0]
|
||||
paths[0],
|
||||
//new Wait(drive, 0.01, 1),
|
||||
new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,8 +7,12 @@
|
||||
|
||||
package frc4388.robot.commands.auto;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
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.SequentialCommandGroup;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
@@ -18,7 +22,7 @@ public class Bounce extends SequentialCommandGroup {
|
||||
/**
|
||||
* Creates a new Bounce.
|
||||
*/
|
||||
public Bounce(Drive drive, RamseteCommand[] paths) {
|
||||
public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
addCommands(
|
||||
|
||||
@@ -7,8 +7,11 @@
|
||||
|
||||
package frc4388.robot.commands.auto;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RamseteCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
|
||||
/**
|
||||
* Creates a new DriveOffLineForward.
|
||||
*/
|
||||
public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
|
||||
public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
|
||||
addCommands(
|
||||
new InstantCommand(() -> drive.switchReversed(true)),
|
||||
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
|
||||
paths[0]
|
||||
);
|
||||
}
|
||||
|
||||
@@ -15,10 +15,10 @@ public class TankDriveVelocity extends CommandBase {
|
||||
double m_leftTargetVel;
|
||||
double m_rightTargetVel;
|
||||
|
||||
double m_targetTime;
|
||||
double m_firstTimeSec;
|
||||
double m_currentTimeSec;
|
||||
double m_diffSec;
|
||||
long m_targetTime;
|
||||
long m_firstTime;
|
||||
long m_currentTime;
|
||||
long m_diffTime;
|
||||
|
||||
/**
|
||||
* Creates a new TankDriveVelocity.
|
||||
@@ -28,24 +28,24 @@ public class TankDriveVelocity extends CommandBase {
|
||||
m_drive = subsystem;
|
||||
m_leftTargetVel = leftTargetVel;
|
||||
m_rightTargetVel = rightTargetVel;
|
||||
m_targetTime = targetTime;
|
||||
m_targetTime = (long) (targetTime * 1000);
|
||||
addRequirements(subsystem);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_firstTimeSec = (System.currentTimeMillis() / 1000);
|
||||
m_diffSec = 0;
|
||||
m_firstTime = System.currentTimeMillis();
|
||||
m_diffTime = 0;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_currentTimeSec = (System.currentTimeMillis() / 1000);
|
||||
m_diffSec = m_currentTimeSec - m_firstTimeSec;
|
||||
m_currentTime = System.currentTimeMillis();
|
||||
m_diffTime = m_currentTime - m_firstTime;
|
||||
|
||||
if (m_diffSec < m_targetTime) {
|
||||
if (m_diffTime < m_targetTime) {
|
||||
m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
|
||||
}
|
||||
|
||||
@@ -59,7 +59,7 @@ public class TankDriveVelocity extends CommandBase {
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (m_diffSec >= m_targetTime) {
|
||||
if (m_diffTime >= m_targetTime) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
@@ -29,7 +29,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
|
||||
m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
|
||||
@@ -94,7 +94,7 @@ public class DriveWithJoystick extends CommandBase {
|
||||
}
|
||||
*/
|
||||
|
||||
m_drive.driveWithInput(-moveOutput, steerOutput);
|
||||
m_drive.driveWithInput(moveOutput, steerOutput * .8);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -46,7 +46,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
|
||||
addCommands( new TurnDegrees(m_drive, m_currentAngle),
|
||||
new Wait(m_drive, 0, 0),
|
||||
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
|
||||
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
|
||||
new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
|
||||
}
|
||||
|
||||
public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
|
||||
|
||||
@@ -61,7 +61,7 @@ public class CalibrateShooter extends CommandBase {
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() &&
|
||||
m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) {
|
||||
m_shooterHood.m_hoodDownLimit.get()) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
@@ -35,7 +35,7 @@ public class ShooterGoalPosition extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
m_shooter.runDrumShooterVelocityPID(5000);
|
||||
m_hood.runAngleAdjustPID(3);
|
||||
m_hood.runAngleAdjustPID(4);
|
||||
m_aim.runshooterRotatePID(-26.5);
|
||||
}
|
||||
|
||||
|
||||
@@ -34,9 +34,9 @@ public class ShooterTrenchPosition extends CommandBase {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_shooter.runDrumShooterVelocityPID(5000);
|
||||
m_hood.runAngleAdjustPID(3);
|
||||
m_aim.runshooterRotatePID(-26.5);
|
||||
m_shooter.runDrumShooterVelocityPID(5500);
|
||||
m_hood.runAngleAdjustPID(11);
|
||||
//m_aim.runshooterRotatePID(-28);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -35,7 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
//Tells whether the target velocity has been reached
|
||||
m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition();
|
||||
m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition();
|
||||
m_targetVel = m_shooter.addFireVel();
|
||||
double error = m_actualVel - m_targetVel;
|
||||
if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){
|
||||
|
||||
@@ -70,24 +70,26 @@ public class TrackTarget extends CommandBase {
|
||||
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
|
||||
|
||||
// Finding Distance
|
||||
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
|
||||
distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
|
||||
realDistance = (1.09 * distance) - 12.8;
|
||||
|
||||
|
||||
if (target == 1.0) { // If target in view
|
||||
// Aiming Left/Right
|
||||
xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance);
|
||||
turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
|
||||
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;
|
||||
else if (turnAmount > 0 && turnAmount < 0.1){// VisionConstants.MOTOR_DEAD_ZONE) {
|
||||
turnAmount = 0.1;//VisionConstants.MOTOR_DEAD_ZONE;
|
||||
} else if (turnAmount < 0 && turnAmount > -0.1){//-VisionConstants.MOTOR_DEAD_ZONE) {
|
||||
turnAmount = -0.1;//-VisionConstants.MOTOR_DEAD_ZONE;
|
||||
}
|
||||
|
||||
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
|
||||
//m_shooterAim.runshooterRotatePID(targetAngle);
|
||||
|
||||
SmartDashboard.putNumber("Distance to Target", realDistance);
|
||||
SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));
|
||||
|
||||
@@ -46,6 +46,7 @@ public class Drive extends SubsystemBase {
|
||||
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 static GyroBase m_pigeonGyro;
|
||||
public boolean m_isReversed;
|
||||
|
||||
/* Drive objects to manage Drive Train */
|
||||
public DifferentialDrive m_driveTrain;
|
||||
@@ -312,7 +313,7 @@ public class Drive extends SubsystemBase {
|
||||
public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) {
|
||||
m_pneumaticsSubsystem = subsystem;
|
||||
m_shooter = shooter;
|
||||
m_orchestra.addInstrument(m_shooter.m_shooterFalcon);
|
||||
m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
|
||||
}
|
||||
|
||||
public void updateTime() {
|
||||
@@ -339,9 +340,27 @@ public class Drive extends SubsystemBase {
|
||||
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
|
||||
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
updateOdometry(m_isReversed);
|
||||
}
|
||||
|
||||
public void updateOdometry(boolean reversed){
|
||||
if (reversed){
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees( -getGyroYaw()-180),
|
||||
-inchesToMeters(getDistanceInches(m_rightFrontMotor)),
|
||||
inchesToMeters(getDistanceInches(m_leftFrontMotor)));
|
||||
}
|
||||
else
|
||||
{
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
|
||||
-inchesToMeters(getDistanceInches(m_rightFrontMotor)));
|
||||
}
|
||||
}
|
||||
|
||||
public void switchReversed(boolean reversed)
|
||||
{
|
||||
m_isReversed = reversed;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -401,7 +420,7 @@ public class Drive extends SubsystemBase {
|
||||
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_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);
|
||||
@@ -426,7 +445,7 @@ public class Drive extends SubsystemBase {
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -23,15 +23,15 @@ public class LimeLight extends SubsystemBase {
|
||||
}
|
||||
|
||||
public void limeOff(){
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
|
||||
}
|
||||
|
||||
|
||||
public void limeOn(){
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
|
||||
}
|
||||
|
||||
|
||||
public void changePipeline(int pipelineId)
|
||||
{
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
|
||||
|
||||
@@ -21,8 +21,8 @@ import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
|
||||
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
|
||||
|
||||
public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
|
||||
public WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static Shooter m_shooter;
|
||||
public static IHandController m_controller;
|
||||
@@ -50,24 +50,42 @@ public class Shooter extends SubsystemBase {
|
||||
//SmartDashboard.putNumber("Velocity Target", 10000);
|
||||
//SmartDashboard.putNumber("Angle Target", 3);
|
||||
|
||||
m_shooterFalcon.configFactoryDefault();
|
||||
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
|
||||
m_shooterFalcon.setInverted(true);
|
||||
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
//LEFT FALCON
|
||||
m_shooterFalconLeft.configFactoryDefault();
|
||||
m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
m_shooterFalconLeft.setInverted(true);
|
||||
m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
//RIGHT FALCON
|
||||
m_shooterFalconRight.configFactoryDefault();
|
||||
m_shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
m_shooterFalconRight.setInverted(false);
|
||||
m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
setShooterGains();
|
||||
|
||||
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
int closedLoopTimeMs = 1;
|
||||
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
//LEFT FALCON
|
||||
m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
|
||||
//RIGHT FALCON
|
||||
//m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterTable = new ShooterTables();
|
||||
|
||||
m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
//SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
|
||||
//SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
|
||||
//SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
|
||||
@@ -83,13 +101,13 @@ public class Shooter extends SubsystemBase {
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
try{
|
||||
SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
|
||||
|
||||
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
|
||||
|
||||
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
|
||||
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature());
|
||||
|
||||
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent());
|
||||
|
||||
SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
|
||||
}
|
||||
@@ -118,18 +136,20 @@ public class Shooter extends SubsystemBase {
|
||||
* @param speed Speed to set the motor at
|
||||
*/
|
||||
public void runDrumShooter(double speed) {
|
||||
m_shooterFalcon.set(speed);
|
||||
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
|
||||
m_shooterFalconRight.follow(m_shooterFalconLeft);
|
||||
//m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed);
|
||||
}
|
||||
|
||||
/**
|
||||
* Configures gains for shooter PID.
|
||||
*/
|
||||
public void setShooterGains() {
|
||||
m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
|
||||
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
|
||||
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -138,6 +158,7 @@ public class Shooter extends SubsystemBase {
|
||||
*/
|
||||
public void runDrumShooterVelocityPID(double targetVel) {
|
||||
//System.out.println("Target Velocity" + targetVel);
|
||||
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
|
||||
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init PID
|
||||
m_shooterFalconRight.follow(m_shooterFalconLeft);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user