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

SWERVE FIXES n SUCH

Browse Source
This commit is contained in:
Ryan Manley
2022-04-02 15:28:55 -06:00
parent 232cd94fa4
commit 5ff011ef73
8 changed files with 59 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/ShooterData.csv
+5 -3
View File
@@ -1,14 +1,16 @@
Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
0 ,-29.5 ,8000
78.5 ,-29.5 ,8000
99 ,-39.62 ,8500
88 ,-34.2 ,8600
99 ,-39.62 ,8600
111 ,-42 ,9000
127.25 ,-49.12 ,9500
141 ,-59.4 ,9900
150 ,-66.22 ,10000
164.5 ,-75.52 ,10000
186 ,-76.24 ,10000
189.9 ,-81.39 ,11000
207 ,-104.07 ,11000
227 ,-105.32 ,11500
239 ,-105.5 ,12380
255.5 ,-105.8 ,13500
999 ,-105.8 ,13500
999 ,-105.8 ,13500
1 Distance (in) Hood Ext. (u) Drum Velocity (u/ds)
2 0 -29.5 8000
3 78.5 -29.5 8000
4 99 88 -39.62 -34.2 8500 8600
5 99 -39.62 8600
6 111 -42 9000
7 127.25 -49.12 9500
8 141 -59.4 9900
9 150 -66.22 10000
10 164.5 -75.52 10000
11 186 189.9 -76.24 -81.39 10000 11000
12 207 -104.07 11000
13 227 -105.32 11500
14 239 -105.5 12380
15 255.5 -105.8 13500
16 999 -105.8 13500
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -156,7 +156,7 @@ public final class Constants {
}
public static final class ExtenderConstants {
public static final double EXTENDER_FORWARD_LIMIT = 235.0;//250.0;
public static final double EXTENDER_FORWARD_LIMIT = 200.3;//250.0;
public static final double EXTENDER_REVERSE_LIMIT = 0.0;
}
@@ -164,7 +164,7 @@ public final class Constants {
public static final int STORAGE_CAN_ID = 18;
public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
public static final int BEAM_SENSOR_INTAKE = 29; //TODO
public static final double STORAGE_SPEED = 0.9;
public static final double STORAGE_SPEED = 1.0;//0.9;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
src/main/java/frc4388/robot/Robot.java
+5 -3
View File
@@ -75,7 +75,7 @@ public class Robot extends TimedRobot {
SmartDashboard.putData(CommandScheduler.getInstance());
// desmosServer.start();
m_robotContainer.m_robotVisionOdometry.setLEDs(true);
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
// DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
}
@@ -146,11 +146,13 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
m_robotContainer.m_robotVisionOdometry.setLEDs(true);
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
}
@Override
public void disabledPeriodic() {}
public void disabledPeriodic() {
// m_robotContainer.m_robotVisionOdometry.setLEDs(false);
}
/**
* This autonomous runs the autonomous command selected by your
src/main/java/frc4388/robot/RobotMap.java
+1
View File
@@ -300,6 +300,7 @@ void configureExtenderMotors() {
void configureStorageMotors() {
storageMotor.restoreFactoryDefaults();
storageMotor.setIdleMode(IdleMode.kCoast);
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+9 -11
View File
@@ -94,7 +94,7 @@ public class TrackTarget extends CommandBase {
//// points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2.1;
m_turret.runTurretWithInput(output);
@@ -132,8 +132,6 @@ public class TrackTarget extends CommandBase {
} catch (Exception e){
e.printStackTrace();
}
// run storage
}
@@ -197,14 +195,14 @@ public class TrackTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (this.isAuto) {
if (targetLocked && !timerStarted) {
timerStarted = true;
startTime = System.currentTimeMillis();
}
return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
} else {
// if (this.isAuto) {
// if (targetLocked && !timerStarted) {
// timerStarted = true;
// startTime = System.currentTimeMillis();
// }
// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
// } else {
return false;
}
// }
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+9 -9
View File
@@ -106,9 +106,9 @@ public class SwerveDrive extends SubsystemBase {
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
-rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
-rot * SwerveDriveConstants.ROTATION_SPEED * 2);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
@@ -128,8 +128,8 @@ public class SwerveDrive extends SubsystemBase {
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 2);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
// if (ignoreAngles) {
@@ -173,12 +173,12 @@ public class SwerveDrive extends SubsystemBase {
public void periodic() {
updateOdometry();
updateSmartDash();
// updateSmartDash();
SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
// SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
// SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
// SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
// SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
m_field.setRobotPose(m_odometry.getPoseMeters());
super.periodic();
src/main/java/frc4388/robot/subsystems/Turret.java
+25 -25
View File
@@ -114,41 +114,41 @@ public class Turret extends SubsystemBase {
// SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
// SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed());
SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed());
// SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
// SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
// SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed());
// SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed());
// limit switch annoying time thing but actually worked first try wtf
leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed).
// leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed).
hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state.
// hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state.
if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time.
leftCurrentTime = System.currentTimeMillis();
leftElapsedTime = 0;
}
// if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time.
// leftCurrentTime = System.currentTimeMillis();
// leftElapsedTime = 0;
// }
if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false;
// if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false;
if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){
recentlyPressed = true;
m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/);
}
SmartDashboard.putBoolean("Recently Pressed", recentlyPressed);
// if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){
// recentlyPressed = true;
// m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/);
// }
// // SmartDashboard.putBoolean("Recently Pressed", recentlyPressed);
if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time.
leftElapsedTime = System.currentTimeMillis() - leftCurrentTime;
}
// if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time.
// leftElapsedTime = System.currentTimeMillis() - leftCurrentTime;
// }
if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position.
m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);
}
// if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position.
// m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);
// }
leftPrevState = leftState; // * Update the state of the left limit switch.
// leftPrevState = leftState; // * Update the state of the left limit switch.
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
runVelocityRamping();
// // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
// runVelocityRamping();
}
public void runVelocityRamping() {
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+3 -1
View File
@@ -44,7 +44,9 @@ public class VisionOdometry extends SubsystemBase {
* @param shooter The turret subsystem
*/
public VisionOdometry(SwerveDrive drive, Turret shooter) {
m_camera = new PhotonCamera(VisionConstants.NAME);
// do{
m_camera = new PhotonCamera(VisionConstants.NAME);
// }while(m_camera.getLatestResult().getLatencyMillis() == 0.d);
m_drive = drive;
m_shooter = shooter;