Skip to content

Commit

Permalink
jhj
Browse files Browse the repository at this point in the history
  • Loading branch information
pratyush-p committed Dec 2, 2022
1 parent 0be209d commit 7feb5f5
Show file tree
Hide file tree
Showing 4 changed files with 21 additions and 17 deletions.
12 changes: 6 additions & 6 deletions src/main/deploy/pathplanner/Test1.path
Original file line number Diff line number Diff line change
Expand Up @@ -19,16 +19,16 @@
},
{
"anchorPoint": {
"x": 4.945682968293786,
"y": 0.8506574705433692
"x": 5.9286110013018885,
"y": 3.838922371140035
},
"prevControl": {
"x": 3.6899668013911455,
"y": 0.9393765475527959
"x": 4.672894834399248,
"y": 3.927641448149462
},
"nextControl": {
"x": 6.7656943006241415,
"y": 0.7220697133678535
"x": 7.748622333632244,
"y": 3.7103346139645192
},
"holonomicAngle": 0,
"isReversal": false,
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,10 @@ public final class Constants {
public static final boolean BACK_LEFT_TURNING_INVERT = false;
public static final boolean BACK_RIGHT_TURNING_INVERT = false;

public static final boolean FRONT_LEFT_DRIVE_INVERT = false;
public static final boolean FRONT_RIGHT_DRIVE_INVERT = true;
public static final boolean BACK_LEFT_DRIVE_INVERT = false;
public static final boolean BACK_RIGHT_DRIVE_INVERT = true;
public static final boolean FRONT_LEFT_DRIVE_INVERT = true;
public static final boolean FRONT_RIGHT_DRIVE_INVERT = false;
public static final boolean BACK_LEFT_DRIVE_INVERT = true;
public static final boolean BACK_RIGHT_DRIVE_INVERT = false;

public static final boolean FRONT_LEFT_CANCODER_INVERT = false;
public static final boolean FRONT_RIGHT_CANCODER_INVERT = false;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/JoystickSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public void execute() {
if (joy.getAButtonPressed()) {swerve.toggleFieldRelative();}
if (joy.getBButtonPressed()) {
swerve.zeroHeading();
swerve.resetOdometry(new Pose2d(0, 0, new Rotation2d(0)));
swerve.resetOdometry(new Pose2d(5.93, 3.84, new Rotation2d(0)));
swerve.resetMods();
}

Expand Down
16 changes: 10 additions & 6 deletions src/main/java/frc/robot/subsystems/SwerveDrive.java
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ public class SwerveDrive extends SubsystemBase {
private PIDController faceTargetPID;
private SwerveDriveOdometry odometer;
private Field2d field;
private boolean usingOdometryTargeting = false;

public SwerveDrive (AHRS m_gyro) {
this.frontLeft = new SwerveModule(Constants.SwerveModuleType.FRONT_LEFT);
Expand All @@ -55,7 +56,7 @@ public SwerveDrive (AHRS m_gyro) {
odometer = new SwerveDriveOdometry(Constants.SWERVE_DRIVE_KINEMATICS, new Rotation2d(0));
faceTargetPID = new PIDController(.065, 0, 0);
//edit and add to constants //FIXME tune and maybe invert P (if it goes in wrong direction)
faceTargetPID.setTolerance(2);
faceTargetPID.setTolerance(5);

field = new Field2d();
zeroHeading();
Expand Down Expand Up @@ -118,21 +119,23 @@ public SwerveModuleState[] controllerToModuleStates(XboxController controller, L
if (shootButton > .1) {

if (targetVis) {
usingOdometryTargeting = false;
double visionSpeed = faceTargetPID.calculate(limelight.targetX(), 0);
if (faceTargetPID.atSetpoint()) {
resetTargetingPID(limelight.targetX(), Math.toDegrees(visionSpeed));
setGyroOffset(OdometryMath2022.gyroTargetOffset()); //might need to negate //FIXME
// setGyroOffset(OdometryMath2022.gyroTargetOffset()); //might need to negate //FIXME
}
if (shootButton > .9) {
x2Speed = visionSpeed;
} else {
// x2Speed = x2Speed * (1 - shootButton) + visionSpeed * shootButton;
x2Speed = visionSpeed;
x2Speed = x2Speed * (1 - shootButton) + visionSpeed * shootButton;
// x2Speed = visionSpeed;
}
// System.out.println(x2Speed);

} else {
x2Speed = visionLimiter.calculate(OdometryMath2022.robotEasiestTurnToTarget()) * Constants.MAX_ANGULAR_SPEED_TELEOP_RAD_PER_S ;
usingOdometryTargeting = true;
x2Speed = visionLimiter.calculate(OdometryMath2022.robotEasiestTurnToTarget()) * Constants.MAX_ANGULAR_SPEED_TELEOP_RAD_PER_S;

}
}
Expand Down Expand Up @@ -229,6 +232,7 @@ private void log() {

Logger.Work.post("state", frontRight.getModState().toString());
// Logger.Work.post("gyro yaw", OdometryMath2022.gyroTargetOffset());
Logger.Work.post("x2speed", x2Speed);
Logger.Work.post("x2speed", x2Speed);
Logger.Work.post("usingOdom", usingOdometryTargeting);
}
}

0 comments on commit 7feb5f5

Please sign in to comment.