Skip to content
This repository has been archived by the owner on Jan 13, 2025. It is now read-only.

Commit

Permalink
update latest from 2023-Gatorade
Browse files Browse the repository at this point in the history
  • Loading branch information
@dejabot
dejabot committed Jan 9, 2024
1 parent 2a12064 commit 8f12b14
Show file tree
Hide file tree
Showing 28 changed files with 668 additions and 461 deletions.
10 changes: 7 additions & 3 deletions src/main/java/com/team766/robot/gatorade/AutonomousModes.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package com.team766.robot.gatorade;

import com.team766.framework.AutonomousMode;
import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType;
import com.team766.robot.gatorade.procedures.*;

public class AutonomousModes {
Expand All @@ -16,10 +17,13 @@ public class AutonomousModes {
// new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
// new AutonomousMode("FollowPoints", () -> new FollowPoints()),
// new AutonomousMode("ReverseIntake", () -> new ReverseIntake()),
// new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()),
// new AutonomousMode("ScoreHighCube", () -> new ScoreHighCube(GamePieceType.CUBE)),
// new AutonomousMode("OnePieceExitCommunity", () -> new
// OnePieceExitCommunity(GamePieceType.CUBE)),
// new AutonomousMode("OnePieceExitCommunityBalance", () -> new
// OnePieceExitCommunityBalance(GamePieceType.CUBE)),
new AutonomousMode(
"OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()),
// AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()),
"OnePieceBalanceCube", () -> new OnePieceBalance(GamePieceType.CUBE)),
// new AutonomousMode("FollowPointsFile", () -> new
// FollowPoints("FollowPoints.json")),
// //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new
Expand Down
150 changes: 61 additions & 89 deletions src/main/java/com/team766/robot/gatorade/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ public void run(Context context) {
> 0) {
context.takeOwnership(Robot.drive);
// If a button is pressed, drive is just fine adjustment
if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) {
if (rightJoystick.getButton(InputConstants.FINE_DRIVING)) {
Robot.drive.controlFieldOriented(
Math.toRadians(Robot.gyro.getGyroYaw()),
(leftJoystickX * FINE_DRIVING_COEFFICIENT),
Expand All @@ -133,41 +133,38 @@ public void run(Context context) {
if (boxopGamepad.getPOV() == InputConstants.POV_UP) {
new GoForCones().run(context);
setLightsForGamePiece();
SmartDashboard.putBoolean("Game Piece", true);
} else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) {
new GoForCubes().run(context);
setLightsForGamePiece();
SmartDashboard.putBoolean("Game Piece", false);
}

// look for button presses to queue placement of intake/wrist/elevator superstructure
if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) {
placementPosition = PlacementPosition.NONE;
// setLightsForPlacement();
} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
placementPosition = PlacementPosition.LOW_NODE;
setLightsForPlacement();
// setLightsForPlacement();
} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) {
placementPosition = PlacementPosition.MID_NODE;
setLightsForPlacement();
// setLightsForPlacement();
} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) {
placementPosition = PlacementPosition.HIGH_NODE;
setLightsForPlacement();
// setLightsForPlacement();
} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) {
placementPosition = PlacementPosition.HUMAN_PLAYER;
setLightsForPlacement();
// setLightsForPlacement();
}

// look for button hold to start intake, release to idle intake
if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_INTAKE_IN)) {
new IntakeIn().run(context);
} else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) {
new IntakeIdle().run(context);
}

// see if we should reset the button states
if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) {
// stop the intake
} else if (boxopGamepad.getButton(InputConstants.BUTTON_INTAKE_STOP)) {
new IntakeStop().run(context);
// reset the placement position
placementPosition = PlacementPosition.NONE;
// reset the cone/cube selection to cones
new GoForCones().run(context);
}

// look for button hold to extend intake/wrist/elevator superstructure,
Expand All @@ -187,7 +184,7 @@ public void run(Context context) {
break;
case HUMAN_PLAYER:
context.startAsync(
new ExtendWristvatorToHuman(Robot.intake.getGamePieceType()));
new ExtendToHumanWithIntake(Robot.intake.getGamePieceType()));
break;
default:
// warn, ignore
Expand All @@ -197,103 +194,78 @@ public void run(Context context) {
break;
}
} else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) {
context.startAsync(new RetractWristvator());
if (placementPosition == PlacementPosition.HUMAN_PLAYER) {
context.startAsync(new RetractWristvatorIdleIntake());
} else {
context.startAsync(new RetractWristvator());
}
}

// TODO: refactor this code. it's getting gnarly.
// look for manual nudges
// we only allow these if the extend elevator trigger is extended
if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) {
// the y axis is flipped from what we expect. invert so up is positive, down is
// negative.

// look for elevator nudges
double elevatorNudgeAxis =
-1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT);
double wristNudgeAxis =
-1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);

if (boxopGamepad.getButtonPressed(
InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) {
// bypass PID
if (Math.abs(elevatorNudgeAxis) > 0.05) {
elevatorManual = true;
context.takeOwnership(Robot.elevator);
Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
context.releaseOwnership(Robot.elevator);
} else if (elevatorManual) {
context.takeOwnership(Robot.elevator);
Robot.elevator.stopElevator();
context.releaseOwnership(Robot.elevator);
elevatorManual = false;
}

if ((Math.abs(wristNudgeAxis) > 0.05)) {
wristManual = true;
context.takeOwnership(Robot.wrist);
Robot.wrist.nudgeNoPID(wristNudgeAxis);
context.releaseOwnership(Robot.wrist);
} else if (wristManual) {
context.takeOwnership(Robot.wrist);
Robot.wrist.stopWrist();
context.releaseOwnership(Robot.wrist);
elevatorManual = false;
}
} else if (boxopGamepad.getButtonReleased(
InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) {
context.takeOwnership(Robot.wrist);
if (Math.abs(elevatorNudgeAxis) > 0.05) {
// elevatorManual = true;
context.takeOwnership(Robot.elevator);
Robot.wrist.resetEncoder();
Robot.elevator.resetEncoder();
context.releaseOwnership(Robot.wrist);
context.releaseOwnership(Robot.elevator);
} else {
// look for elevator nudges
if (Math.abs(elevatorNudgeAxis) > 0.05) {
context.takeOwnership(Robot.elevator);
if (elevatorNudgeAxis > 0) {
Robot.elevator.nudgeUp();
} else {
Robot.elevator.nudgeDown();
}
context.releaseOwnership(Robot.elevator);
// Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
if (elevatorNudgeAxis > 0) {
Robot.elevator.nudgeUp();
} else {
Robot.elevator.nudgeDown();
}
context.releaseOwnership(Robot.elevator);
} else if (false && elevatorManual) {
Robot.elevator.stopElevator();
elevatorManual = false;
}

// look for wrist nudges
if (Math.abs(wristNudgeAxis) > 0.05) {
context.takeOwnership(Robot.wrist);
if (wristNudgeAxis > 0) {
Robot.wrist.nudgeUp();
} else {
Robot.wrist.nudgeDown();
}
context.releaseOwnership(Robot.wrist);
// look for wrist nudges
double wristNudgeAxis =
-1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);
if (Math.abs(wristNudgeAxis) > 0.05) {
// wristManual = true;
context.takeOwnership(Robot.wrist);
// Robot.wrist.nudgeNoPID(wristNudgeAxis);
if (wristNudgeAxis > 0) {
Robot.wrist.nudgeUp();
} else {
Robot.wrist.nudgeDown();
}
context.releaseOwnership(Robot.wrist);
} else if (false && wristManual) {
Robot.wrist.stopWrist();
wristManual = true;
}
}

if (lightsRateLimit.next()) {
if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) {
if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 17) {
Robot.lights.rainbow();
} else {
setLightsForPlacement();
setLightsForGamePiece();
}
}
}
}

private void setLightsForPlacement() {
switch (placementPosition) {
case NONE:
Robot.lights.white();
break;
case LOW_NODE:
Robot.lights.green();
break;
case MID_NODE:
Robot.lights.red();
break;
case HIGH_NODE:
Robot.lights.orange();
break;
// case NONE:
// Robot.lights.white();
// break;
// case LOW_NODE:
// Robot.lights.green();
// break;
// case MID_NODE:
// Robot.lights.red();
// break;
// case HIGH_NODE:
// Robot.lights.orange();
// break;
case HUMAN_PLAYER:
setLightsForGamePiece();
break;
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/com/team766/robot/gatorade/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ public class Robot {
public static Intake intake;
public static Wrist wrist;
public static Elevator elevator;
public static Shoulder shoulder;
public static Drive drive;
public static Gyro gyro;
public static Lights lights;
Expand All @@ -16,6 +17,7 @@ public static void robotInit() {
intake = new Intake();
wrist = new Wrist();
elevator = new Elevator();
shoulder = new Shoulder();
drive = new Drive();
gyro = new Gyro();
lights = new Lights();
Expand Down
11 changes: 11 additions & 0 deletions src/main/java/com/team766/robot/gatorade/constants/ConfigConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,4 +36,15 @@ private ConfigConstants() {}
public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity";
public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity";
public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel";

// shoulder config values
public static final String SHOULDER_LEFT_MOTOR = "shoulder.leftMotor";
public static final String SHOULDER_RIGHT_MOTOR = "shoulder.rightMotor";
public static final String SHOULDER_PGAIN = "shoulder.sparkPID.pGain";
public static final String SHOULDER_IGAIN = "shoulder.sparkPID.iGain";
public static final String SHOULDER_DGAIN = "shoulder.sparkPID.dGain";
public static final String SHOULDER_FFGAIN = "shoulder.sparkPID.ffGain";
public static final String SHOULDER_MAX_VELOCITY = "shoulder.sparkPID.maxVelocity";
public static final String SHOULDER_MIN_OUTPUT_VELOCITY = "shoulder.sparkPID.minOutputVelocity";
public static final String SHOULDER_MAX_ACCEL = "shoulder.sparkPID.maxAccel";
}
18 changes: 9 additions & 9 deletions src/main/java/com/team766/robot/gatorade/constants/InputConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,24 +24,24 @@ public final class InputConstants {

// Boxop Gamepad Buttons

// LT
public static final int BUTTON_INTAKE_IN = 7;
// RT
public static final int BUTTON_EXTEND_WRISTVATOR = 8;
public static final int BUTTON_INTAKE_IN = 8;
// LT
public static final int BUTTON_EXTEND_WRISTVATOR = 7;
// Start
public static final int BUTTON_RESET_STATE = 10;
public static final int BUTTON_INTAKE_STOP = 10; // used for development

// left axis
public static final int AXIS_WRIST_MOVEMENT = 1;
// right axis
public static final int AXIS_ELEVATOR_MOVEMENT = 3;
public static final int AXIS_WRIST_MOVEMENT = 3;
// left axis
public static final int AXIS_ELEVATOR_MOVEMENT = 1;

// pov
public static final int POV_UP = 0;
public static final int POV_DOWN = 180;

// LB
public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5;
// RB
public static final int BUTTON_PLACEMENT_NONE = 6;
// X/A/B/Y
public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X
public static final int BUTTON_PLACEMENT_HIGH = 4; // Y
Expand Down
Loading

0 comments on commit 8f12b14

Please sign in to comment.