Skip to content

Commit

Permalink
Merge branch 'MrCoyne' of https://github.com/vikings204/crescendo2024
Browse files Browse the repository at this point in the history
…into MrCoyne
  • Loading branch information
xorbotz committed Feb 9, 2024
2 parents 575198e + a3d6ff7 commit ec13658
Show file tree
Hide file tree
Showing 8 changed files with 238 additions and 53 deletions.
50 changes: 50 additions & 0 deletions src/main/deploy/pathplanner/autos/Intake Auto.auto
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 0.0,
"y": 0.0
},
"rotation": 0
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intakeStart"
}
},
{
"type": "wait",
"data": {
"waitTime": 5.0
}
}
]
}
},
{
"type": "named",
"data": {
"name": "intakeStop"
}
},
{
"type": "path",
"data": {
"pathName": "Path 2"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}
25 changes: 0 additions & 25 deletions src/main/deploy/pathplanner/autos/New Auto.auto

This file was deleted.

49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/Path 2.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.0,
"y": 0.0
},
"prevControl": null,
"nextControl": {
"x": 0.0204051190332743,
"y": 0.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.0,
"y": 1.0
},
"prevControl": {
"x": -0.07244618391805213,
"y": 1.0
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 180.0,
"maxAngularAcceleration": 360.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
}
82 changes: 82 additions & 0 deletions src/main/deploy/pathplanner/paths/Path 3.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.0,
"y": 0.0
},
"prevControl": null,
"nextControl": {
"x": 0.02023671888241695,
"y": 0.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.0,
"y": 0.0
},
"prevControl": {
"x": -0.002977187654856876,
"y": 0.0
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [
{
"name": "intake",
"waypointRelativePos": 0,
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intakeStart"
}
},
{
"type": "wait",
"data": {
"waitTime": 5.0
}
},
{
"type": "named",
"data": {
"name": "intakeStop"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 180.0,
"maxAngularAcceleration": 360.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 0,
"velocity": 0
},
"useDefaultConstraints": true
}
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@ public static final class Shooter {
/* Motor Inverts */
public static final boolean driveInvert = true;

public static final int shooterID_1 = 25;
public static final int shooterID_2 = 22;
public static final int shooterID_1 = 22;
public static final int shooterID_2 = 25;
public static final int bumpID = 26;


Expand Down Expand Up @@ -160,8 +160,8 @@ public static final class Swerve {
/* Module Specific Constants */
/* Front Left Module - Module 0 */
public static final class Mod0 {
public static final int driveMotorID = 2;
public static final int angleMotorID = 9;
public static final int driveMotorID = 4;
public static final int angleMotorID = 3;
//public static final int canCoderID = 4;
//public static final Rotation2d angleOffset = Rotation2d.fromDegrees(42.58);
public static final Rotation2d angleOffset = Rotation2d.fromDegrees(62.99);
Expand All @@ -171,8 +171,8 @@ public static final class Mod0 {

/* Front Right Module - Module 1 */
public static final class Mod1 {
public static final int driveMotorID = 4;
public static final int angleMotorID = 3;
public static final int driveMotorID = 6;
public static final int angleMotorID = 5;
//public static final int canCoderID = 1;
//public static final Rotation2d angleOffset = Rotation2d.fromDegrees(296.67);
public static final Rotation2d angleOffset = Rotation2d.fromDegrees(312.58);
Expand All @@ -182,8 +182,8 @@ public static final class Mod1 {

/* Back Left Module - Module 2 */
public static final class Mod2 {
public static final int driveMotorID = 7;
public static final int angleMotorID = 8;
public static final int driveMotorID = 2;
public static final int angleMotorID = 9;
//public static final int canCoderID = 3;
public static final Rotation2d angleOffset = Rotation2d.fromDegrees(470.5);
//public static final Rotation2d angleOffset = Rotation2d.fromDegrees(204.7);
Expand All @@ -193,8 +193,8 @@ public static final class Mod2 {

/* Back Right Module - Module 3 */
public static final class Mod3 {
public static final int driveMotorID = 6;
public static final int angleMotorID = 5;
public static final int driveMotorID = 7;
public static final int angleMotorID = 8;
//public static final int canCoderID = 2;
public static final Rotation2d angleOffset = Rotation2d.fromDegrees(296.67);
//public static final Rotation2d angleOffset = Rotation2d.fromDegrees(9.2);
Expand Down
36 changes: 28 additions & 8 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -1,7 +1,13 @@
package frc.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.auto.NamedCommands.*;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathPlannerPath;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Robot.ControlMode;
Expand All @@ -11,6 +17,8 @@
import frc.robot.subsystems.SwerveSubsystem;
import frc.robot.util.Gamepad;

import java.util.List;

import static frc.robot.Robot.ControlModeChooser;

/*
Expand Down Expand Up @@ -41,6 +49,12 @@ public RobotContainer() {
System.out.println("Switched control mode to " + mode);
});



NamedCommands.registerCommand("intakeStart", new InstantCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem));
NamedCommands.registerCommand("intakeStop", new InstantCommand(() -> ShooterSubsystem.receive(false), ShooterSubsystem));


configureButtonBindings();
SwerveSubsystem.setDefaultCommand(
new TeleopSwerve(
Expand All @@ -52,13 +66,6 @@ public RobotContainer() {
() -> DRIVER.getYButton(),
() -> DRIVER.getAButton()));


if (DRIVER.getBButton()) {
SwerveSubsystem.gyro.reset();
// s_Swerve.m_gyro_P2.calib;
System.out.println("you have calibed the gyro");
}

ShooterSubsystem.setDefaultCommand(
new RunCommand(
() -> ShooterSubsystem.speakerShot(OPERATOR.getXButton()),
Expand All @@ -85,12 +92,25 @@ private void configureButtonBindings() {
.whileTrue(
new RunCommand(() -> ShooterSubsystem.ampShot(true), ShooterSubsystem));

new JoystickButton(DRIVER, 2)
.whileTrue(
new RunCommand(SwerveSubsystem::resetEverything, SwerveSubsystem));

}

public Command getAutonomousCommand() {
return new PathPlannerAuto("Path 1");
return new PathPlannerAuto("Intake Auto");
}

// public Command getAutonomousCommand() {
// // Load the path you want to follow using its name in the GUI
// PathPlannerPath path = PathPlannerPath.fromPathFile("Path 2");
//
// // Create a path following command using AutoBuilder. This will also trigger event markers.
// //noinspection removal
// return AutoBuilder.followPathWithEvents(path);
// }

/*public Command getAutonomousCommand() {
return AutonomousManager.getCommand();
}*/
Expand Down
14 changes: 4 additions & 10 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -157,16 +157,10 @@ public void receive(boolean shoot) {
shooterMotor_1.set(-.05);
shooterMotor_2.set(-.05);
bumpMotor.set(-.05);
}
else {
// shooterPidController_1.setReference(
// 0,
// ControlType.kVelocity);
//shooterMotor_1.set(0);
//shooterPidController_2.setReference(
// 0,
// ControlType.kVelocity);
//shooterMotor_2.set(0);
} else {
shooterMotor_1.set(0);
shooterMotor_2.set(0);
bumpMotor.set(0);
}
}
public void bump(boolean shoot) {
Expand Down
15 changes: 15 additions & 0 deletions src/main/java/frc/robot/subsystems/SwerveSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,21 @@ public Rotation2d getYaw() {
// return 0.0;//gyro.getRoll();
// }

public void resetEverything() {
// modules[0].setAngleForX(0);
// modules[1].setAngleForX(0);
// modules[2].setAngleForX(0);
// modules[3].setAngleForX(0);

modules[0].resetEncoder();
modules[1].resetEncoder();
modules[2].resetEncoder();
modules[3].resetEncoder();

resetOdometry(new Pose2d());
gyro.reset();
}

@Override
public void periodic() {
swerveOdometry.update(getYaw(), getPositions());
Expand Down

0 comments on commit ec13658

Please sign in to comment.