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

Rework Command Groups to use Prep Checker

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-03 01:12:07 -07:00
parent 1b603d4f5d
commit 9288d2b934
13 changed files with 105 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/shooter/HoldTarget.java
-121
View File
@@ -1,121 +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.shooter;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
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 HoldTarget extends CommandBase {
// Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
ShooterHood m_shooterHood;
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(ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = m_shooterAim.m_shooterSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
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);
//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) * (180/Math.PI);
*/
//END Equation 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
m_shooter.m_fireVel = fireVel;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
}
// 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/shooter/HoodPositionPID.java
+3 -1
View File
@@ -48,7 +48,9 @@ public class HoodPositionPID extends CommandBase {
public boolean isFinished() {
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/shooter/ShootFireGroup.java
+1 -2
View File
@@ -26,11 +26,10 @@ public class ShootFireGroup extends ParallelRaceGroup {
* @param m_storage The Storage subsytem
*/
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),
new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
new HoldTarget(m_shooterAim),
new TrackTarget(m_shooterAim),
new StorageRun(m_storage)
);
}
src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+8 -4
View File
@@ -7,7 +7,10 @@
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.commands.storage.StoragePrepAim;
import frc4388.robot.subsystems.Shooter;
@@ -18,20 +21,21 @@ 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, ShooterHood m_shooterHood, Storage m_storage) {
addCommands(
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 HoodPositionPID(m_shooterHood),
//new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
new StoragePrepAim(m_storage)
//new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
);
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+4 -4
View File
@@ -54,12 +54,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/shooter/TrackTarget.java
+4 -3
View File
@@ -116,10 +116,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/storage/StoragePrepAim.java
+4 -3
View File
@@ -50,10 +50,11 @@ public class StoragePrepAim extends CommandBase {
@Override
public boolean isFinished() {
if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
SmartDashboard.putBoolean("StoragePrepAim Finished", true);
m_storage.m_isStorageReadyToFire = true;
return true;
} else {
m_storage.m_isStorageReadyToFire = false;
return false;
}
SmartDashboard.putBoolean("StoragePrepAim Finished", false);
return false;
}
}