Skip to content

Commit 5dbea16

Browse files
committed
update absolutedrive adv command
1 parent 898e0c9 commit 5dbea16

File tree

1 file changed

+6
-5
lines changed

1 file changed

+6
-5
lines changed

src/main/java/frc/robot/commands/AbsoluteDriveAdv.java

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,9 @@
99
import edu.wpi.first.math.kinematics.ChassisSpeeds;
1010
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1111
import edu.wpi.first.wpilibj2.command.Command;
12+
import frc.robot.Constants;
1213
import frc.robot.Constants.Dimensoes;
13-
import frc.robot.Constants.Tracao;
14+
import frc.robot.Constants.SwerveConfigs;
1415
import frc.robot.subsystems.SwerveSubsystem;
1516

1617
import java.util.List;
@@ -99,7 +100,7 @@ public void execute()
99100

100101
//Dont overwrite a button press
101102
if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){
102-
newHeading = Rotation2d.fromRadians(Tracao.TURN_CONSTANT * -headingAdjust.getAsDouble())
103+
newHeading = Rotation2d.fromRadians(Constants.Dimensoes.MAX_ANGULAR_SPEED* -headingAdjust.getAsDouble())
103104
.plus(swerve.getHeading());
104105
headingX = newHeading.getSin();
105106
headingY = newHeading.getCos();
@@ -128,9 +129,9 @@ public void execute()
128129
Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
129130

130131
// Caso essa função seja verdadeira a aceleração do robô será limitada
131-
if(Tracao.accelCorrection) {
132+
if(SwerveConfigs.accelCorrection) {
132133
translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
133-
Dimensoes.LOOP_TIME, Dimensoes.ROBOT_MASS,
134+
Dimensoes.LOOP_TIME, Dimensoes.ROBOT_MASS,
134135
List.of(Dimensoes.CHASSIS),
135136
swerve.getSwerveDriveConfiguration());
136137

@@ -141,7 +142,7 @@ public void execute()
141142

142143

143144
// Make the robot move
144-
swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, Tracao.fieldRelative);
145+
swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
145146
}
146147

147148
// Called once the command ends or is interrupted.

0 commit comments

Comments
 (0)