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

hood turret and tracking working

Browse Source
This commit is contained in:
ryan123rudder
2020-02-29 18:42:37 -07:00
parent 7c9bdc030f
commit 493c4f69de
8 changed files with 184 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -3
View File
@@ -122,7 +122,7 @@ public final class Constants {
//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.5, 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;
@@ -169,8 +169,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
+18 -9
View File
@@ -16,22 +16,26 @@ 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;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
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.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;
@@ -169,14 +173,18 @@ public class RobotContainer {
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
//.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
//.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
.whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Angle Current", m_robotShooter.m_angleAdjustMotor.getOutputCurrent())))
.whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Rotate Current", m_robotShooterAim.m_shooterRotateMotor.getOutputCurrent())));
// starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
//.whileHeld(new TrackTarget(m_robotShooter,m_robotShooterAim))
.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
//.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
.whileHeld(new ParallelCommandGroup(
new HoldTarget(m_robotShooter,m_robotShooterAim),
new HoodPositionPID(m_robotShooter)));
//.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//Prepares storage for intaking
@@ -188,16 +196,17 @@ public class RobotContainer {
.whileHeld(new StorageOutake(m_robotStorage));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.2)))
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2)))
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(7)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
}
/**
src/main/java/frc4388/robot/commands/CalibrateShooter.java
+63
View File
@@ -0,0 +1,63 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
/**
* Creates a new CalibrateShooter.
*/
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(-0.1);
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(0.075);
}
// 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
+16 -4
View File
@@ -41,7 +41,7 @@ public class HoldTarget extends CommandBase {
public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
addRequirements(m_shooter);
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
@@ -69,7 +69,7 @@ public class HoldTarget extends CommandBase {
//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/3);
m_shooterAim.runShooterWithInput(-turnAmount);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
@@ -79,17 +79,29 @@ public class HoldTarget extends CommandBase {
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);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
}/*
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.
src/main/java/frc4388/robot/commands/HoodPositionPID.java
+48
View File
@@ -0,0 +1,48 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems.Shooter;
public class HoodPositionPID extends CommandBase {
Shooter m_shooter;
/**
* 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 firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
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() {
return false;
}
}
src/main/java/frc4388/robot/commands/TrackTarget.java
+4 -4
View File
@@ -74,7 +74,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_shooterAim.runShooterWithInput(-turnAmount);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
@@ -84,17 +84,17 @@ public class TrackTarget extends CommandBase {
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);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
}
}
// 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.
src/main/java/frc4388/robot/subsystems/Shooter.java
+15 -8
View File
@@ -13,6 +13,7 @@ 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;
@@ -35,7 +36,7 @@ public class Shooter extends SubsystemBase {
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
@@ -57,10 +58,10 @@ public class Shooter extends SubsystemBase {
*/
public Shooter() {
//Testing purposes reseting gyros
resetGyroAngleAdj();
//resetGyroAngleAdj();
m_shooterFalcon.configFactoryDefault();
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();
@@ -91,6 +92,11 @@ public class Shooter extends SubsystemBase {
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, 33);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3);
}
@Override
@@ -106,6 +112,7 @@ public class Shooter extends SubsystemBase {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
@@ -126,9 +133,8 @@ public class Shooter extends SubsystemBase {
}
/* Angle Adjustment PID Control */
public void runAngleAdjustPID(double mmtargetAngle)
public void runAngleAdjustPID(double targetAngle)
{
double targetAngle = addFireAngle();
// Set PID Coefficients
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
@@ -137,9 +143,6 @@ public class Shooter extends SubsystemBase {
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
@@ -162,4 +165,8 @@ public class Shooter extends SubsystemBase {
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
public double getAnglePosition(){
return m_angleAdjustMotor.getEncoder().getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+17 -7
View File
@@ -14,9 +14,11 @@ 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;
@@ -28,18 +30,24 @@ public class ShooterAim extends SubsystemBase {
// Configure PID Controllers
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
/**
* Creates a new ShooterAim.
*/
public ShooterAim() {
resetGyroShooterRotate();
//resetGyroShooterRotate();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterRightLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterLeftLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
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, -2);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, -56);
}
public void runShooterWithInput(double input) {
@@ -64,11 +72,13 @@ public class ShooterAim extends SubsystemBase {
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
/* For Testing Purposes, reseting gyro for shooter rotation */
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
m_shooterRotateEncoder.setPosition(0);
}
public double getShooterRotatePosition(){
return m_shooterRotateMotor.getEncoder().getPosition();
}
@Override