Skip to content

Commit

Permalink
changed drive code to toggle speed (from hold)
Browse files Browse the repository at this point in the history 
changed swerve code to use motor encoders

Co-authored-by: Andy Killorin <[email protected]>
  • Loading branch information
@Sha-dos @Speedy6451
Sha-dos and Speedy6451 committed Oct 7, 2023
1 parent fad1573 commit 90b297f
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 25 deletions.
17 changes: 14 additions & 3 deletions src/main/java/com/team2502/robot2023/commands/DriveCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,9 @@ private enum Drivetype {
private double dlxDrift;
private double dlyDrift;

private boolean tgoggla = false;
private boolean prevTog = false;

private final SendableChooser<Drivetype> typeEntry = new SendableChooser<>();
private final SendableChooser<DriveController> controllerEntry = new SendableChooser<>();

Expand Down Expand Up @@ -71,6 +74,14 @@ public void execute() {
resetDrift();
}

boolean tog = leftJoystick.getRawButton(OI.RET_MODE);
if (prevTog != tog && tog) {
tgoggla = !tgoggla;
}
prevTog = tog;

SmartDashboard.putBoolean("iosajioj", tgoggla);

ChassisSpeeds speeds;
Translation2d centerOfRotation;
drivetrain.setTurnNeutralMode(NeutralMode.Brake);
Expand Down Expand Up @@ -106,9 +117,9 @@ public void execute() {
break;
case FieldOrientedTwist:
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
leftJoystick.getY() * (leftJoystick.getRawButton(OI.RET_MODE) ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-leftJoystick.getX() * (leftJoystick.getRawButton(OI.RET_MODE) ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-rightJoystick.getZ() * (leftJoystick.getRawButton(OI.RET_MODE) ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
leftJoystick.getY() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-leftJoystick.getX() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-rightJoystick.getZ() * (tgoggla ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset));
centerOfRotation = new Translation2d(0, 0);
drivetrain.setSpeeds(speeds, centerOfRotation);
Expand Down
48 changes: 26 additions & 22 deletions src/main/java/com/team2502/robot2023/subsystems/DrivetrainSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,17 +21,15 @@
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.team2502.robot2023.Utils;
import com.team2502.robot2023.Constants.HardwareMap;
import com.team2502.robot2023.Constants.Subsystems.Field;
import com.team2502.robot2023.Constants.Subsystems.Drivetrain;
import com.team2502.robot2023.Constants.Subsystems.Drivetrain.*;

import java.lang.Math;
import edu.wpi.first.wpilibj2.command.SubsystemBase;


public class DrivetrainSubsystem extends SubsystemBase{
public class DrivetrainSubsystem extends SubsystemBase {
private SwerveDriveKinematics kinematics;
private SwerveDriveOdometry odometry;

Expand All @@ -50,10 +48,10 @@ public class DrivetrainSubsystem extends SubsystemBase{
private CANSparkMax drivetrainEncoderBackRight;
private CANSparkMax drivetrainEncoderFrontRight;*/

private CANCoder drivetrainEncoderBackLeft;
/*private CANCoder drivetrainEncoderBackLeft;
private CANCoder drivetrainEncoderFrontLeft;
private CANCoder drivetrainEncoderBackRight;
private CANCoder drivetrainEncoderFrontRight;
private CANCoder drivetrainEncoderFrontRight;*/

private Field2d field;

Expand Down Expand Up @@ -93,10 +91,10 @@ public DrivetrainSubsystem(){
drivetrainTurnFrontRight = new WPI_TalonFX(HardwareMap.FR_TURN_MOTOR, "can0");
drivetrainTurnBackRight = new WPI_TalonFX(HardwareMap.BR_TURN_MOTOR, "can0");

drivetrainEncoderBackLeft = new CANCoder(HardwareMap.BL_TURN_ENCODER, "can0");
/*drivetrainEncoderBackLeft = new CANCoder(HardwareMap.BL_TURN_ENCODER, "can0");
drivetrainEncoderFrontLeft = new CANCoder(HardwareMap.FL_TURN_ENCODER, "can0");
drivetrainEncoderFrontRight = new CANCoder(HardwareMap.FR_TURN_ENCODER, "can0");
drivetrainEncoderBackRight = new CANCoder(HardwareMap.BR_TURN_ENCODER, "can0");
drivetrainEncoderBackRight = new CANCoder(HardwareMap.BR_TURN_ENCODER, "can0");*/

Translation2d m_frontLeftLocation = new Translation2d(Drivetrain.SWERVE_WIDTH, -Drivetrain.SWERVE_LENGTH);
Translation2d m_frontRightLocation = new Translation2d(Drivetrain.SWERVE_WIDTH, Drivetrain.SWERVE_LENGTH);
Expand Down Expand Up @@ -185,6 +183,7 @@ public Pose2d reflectPose(Pose2d input) {
* call once after aligning turn motors after motor rebuild
* ensures that swerve zeros persist across restarts
*/
/*
public void setSwerveInit() {
drivetrainEncoderFrontLeft.configMagnetOffset(
drivetrainEncoderFrontLeft.getAbsolutePosition());
Expand All @@ -201,20 +200,24 @@ public void setSwerveInit() {
drivetrainEncoderBackRight.configMagnetOffset(
drivetrainEncoderBackRight.getAbsolutePosition());
drivetrainEncoderBackRight.setPositionToAbsolute();
}
}*/

private SwerveModulePosition[] getModulePositions() {
Rotation2d FLRotation = Rotation2d.fromDegrees(
drivetrainEncoderFrontLeft.getPosition()
//drivetrainEncoderFrontLeft.getPosition()
drivetrainTurnFrontLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
);
Rotation2d FRRotation = Rotation2d.fromDegrees(
drivetrainEncoderFrontRight.getPosition()
//drivetrainEncoderFrontRight.getPosition()
drivetrainTurnFrontRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
);
Rotation2d BLRotation = Rotation2d.fromDegrees(
drivetrainEncoderBackLeft.getPosition()
//drivetrainEncoderBackLeft.getPosition()
drivetrainTurnBackLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
);
Rotation2d BRRotation = Rotation2d.fromDegrees(
drivetrainEncoderBackRight.getPosition()
//drivetrainEncoderBackRight.getPosition()
drivetrainTurnBackRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
);

SwerveModulePosition FRPosition = new SwerveModulePosition(
Expand Down Expand Up @@ -300,23 +303,23 @@ public void setSpeeds(ChassisSpeeds speed, Translation2d centerOfRotation) {

Rotation2d FLRotation = Rotation2d.fromDegrees(
//drivetrainEncoderFrontLeft.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360
//drivetrainTurnFrontLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
-drivetrainEncoderFrontLeft.getPosition()
drivetrainTurnFrontLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
//-drivetrainEncoderFrontLeft.getPosition()
);
Rotation2d FRRotation = Rotation2d.fromDegrees(
//drivetrainEncoderFrontRight.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360
//drivetrainTurnFrontRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
-drivetrainEncoderFrontRight.getPosition()
drivetrainTurnFrontRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
//-drivetrainEncoderFrontRight.getPosition()
);
Rotation2d BLRotation = Rotation2d.fromDegrees(
//drivetrainEncoderBackLeft.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360
//drivetrainTurnBackLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
-drivetrainEncoderBackLeft.getPosition()
drivetrainTurnBackLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
//-drivetrainEncoderBackLeft.getPosition()
);
Rotation2d BRRotation = Rotation2d.fromDegrees(
//drivetrainEncoderBackRight.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360
//drivetrainTurnBackRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
-drivetrainEncoderBackRight.getPosition()
drivetrainTurnBackRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES
//-drivetrainEncoderBackRight.getPosition()
);

SwerveModuleState FLState = SwerveModuleState.optimize(moduleStates[0], FLRotation);
Expand Down Expand Up @@ -459,12 +462,13 @@ public double getMaxTemp() {
return Math.max(Math.max(fl, fr), Math.max(bl, br));
}

/*
public void zeroTurn() {
drivetrainEncoderFrontRight.setPosition(0);
drivetrainEncoderFrontLeft.setPosition(0);
drivetrainEncoderBackRight.setPosition(0);
drivetrainEncoderBackLeft.setPosition(0);
}
}*/

@Override
public void periodic(){
Expand Down

0 comments on commit 90b297f

Please sign in to comment.