diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 35672cc..c241591 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -39,7 +39,7 @@ public final class Constants { /* Trajectory Constants */ public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kTrackwidthMeters = 0.69; ///TODO: SET THIS SOON! + public static final double kTrackwidthMeters = 0.648; ///TODO: SET THIS SOON! public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters); /* Remote Sensors */ diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 161b7f1..3cbf888 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -372,7 +372,7 @@ public class Drive extends SubsystemBase { * @param rightSpeed the commanded right output */ public void tankDriveVelocity(double leftSpeed, double rightSpeed) { - DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); + /*DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds); double moveVelMPS = chassisSpeeds.vxMetersPerSecond; double angleVelRad = chassisSpeeds.omegaRadiansPerSecond; @@ -382,10 +382,14 @@ public class Drive extends SubsystemBase { m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle, m_currentAngleYaw-(360), m_currentAngleYaw+(360)); - double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - double moveVel = inchesToMeters(metersToInches(moveVelMPS))/DriveConstants.SECONDS_TO_TICK_TIME; + double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;*/ + double moveVelLeft = inchesToMeters(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelRight = inchesToMeters(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; - runDriveStraightVelocityPID(moveVel, targetGyro); + //runDriveStraightVelocityPID(moveVel, targetGyro); + + m_rightFrontMotor.set(TalonFXControlMode.Velocity, moveVelRight); + m_leftFrontMotor.set(TalonFXControlMode.Velocity, moveVelLeft); } /**