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

calibration speed + velocity ramp only if soft limits

Browse Source
This commit is contained in:
aarav18
2022-03-19 21:03:01 -06:00
parent be850585ec
commit 6b38e89660
5 changed files with 116 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+10 -2
View File
@@ -271,7 +271,7 @@ public class RobotContainer {
.whenReleased(() -> m_robotSwerveDrive.stopModules());
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
.whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry))
.whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
@@ -340,12 +340,20 @@ public class RobotContainer {
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
// .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
// .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber))
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+21 -9
View File
@@ -4,6 +4,9 @@
package frc4388.robot.commands.ShooterCommands;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
@@ -15,9 +18,11 @@ import frc4388.robot.subsystems.VisionOdometry;
public class AimToCenter extends CommandBase {
/** Creates a new AimWithOdometry. */
Turret m_turret;
SwerveDrive m_drive;
VisionOdometry m_visionOdometry;
Supplier<Pose2d> supplier;
Pose2d odo;
// use odometry to find x and y later
double x;
double y;
@@ -25,28 +30,35 @@ public class AimToCenter extends CommandBase {
// public static Gains m_aimGains;
public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) {
public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier<Pose2d> supplier) {
// Use addRequirements() here to declare subsystem dependencies.
m_turret = turret;
m_drive = drive;
m_visionOdometry = visionOdometry;
addRequirements(m_turret, m_drive, m_visionOdometry);
this.supplier = supplier;
addRequirements(m_turret, m_visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = -m_drive.getOdometry().getY();
y = -m_drive.getOdometry().getX();
odo = this.supplier.get();
// ! Yes I realize this stupid, yes it works I promise, coordinate system is funky
x = -odo.getY();
y = -odo.getX();
SmartDashboard.putNumber("trans x", x);
SmartDashboard.putNumber("trans y", y);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360;
odo = this.supplier.get(); // * update odometry using really cool supplier -aarav
m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360;
SmartDashboard.putNumber("Target Angle", m_targetAngle);
m_turret.runShooterRotatePID(m_targetAngle);
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+43 -41
View File
@@ -20,22 +20,22 @@ import frc4388.utility.Gains;
public class Shoot extends CommandBase {
// subsystems
private SwerveDrive m_swerve;
private BoomBoom m_boomBoom;
private Turret m_turret;
private Hood m_hood;
private SwerveDrive swerve;
private BoomBoom drum;
private Turret turret;
private Hood hood;
// given
private double m_gyroAngle;
private double m_odoX;
private double m_odoY;
private double m_distance;
private double gyroAngle;
private double odoX;
private double odoY;
private double distance;
// targets
private double m_targetVel;
private double m_targetHood;
private double m_targetAngle;
private Pose2d m_targetPoint;
private double targetVel;
private double targetHood;
private double targetAngle;
private Pose2d targetPoint;
// pid
private double error;
@@ -59,20 +59,22 @@ public class Shoot extends CommandBase {
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
* TODO: Velocity Correction
* @param sDrive Drive Train
* @param sShooter Shooter Drum
* @param sTurret Shooter Turret
* @param sHood Shooter Hood
*
* @param swerve Drive Train
* @param drum Shooter Drum
* @param turret Shooter Turret
* @param hood Shooter Hood
*
* @author Aarav Shah
*/
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
// Use addRequirements() here to declare subsystem dependencies.
m_swerve = sDrive;
m_boomBoom = sShooter;
m_turret = sTurret;
m_hood = sHood;
this.swerve = swerve;
this.drum = drum;
this.turret = turret;
this.hood = hood;
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
addRequirements(this.swerve, this.drum, this.turret, this.hood);
kP = gains.kP;
kI = gains.kI;
@@ -98,16 +100,16 @@ public class Shoot extends CommandBase {
* Updates error for custom PID.
*/
public void updateError() {
m_targetPoint = SwerveDriveConstants.HUB_POSE;
targetPoint = SwerveDriveConstants.HUB_POSE;
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
// error = (m_targetAngle - turretDummy.get() + 360) % 360;
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360;
isAimedInTolerance = (Math.abs(error) <= tolerance);
if (simMode) {
SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
System.out.println("Target Angle: " + m_targetAngle);
System.out.println("Target Angle: " + targetAngle);
System.out.println("Error: " + error);
}
}
@@ -116,23 +118,23 @@ public class Shoot extends CommandBase {
@Override
public void initialize() {
m_turret.gotoMidpoint();
turret.gotoMidpoint();
m_odoX = 0;//-m_swerve.getOdometry().getY();
m_odoY = -8;//-m_swerve.getOdometry().getX();
odoX = 0;//-m_swerve.getOdometry().getY();
odoY = -8;//-m_swerve.getOdometry().getX();
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
initialSwerveRotation = m_gyroAngle;
gyroAngle = swerve.getRegGyro().getDegrees();
initialSwerveRotation = gyroAngle;
// get targets (shooter tables)
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
targetVel = drum.getVelocity(distance);
targetHood = drum.getHood(distance);
m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
// m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
// deadzone processing
if (AimToCenter.isDeadzone(m_targetAngle)) {}
if (AimToCenter.isDeadzone(targetAngle)) {}
// initial error
updateError();
@@ -174,12 +176,12 @@ public class Shoot extends CommandBase {
driveDummy.apply(normOutput);
System.out.println("Drive Dummy: " + driveDummy.get());
}
runPID();
SmartDashboard.putNumber("Error", this.error);
SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle);
SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
SmartDashboard.putNumber("Normalized Output", normOutput);
m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
// entire swerve drive or its the line below
// m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
@@ -190,14 +192,14 @@ public class Shoot extends CommandBase {
turretDummy.apply(normOutput);
System.out.println("Turret Dummy: " + turretDummy.get());
}
m_turret.m_boomBoomRotateMotor.set(normOutput);
turret.m_boomBoomRotateMotor.set(normOutput);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
// return to initial swerve rotation
m_swerve.driveWithInput(0, 0, initialSwerveRotation, true);
swerve.driveWithInput(0, 0, initialSwerveRotation, true);
}
// Returns true when the command should end.
src/main/java/frc4388/robot/subsystems/Hood.java
+20 -12
View File
@@ -35,6 +35,7 @@ public class Hood extends SubsystemBase {
public double m_fireAngle;
public double speedLimiter;
public double calibrationSpeed = 1.0;
/** Creates a new Hood. */
public Hood(CANSparkMax angleAdjusterMotor) {
@@ -63,23 +64,30 @@ public class Hood extends SubsystemBase {
SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (areSoftLimitsEnabled()) {
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
}
}
public boolean areSoftLimitsEnabled() {
return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
}
/**
* Set status of hood motor soft limits.
* @param set Boolean to set soft limits to.
@@ -107,7 +115,7 @@ public class Hood extends SubsystemBase {
* @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle)
*/
public void runHood(double input) {
m_angleAdjusterMotor.set(input * this.speedLimiter);
m_angleAdjusterMotor.set(input * this.speedLimiter * this.calibrationSpeed);
}
/**
src/main/java/frc4388/robot/subsystems/Turret.java
+22 -13
View File
@@ -46,7 +46,8 @@ public class Turret extends SubsystemBase {
long leftCurrentTime;
long leftElapsedTime;
double speedLimiter;
public double speedLimiter;
public double calibrationSpeed = 1.0;
public Turret(CANSparkMax boomBoomRotateMotor) {
@@ -148,23 +149,31 @@ public class Turret extends SubsystemBase {
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if (areSoftLimitsEnabled()) {
if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
}
}
public boolean areSoftLimitsEnabled() {
return this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
}
/**
* Set status of turret motor soft limits.
* @param set Boolean to set soft limits to.
@@ -192,7 +201,7 @@ public class Turret extends SubsystemBase {
* @param input from -1.0 to 1.0, positive is clockwise
*/
public void runTurretWithInput(double input) {
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter);
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed);
}
public void runShooterRotatePID(double targetAngle) {