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

encoder reset thingies

Browse Source
This commit is contained in:
aarav18
2022-03-06 18:02:04 -07:00
parent 1699ebeda8
commit b2b1451761
6 changed files with 117 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -6
View File
@@ -115,7 +115,6 @@ public final class Constants {
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
// TODO: put in real numbers for the hub
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
}
@@ -172,8 +171,7 @@ public final class Constants {
public static final double ENCODER_TICKS_PER_REV = 2048;
// Shoot Command Constants
public static final Gains SHOOT_DRIVE_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_TURRET_GAINS = new Gains(2.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
@@ -193,7 +191,7 @@ public final class Constants {
public static final int SHOOTER_ANGLE_ADJUST_ID = 20;
public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find
public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find
public static final float HOOD_FORWARD_LIMIT = 0; // TODO: find
public static final float HOOD_FORWARD_LIMIT = 200; // TODO: find
public static final float HOOD_REVERSE_LIMIT = 0; // TODO: find
}
@@ -209,8 +207,8 @@ public final class Constants {
public static final String NAME = "photonCamera";
public static final double TARGET_HEIGHT = 8*12 + 8; // Convert to metric
public static final double TARGET_RADIUS = 4*12; // Convert to metric
public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?)
public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?)
public static final double H_FOV = 59.6;
public static final double V_FOV = 49.7;
public static final double LIME_VIXELS = 960;
src/main/java/frc4388/robot/Robot.java
+3
View File
@@ -136,6 +136,9 @@ public class Robot extends TimedRobot {
// robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
// odo chooser stuff
addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)),
src/main/java/frc4388/robot/RobotContainer.java
+13 -6
View File
@@ -95,10 +95,10 @@ public class RobotContainer {
private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
// private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
/*private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);*/
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
/* Controllers */
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -214,6 +214,13 @@ public class RobotContainer {
// Y > Retract Intake
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(() -> m_robotIntake.runExtender(false));
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0)));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0)));
// Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
@@ -402,8 +409,8 @@ public class RobotContainer {
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
// actual robot velocity.
Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
m_robotSwerveDrive.getChassisSpeeds()[1]);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
pathPoints.add(waypoint);
src/main/java/frc4388/robot/RobotMap.java
+3 -3
View File
@@ -35,7 +35,7 @@ public class RobotMap {
public RobotMap() {
// configureLEDMotorControllers();
configureSwerveMotorControllers();
// configureShooterMotorControllers();
configureShooterMotorControllers();
}
/* LED Subsystem */
@@ -213,8 +213,8 @@ public class RobotMap {
// numbers out of our ass anymore
// hood subsystem
// angleAdjusterMotor.restoreFactoryDefaults();
// angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
angleAdjusterMotor.restoreFactoryDefaults();
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
}
src/main/java/frc4388/robot/commands/Shoot.java
+77 -21
View File
@@ -4,9 +4,12 @@
package frc4388.robot.commands;
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;
@@ -32,13 +35,12 @@ public class Shoot extends CommandBase {
public double m_targetVel;
public double m_targetHood;
public double m_targetAngle;
public double m_driveTargetAngle;
public Pose2d m_targetPoint;
// pid
public double error;
public double prevError;
public Gains driveGains = ShooterConstants.SHOOT_DRIVE_GAINS;
public Gains turretGains = ShooterConstants.SHOOT_TURRET_GAINS;
public Gains gains = ShooterConstants.SHOOT_GAINS;
public double kP, kI, kD;
public double proportional, integral, derivative;
public double time;
@@ -49,6 +51,7 @@ public class Shoot extends CommandBase {
public int inverted;
// testing
public boolean simMode = true;
public DummySensor driveDummy;
public DummySensor turretDummy;
@@ -69,9 +72,9 @@ public class Shoot extends CommandBase {
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
kP = driveGains.kP;
kI = driveGains.kI;
kD = driveGains.kD;
kP = gains.kP;
kI = gains.kI;
kD = gains.kD;
proportional = 0;
integral = 0;
@@ -81,24 +84,30 @@ public class Shoot extends CommandBase {
tolerance = 5.0;
isAimedInTolerance = false;
driveDummy = new DummySensor(180);
turretDummy = new DummySensor(180);
if (simMode) {
driveDummy = new DummySensor(180);
turretDummy = new DummySensor(180);
DummySensor.resetAll();
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);
SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
System.out.println("Target Angle: " + m_targetAngle);
System.out.println("Error: " + error);
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.
@@ -143,24 +152,68 @@ public class Shoot extends CommandBase {
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() {
System.out.println("Normalized Output: " + normOutput);
// custom pid
if (simMode) {
System.out.println("Normalized Output: " + normOutput);
}
// custom pid
runPID();
driveDummy.apply(normOutput);
System.out.println("Drive Dummy: " + driveDummy.get());
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);
turretDummy.apply(normOutput);
System.out.println("Turret Dummy: " + turretDummy.get());
if (simMode) {
turretDummy.apply(normOutput);
System.out.println("Turret Dummy: " + turretDummy.get());
}
m_turret.m_boomBoomRotateMotor.set(normOutput);
}
@@ -171,6 +224,9 @@ public class Shoot extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return isAimedInTolerance;
if (simMode) {
return isAimedInTolerance;
}
return false;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -9
View File
@@ -8,6 +8,7 @@ import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -63,7 +64,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private final Field2d m_field = new Field2d();
@@ -113,12 +114,12 @@ public class SwerveDrive extends SubsystemBase {
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED));
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
@@ -175,13 +176,20 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
// chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have
// accurate chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
* Gets the current chassis speeds in m/s and rad/s.
* @return Current chassis speeds (vx, vy, ω)
*/
public double[] getChassisSpeeds() {
return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond};
}
/**
* Gets the current pose of the robot.
*