Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

SwerveModule done

Browse Source
This commit is contained in:
aarav18
2023-01-13 19:39:24 -07:00
parent 1181de776f
commit c62ca6bd2f
3 changed files with 69 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/DiffDrive.java
-85
View File
@@ -1,85 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
/**
* Add your docs here.
*/
public class DiffDrive extends SubsystemBase {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_TalonFX m_leftFrontMotor;
private WPI_TalonFX m_rightFrontMotor;
private WPI_TalonFX m_leftBackMotor;
private WPI_TalonFX m_rightBackMotor;
private DifferentialDrive m_driveTrain;
private RobotGyro m_gyro;
/**
* Add your docs here.
*/
public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
m_leftFrontMotor = leftFrontMotor;
m_rightFrontMotor = rightFrontMotor;
m_leftBackMotor = leftBackMotor;
m_rightBackMotor = rightBackMotor;
m_driveTrain = driveTrain;
m_gyro = gyro;
}
@Override
public void periodic() {
m_gyro.updatePigeonDeltas();
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
updateSmartDashboard();
}
}
/**
* Add your docs here.
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
}
/**
* Add your docs here.
*/
public void tankDriveWithInput(double leftMove, double rightMove) {
m_leftFrontMotor.set(leftMove);
m_rightFrontMotor.set(rightMove);
}
/**
* Add your docs here.
*/
private void updateSmartDashboard() {
// Examples of the functionality of RobotGyro
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
// SmartDashboard.putData(m_gyro);
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+53 -3
View File
@@ -4,17 +4,27 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
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.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
private CANCoder canCoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
@@ -24,11 +34,51 @@ public class SwerveModule extends SubsystemBase {
this.canCoder = canCoder;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
angleConfig.slot0.kP = swerveGains.kP;
angleConfig.slot0.kI = swerveGains.kI;
angleConfig.slot0.kD = swerveGains.kD;
// use the CANcoder as the remote sensor for the primary TalonFX PID
angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleConfig);
CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
canCoderConfig.magnetOffsetDegrees = offset;
canCoder.configAllSettings(canCoderConfig);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
/**
* Get the angle of a SwerveModule as a Rotation2d
* @return the angle of a SwerveModule as a Rotation2d
*/
public Rotation2d getAngle() {
// Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}
/**
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
*/
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// calculate the new absolute position of the SwerveModule based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
// convert the CANCoder from its position reading to ticks
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
}
}