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

AimToCenter works without fixing odometry issues

Browse Source
This commit is contained in:
aarav18
2022-03-19 16:37:59 -06:00
parent 87de6a2c2f
commit dbc1e070e0
7 changed files with 58 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+14 -14
View File
@@ -37,26 +37,26 @@ public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 3.0;
public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5;
public static final double WIDTH = 23.75;
public static final double HEIGHT = 23.75;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant?
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2; //
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; //
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; //
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; //
public static final int LEFT_BACK_STEER_CAN_ID = 6; //
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; //
public static final int RIGHT_BACK_STEER_CAN_ID = 8; //
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; //
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; //
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; //
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;//
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; //
public static final int LEFT_FRONT_STEER_CAN_ID = 2; // *
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // *
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // *
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // *
public static final int LEFT_BACK_STEER_CAN_ID = 6; // *
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // *
public static final int RIGHT_BACK_STEER_CAN_ID = 8; // *
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // *
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // *
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // *
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // *
public static final int GYRO_ID = 14;
// offsets are in degrees
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -262,7 +262,7 @@ public class Robot extends TimedRobot {
if (selectedOdo == null) {
selectedOdo = m_robotContainer.getOdometry();
}
m_robotContainer.resetOdometry(selectedOdo);
// m_robotContainer.resetOdometry(selectedOdo);
// This makes sure that the autonomous stops running when
src/main/java/frc4388/robot/RobotContainer.java
+3 -1
View File
@@ -273,8 +273,10 @@ public class RobotContainer {
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
.whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
.whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-45), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
/* Operator Buttons */
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+17 -11
View File
@@ -4,6 +4,7 @@
package frc4388.robot.commands.ShooterCommands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
@@ -35,15 +36,18 @@ public class AimToCenter extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = m_drive.getOdometry().getX();
y = m_drive.getOdometry().getY();
x = -m_drive.getOdometry().getY();
y = -m_drive.getOdometry().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.getRegGyro().getDegrees())) % 360;
System.out.println("Target Angle" + m_targetAngle);
m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360;
SmartDashboard.putNumber("Target Angle", m_targetAngle);
m_turret.runShooterRotatePID(m_targetAngle);
// Check if limelight is within range (comment out to disable vision odo)
@@ -62,15 +66,17 @@ public class AimToCenter extends CommandBase {
}
public static double aaravAngleToCenter(double x, double y, double gyro) {
double yes = 360 - gyro;
double exp = Math.toDegrees(Math.atan(y/x)) - yes;
if (x > 0) { return exp; }
if (x < 0) { return (180 + exp); }
double actualGyro = -gyro + 90;
double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro;
if (-x > 0) { return (180 + exp); }
if (-x < 0) { return (360 + exp); }
if (x == 0 && y > 0) { return (90 - yes); }
if (x == 0 && y < 0) { return (-90 - yes); }
if (x == 0 && y > 0) { return (270 - actualGyro); }
if (x == 0 && y < 0) { return (90 - actualGyro); }
System.out.println("Invalid case.");
System.err.println("Invalid case.");
return 0;
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+11 -11
View File
@@ -36,10 +36,10 @@ public class SwerveDrive extends SubsystemBase {
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(-halfHeight));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(halfHeight));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(-halfHeight));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(halfHeight));
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
@@ -48,7 +48,7 @@ public class SwerveDrive extends SubsystemBase {
public WPI_Pigeon2 m_gyro;
public SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDriveOdometry m_odometry;
// public SwerveDriveOdometry m_odometry;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
@@ -69,14 +69,14 @@ public class SwerveDrive extends SubsystemBase {
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
m_poseEstimator = new SwerveDrivePoseEstimator(
m_gyro.getRotation2d(),
getRegGyro(),//m_gyro.getRotation2d(),
new Pose2d(),
m_kinematics,
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
// m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
@@ -160,7 +160,7 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
@@ -172,9 +172,9 @@ public class SwerveDrive extends SubsystemBase {
// 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);
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+2 -3
View File
@@ -79,6 +79,7 @@ public class SwerveModule extends SubsystemBase {
// driveMotor.configAllSettings(driveTalonFXConfiguration);
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.sensorCoefficient = 0.087890625;
canCoderConfiguration.magnetOffsetDegrees = offset;
canCoderConfiguration.sensorDirection = true;
canCoder.configAllSettings(canCoderConfiguration);
@@ -90,9 +91,7 @@ public class SwerveModule extends SubsystemBase {
}
private Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback
// coefficient
// and the sensor value reports degrees.
// ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}
src/main/java/frc4388/robot/subsystems/Turret.java
+10
View File
@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
import frc4388.utility.Gains;
public class Turret extends SubsystemBase {
@@ -195,6 +196,15 @@ public class Turret extends SubsystemBase {
}
public void runShooterRotatePID(double targetAngle) {
double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2;
targetAngle = (targetAngle % 360);
if (targetAngle > softMid) {
targetAngle = targetAngle - 360;
}
targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT;
m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}