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

Merge branch 'swerve' of https://github.com/Team4388/2022NoWayHome into swerve

Browse Source
This commit is contained in:
aarav18
2022-02-19 11:18:31 -07:00
parent 997540b2c3 a3fc1b4d14
commit 91ad9521c1
4 changed files with 29 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -53,10 +53,10 @@ public final class Constants {
public static final int GYRO_ID = 14;
// ofsets are in degrees
public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45;
// swerve PID constants
public static final int SWERVE_SLOT_IDX = 0;
src/main/java/frc4388/robot/RobotMap.java
+9 -8
View File
@@ -91,14 +91,15 @@ public class RobotMap {
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
NeutralMode mode = NeutralMode.Brake;
leftFrontSteerMotor.setNeutralMode(mode);
leftFrontWheelMotor.setNeutralMode(mode);//Coast
rightFrontSteerMotor.setNeutralMode(mode);
rightFrontWheelMotor.setNeutralMode(mode);//Coast
leftBackSteerMotor.setNeutralMode(mode);
leftBackWheelMotor.setNeutralMode(mode);//Coast
rightBackSteerMotor.setNeutralMode(mode);
rightBackWheelMotor.setNeutralMode(mode);//Coast
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+15 -4
View File
@@ -15,6 +15,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.math.geometry.Rotation2d;
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.Gains;
@@ -49,9 +50,9 @@ public class SwerveModule extends SubsystemBase {
angleMotor.configAllSettings(angleTalonFXConfiguration);
TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
driveTalonFXConfiguration.slot0.kP = 0.15;
driveTalonFXConfiguration.slot0.kP = 0.1;
driveTalonFXConfiguration.slot0.kI = 0.0;
driveTalonFXConfiguration.slot0.kD = 0.5;
driveTalonFXConfiguration.slot0.kD = 0.0;
driveMotor.configAllSettings(driveTalonFXConfiguration);
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
@@ -72,8 +73,7 @@ public class SwerveModule extends SubsystemBase {
*/
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
Rotation2d currentRotation = getAngle();
//SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
// SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
state = SwerveModuleState.optimize(desiredState, currentRotation);
// Find the difference between our current rotational position + our new rotational position
@@ -84,17 +84,23 @@ public class SwerveModule extends SubsystemBase {
// Convert the CANCoder from it's position reading back to ticks
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle){
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
<<<<<<< Updated upstream
// Add this line to correct for the slight drive movement the angle motor makes when turning in place.
// driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
=======
// driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC) + angleMotor.get());
>>>>>>> Stashed changes
}
/**
@@ -114,5 +120,10 @@ public class SwerveModule extends SubsystemBase {
driveMotor.set(0);
angleMotor.set(0);
}
@Override
public void periodic(){
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
}
}