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 auto-programming

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-06 20:44:58 -07:00
parent 3c0b92a4ac 733ed01d9e
commit 6c1265a872
59 changed files with 822 additions and 589 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java
+2 -2
View File
@@ -5,10 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
src/main/java/frc4388/robot/commands/HoldTarget.java
-117
View File
@@ -1,117 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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/AutoPath1FromCenter.java → src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java
+2 -1
View File
@@ -5,9 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.GotoCoordinates;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
src/main/java/frc4388/robot/commands/AutoPath2FromRight.java → src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java
+2 -1
View File
@@ -5,9 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.GotoCoordinates;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
src/main/java/frc4388/robot/commands/Wait.java → src/main/java/frc4388/robot/commands/auto/Wait.java
+3 -4
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -46,11 +45,11 @@ public class Wait extends CommandBase {
public void execute() {
if (counter == 0) {
SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
//SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
}
m_currentTime = System.currentTimeMillis();
SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
//SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
counter ++;
}
src/main/java/frc4388/robot/commands/climber/DisengageRachet.java
+47
View File
@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Climber;
public class DisengageRachet extends CommandBase {
Climber m_climber;
/**
* Creates a new DisengageRachet command.
*/
public DisengageRachet(Climber subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_climber = subsystem;
}
// 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_climber.climberSafety) {
m_climber.shiftServo(false);
}
}
// 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 true;
}
}
src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java → src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java
+2 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Climber;
@@ -43,6 +43,7 @@ public class RunClimberWithTriggers extends CommandBase {
}
if (leftTrigger > rightTrigger) {
output = -leftTrigger;
m_climber.shiftServo(true);
}
} else {
output = rightTrigger;
src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java → src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java
+2 -2
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Leveler;
@@ -36,7 +36,7 @@ public class RunLevelerWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double input = m_controller.getLeftXAxis();
double input = m_controller.getRightXAxis();
m_leveler.runLeveler(input);
}
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java → src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java → src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java
+1 -2
View File
@@ -4,8 +4,7 @@
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java → src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java
+3 -3
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -61,7 +61,7 @@ public class DriveStraightToPositionMM extends CommandBase {
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
SmartDashboard.putBoolean("MM Run", true);
//SmartDashboard.putBoolean("MM Run", true);
i++;
}
@@ -74,7 +74,7 @@ public class DriveStraightToPositionMM extends CommandBase {
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
SmartDashboard.putBoolean("MM Run", false);
//SmartDashboard.putBoolean("MM Run", false);
return true;
} else {
if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java → src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java
+1 -2
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
src/main/java/frc4388/robot/commands/DriveWithJoystick.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
+6 -6
View File
@@ -5,10 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
@@ -42,7 +42,7 @@ public class DriveWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double moveInput = -m_controller.getLeftYAxis();
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
@@ -52,13 +52,13 @@ public class DriveWithJoystick extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
double cosMultiplier = 1.0;
double cosMultiplier;
double deadzone = .1;
if (m_pneumatics.m_isSpeedShiftHigh) {
cosMultiplier = 0.8;
cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
} else {
cosMultiplier = 1.0;
cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
}
if (steerInput > 0){
src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java
+4 -6
View File
@@ -5,9 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
@@ -61,9 +59,9 @@ public class DriveWithJoystickAuxPID extends CommandBase {
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
System.err.println("Target: " + m_targetGyro);
System.err.println("Current: " + currentGyro);
System.err.println();
//System.err.println("Target: " + m_targetGyro);
//System.err.println("Current: " + currentGyro);
//System.err.println();
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java
+24 -7
View File
@@ -5,16 +5,17 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickDriveStraight extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
@@ -22,6 +23,8 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
long m_deadTimeout = 100;
IHandController m_controller;
boolean m_isInterrupted;
double highGearMultiplier = 1;
double lowGearMultiplier = 1;
/**
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
@@ -35,6 +38,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_pneumatics = m_drive.m_pneumaticsSubsystem;
m_controller = controller;
addRequirements(m_drive);
}
@@ -84,9 +88,17 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .7;
double cosMultiplier;
double steerOutput = 0;
double deadzone = .1;
if (m_pneumatics.m_isSpeedShiftHigh) {
cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
} else {
cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
}
/* Curves the steer output to be similarily gradual */
if (steer > 0) {
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
@@ -94,12 +106,12 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
//System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
//System.out.println("Driving Straight with Target: " + m_targetGyro);
}
/**
@@ -107,8 +119,13 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
*/
private void resetGyroTarget() {
//m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetGyro = m_currentGyro
+ highGearMultiplier * m_drive.getTurnRate();
} else {
m_targetGyro = m_currentGyro
+ lowGearMultiplier * m_drive.getTurnRate();
}
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java
+4 -5
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
@@ -133,12 +132,12 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
//System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
//System.out.println("Driving Straight with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
@@ -159,7 +158,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
} else {
m_drive.driveWithInputAux(0, m_targetGyro);
}
System.out.println("Turning with Target: " + m_targetGyro);
//System.out.println("Turning with Target: " + m_targetGyro);
}
/**
src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java
+87
View File
@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
// 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 GotoCoordinatesRobotRelative extends SequentialCommandGroup {
Drive m_drive;
Pneumatics m_pneumatics;
double m_xTarget;
double m_yTarget;
double m_currentAngle;
double m_hypotDist;
double m_endAngle;
/**
* Creates a new GotoPosition.
*/
public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
m_pneumatics = subsystem2;
m_xTarget = xTarget;
m_yTarget = yTarget;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = endAngle;
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));
}
public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
m_drive = subsystem;
m_pneumatics = subsystem2;
m_xTarget = xTarget;
m_yTarget = yTarget;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = m_currentAngle;
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));
}
public boolean isQuadrantThree() {
if ((m_xTarget < 0) && (m_yTarget < 0)) {
return true;
} else {
return false;
}
}
public double calcAngle() {
if (isQuadrantThree()) {
return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
} else {
return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
}
}
}
src/main/java/frc4388/robot/commands/PlaySongDrive.java → src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
src/main/java/frc4388/robot/commands/TurnDegrees.java → src/main/java/frc4388/robot/commands/drive/TurnDegrees.java
+3 -3
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -50,8 +50,8 @@ public class TurnDegrees extends CommandBase {
m_drive.runTurningPID(m_targetAngleTicksOut);
SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
//SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
//SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
i++;
}
src/main/java/frc4388/robot/commands/RunExtenderOutIn.java → src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java
+1 -8
View File
@@ -5,17 +5,15 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.intake;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.controller.IHandController;
public class RunExtenderOutIn extends CommandBase {
private Intake m_intake;
@@ -36,11 +34,6 @@ public class RunExtenderOutIn extends CommandBase {
// Use addRequirements() here to declare subsystem dependencies.
m_intake = subsystem;
addRequirements(m_intake);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(false);
m_extenderReverseLimit.enableLimitSwitch(false);
}
// Called when the command is initially scheduled.
src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java → src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java
+5 -9
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.intake;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Intake;
@@ -41,16 +41,12 @@ public class RunIntakeWithTriggers extends CommandBase {
double rightTrigger = m_controller.getRightTriggerAxis();
double leftTrigger = m_controller.getLeftTriggerAxis();
double output = 0;
if (leftTrigger < .5) {
if(leftTrigger > rightTrigger) {
output = leftTrigger;
}
if (rightTrigger > leftTrigger) {
output = -leftTrigger;
}
} else {
if(leftTrigger >= rightTrigger) {
output = leftTrigger;
}
if (rightTrigger > leftTrigger) {
output = -rightTrigger;
}
m_intake.runIntake(output);
}
src/main/java/frc4388/robot/commands/CalibrateShooter.java → src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
+9 -11
View File
@@ -5,28 +5,31 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
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;
import frc4388.robot.subsystems.ShooterHood;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
/**
* 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) {
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) {
m_shooter = shootSub;
m_shooterAim = aimSub;
addRequirements(m_shooter, m_shooterAim);
m_shooterHood = hoodSub;
addRequirements(m_shooter, m_shooterHood, m_shooterAim);
}
// Called when the command is initially scheduled.
@@ -37,10 +40,8 @@ public class CalibrateShooter extends CommandBase {
// 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_shooterHood.m_angleEncoder.setPosition(0);
m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
@@ -51,9 +52,6 @@ public class CalibrateShooter extends CommandBase {
// 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);
}
src/main/java/frc4388/robot/commands/HoodPositionPID.java → src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java
+18 -15
View File
@@ -5,22 +5,22 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
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.ShooterHood;
public class HoodPositionPID extends CommandBase {
Shooter m_shooter;
ShooterHood m_shooterHood;
double firingAngle;
/**
* Creates a new HoodPositionPID.
*/
public HoodPositionPID(Shooter subSystem) {
m_shooter = subSystem;
//addRequirements(m_shooter);
public HoodPositionPID(ShooterHood subSystem) {
m_shooterHood = subSystem;
addRequirements(m_shooterHood);
}
// Called when the command is initially scheduled.
@@ -31,12 +31,13 @@ public class HoodPositionPID extends CommandBase {
// 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);
//double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
//double b = ShooterConstants.HOOD_CONVERT_B;
//firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
firingAngle = m_shooterHood.addFireAngle();
//SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
//SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooterHood.runAngleAdjustPID(firingAngle);
}
// Called once the command ends or is interrupted.
@@ -47,9 +48,11 @@ public class HoodPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
return true;
m_shooterHood.m_isHoodReady = true;
} else {
m_shooterHood.m_isHoodReady = false;
}
return false;
}
src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
+75
View File
@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* 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.shooter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
public class PrepChecker extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
Storage m_storage;
boolean m_isDrumReady = false;
boolean m_isAimReady = false;
boolean m_isHoodReady = false;
boolean m_isStorageReady = false;
/**
* Creates a new PrepChecker.
* @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands.
* @param storage reads storage in a similar way to shooter. Not used as a requirement.
*/
public PrepChecker(Shooter shooter, Storage storage) {
m_shooter = shooter;
m_shooterAim = m_shooter.m_shooterAimSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
m_storage = storage;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isDrumReady = false;
m_isAimReady = false;
m_isHoodReady = false;
m_isStorageReady = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady);
m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady);
m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady);
m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooter.m_isDrumReady = false;
m_shooterAim.m_isAimReady = false;
m_shooterHood.m_isHoodReady = false;
m_storage.m_isStorageReadyToFire = false;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/ShootFireGroup.java → src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+8 -6
View File
@@ -5,12 +5,14 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.commands.storage.StorageRun;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
@@ -23,12 +25,12 @@ public class ShootFireGroup extends ParallelRaceGroup {
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, 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)
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
new TrackTarget(m_shooterAim)
//new StorageRun(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShootFullGroup.java → src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java
+5 -4
View File
@@ -5,11 +5,12 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
@@ -22,10 +23,10 @@ public class ShootFullGroup extends SequentialCommandGroup {
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
addCommands(
new ShootPrepGroup(m_shooter, m_shooterAim, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShootPrepGroup.java → src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+11 -9
View File
@@ -5,30 +5,32 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import frc4388.robot.commands.storage.StoragePrepAim;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
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 {
public class ShootPrepGroup extends ParallelDeadlineGroup {
/**
* 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),
public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
super(
new PrepChecker(m_shooter, m_storage),
new TrackTarget(m_shooterAim),
new ShooterVelocityControlPID(m_shooter),
new StoragePrepAim(m_storage),
new HoodPositionPID(m_shooter)
new HoodPositionPID(m_shooterHood),
new StoragePrepAim(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java → src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+10 -10
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
@@ -16,6 +15,7 @@ public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
double m_actualVel;
/**
* Runs the drum at a velocity
* @param subsystem The Shooter subsytem
@@ -34,10 +34,10 @@ public class ShooterVelocityControlPID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
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());
m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
//m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
//SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
//SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
}
// Called once the command ends or is interrupted.
@@ -52,12 +52,12 @@ public class ShooterVelocityControlPID extends CommandBase {
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;
m_shooter.m_isDrumReady = true;
}
else{
SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
return false;
m_shooter.m_isDrumReady = false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/TrackTarget.java → src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+29 -32
View File
@@ -5,31 +5,25 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
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;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class TrackTarget extends CommandBase {
// Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
ShooterHood m_shooterHood;
NetworkTableEntry xEntry;
IHandController m_driverController;
// Aiming
double turnAmount = 0;
@@ -43,16 +37,15 @@ public class TrackTarget extends CommandBase {
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, ShooterAim aimSubsystem) {
public TrackTarget(ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
m_shooter = m_shooterAim.m_shooterSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
@@ -63,7 +56,6 @@ public class TrackTarget extends CommandBase {
// 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.
@@ -85,7 +77,7 @@ public class TrackTarget extends CommandBase {
} else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
}
m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
@@ -95,18 +87,22 @@ public class TrackTarget extends CommandBase {
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);
//fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
//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*/
//START CSV Code
fireVel = m_shooter.m_shooterTable.getVelocity(distance);
fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
//fireAngle = 33;
//END CSV Code
//fireVel = SmartDashboard.getNumber("Velocity Target", 0);
//fireAngle = SmartDashboard.getNumber("Angle Target", 3);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
}
@@ -121,10 +117,11 @@ public class TrackTarget extends CommandBase {
public boolean isFinished() {
if (xAngle < 1 && xAngle > -1 && target == 1)
{
SmartDashboard.putBoolean("TrackTarget Finished", true);
return true;
m_shooterAim.m_isAimReady = true;
} else {
m_shooterAim.m_isAimReady = false;
}
SmartDashboard.putBoolean("TrackTarget Finished", false);
return false;
}
}
src/main/java/frc4388/robot/commands/TrimShooter.java → src/main/java/frc4388/robot/commands/shooter/TrimShooter.java
+2 -6
View File
@@ -5,16 +5,12 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
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;
import frc4388.utility.controller.XboxController;
public class TrimShooter extends CommandBase {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
src/main/java/frc4388/robot/commands/StorageIntake.java → src/main/java/frc4388/robot/commands/storage/StorageIntake.java
+3 -4
View File
@@ -5,10 +5,9 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
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;
@@ -39,7 +38,7 @@ public class StorageIntake extends CommandBase {
@Override
public void execute() {
if (!m_storage.getBeam(0)){
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
intook = true;
}
@@ -56,7 +55,7 @@ public class StorageIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(0) && intook){
if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){
return true;
}
return false;
src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java
+40
View File
@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
public class StorageIntakeFinal extends CommandBase {
/**
* Creates a new StorageIntakeFinal.
*/
public StorageIntakeFinal() {
// Use addRequirements() here to declare subsystem dependencies.
}
// 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() {
}
// 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/commands/StorageOutake.java → src/main/java/frc4388/robot/commands/storage/StorageOutake.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
src/main/java/frc4388/robot/commands/StoragePositionPID.java → src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
src/main/java/frc4388/robot/commands/StoragePrepAim.java → src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java
+9 -7
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -34,7 +34,7 @@ public class StoragePrepAim extends CommandBase {
@Override
public void execute() {
if (m_storage.getBeam(1)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
//m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
m_storage.runStorage(0);
@@ -49,11 +49,13 @@ public class StoragePrepAim extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
SmartDashboard.putBoolean("StoragePrepAim Finished", true);
/*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
m_storage.m_isStorageReadyToFire = true;
return true;
}
SmartDashboard.putBoolean("StoragePrepAim Finished", false);
return false;
} else {
m_storage.m_isStorageReadyToFire = false;
return false;
}*/
return true;
}
}
src/main/java/frc4388/robot/commands/StoragePrepIntake.java → src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java
+4 -4
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
@@ -38,7 +38,7 @@ public class StoragePrepIntake extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(0)){
if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
else{
@@ -54,8 +54,8 @@ public class StoragePrepIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return true;
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return false;
}
return false;
}
src/main/java/frc4388/robot/commands/StorageRun.java → src/main/java/frc4388/robot/commands/storage/StorageRun.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;