Skip to content
This repository has been archived by the owner on Nov 10, 2024. It is now read-only.

Commit

Permalink
commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@MorphaxTheDeveloper
MorphaxTheDeveloper committed Mar 13, 2024
1 parent 403f432 commit 7ede5af
Show file tree
Hide file tree
Showing 2 changed files with 153 additions and 82 deletions.
13 changes: 13 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.ShooterSubsystem;
Expand All @@ -14,15 +16,26 @@ public class Robot extends TimedRobot {

private RobotContainer m_robotContainer;

public static SendableChooser<Integer> auto_chooser = new SendableChooser<>();


@Override
public void robotInit() {

m_robotContainer = new RobotContainer();

auto_chooser.setDefaultOption("2 Note", 0);
auto_chooser.addOption("3 Note", 1);
auto_chooser.addOption("4 Note", 2);
auto_chooser.setDefaultOption("1 Note", 3);
}

@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
m_robotContainer.autonomous_case = auto_chooser.getSelected();

SmartDashboard.putNumber("Autonomous Case",m_robotContainer.autonomous_case) ;
}

@Override
Expand Down
222 changes: 140 additions & 82 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.SwerveSubsystem;
//import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.proto.Trajectory;
import edu.wpi.first.math.trajectory.Trajectory;


public class RobotContainer {
Expand All @@ -67,6 +67,8 @@ public class RobotContainer {
private final XboxController subJoytick = new XboxController(3);
private final FeederSubsystem m_feeder = new FeederSubsystem();
private final JoystickSubsystem m_joystick = new JoystickSubsystem();
public int autonomous_case;

public RobotContainer() {

swerveSubsystem.setDefaultCommand(new SwerveJoystickCmd(
Expand Down Expand Up @@ -304,90 +306,146 @@ private void configureBindings() {

}

public Command getAutonomousCommand() {

// TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
// AutoConstants.kMaxSpeedMetersPerSecond,
// AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// .setKinematics(DriveConstants.kDriveKinematics);

TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
4,
3)
.setKinematics(DriveConstants.kDriveKinematics);
public Command getAutonomousCommand() {

var trajectoryOne =
// TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
// AutoConstants.kMaxSpeedMetersPerSecond,
// AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// .setKinematics(DriveConstants.kDriveKinematics);

PIDController xController = new PIDController(0.08, 0, 0);
PIDController yController = new PIDController(0.08, 0, 0);

ProfiledPIDController thetaController = new ProfiledPIDController(
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);

TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
4,
3)
.setKinematics(DriveConstants.kDriveKinematics);

// Pathler

var trajectoryOne =
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(-1.28, 0),new Translation2d(-0.56, -0.91)),
new Pose2d(-1.28, -1.44, new Rotation2d(3.1)),
trajectoryConfig);
new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(-1.28, 0),new Translation2d(-0.56, -0.91)),
new Pose2d(-1.28, -1.44, new Rotation2d(3.1)),
trajectoryConfig);

var trajectoryTwo =
TrajectoryGenerator.generateTrajectory(
new Pose2d(-1.28, -1.44, new Rotation2d(0)),
List.of(new Translation2d(-1.28, 0),new Translation2d(0.5, -0.5)),
new Pose2d(-0,0, new Rotation2d(0)),
trajectoryConfig);

var trajectoryThree =
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(1, 0),new Translation2d(1, -1)),
new Pose2d(0,0, new Rotation2d(2)),
trajectoryConfig);

var goSecondNote =
TrajectoryGenerator.generateTrajectory(
List.of(new Pose2d(0,0, new Rotation2d(0)),
new Pose2d(-1.3,0, new Rotation2d(0))),
trajectoryConfig);

var secondNoteToSpeaker =
TrajectoryGenerator.generateTrajectory(
List.of(new Pose2d(-1.3,0, new Rotation2d(0)),
new Pose2d(0,0, new Rotation2d(0))),
trajectoryConfig);


// var trajectoryOne =
// TrajectoryGenerator.generateTrajectory(
// List.of(
// new Pose2d(0, 0, new Rotation2d(0)),
// new Pose2d(-1.4, -1.8, new Rotation2d(1.2)),
// new Pose2d(0, -1, new Rotation2d(-2)),
// new Pose2d(-1, -1, new Rotation2d(0))),
// trajectoryConfig);

// SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
// trajectoryOne,
// swerveSubsystem::getPose,
// DriveConstants.kDriveKinematics,
// xController,
// yController,
// thetaController,
// swerveSubsystem::OtosetModuleStates,
// swerveSubsystem);

// SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand(
// trajectoryTwo,
// swerveSubsystem::getPose,
// DriveConstants.kDriveKinematics,
// xController,
// yController,
// thetaController,
// swerveSubsystem::OtosetModuleStates,
// swerveSubsystem);

// SwerveControllerCommand swerveControllerCommand3 = new SwerveControllerCommand(
// trajectoryThree,
// swerveSubsystem::getPose,
// DriveConstants.kDriveKinematics,
// xController,
// yController,
// thetaController,
// swerveSubsystem::OtosetModuleStates,
// swerveSubsystem);

switch(autonomous_case) {
case 0: // Önce içindekini atacak sonrasında arkasındakini atıp atabiliyorsa onu oradan atacak sonra en arkadaki
return pathCommand(goSecondNote).andThen(pathCommand(secondNoteToSpeaker));

case 1:

case 2:

default: return pathCommand(goSecondNote).andThen(pathCommand(secondNoteToSpeaker));
}



var trajectoryTwo =
TrajectoryGenerator.generateTrajectory(
new Pose2d(-1.28, -1.44, new Rotation2d(0)),
List.of(new Translation2d(-1.28, 0),new Translation2d(0.5, -0.5)),
new Pose2d(-0,0, new Rotation2d(0)),
trajectoryConfig);

var trajectoryThree =
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(1, 0),new Translation2d(1, -1)),
new Pose2d(0,0, new Rotation2d(2)),
trajectoryConfig);


// var trajectoryOne =
// TrajectoryGenerator.generateTrajectory(
// List.of(
// new Pose2d(0, 0, new Rotation2d(0)),
// new Pose2d(-1.4, -1.8, new Rotation2d(1.2)),
// new Pose2d(0, -1, new Rotation2d(-2)),
// new Pose2d(-1, -1, new Rotation2d(0))),
// trajectoryConfig);
// return swerveControllerCommand.andThen(swerveControllerCommand2).andThen(swerveControllerCommand3);
// PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path");
// Create a path following command using AutoBuilder. This will also trigger event markers.
// return swerveSubsystem.autobuilder.followPath(path);

}


public SwerveControllerCommand pathCommand(Trajectory m_tra){

PIDController xController = new PIDController(0.08, 0, 0);
PIDController yController = new PIDController(0.08, 0, 0);
PIDController xController = new PIDController(0.08, 0, 0);
PIDController yController = new PIDController(0.08, 0, 0);

ProfiledPIDController thetaController = new ProfiledPIDController(
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);

ProfiledPIDController thetaController = new ProfiledPIDController(
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);

SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
trajectoryOne,
swerveSubsystem::getPose,
DriveConstants.kDriveKinematics,
xController,
yController,
thetaController,
swerveSubsystem::OtosetModuleStates,
swerveSubsystem);

SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand(
trajectoryTwo,
swerveSubsystem::getPose,
DriveConstants.kDriveKinematics,
xController,
yController,
thetaController,
swerveSubsystem::OtosetModuleStates,
swerveSubsystem);

SwerveControllerCommand swerveControllerCommand3 = new SwerveControllerCommand(
trajectoryThree,
swerveSubsystem::getPose,
DriveConstants.kDriveKinematics,
xController,
yController,
thetaController,
swerveSubsystem::OtosetModuleStates,
swerveSubsystem);

return swerveControllerCommand.andThen(swerveControllerCommand2).andThen(swerveControllerCommand3);
// PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path");
// Create a path following command using AutoBuilder. This will also trigger event markers.
// return swerveSubsystem.autobuilder.followPath(path);

}
TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
4,
3)
.setKinematics(DriveConstants.kDriveKinematics);

SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
m_tra,
swerveSubsystem::getPose,
DriveConstants.kDriveKinematics,
xController,
yController,
thetaController,
swerveSubsystem::OtosetModuleStates,
swerveSubsystem);

return swerveControllerCommand;

}
}

0 comments on commit 7ede5af

Please sign in to comment.