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

attempt at fix all

Browse Source 
notes: failed
This commit is contained in:
ryan123rudder
2020-03-02 21:03:39 -07:00
parent a6e5103193
commit ddf18c3a93
14 changed files with 100 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -121,8 +121,8 @@ 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 DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 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.0, 0.0, 0.0, 0.05, 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;
@@ -139,7 +139,7 @@ public final class Constants {
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 HOOD_CALIBRATE_SPEED = 0.2;
public static final double DRUM_RAMP_LIMIT = 1000;
public static final double DRUM_VELOCITY_BOUND = 300;
@@ -194,7 +194,7 @@ 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 = 64;
public static final double TARGET_HEIGHT = 71;
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;
src/main/java/frc4388/robot/RobotContainer.java
+22 -23
View File
@@ -181,22 +181,31 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
/* Operator Buttons */
// shoots until released
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)));
// shoots one ball
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
//.whenPressed(new RunExtenderOutIn(m_robotIntake));
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.3)));
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
.whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0)));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0)));
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
@@ -206,9 +215,9 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
//.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
//.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3)));
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
//Prepares storage for intaking
//new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
// .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
@@ -217,21 +226,11 @@ public class RobotContainer {
//new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
// .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)));
.whileHeld(new RunCommand(() -> m_robotIntake.runIntakeOut(0.1)))
.whenReleased(new RunCommand(() -> m_robotIntake.runIntakeOut(0.0)));
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3)));
//TEST FOR HOOD
//Run drum
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)));
.whileHeld(new RunCommand(() -> m_robotIntake.runIntakeIn(0.1)))
.whenReleased(new RunCommand(() -> m_robotIntake.runIntakeIn(0.0)));
//.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
.whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
//.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
//Trims shooter
new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
src/main/java/frc4388/robot/commands/HoldTarget.java
+30 -24
View File
@@ -13,6 +13,7 @@ import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.networktables.NetworkTable;
@@ -56,7 +57,7 @@ public class HoldTarget extends CommandBase {
//Vision Processing Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
}
@@ -67,39 +68,44 @@ public class HoldTarget extends CommandBase {
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;}
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));
// 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) * (180/Math.PI);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
*/
//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
}/*
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);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
*/
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/HoodPositionPID.java
+3 -3
View File
@@ -31,9 +31,9 @@ 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 slope = ShooterConstants.HOOD_CONVERT_SLOPE;
double b = ShooterConstants.HOOD_CONVERT_B;
firingAngle = (-slope*m_shooter.addFireAngle())+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);
@@ -49,7 +49,7 @@ public class HoodPositionPID extends CommandBase {
public boolean isFinished() {
double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
return true;
return false;
}
return false;
}
src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
-5
View File
@@ -36,11 +36,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.kNormallyOpen);
m_extenderForwardLimit.enableLimitSwitch(true);
m_extenderReverseLimit.enableLimitSwitch(true);
}
// Called when the command is initially scheduled.
src/main/java/frc4388/robot/commands/ShootFireGroup.java
+1 -1
View File
@@ -25,7 +25,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
*/
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.runDrumShooterVelocityPID(m_shooter.addFireVel())),
new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
new HoldTarget(m_shooter, m_shooterAim),
new StorageRun(m_storage)
src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+4 -4
View File
@@ -25,10 +25,10 @@ public class ShootPrepGroup extends ParallelCommandGroup {
*/
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)
//new TrackTarget(m_shooter, m_shooterAim),
//new ShooterVelocityControlPID(m_shooter)
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()))
//new StoragePrepAim(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+1 -1
View File
@@ -34,7 +34,7 @@ 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.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
src/main/java/frc4388/robot/commands/TrackTarget.java
+8 -8
View File
@@ -43,8 +43,6 @@ 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
@@ -63,7 +61,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.
@@ -92,21 +89,24 @@ public class TrackTarget extends CommandBase {
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_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
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+1 -1
View File
@@ -786,7 +786,7 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putString("Odometry Values Meters", getPose().toString());
//SmartDashboard.putNumber("Odometry Heading", getHeading());
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
if (m_currentSong != m_songChooser.getSelected()){
src/main/java/frc4388/robot/subsystems/Intake.java
+3 -7
View File
@@ -60,18 +60,14 @@ public class Intake extends SubsystemBase {
m_intakeMotor.set(input);
}
public void runIntakeIn(double input){
m_extenderMotor.set(-input);
}
public void runIntakeOut(double input){
public void runExtender(double input){
m_extenderMotor.set(input);
}
/**
* Runs extender motor
* @param input the percent output to run motor at
*/
public void runExtender(double input) {
/*public void runExtender(double input) {
if (m_extenderForwardLimit.get()) {
isExtended = true;
}
@@ -88,5 +84,5 @@ public class Intake extends SubsystemBase {
if (isExtended == true) {
m_extenderMotor.set(-input);
}
}
}*/
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+19 -15
View File
@@ -47,7 +47,7 @@ public class Shooter extends SubsystemBase {
double velP;
double input;
ShooterTables m_shooterTable;
public ShooterTables m_shooterTable;
public boolean velReached;
public double m_fireVel;
@@ -69,6 +69,7 @@ public class Shooter extends SubsystemBase {
m_shooterFalcon.setInverted(true);
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -77,7 +78,6 @@ public class Shooter extends SubsystemBase {
int closedLoopTimeMs = 1;
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterTable = new ShooterTables();
@@ -91,8 +91,6 @@ public class Shooter extends SubsystemBase {
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);
@@ -108,7 +106,19 @@ public class Shooter extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
try{
SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
SmartDashboard.putNumber("Fire Velocity From CSV", m_fireVel);
SmartDashboard.putNumber("Fire Angle From CSV", m_fireAngle);
//SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
}
catch(Exception e)
{
}
}
public double addFireVel() {
@@ -157,15 +167,9 @@ public class Shooter extends SubsystemBase {
* @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();
public void runDrumShooterVelocityPID(double targetVel) {
System.out.println("dddddddddddddddddddddddd" + targetVel);
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
public void resetGyroAngleAdj(){
@@ -173,6 +177,6 @@ public class Shooter extends SubsystemBase {
}
public double getAnglePosition(){
return m_angleAdjustMotor.getEncoder().getPosition();
return m_angleEncoder.getPosition();
}
}