Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Make code work

Browse Source 
Update basic functions for apriltags and outline for integration
This commit is contained in:
Astatin3
2023-02-16 18:46:43 -07:00
parent dd8209d8c5
commit a91d334e2c
10 changed files with 282 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Apriltags.java
+35
View File
@@ -0,0 +1,35 @@
package frc4388.robot.subsystems;
//import edu.wpi.first.apriltag.AprilTag;
//import edu.wpi.first.math.geometry.Pose3d;
//import edu.wpi.first.math.geometry.Rotation3d;
//import edu.wpi.first.networktables.NetworkTable;
//import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Apriltags {
public Object[] getApriltagPosition() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
return new Object[] {true,
tagTable.getEntry("TagPosX"),
tagTable.getEntry("TagPosY"),
tagTable.getEntry("TagPosZ")
};
}
public Object[] getApriltagRotation() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
return new Object[] {true,
tagTable.getEntry("TagRotY"),
tagTable.getEntry("TagRotP"),
tagTable.getEntry("TagRotR")
};
}
public boolean isAprilTag() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
return tagTable.getEntry("IsTag").getBoolean(false);
}
}
src/main/java/frc4388/robot/subsystems/Location.java
+44
View File
@@ -0,0 +1,44 @@
package frc4388.robot.subsystems;
import frc4388.robot.subsystems.Apriltags;
public class Location {
final Apriltags Apriltag = new Apriltags();
public boolean isLimelight = false;
public boolean isApriltag = false;
//Determines which source to get pos and rot from and also resets
public void reoderPrio(){
isLimelight = false; //If limelight gets position and if within a certain range of poles
isApriltag = Apriltag.isAprilTag();
}
public Object[] getPosition() {
Object[] Position = {};
if(isLimelight){
//Return Limelight Position
}else if(isApriltag){
return Apriltag.getApriltagPosition();
}else{
//Return odometry Position, last resort
}
return Position;
}
public Object[] getRotation() {
Object[] Rotation = {};
if(isLimelight){
//Return Limelight Rotation
}else if(isApriltag){
return Apriltag.getApriltagRotation();
}else{
//Return odometry Rotation, last resort
}
return Rotation;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+66 -106
View File
@@ -4,27 +4,20 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
private SwerveModule[] modules;
@@ -33,64 +26,39 @@ public class SwerveDrive extends SubsystemBase {
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private RobotGyro gyro;
private SwerveDriveOdometry odometry;
// private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
// kinematics,
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public Rotation2d rotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.gyro = gyro;
this.odometry = new SwerveDriveOdometry(
kinematics,
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
}
);
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
if (rightStick.getNorm() > 0.1) {
rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
}
double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// if (rightStick.getNorm() < .1) {
// rot = 0;
// }
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
// WPILib swerve drive example
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
// );
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
/**
@@ -98,71 +66,70 @@ public class SwerveDrive extends SubsystemBase {
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state, false);
module.setDesiredState(state);
}
}
public double getGyroAngle() {
return gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
setOdometry(getOdometry());
rotTarget = new Rotation2d(0);
}
/**
* Updates the odometry of the SwerveDrive.
*/
public void updateOdometry() {
odometry.update(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
}
);
}
// public void updateOdometry() {
// odometry.update(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
// }
/**
* Gets the odometry of the SwerveDrive.
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
*/
public Pose2d getOdometry() {
return odometry.getPoseMeters();
}
// public Pose2d getOdometry() {
// return odometry.getPoseMeters();
// }
/**
* Sets the odometry of the SwerveDrive.
* @param pose Pose to set the odometry to.
*/
public void setOdometry(Pose2d pose) {
odometry.resetPosition(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
},
pose
);
}
// public void setOdometry(Pose2d pose) {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// pose
// );
// }
/**
* Resets the odometry of the SwerveDrive to 0.
* *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
* *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
*/
public void resetOdometry() {
setOdometry(new Pose2d());
}
// public void resetOdometry() {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// new Pose2d()
// );
// }
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
@@ -171,14 +138,7 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
updateOdometry();
SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
// updateOdometry();
}
/**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+7 -29
View File
@@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
public WPI_TalonFX driveMotor;
public WPI_TalonFX angleMotor;
private CANCoder encoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -44,9 +44,6 @@ public class SwerveModule extends SubsystemBase {
angleMotor.configAllSettings(angleConfig);
encoder.configMagnetOffset(offset);
driveMotor.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
}
/**
@@ -82,18 +79,6 @@ public class SwerveModule extends SubsystemBase {
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
}
public double getAngularVel() {
return this.angleMotor.getSelectedSensorVelocity();
}
public double getDrivePos() {
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
}
public double getDriveVel() {
return this.driveMotor.getSelectedSensorVelocity(0);
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
@@ -126,32 +111,25 @@ public class SwerveModule extends SubsystemBase {
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
*/
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// calculate the new absolute position of the SwerveModule based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
// convert the CANCoder from its position reading to ticks
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
}
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
// double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12;
driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
// driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
// driveMotor.set(0.1);
// double angleCorrection = getAngularVel() * 2.69;
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
}
public void reset(double position) {