Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into CAN-and-Limits

Browse Source
This commit is contained in:
aarav18
2020-03-01 13:13:32 -07:00
committed by GitHub
parent 7adb1edb0c 110bee64e4
commit 89bc6d58de
25 changed files with 908 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+27 -7
View File
@@ -103,7 +103,8 @@ public final class Constants {
public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
}
public static final class IntakeConstants {
public static final class IntakeConstants {;
public static final double EXTENDER_SPEED = 0.3;
public static final int INTAKE_SPARK_ID = 12;
public static final int EXTENDER_SPARK_ID = 13;
}
@@ -118,12 +119,28 @@ 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(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 Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 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, 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 int TURRET_RIGHT_SOFT_LIMIT = -2;
public static final int TURRET_LEFT_SOFT_LIMIT = -55;
public static final double TURRET_SPEED_MULTIPLIER = 0.3;
public static final double TURRET_CALIBRATE_SPEED = 0.075;
public static final int HOOD_UP_SOFT_LIMIT = 33;
public static final int HOOD_DOWN_SOFT_LIMIT = 3;
public static final double HOOD_CONVERT_SLOPE = 0.47;
public static final double HOOD_CONVERT_B = 40.5;
public static final double HOOD_CALIBRATE_SPEED = 0.1;
public static final double DRUM_RAMP_LIMIT = 1000;
public static final double DRUM_VELOCITY_BOUND = 300;
}
public static final class ClimberConstants {
@@ -136,6 +153,10 @@ public final class Constants {
public static final class StorageConstants {
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_TIMEOUT = 2000;
/* Ball Indexes */
public static final int BEAM_SENSOR_DIO_0 = 0;
@@ -152,9 +173,8 @@ public final class Constants {
public static final int PID_PRIMARY = 0;
/* PID Gains */
public static final double STORAGE_MIN_OUTPUT = -1.0;
public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class PneumaticsConstants {
@@ -174,8 +194,8 @@ 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 = 82.75;
public static final double LIME_ANGLE = 18.7366;
public static final double TARGET_HEIGHT = 64;
public static final double LIME_ANGLE = 25;
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;
src/main/java/frc4388/robot/RobotContainer.java
+83 -31
View File
@@ -16,6 +16,7 @@ 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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -28,26 +29,52 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.CalibrateShooter;
import frc4388.robot.commands.DrivePositionMPAux;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
import frc4388.robot.commands.HoldTarget;
import frc4388.robot.commands.HoodPositionPID;
import frc4388.robot.commands.DriveWithJoystickDriveStraight;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.commands.ShooterVelocityControlPID;
import frc4388.robot.commands.StorageIntake;
import frc4388.robot.commands.GotoCoordinates;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.commands.StorageIntakeGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.commands.ShootFireGroup;
import frc4388.robot.commands.ShootFullGroup;
import frc4388.robot.commands.ShootPrepGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.commands.TrimShooter;
import frc4388.robot.commands.StorageOutake;
import frc4388.robot.commands.StoragePrepAim;
import frc4388.robot.commands.StoragePrepIntake;
import frc4388.robot.commands.StorageRun;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.commands.TurnDegrees;
import frc4388.robot.commands.Wait;
import frc4388.robot.commands.storageOutake;
import frc4388.robot.commands.StorageOutake;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.Pneumatics;
@@ -69,20 +96,23 @@ public class RobotContainer {
private final LED m_robotLED = new LED();
private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter();
private final ShooterAim m_robotShooterAim = new ShooterAim();
private final Climber m_robotClimber = new Climber();
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);
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();
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
/* Passing Drive and Pneumatics Subsystems */
@@ -96,23 +126,26 @@ public class RobotContainer {
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
// drives intake with input from triggers on the opperator controller
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// runs the drum shooter in idle mode
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
// runs the turret with joystick
m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
// moves the drum not
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), 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
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
/* Test Buttons */
// A driver test button
@@ -129,8 +162,8 @@ public class RobotContainer {
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
.whenPressed(new InstantCommand());
/* Driver Buttons */
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -140,21 +173,19 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
/* Operator Buttons */
//TODO: Shooter Buttons
// shoots until released
//new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
// shoots one ball
//new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
// aims the turret
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
//.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
@@ -164,17 +195,38 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
/* Storage Neo PID Test */
// starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
//Prepares storage for intaking
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
.whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
//Runs storage to outtake
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
.whileHeld(new storageOutake(m_robotStorage));
.whileHeld(new StorageOutake(m_robotStorage));
//TEST FOR HOOD
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
//TEST FOR HOOD
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
//Trims shooter
new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
.whenPressed(new TrimShooter(m_robotShooter));
//Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
}
/**
@@ -313,4 +365,4 @@ public class RobotContainer {
{
return m_driverXbox.getJoyStick();
}
}
}
src/main/java/frc4388/robot/Trims.java
+18
View File
@@ -0,0 +1,18 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-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;
public class Trims{
public double m_turretTrim;
public double m_hoodTrim;
public Trims(double turretTrim, double hoodTrim){
m_turretTrim = turretTrim;
m_hoodTrim = hoodTrim;
}
}
src/main/java/frc4388/robot/commands/CalibrateShooter.java
+66
View File
@@ -0,0 +1,66 @@
/*----------------------------------------------------------------------------*/
/* 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 com.revrobotics.CANSparkMax.SoftLimitDirection;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
/**
* Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders
* @param shootSub The Shooter subsystem
* @param aimSub The ShooterAim subsystem
*/
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) {
m_shooter = shootSub;
m_shooterAim = aimSub;
addRequirements(m_shooter, m_shooterAim);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooter.m_angleEncoder.setPosition(0);
m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooterAim.m_shooterRotateEncoder.setPosition(0);
m_shooterAim.m_shooterRotateMotor.set(ShooterConstants.TURRET_CALIBRATE_SPEED);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/HoldTarget.java
+117
View File
@@ -0,0 +1,117 @@
/*----------------------------------------------------------------------------*/
/* 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.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
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 HoldTarget extends CommandBase {
//Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
IHandController m_driverController;
//Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
double target = 0;
public double distance;
public double fireVel;
public double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
/**
* Uses the Limelight to track the target
* @param shooterSubsystem The Shooter subsystem
* @param aimSubsystem The ShooterAim subsystem
*/
public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
addRequirements(m_shooterAim);
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_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
SmartDashboard.putNumber("Distance to Target", distance);
double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
double xVel = (distance*VisionConstants.GRAV)/(yVel);
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
}/*
else{
System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition());
double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1;
if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){
curveInput = -curveInput;
}
System.err.println("Curve Input: " + curveInput);
m_shooterAim.runShooterWithInput(curveInput);
}
*/
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
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;
}
}
src/main/java/frc4388/robot/commands/HoodPositionPID.java
+56
View File
@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* 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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
public class HoodPositionPID extends CommandBase {
Shooter m_shooter;
double firingAngle;
/**
* Creates a new HoodPositionPID.
*/
public HoodPositionPID(Shooter subSystem) {
m_shooter = subSystem;
//addRequirements(m_shooter);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
double b = ShooterConstants.HOOD_CONVERT_B;
firingAngle = (-slope*m_shooter.addFireAngle())+b;
SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooter.runAngleAdjustPID(firingAngle);
}
// 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() {
double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/ShootFireGroup.java
+34
View File
@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class ShootFireGroup extends ParallelRaceGroup {
/**
* Fires the shooter
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
addCommands(
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
new HoldTarget(m_shooter, m_shooterAim),
new StorageRun(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/StorageIntakeGroup.java → src/main/java/frc4388/robot/commands/ShootFullGroup.java
+11 -7
View File
@@ -8,20 +8,24 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class StorageIntakeGroup extends SequentialCommandGroup {
public class ShootFullGroup extends SequentialCommandGroup {
/**
* Creates a new StorageIntakeGroup.
* Preps and Fires the Shooter
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
addCommands(
new storagePrepIntake(m_intake, m_storage),
new storageIntake(m_intake, m_storage),
new StorageIntakeFinal(m_storage));
new ShootPrepGroup(m_shooter, m_shooterAim, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+34
View File
@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class ShootPrepGroup extends ParallelCommandGroup {
/**
* Prepares the Shooter to be fired
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
addCommands(
new TrackTarget(m_shooter, m_shooterAim),
new ShooterVelocityControlPID(m_shooter),
new StoragePrepAim(m_storage),
new HoodPositionPID(m_shooter)
);
}
}
src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+22 -7
View File
@@ -7,32 +7,37 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
double m_actualVel;
/**
* Creates a new ShooterVelocityControlPID.
* Runs the drum at a velocity
* @param subsystem The Shooter subsytem
*/
public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
public ShooterVelocityControlPID(Shooter subsystem) {
m_shooter = subsystem;
m_targetVel = targetVel;
addRequirements(m_shooter);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
}
// Called once the command ends or is interrupted.
@@ -43,6 +48,16 @@ public class ShooterVelocityControlPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
//Tells whether the target velocity has been reached
double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
if (m_actualVel < upperBound && m_actualVel > lowerBound){
SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
return true;
}
else{
SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
return false;
}
}
}
src/main/java/frc4388/robot/commands/StorageIntake.java
+64
View File
@@ -0,0 +1,64 @@
/*----------------------------------------------------------------------------*/
/* 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.Constants.IntakeConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Storage;
public class StorageIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
public boolean intook;
/**
* Runs the Storage in for intaking
* @param inSub The Intake subsystem
* @param storeSub The Storage subsytem
*/
public StorageIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
intook = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (!m_storage.getBeam(0)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
intook = true;
}
else{
m_storage.runStorage(0);
}
}
// 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() {
if (m_storage.getBeam(0) && intook){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/storageOutake.java → src/main/java/frc4388/robot/commands/StorageOutake.java
+6 -4
View File
@@ -8,14 +8,16 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class storageOutake extends CommandBase {
public class StorageOutake extends CommandBase {
Storage m_storage;
/**
* Creates a new storageOutake.
* Runs the Storage out for outaking
* @param storeSub The Storage subsystem
*/
public storageOutake(Storage storeSub) {
public StorageOutake(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
@@ -28,7 +30,7 @@ public class storageOutake extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_storage.runStorage(-0.5);
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/storagePrepIntake.java → src/main/java/frc4388/robot/commands/StoragePositionPID.java
+12 -16
View File
@@ -8,36 +8,31 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class storagePrepIntake extends CommandBase {
public Intake m_intake;
public class StoragePositionPID extends CommandBase {
public Storage m_storage;
double startPos;
/**
* Creates a new storagePrepIntake.
* Moves the storage a number of rotations
* @param subsystem The Storage subsystem
*/
public storagePrepIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
public StoragePositionPID(Storage subsystem) {
m_storage = subsystem;
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(1) == false){
m_storage.runStorage(-0.5);
}
else{
m_storage.runStorage(0);
}
m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
}
// Called once the command ends or is interrupted.
@@ -48,9 +43,10 @@ public class storagePrepIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(1)){
/*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
{
return true;
}
}*/
return false;
}
}
src/main/java/frc4388/robot/commands/storagePrepAim.java → src/main/java/frc4388/robot/commands/StoragePrepAim.java
+13 -6
View File
@@ -7,15 +7,19 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class storagePrepAim extends CommandBase {
public class StoragePrepAim extends CommandBase {
Storage m_storage;
double startTime;
/**
* Creates a new storagePrepAim.
* Prepares the Storage for aiming
* @param storeSub The Storage subsystem
*/
public storagePrepAim(Storage storeSub) {
public StoragePrepAim(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
@@ -23,13 +27,14 @@ public class storagePrepAim extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(2) == false){
m_storage.runStorage(0.5);
if (m_storage.getBeam(1)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
m_storage.runStorage(0);
@@ -44,9 +49,11 @@ public class storagePrepAim extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(2)){
if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
SmartDashboard.putBoolean("StoragePrepAim Finished", true);
return true;
}
SmartDashboard.putBoolean("StoragePrepAim Finished", false);
return false;
}
}
src/main/java/frc4388/robot/commands/storageIntake.java → src/main/java/frc4388/robot/commands/StoragePrepIntake.java
+12 -8
View File
@@ -8,16 +8,21 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Storage;
public class storageIntake extends CommandBase {
public class StoragePrepIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
public double startTime;
/**
* Creates a new storageIntake.
* Prepares the Storage for intaking
* @param inSub The Intake subsystem
* @param storeSub the Storage subsystem
*/
public storageIntake(Intake inSub, Storage storeSub) {
public StoragePrepIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
@@ -27,18 +32,17 @@ public class storageIntake extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(0)){
m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
m_intake.runExtender(-0.3);
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
else{
m_intake.runExtender(0.3);
m_storage.runStorage(0);
}
}
@@ -50,7 +54,7 @@ public class storageIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(0)){
if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return true;
}
return false;
src/main/java/frc4388/robot/commands/StorageIntakeFinal.java → src/main/java/frc4388/robot/commands/StorageRun.java
+8 -8
View File
@@ -8,16 +8,17 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StorageIntakeFinal extends CommandBase {
public class StorageRun extends CommandBase {
Storage m_storage;
/**
* Creates a new StorageIntakeFinal.
* Runs the Storage at a speed
* @param storageSub The Storage subsytem
*/
public StorageIntakeFinal(Storage subsystem) {
m_storage = subsystem;
addRequirements(m_storage);
public StorageRun(Storage storageSub) {
m_storage = storageSub;
}
// Called when the command is initially scheduled.
@@ -28,14 +29,13 @@ public class StorageIntakeFinal extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(1)){
m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
}
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storage.runStorage(0);
}
// Returns true when the command should end.
src/main/java/frc4388/robot/commands/TrackTarget.java
+58 -25
View File
@@ -9,21 +9,29 @@ package frc4388.robot.commands;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.commands.TrimShooter;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class TrackTarget extends CommandBase {
//Setup
// Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
IHandController m_driverController;
//Aiming
// Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
@@ -32,66 +40,91 @@ public class TrackTarget extends CommandBase {
public static double fireVel;
public static double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
ShooterTables m_shooterTable;
/**
* Uses the Limelight to track the target
* @param shooterSubsystem The Shooter subsystem
* @param aimSubsystem The ShooterAim subsystem
*/
public TrackTarget(Shooter shooterSubsystem) {
public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
addRequirements(m_shooter);
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
//Vision Processing Mode
// Vision Processing Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
m_shooterTable = new ShooterTables();
}
// 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));
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_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
SmartDashboard.putNumber("Distance to Target", distance);
double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
double xVel = (distance*VisionConstants.GRAV)/(yVel);
//START Equation Code
double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
double xVel = (distance * VisionConstants.GRAV) / (yVel);
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel);
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
//END Equation Code
/*//START CSV Code
fireVel = m_shooterTable.getVelocity(distance);
fireAngle = m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
//END CSV Code*/
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
}
}
// 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() {
if (xAngle < 1 && xAngle > -1 && target == 1)
{
SmartDashboard.putBoolean("TrackTarget Finished", true);
return true;
}
SmartDashboard.putBoolean("TrackTarget Finished", false);
return false;
}
}
src/main/java/frc4388/robot/commands/TrimShooter.java
+69
View File
@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* 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.wpilibj.Joystick;
import frc4388.utility.controller.XboxController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Trims;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
public class TrimShooter extends CommandBase {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
public double turretTrim = 0;
public double hoodTrim = 0;
public Shooter m_shooter;
/**
* Trims the shooter based on the D-Pad inputs
* @param shootSub The Shooter subsytems
*/
public TrimShooter(Shooter shootSub) {
m_shooter = shootSub;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_operatorXbox.getDPadTop()){
hoodTrim += 1;
}
else if(m_operatorXbox.getDPadBottom()){
hoodTrim -= 1;
}
else if(m_operatorXbox.getDPadRight()){
turretTrim += 1;
}
else if(m_operatorXbox.getDPadLeft()){
turretTrim -= 1;
}
m_shooter.shooterTrims.m_turretTrim = turretTrim;
m_shooter.shooterTrims.m_hoodTrim = hoodTrim;
}
// 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;
}
}
src/main/java/frc4388/robot/subsystems/Camera.java
+2 -2
View File
@@ -11,6 +11,7 @@ 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.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -38,8 +39,7 @@ public class Camera extends SubsystemBase {
catch(Exception e){
System.err.println("Camera broken, pls nerf");
}
}
}
@Override
public void periodic() {
src/main/java/frc4388/robot/subsystems/Drive.java
+1
View File
@@ -107,6 +107,7 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
/* Config Open Loop Ramp so we don't make sudden output changes */
m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
src/main/java/frc4388/robot/subsystems/LimeLight.java
+35
View File
@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class LimeLight extends SubsystemBase {
/**
* Creates a new LimeLight.
*/
public LimeLight() {
}
public void limeOff(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
public void limeOn(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+59 -87
View File
@@ -13,9 +13,12 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.ControlType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Joystick;
@@ -23,6 +26,7 @@ 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.Trims;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
@@ -31,22 +35,15 @@ 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);
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_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;
@@ -55,20 +52,23 @@ public class Shooter extends SubsystemBase {
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
public Trims shooterTrims;
/*
* Creates a new Shooter subsystem.
* Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
*/
public Shooter() {
//Testing purposes reseting gyros
resetGyroAngleAdj();
resetGyroShooterRotate();
//resetGyroAngleAdj();
shooterTrims = new Trims(0, 0);
m_shooterFalcon.configFactoryDefault();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(false);
m_shooterFalcon.setInverted(true);
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -90,6 +90,19 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodUpLimit.enableLimitSwitch(true);
m_hoodDownLimit.enableLimitSwitch(true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
}
@Override
@@ -105,6 +118,7 @@ public class Shooter extends SubsystemBase {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
@@ -118,89 +132,47 @@ 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_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.
* @param falcon Motor to use
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
velP = actualVel/targetVel; //Percent of target
double runSpeed = actualVel + (12000*velP); //Ramp up equation
SmartDashboard.putNumber("shoot", actualVel);
runSpeed = runSpeed/targetVel; //Convert to percent
if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
}
else{ //PID Based on targetVel
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
//Tells wether the target velocity has been reached
double upperBound = targetVel + 300;
double lowerBound = targetVel - 300;
if (actualVel < upperBound && actualVel > lowerBound)
{
velReached = true;
}
else{
velReached = false;
}
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input);
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);
}
/* 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.setP(m_angleAdjustGains.m_kP);
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
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);
/**
* Runs drum shooter velocity PID.
* @param falcon Motor to use
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
SmartDashboard.putNumber("shoot", actualVel);
if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
}
else{ //PID Based on targetVel
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
m_shooterFalcon.feed();
}
/* For Testing Purposes, reseting gyro for angle adjuster */
public void resetGyroAngleAdj()
{
m_angleEncoder.setPosition(0);
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
/* For Testing Purposes, reseting gyro for shooter rotation */
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
public double getAnglePosition(){
return m_angleAdjustMotor.getEncoder().getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+89
View File
@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* 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 com.revrobotics.CANDigitalInput;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.ShooterConstants;
public class ShooterAim extends SubsystemBase {
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
// Configure PID Controllers
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
/**
* Creates a subsytem for the turret aiming
*/
public ShooterAim() {
//resetGyroShooterRotate();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit.enableLimitSwitch(true);
m_shooterLeftLimit.enableLimitSwitch(true);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
}
/* 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);
}
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
public double getShooterRotatePosition(){
return m_shooterRotateMotor.getEncoder().getPosition();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+10 -1
View File
@@ -19,12 +19,14 @@ 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.wpilibj.command.WaitUntilCommand;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput[] m_beamSensors = new DigitalInput[6];
CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -68,6 +70,11 @@ public class Storage extends SubsystemBase {
m_encoder.setPosition(0);
}
public void testBeams(){
SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
}
/* Storage PID Control */
public void runStoragePositionPID(double targetPos){
// Set PID Coefficients
@@ -78,6 +85,8 @@ public class Storage extends SubsystemBase {
m_storagePIDController.setFF(storageGains.m_kF);
m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
SmartDashboard.putNumber("Storage Position PID Target", targetPos);
SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}
src/main/java/frc4388/utility/ShooterTables.java
+2 -2
View File
@@ -93,7 +93,7 @@ public class ShooterTables {
SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
}
public double getHood(double distance) {
public double getHood(double distance) { //Rotations of motor
int i = 0;
while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
i ++;
@@ -108,7 +108,7 @@ public class ShooterTables {
}
}
public double getVelocity(double distance) {
public double getVelocity(double distance) { //Units per 100ms
int i = 0;
while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
i ++;