Skip to content

Commit

Permalink
Revamped auto path chooser + MBR paths
Browse files Browse the repository at this point in the history 
(Need to add x-negation instead of y-negation for amp paths in driveToPose)
* moved swerve drive code to new repo

* fixed import errors in 2024 update

* removed out of date sim spark json file

* added drive system to auto handler

* added update auto method to drive fsm

* cleaned up teleop input file

* Pose resets to (0,0,0) at start of auto and teleop

* fixed update(null) nullpointer issue

* removed stale comments

* fixed bugs with running chassis

* resolved merge conflicts

* working movement along multi-point paths

* removed unused travel angle

* checkstyle

* added seperate tag and object alignment states

* added auto path chooser for choosing paths via shuffleboard

* checkstyle

* deleted old naxx file

* added new naxX file

* fixing bugs with auto path chooser

* test auto path chooser -- working

* added maxswerve module copywright and code link

* added more descriptive names for the paths on the path chooser

* Update build job to Java 17

Fix build issue due to out of data JDK.

* implemented blue aliance auto paths

* added red alliance auto path temp code

added the code for the auto paths assuming the robot is on the red alliance. some angles are to be determined.

* reset odometry when enabled

* commented out 0 for start auto

* added PENDING state in AutoHandlerSystem and declared constants for auto path coordinates in SwerveConstants

* paths 1 and 4 tested -- need to figure out accelation constant

* paths 1 and 4 tested -- need to figure out accelation constant

* ensured that the acceleration constant would always produce a speed within max speed

* tested path 2, added turn movement for paths

* checkstyle and fixed points

* cleaned up code by removing uncessary comments, constants, and prints

* checkstyle

* checkstyle + merge cleanup

* added pause(seconds) for the pending state in drive -- to be implemented

* Test/week 1/apriltags and optimization (#4)

* added rpi java code

* added CV python files

* added apriltag module and test script

* streamlined code for only apriltag computation, replaced cv2.aruco with apriltag library

* april tag code

* working calibration code

* added apriltag network table code

* added pupil apriltag code

* added working aruco code on opencv 4.5

* added calibration data for cam 1

* code reformat

* removed image annotation to speed up april tag pipeline

* modified driveToTag() to only align horizontally for testing purposes

* tested april tag alignment with robot

* switched april tag pose from camera-relative to tag-relative

* switched aruco detection method to opencv 4.5

* added only rotation in driveToTag

* added transformation to april tag values for alignment

* negated x speed

* saved last seen value of april tag info so it can be used for alignment

* added apriltag yaw

* made robot turn to april tag instead of parallel

* added tag values on shuffleboard

* changed constants

* commented out transformations

* only y movement

* removed abs val from april tag vals

* fixing merge conflicts

* fixed merge errors

---------

Co-authored-by: Akshay Anand <95551900+AknA13@users.noreply.github.com>
Co-authored-by: Tanmayi <tanmayi.dasari@gmail.com>
Co-authored-by: AknA13 <akshaya91306@gmail.com>

* fixing errors, removing uncessary code

* removed untested automation code

* cleared automation code

* added constant

* added constant

* implemented pause method

* tested all autonomus paths at half distance --working

* implemented double speed with trigger for drive

* mbr auto paths

* need to pull

* started mbr paths

* cleaning auto code

* converted to mbr paths

* converted to mbr paths

* clean up paths

* clean up paths

* clean up paths

* added path 3 and 6

* progress on mbr paths code

* added mbr paths 1-3

* fixed can id for new chassis

* fixing errors in code

* checkstyle

* sucessfully merged auto handler

* sucessfully merged auto handler

* added mech chooser on shuffle board

* completed mbr svr auto paths for speaker merge

* renamed paths

* checkstyle

* checkstyle

* compiler errors fix

* tested all svr and mbr speaker autonomus paths combined and made changes

* removed pickup 4 (unfeasable), simplified points and differenciation between alliance paths

* removed pickup 4 (unfeasable), simplified points and differenciation between alliance paths

* simplified paths

* set up amp paths

* simplified paths

* completed code for amp paths

---------

Co-authored-by: Akshay Anand <95551900+AknA13@users.noreply.github.com>
Co-authored-by: AG6GR <AG6GR@users.noreply.github.com>
Co-authored-by: Martin <mlee187@student.fuhsd.org>
Co-authored-by: Nikhil <koalaprismo@gmail.com>
Co-authored-by: Tanmayi <tanmayi.dasari@gmail.com>
Co-authored-by: AknA13 <akshaya91306@gmail.com>
  • Loading branch information
@mirapanigrahy @AknA13
@AG6GR @mlee187 @koalabearrawr @tanmayi2
7 people authored Feb 8, 2024
1 parent 7e0faee commit 59f9bac
Show file tree
Hide file tree
Showing 7 changed files with 256 additions and 156 deletions.
64 changes: 54 additions & 10 deletions src/main/java/frc/robot/AutoPathChooser.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,44 +7,72 @@
public class AutoPathChooser {
private static SendableChooser<AutoPath> autoPathChooser;
private static SendableChooser<Boolean> allianceChooser;
private static SendableChooser<Integer> startingPosChooser;
private static SendableChooser<Boolean> mechChooser;


/**
* Constructor for creating the AutoPathChooser class. It is used in order to select the
* auto path and node for the robot without re-deploying.
*/
public AutoPathChooser() {
autoPathChooser = new SendableChooser<>();
autoPathChooser.setDefaultOption("Center Shoot and Leave", AutoPath.PATH1);
autoPathChooser.addOption("Source Side Shoot and Leave", AutoPath.PATH2);
autoPathChooser.addOption("Amp Side Shoot and Leave", AutoPath.PATH3);
autoPathChooser.addOption("Wait then Shoot and Leave", AutoPath.PATH4);
autoPathChooser.addOption("Leave Only", AutoPath.PATH5);
autoPathChooser.addOption("Shoot Only", AutoPath.PATH6);
autoPathChooser.setDefaultOption("Score and Leave", AutoPath.PATH1);
autoPathChooser.addOption("MBR ONLY: Score Multiple Times", AutoPath.PATH2);
autoPathChooser.addOption("Score Only", AutoPath.PATH3);
SmartDashboard.putData("Auto Path", autoPathChooser);

allianceChooser = new SendableChooser<>();
allianceChooser.setDefaultOption("Blue", true);
allianceChooser.addOption("Red", false);
SmartDashboard.putData("Alliance", allianceChooser);

startingPosChooser = new SendableChooser<>();
startingPosChooser.setDefaultOption("Speaker (center)", 0);
startingPosChooser.addOption("Speaker (source side)", 1);
startingPosChooser.addOption("Speaker (amp side)", 2);
startingPosChooser.addOption("Amp", 2 + 1);
startingPosChooser.addOption("Other", 2 + 2);
SmartDashboard.putData("Starting Position", startingPosChooser);

mechChooser = new SendableChooser<>();
mechChooser.setDefaultOption("SVR Mech", true);
mechChooser.addOption("MBR Mech", false);
SmartDashboard.putData("Mech", mechChooser);
}

/**
* Returns the sendable chooser object which contains information on the selected auto path.
* @return The sendable chooser for the auto path.SendableChooser contains FSMState.
* @return The sendable chooser for the auto path. SendableChooser contains AutoPath.
*/
public static SendableChooser<AutoPath> getAutoPathChooser() {
return autoPathChooser;
}

/**
* Returns the sendable chooser object containing information on the selected auto path
* alliance.
* Returns the sendable chooser object containing information on the selected alliance.
* @return the sendable chooser for the alliance. SendableChooser contains Boolean.
*/
public static SendableChooser<Boolean> getAllianceChooser() {
return allianceChooser;
}

/**
* Returns the sendable chooser object containing information on the selected starting position.
* @return the sendable chooser for the starting position. SendableChooser contains Integer.
*/
public static SendableChooser<Integer> getStartPosChooser() {
return startingPosChooser;
}

/**
* Returns the sendable chooser object containing information on the selected mechanism.
* @return the sendable chooser for the mechanism. SendableChooser contains Boolean.
*/
public static SendableChooser<Boolean> getMechChooser() {
return mechChooser;
}

/**
* Returs the selected auto path.
* @return FSMState object which has the selected auto path.
Expand All @@ -55,9 +83,25 @@ public static AutoPath getSelectedPath() {

/**
* Returns the selected alliance.
* @return boolean for the selected node. true for blue, false for red.
* @return boolean for the selected alliance.
*/
public static boolean getSelectedAlliance() {
return allianceChooser.getSelected();
}

/**
* Returns the selected starting position.
* @return integer for the selected starting position.
*/
public static int getStartingPos() {
return startingPosChooser.getSelected();
}

/**
* Returns the selected mechanism.
* @return boolean for the selected mechanism.
*/
public static boolean getSelectedMech() {
return mechChooser.getSelected();
}
}
33 changes: 24 additions & 9 deletions src/main/java/frc/robot/HardwareMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,30 @@
*/
public final class HardwareMap {
// ID numbers for devices on the CAN bus
public static final int FRONT_LEFT_DRIVING_CAN_ID = 6;
public static final int FRONT_RIGHT_DRIVING_CAN_ID = 4;
public static final int REAR_LEFT_DRIVING_CAN_ID = 8;
public static final int REAR_RIGHT_DRIVING_CAN_ID = 2;

public static final int FRONT_LEFT_TURNING_CAN_ID = 5;
public static final int FRONT_RIGHT_TURNING_CAN_ID = 3;
public static final int REAR_LEFT_TURNING_CAN_ID = 7;
public static final int REAR_RIGHT_TURNING_CAN_ID = 1;

// NEW Chassis

// public static final int FRONT_LEFT_DRIVING_CAN_ID = 6;
// public static final int FRONT_RIGHT_DRIVING_CAN_ID = 4;
// public static final int REAR_LEFT_DRIVING_CAN_ID = 8;
// public static final int REAR_RIGHT_DRIVING_CAN_ID = 2;

// public static final int FRONT_LEFT_TURNING_CAN_ID = 5;
// public static final int FRONT_RIGHT_TURNING_CAN_ID = 3;
// public static final int REAR_LEFT_TURNING_CAN_ID = 7;
// public static final int REAR_RIGHT_TURNING_CAN_ID = 1;

// OLD Chassis

public static final int FRONT_LEFT_DRIVING_CAN_ID = 8;
public static final int FRONT_RIGHT_DRIVING_CAN_ID = 6;
public static final int REAR_LEFT_DRIVING_CAN_ID = 2;
public static final int REAR_RIGHT_DRIVING_CAN_ID = 4;

public static final int FRONT_LEFT_TURNING_CAN_ID = 7;
public static final int FRONT_RIGHT_TURNING_CAN_ID = 5;
public static final int REAR_LEFT_TURNING_CAN_ID = 1;
public static final int REAR_RIGHT_TURNING_CAN_ID = 3;

public static final int CAN_ID_SPARK_SHOOTER_UPPER = 31;
public static final int CAN_ID_SPARK_SHOOTER_LOWER = 30;
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/SwerveConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,7 @@ public static final class AutoConstants {
public static final double WAIT_TIME = 5; // seconds

// constants for auto path points
public static final double N_0_5 = 0.5;
public static final double N_1_5 = 1.5;
public static final double N_2 = 2;
public static final double N_2_5 = 2.5;
Expand All @@ -150,6 +151,7 @@ public static final class AutoConstants {
public static final double DEG_45 = 45;
public static final double DEG_90 = 90;
public static final double DEG_180 = 180;
public static final double DEG_360 = 360;

// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints THETA_CONTROLLER_CONSTRAINTS
Expand Down Expand Up @@ -179,7 +181,6 @@ public static final class VisionConstants {
public static final double SOURCE_DRIVE_FORWARD_POWER = 0.25;
public static final double SOURCE_TARGET_DISTANCE = 0.85;


public static final double UNABLE_TO_SEE_TAG_CONSTANT = 4000;

public static final int RED_SPEAKER_TAG_ID = 4;
Expand Down
89 changes: 41 additions & 48 deletions src/main/java/frc/robot/systems/AutoHandlerSystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,26 @@
package frc.robot.systems;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
//import frc.robot.AutoPathChooser;

public class AutoHandlerSystem {
/* ======================== Constants ======================== */
// Auto FSM state definitions
public enum AutoFSMState {
DRIVE_PATH_1,
DRIVE_PATH_2,
DRIVE_PATH_3,
DRIVE_PATH_4_STATE_1,
DRIVE_PATH_4_STATE_2,
DRIVE_PATH_5,
TURN_TO_SCORE,
LEAVE,
DRIVE_TO_SCORE,
PICK_UP_1,
PICK_UP_2,
PICK_UP_3,
PICK_UP_4,
SHOOTER_STATE,
INTAKE_STATE,
PENDING
}

public enum AutoPath {
PATH1,
PATH2,
PATH3,
PATH4,
PATH5,
PATH6
PATH1, // score and leave
PATH2, // score multiple times
PATH3 // tbd emergency path
}

/* ======================== Private variables ======================== */
Expand All @@ -40,22 +35,18 @@ public enum AutoPath {
private KitBotShooterFSM shooterFSM;

//Predefined auto paths

private static final AutoFSMState[] PATH1 = new AutoFSMState[]{
AutoFSMState.DRIVE_PATH_1};
AutoFSMState.TURN_TO_SCORE, AutoFSMState.LEAVE};

private static final AutoFSMState[] PATH2 = new AutoFSMState[]{
AutoFSMState.DRIVE_PATH_2};
AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_1,
AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_2,
AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_3,
AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_4};

private static final AutoFSMState[] PATH3 = new AutoFSMState[]{
AutoFSMState.DRIVE_PATH_3};
private static final AutoFSMState[] PATH3 = new AutoFSMState[]{};

private static final AutoFSMState[] PATH4 = new AutoFSMState[]{
AutoFSMState.PENDING, AutoFSMState.DRIVE_PATH_4_STATE_1, AutoFSMState.DRIVE_PATH_4_STATE_2};

private static final AutoFSMState[] PATH5 = new AutoFSMState[]{
AutoFSMState.DRIVE_PATH_5};

private static final AutoFSMState[] PATH6 = new AutoFSMState[]{};
/* ======================== Constructor ======================== */
/**
* Create FSMSystem and initialize to starting state.
Expand Down Expand Up @@ -88,22 +79,16 @@ public AutoFSMState getCurrentState() {
*/
public void reset(AutoPath path) {
driveSystem.resetAutonomus();
shooterFSM.reset();
//shooterFSM.reset();

currentStateIndex = 0;
if (path == AutoPath.PATH1) {
currentStateList = PATH1;
} else if (path == AutoPath.PATH2) {
currentStateList = PATH2;
} else if (path == AutoPath.PATH3) {
currentStateList = PATH3;
} else if (path == AutoPath.PATH4) {
currentStateList = PATH4;
} else if (path == AutoPath.PATH5) {
currentStateList = PATH5;
} else if (path == AutoPath.PATH6) {
currentStateList = PATH6;
}
currentStateIndex = 0;
}

/**
Expand All @@ -113,29 +98,37 @@ public void update() {
if (currentStateIndex >= currentStateList.length) {
return;
}

boolean isCurrentStateFinished;
SmartDashboard.putString("In Auto State: ", "" + getCurrentState());
switch (getCurrentState()) {
case DRIVE_PATH_1:
isCurrentStateFinished = driveSystem.updateAutonomous(AutoFSMState.DRIVE_PATH_1);

case TURN_TO_SCORE:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.TURN_TO_SCORE);
break;
case LEAVE:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.LEAVE);
break;
case DRIVE_PATH_2:
isCurrentStateFinished = driveSystem.updateAutonomous(AutoFSMState.DRIVE_PATH_2);
case PICK_UP_1:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.PICK_UP_1);
break;
case DRIVE_PATH_3:
isCurrentStateFinished = driveSystem.updateAutonomous(AutoFSMState.DRIVE_PATH_3);
case DRIVE_TO_SCORE:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.DRIVE_TO_SCORE);
break;
case DRIVE_PATH_4_STATE_1:
case PICK_UP_2:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.DRIVE_PATH_4_STATE_1);
AutoFSMState.PICK_UP_2);
break;
case DRIVE_PATH_4_STATE_2:
case PICK_UP_3:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.DRIVE_PATH_4_STATE_2);
AutoFSMState.PICK_UP_3);
break;
case DRIVE_PATH_5:
isCurrentStateFinished = driveSystem.updateAutonomous(AutoFSMState.DRIVE_PATH_5);
case PICK_UP_4:
isCurrentStateFinished = driveSystem.updateAutonomous(
AutoFSMState.PICK_UP_4);
break;
case SHOOTER_STATE:
isCurrentStateFinished = shooterFSM.updateAutonomous(AutoFSMState.SHOOTER_STATE);
Expand Down
3 changes: 0 additions & 3 deletions src/main/java/frc/robot/systems/ClimberMechFSMLeft.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,4 @@ private void handleRetractingState(TeleopInput input) {
private boolean peakLimitSwitchHit() {
return limitPressed;
}



}
3 changes: 0 additions & 3 deletions src/main/java/frc/robot/systems/ClimberMechFSMRight.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,4 @@ private void handleRetractingState(TeleopInput input) {
private boolean peakLimitSwitchHit() {
return limitPressed;
}



}
Loading

0 comments on commit 59f9bac

Please sign in to comment.