mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
extender and intake changes
This commit is contained in:
aarav18
@@ -0,0 +1,85 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
|
||||
public class AimToCenter extends CommandBase {
|
||||
/** Creates a new AimWithOdometry. */
|
||||
Turret m_turret;
|
||||
SwerveDrive m_drive;
|
||||
VisionOdometry m_visionOdometry;
|
||||
|
||||
// use odometry to find x and y later
|
||||
double x;
|
||||
double y;
|
||||
double m_targetAngle;
|
||||
|
||||
// public static Gains m_aimGains;
|
||||
|
||||
public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_turret = turret;
|
||||
m_drive = drive;
|
||||
m_visionOdometry = visionOdometry;
|
||||
addRequirements(m_turret, m_drive, m_visionOdometry);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
|
||||
m_turret.runShooterRotatePID(m_targetAngle);
|
||||
|
||||
// Check if limelight is within range (comment out to disable vision odo)
|
||||
if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
|
||||
m_visionOdometry.updateOdometryWithVision();
|
||||
m_visionOdometry.setLEDs(true);
|
||||
}
|
||||
else{
|
||||
m_visionOdometry.setLEDs(false);
|
||||
}
|
||||
}
|
||||
|
||||
public static double angleToCenter(double x, double y, double gyro) {
|
||||
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
|
||||
return angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if in deadzone.
|
||||
* @param angle Angle to check.
|
||||
* @return True if in deadzone.
|
||||
*/
|
||||
public static boolean isDeadzone(double angle) {
|
||||
if (angle == Double.NaN) {
|
||||
return false;
|
||||
}
|
||||
return !((ShooterConstants.TURRET_REVERSE_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_LIMIT));
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,232 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.utility.DummySensor;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class Shoot extends CommandBase {
|
||||
|
||||
// subsystems
|
||||
public SwerveDrive m_swerve;
|
||||
public BoomBoom m_boomBoom;
|
||||
public Turret m_turret;
|
||||
public Hood m_hood;
|
||||
|
||||
// given
|
||||
public double m_gyroAngle;
|
||||
public double m_odoX;
|
||||
public double m_odoY;
|
||||
public double m_distance;
|
||||
|
||||
// targets
|
||||
public double m_targetVel;
|
||||
public double m_targetHood;
|
||||
public double m_targetAngle;
|
||||
public Pose2d m_targetPoint;
|
||||
|
||||
// pid
|
||||
public double error;
|
||||
public double prevError;
|
||||
public Gains gains = ShooterConstants.SHOOT_GAINS;
|
||||
public double kP, kI, kD;
|
||||
public double proportional, integral, derivative;
|
||||
public double time;
|
||||
public double output;
|
||||
public double normOutput;
|
||||
public double tolerance;
|
||||
public boolean isAimedInTolerance;
|
||||
public int inverted;
|
||||
|
||||
// testing
|
||||
public boolean simMode = true;
|
||||
public DummySensor driveDummy;
|
||||
public DummySensor turretDummy;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_swerve = sDrive;
|
||||
m_boomBoom = sShooter;
|
||||
m_turret = sTurret;
|
||||
m_hood = sHood;
|
||||
|
||||
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
|
||||
|
||||
kP = gains.kP;
|
||||
kI = gains.kI;
|
||||
kD = gains.kD;
|
||||
|
||||
proportional = 0;
|
||||
integral = 0;
|
||||
derivative = 0;
|
||||
time = 0.02;
|
||||
|
||||
tolerance = 5.0;
|
||||
isAimedInTolerance = false;
|
||||
|
||||
if (simMode) {
|
||||
driveDummy = new DummySensor(180);
|
||||
turretDummy = new DummySensor(180);
|
||||
|
||||
DummySensor.resetAll();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates error for custom PID.
|
||||
*/
|
||||
public void updateError() {
|
||||
m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation());
|
||||
m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
|
||||
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
|
||||
error = (m_targetAngle - turretDummy.get() + 360) % 360;
|
||||
// error = (m_targetAngle - m_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("Error: " + error);
|
||||
}
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_odoX = 0;//m_swerve.getOdometry().getX();
|
||||
m_odoY = -1;//m_swerve.getOdometry().getY();
|
||||
|
||||
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
|
||||
|
||||
// get targets (shooter tables)
|
||||
m_targetVel = m_boomBoom.getVelocity(m_distance);
|
||||
m_targetHood = m_boomBoom.getHood(m_distance);
|
||||
|
||||
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
|
||||
|
||||
// deadzone processing
|
||||
if (AimToCenter.isDeadzone(m_targetAngle)) {}
|
||||
|
||||
// initial error
|
||||
updateError();
|
||||
System.out.println("Error: " + error);
|
||||
prevError = error;
|
||||
}
|
||||
/**
|
||||
* Run custom PID.
|
||||
*/
|
||||
public void runPID() {
|
||||
if (error > 180){
|
||||
error = 360 - error;
|
||||
inverted = -1;
|
||||
}
|
||||
else{
|
||||
inverted = 1;
|
||||
}
|
||||
prevError = error;
|
||||
updateError();
|
||||
|
||||
proportional = error;
|
||||
integral = integral + error * time;
|
||||
derivative = (error - prevError) / time;
|
||||
output = kP * proportional + kI * integral + kD * derivative;
|
||||
normOutput = output/360 * inverted;
|
||||
}
|
||||
// TODO: horizontal velocity correction
|
||||
public double hTargetDistanceFromHub() {
|
||||
|
||||
double hVel = m_swerve.getChassisSpeeds()[0];
|
||||
double velBeforeCorrection = m_boomBoom.getVelocity(m_distance);
|
||||
double vDistanceFromHub = m_odoY;
|
||||
double approxTravelTime = vDistanceFromHub / velBeforeCorrection;
|
||||
double hTargetDistanceFromHub = hVel * approxTravelTime;
|
||||
|
||||
// return hTargetDistanceFromHub;
|
||||
return 0.0; // this is for no velocity correction
|
||||
}
|
||||
|
||||
public Pose2d findTargetPoint() {
|
||||
|
||||
// position vector and radius
|
||||
Translation2d position = new Translation2d(m_odoX, m_odoY);
|
||||
double radius = position.getNorm();
|
||||
|
||||
// equation of circle = x^2 + y^2 = m_distance^2
|
||||
// derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y
|
||||
|
||||
// velocity vector (x, y)
|
||||
Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]);
|
||||
|
||||
// unit tangential vector
|
||||
Translation2d tangential = new Translation2d(0, 0);
|
||||
// velocity vector (tangential, radial)
|
||||
Translation2d polarVelocity = new Translation2d(0, 0);
|
||||
|
||||
return SwerveDriveConstants.HUB_POSE;
|
||||
}
|
||||
|
||||
// TODO: vertical velocity correction
|
||||
public double vTargetDistanceFromHub() {
|
||||
return 0.0; // this is for no velocity correction
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (simMode) {
|
||||
System.out.println("Normalized Output: " + normOutput);
|
||||
}
|
||||
// custom pid
|
||||
runPID();
|
||||
|
||||
if (simMode) {
|
||||
driveDummy.apply(normOutput);
|
||||
System.out.println("Drive Dummy: " + driveDummy.get());
|
||||
}
|
||||
m_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);
|
||||
|
||||
m_hood.runAngleAdjustPID(m_targetHood);
|
||||
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
|
||||
|
||||
if (simMode) {
|
||||
turretDummy.apply(normOutput);
|
||||
System.out.println("Turret Dummy: " + turretDummy.get());
|
||||
}
|
||||
m_turret.m_boomBoomRotateMotor.set(normOutput);
|
||||
}
|
||||
|
||||
// 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() {
|
||||
if (simMode) {
|
||||
return isAimedInTolerance;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.desmos.DesmosServer;
|
||||
|
||||
public class TrackTarget extends CommandBase {
|
||||
/** Creates a new TrackTarget. */
|
||||
Turret m_turret;
|
||||
VisionOdometry m_visionOdometry;
|
||||
BoomBoom m_boomBoom;
|
||||
Hood m_hood;
|
||||
|
||||
// use odometry to find x and y later
|
||||
double x;
|
||||
double y;
|
||||
double distance;
|
||||
double vel;
|
||||
double hood;
|
||||
double average;
|
||||
double output;
|
||||
Pose2d pos = new Pose2d();
|
||||
ArrayList<Point> points = new ArrayList<>();
|
||||
|
||||
double m=0;
|
||||
double b=0;
|
||||
boolean isExecuted = false;
|
||||
|
||||
// public static Gains m_aimGains;
|
||||
|
||||
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_turret = turret;
|
||||
m_boomBoom = boomBoom;
|
||||
m_hood = hood;
|
||||
m_visionOdometry = visionOdometry;
|
||||
|
||||
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
//m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
|
||||
try {
|
||||
m_visionOdometry.setLEDs(true);
|
||||
points = m_visionOdometry.getTargetPoints();
|
||||
for(int i = 0; i < points.size(); i++) {
|
||||
DesmosServer.putPoint("Point" + i, points.get(i));
|
||||
}
|
||||
|
||||
Point average = VisionOdometry.averagePoint(points);
|
||||
DesmosServer.putPoint("average", average);
|
||||
|
||||
output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
|
||||
output *= 2;
|
||||
DesmosServer.putDouble("output", output);
|
||||
m_turret.runTurretWithInput(output);
|
||||
|
||||
double y_rot = average.y / VisionConstants.LIME_VIXELS;
|
||||
y_rot *= Math.toRadians(VisionConstants.V_FOV);
|
||||
y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2;
|
||||
y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
|
||||
|
||||
double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
|
||||
DesmosServer.putDouble("distance", distance);
|
||||
|
||||
updateRegressionDesmos();
|
||||
double regressedDistance = distanceRegression(distance);
|
||||
DesmosServer.putDouble("distanceReg", regressedDistance);
|
||||
|
||||
//Vision odemetry circle fit based pose estimate
|
||||
Point targetOffset = m_visionOdometry.getTargetOffset();
|
||||
DesmosServer.putPoint("targetOff", targetOffset);
|
||||
|
||||
// isExecuted = true;
|
||||
}
|
||||
catch (Exception e){
|
||||
e.printStackTrace();
|
||||
// System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java");
|
||||
}
|
||||
// vel = m_boomBoom.getVelocity(distance);
|
||||
// hood = m_boomBoom.getHood(distance);
|
||||
// m_boomBoom.runDrumShooter(vel);
|
||||
// m_boomBoom.runDrumShooterVelocityPID(vel);
|
||||
// m_hood.runAngleAdjustPID(hood);
|
||||
// m_turret.runshooterRotatePID(m_targetAngle);
|
||||
}
|
||||
|
||||
public final double distanceRegression(double distance) {
|
||||
return (1.09517561985 * distance + 20.1846165624);
|
||||
}
|
||||
|
||||
public void updateRegressionDesmos() {
|
||||
m = DesmosServer.readDouble("m");
|
||||
b = DesmosServer.readDouble("b");
|
||||
|
||||
DesmosServer.putArray("MB", m, b);
|
||||
}
|
||||
|
||||
// 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;
|
||||
// return isExecuted && Math.abs(output) < .1;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user