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

PID Tuning, Controller Adjust

Browse Source
This commit is contained in:
Aarav Shah
2022-01-14 17:48:53 -07:00
parent 28e970e398
commit 789f4f6a9b
9 changed files with 61 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -3
View File
@@ -26,9 +26,10 @@ public final class Constants {
public static final double WHEEL_SPEED = 0.1;
public static final double WIDTH = 22;
public static final double HEIGHT = 22;
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
public static final double MAX_SPEED_FEET_PER_SEC = 16;
public static final double JOYSTICK_TO_METERS_PER_SECOND = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
//IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
@@ -41,6 +42,7 @@ public final class Constants {
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int GYRO_ID = 14;
// ofsets are in degrees
// public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
@@ -55,7 +57,7 @@ public final class Constants {
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final int SWERVE_TIMEOUT_MS = 30;
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0);
// swerve configuration
public static final double NEUTRAL_DEADBAND = 0.04;
src/main/java/frc4388/robot/Main.java
+3
View File
@@ -24,3 +24,6 @@ public final class Main {
RobotBase.startRobot(Robot::new);
}
}
// hi ryan
src/main/java/frc4388/robot/Robot.java
+1
View File
@@ -114,6 +114,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
System.err.println("TEST");
}
src/main/java/frc4388/robot/RobotContainer.java
+11 -4
View File
@@ -36,7 +36,8 @@ public class RobotContainer {
m_robotMap.leftFrontEncoder,
m_robotMap.rightFrontEncoder,
m_robotMap.leftBackEncoder,
m_robotMap.rightBackEncoder
m_robotMap.rightBackEncoder,
m_robotMap.gyro
);
private final LED m_robotLED = new LED(m_robotMap.LEDController);
@@ -54,8 +55,13 @@ public class RobotContainer {
/* Default Commands */
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
XboxController.ClampJoystickAxis(
getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis()),
-getDriverController().getRightXAxis(),
true),
m_robotSwerveDrive));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
@@ -69,7 +75,8 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
/*new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.resetGyro());*/
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
src/main/java/frc4388/robot/RobotMap.java
+3
View File
@@ -6,7 +6,9 @@ package frc4388.robot;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -42,6 +44,7 @@ public class RobotMap {
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
void configureSwerveMotorControllers() {
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -10
View File
@@ -6,6 +6,7 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -46,14 +47,14 @@ public class SwerveDrive extends SubsystemBase {
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public Gyro gyro; //TODO Add Gyro Lol
public WPI_PigeonIMU m_gyro; //TODO Add Gyro Lol
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
CANCoder rightFrontEncoder,
CANCoder leftBackEncoder,
CANCoder rightBackEncoder)
CANCoder rightBackEncoder, WPI_PigeonIMU gyro)
{
m_leftFrontSteerMotor = leftFrontSteerMotor;
m_leftFrontWheelMotor = leftFrontWheelMotor;
@@ -67,6 +68,7 @@ public class SwerveDrive extends SubsystemBase {
m_rightFrontEncoder = rightFrontEncoder;
m_leftBackEncoder = leftBackEncoder;
m_rightBackEncoder = rightBackEncoder;
m_gyro = gyro;
modules = new SwerveModule[] {
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
@@ -74,35 +76,40 @@ public class SwerveDrive extends SubsystemBase {
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
};
//gyro.reset();
m_gyro.reset();
}
//https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param speeds[0] Speed of the robot in the x direction (forward).
* @param speeds[1] Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
public void driveWithInput(double[] speeds, double rot, boolean fieldRelative)
{
System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]);
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
driveFromSpeeds(speeds);*/
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double xSpeedMetersPerSecond = -speeds[0] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = speeds[1] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
SwerveModuleState[] states =
kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state);
}
}
}
/*public void resetGyro(){
m_gyro.reset();
}*/
//Converts a ChassisSpeed to SwerveModuleStates (targets)
public void driveFromSpeeds(ChassisSpeeds speeds)
{
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+1
View File
@@ -73,6 +73,7 @@ public class SwerveModule extends SubsystemBase {
*/
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);