Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Setting Motors to PID Values

Browse Source
This commit is contained in:
Ryan
2021-12-06 17:23:08 -07:00
parent 57d1826444
commit a7f73ad07f
1 changed files with 16 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+16 -1
View File
@@ -7,6 +7,7 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
@@ -69,7 +70,7 @@ public class SwerveDrive extends SubsystemBase
var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */);
driveFromSpeeds(speeds);
}
//Converts a ChassisSpeed to SwerveModuleStates (targets)
public void driveFromSpeeds(ChassisSpeeds speeds)
{
//https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
@@ -85,14 +86,28 @@ public class SwerveDrive extends SubsystemBase
// Back right module state
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
//Set the motors
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
}
//Sets steering motors to PID values
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
{
//Set the Wheel motor speeds
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
//PID
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees());
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees());
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees());
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
}
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
// {
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary,