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

some auto work (not working at all)

Browse Source
This commit is contained in:
aarav18
2023-02-07 23:32:19 -07:00
parent 90297c044f
commit 7d6ef72cf8
3 changed files with 34 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -8
View File
@@ -51,16 +51,16 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0);
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value
public static final double PATH_MAX_ACC = 3; // TODO: find the actual value
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value
public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value
// public static final double kPX_CONTROLLER = 0.8; //TODO: tune
// public static final double kPY_CONTROLLER = 0.8; //TODO: tune
}
public static final class Conversions {
src/main/java/frc4388/robot/RobotContainer.java
+21 -7
View File
@@ -7,6 +7,7 @@
package frc4388.robot;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.math.controller.PIDController;
@@ -17,6 +18,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -105,20 +107,32 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
//Create Trajectory Settings
TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL,
SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
//Generate Trajactory
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(1, 0), new Translation2d(1, -1)),
new Pose2d(0, 0, Rotation2d.fromDegrees(180)),
// generate trajectory
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(0, 1)),
new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
trajectoryConfig);
// ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
// simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
// simplePath.add(new Pose2d(0, 1, new Rotation2d(0)));
// Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
//Defining PID Controller for tracking trajectory
PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0);
PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0);
PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0);
PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0);
ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP,
AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0);
AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
//Command to follow trajectory
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+5 -5
View File
@@ -77,13 +77,10 @@ public class SwerveDrive extends SubsystemBase {
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
if (rightStick.getNorm() < 0.1) {
rot = 0;
}
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
@@ -178,7 +175,10 @@ public class SwerveDrive extends SubsystemBase {
updateOdometry();
// SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()});
SmartDashboard.putNumber("Odo X", getOdometry().getX());
SmartDashboard.putNumber("Odo Y", getOdometry().getY());
SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees());
SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
}