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

Auto mode #21

Open
maxwell-tang wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Auto mode #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
aa35b19
created a action group for autonomous mode
maxwell-tang Feb 19, 2020
a1ab52b
added 3 autonomous modes
maxwell-tang Feb 22, 2020
4290914
added arc to actions
maxwell-tang Feb 22, 2020
ae94552
changed autonomous modes
maxwell-tang Feb 22, 2020
1a5e4fc
added new auto modes(incomplete)
maxwell-tang Feb 23, 2020
647108d
adds 2 modes: left and right
maxwell-tang Feb 26, 2020
8704fe2
removed todos
maxwell-tang Feb 27, 2020
51b0fb5
fixed action text
maxwell-tang Feb 28, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
231 changes: 194 additions & 37 deletions src/main/java/frc/robot/autonomous/Actions.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import frc.robot.gamepieces.GamePieceController;
import frc.robot.logging.RobotLogManager;
import frc.robot.gamepieces.AbstractLayers.IndexerAL;
import frc.robot.gamepieces.AbstractLayers.IntakeAL;

import java.text.DecimalFormat;

Expand All @@ -21,47 +22,45 @@ public class Actions {
private static GamePieceController gamePieceController = GamePieceController.getInstance();

private static double mirrorTurns = 1.0;

public static void startOnLeft() {
mirrorTurns = -1.0;
}

public static void startOnRight() {
mirrorTurns = 1.0;
}

public static void startInCenter() {
mirrorTurns = 1.0;
}

public static final Action nothing() {
String actionText = "Do Nothing";
return new Action(actionText,
() -> drive.isStopped(),
() -> drive.arcadeDrive(0, 0, false));
return new Action(actionText, () -> drive.isStopped(), () -> drive.arcadeDrive(0, 0, false));
}

public static final Action drive(double duration) {
String actionText = "Do Nothing";
Copy link
Copy Markdown
Contributor

@john-petrangelo john-petrangelo Feb 27, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action text is wrong,

return new Action(actionText, new ActionGroup.Duration(duration), () -> drive.arcadeDrive(1, 0, false));
}

public static final Action Shoot() {
String actionText = "shoot";

IndexerAL indexer = IndexerAL.getInstance();
return new Action(actionText,
() -> indexer.isBallInChamber(),
return new Action(actionText, () -> indexer.isBallInChamber(),
() -> gamePieceController.setAutomousFireWhenReady(true));
}

public static Action wait(double duration) {
String actionText = "Do Nothing";
return new Action(actionText,
new ActionGroup.Duration(duration),
() -> drive.arcadeDrive(0, 0));
return new Action(actionText, new ActionGroup.Duration(duration), () -> drive.arcadeDrive(0, 0));
}

public static final Action nothingForever() {
String actionText = "Do Nothing";
return new Action(actionText,
() -> false,
() -> drive.moveLinearFeet(0));
return new Action(actionText, () -> false, () -> drive.moveLinearFeet(0));
}

public static ActionGroup doNothing() {
Expand All @@ -71,16 +70,11 @@ public static ActionGroup doNothing() {
}

public static Action print(String message) {
return new Action(
"Print custom message",
new ActionGroup.RunOnce(() -> LOGGER.info(message)));
return new Action("Print custom message", new ActionGroup.RunOnce(() -> LOGGER.info(message)));
}


public static Action zeroDistance() {
return new Action(
"Zeroing the distance",
new ActionGroup.RunOnce(() -> drive.zero()));
return new Action("Zeroing the distance", new ActionGroup.RunOnce(() -> drive.zero()));
}

/**
Expand All @@ -90,31 +84,57 @@ public static Action zeroDistance() {
*/
public static Action moveDistanceForward(double distance) {
String actionText = "Move forward " + distance + " feet";
//TODO determine the mult values
double timeAmt = (distance - 0.112)/10.3596;
return new Action(actionText,
new ActionGroup.Duration(timeAmt),
() -> drive.arcadeDrive(0.8,0));
return new Action(actionText, new ActionGroup.ReachDistance(distance), () -> drive.arcadeDrive(0.8, 0));
}

/**
*
* @param distance moves robot in feet.
* @return
*/
public static ActionGroup moveDistanceForwardAndGather(double distance) {
String actionGroupText = "Move forward " + distance + " feet";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addAction( new Action("", new ActionGroup.ReachDistance(distance), new ActionGroup.ConcurrentActions(
() -> drive.arcadeDrive(0.8, 0), () -> IntakeAL.callForward())));
//TODO i dont know if this just does this once and stops;
mode.addAction(new Action("",()->false,()->IntakeAL.callRollerStop()));
return mode;
}

/**
*
* @param distance moves robot in feet.
* @param rotation enter positive degrees for left turn and enter negative
* degrees for right turn.
* @return
*/
public static Action arc(double speed, double rotation) {
String actionText = "Move forward " + speed + " feet";
ActionGroup.MultiCondition condition = new ActionGroup.MultiCondition(new ActionGroup.ReachAngle(rotation),
new ActionGroup.ReachDistance(speed));

return new Action(actionText, condition, () -> drive.arcadeDrive(speed, rotation));
}

/**
*
* @param rotationInDegrees Rotates robot in radians. Enter rotation amount in Degrees.
*
* @param rotationInDegrees Rotates robot in radians. Enter rotation amount in
* Degrees.
*
*/
public static Action moveturn(double rotationInDegrees) {
String actionText = "Rotate " + rotationInDegrees + " degrees.";
return new Action(actionText,
new ActionGroup.ReachAngle(rotationInDegrees),
return new Action(actionText, new ActionGroup.ReachAngle(rotationInDegrees),
// reach distance was here instead of reachAngle
() -> drive.rotateByAngle(rotationInDegrees));
}

public static boolean moveDistanceComplete(double distance) {
double distanceMoved = drive.absoluteDistanceMoved();

LOGGER.debug("Distances - Target: {} Moved: {}",
df.format(Math.abs(distance)), df.format(distanceMoved));
LOGGER.debug("Distances - Target: {} Moved: {}", df.format(Math.abs(distance)), df.format(distanceMoved));
if (distanceMoved >= (Math.abs(distance) - RobotMap.POSITION_ALLOWED_ERROR)) {
LOGGER.info("Finished moving {} feet", df.format(distanceMoved));
return true;
Expand All @@ -124,14 +144,22 @@ public static boolean moveDistanceComplete(double distance) {
}
}

/**
*
* @param distance distance to move in feet
Copy link
Copy Markdown

@jasper77 jasper77 Feb 27, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

"distance" is repeated in comment

*/
public static ActionGroup move(double distance) {
String actionGroupText = "Move forward " + distance + " feet";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addAction(zeroDistance());
mode.addAction(moveDistanceForward(distance));
return mode;
}

/**
*
* @param degrees enter positive degrees for left turn and enter negative
* degrees for right turn.
*/
public static ActionGroup turn(double degrees) {
String actionGroupText = "Turn " + degrees + " degrees";
ActionGroup mode = new ActionGroup(actionGroupText);
Expand All @@ -154,22 +182,39 @@ public static ActionGroup DoNothing() {
}

public static ActionGroup shootGroup() {
String actionGroupText = "doing nothing";
String actionGroupText = "shoots";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addAction(Shoot());
mode.addAction(moveDistanceForward(10));
return mode;
}


public static ActionGroup crossAutoLine() {
String actionGroupText = "Go straight to cross the auto line.";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addActions(start());
mode.addActions(move(10.0));
return mode;
}


public static Action getAngleFromTarget() {
String actionText = "sense angle from target";
return new Action(actionText, //
() -> true, // if an angle has been sensed
() -> {
}// sense for an angle
);
}

public static ActionGroup aimAndShoot() {
String actionGroupText = "aims at the target and shoot";
double thing;
ActionGroup mode = new ActionGroup(actionGroupText);

return mode;

}

// TEST ACTIONS

public static ActionGroup fourFootSquare() {
Expand All @@ -186,4 +231,116 @@ public static ActionGroup fourFootSquare() {
return mode;
}

//
public static ActionGroup collectAndShoot() {
// stuff
String actionGroupText = "align with trench and go back to score";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addActions(move(11));
mode.addActions(turn(90));
// TODO create this
mode.addAction(getAngleFromTarget());
mode.addActions(turn(-90));
// mode.addActions(modeForwardByTheAmountINeedPlus5.66Feet()); TODO
// formula is tan(theta)*10.16+5.66
mode.addActions(turn(-90));
mode.addActions(move(16.06));
mode.addActions(turn(-160.59));
mode.addActions(move(17.03));
mode.addActions(turn(-19.41));
// TODO make these align ans shoot
mode.addAction(Shoot());

return mode;
}

/**
* turns right, moves closer to the power port and shoots at the port. Then it
* turns and moves pas the line and out of the way
*/
public static ActionGroup shootLB() {
// stuff
String actionGroupText = "shoot from loading bay";
ActionGroup mode = new ActionGroup(actionGroupText);
// TODO make these based off of camera readings
mode.addActions(turn(-62.78));
mode.addActions(move(6.56));
mode.addActions(turn(5.78));
// TODO make these align and shoot
mode.addAction(Shoot());
// move out of the way after shot
mode.addActions(turn(57.4));
mode.addActions(move(-10));
return mode;
}

/**
* shoots and moves back out of the way
*/
public static ActionGroup shootPP() {
// stuff
String actionGroupText = "shoot from power port";
ActionGroup mode = new ActionGroup(actionGroupText);
// TODO make these align and shoot
mode.addAction(Shoot());
// move back after shot
mode.addActions(move(-10));
return mode;
}

/**
* turns a little left and shoots, then turns back and drives back
*/
public static ActionGroup shootPS1() {
// stuff
String actionGroupText = "shoot from player station 1";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addActions(turn(28.43));
// TODO make these align and shoot
mode.addAction(Shoot());
mode.addActions(turn(-28.43));
// move out of the way after shot
mode.addActions(move(-10));

return mode;
}

/**
* if the robots shooter is broken, then it moves back out of the way
* immediately
*/
public static ActionGroup noShoot() {
// stuff
String actionGroupText = "dont shoot and back up";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addActions(move(-10));
return mode;
}

// TODO change names
public static ActionGroup leftSide() {
String actionGroupText = "shoot from left side";
double thing;
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addAction(Shoot());
mode.addActions(turn(-150));
mode.addActions(moveDistanceForwardAndGather(16)/* suck TODO */);
mode.addActions(move(-9));
mode.addActions(turn(-160));
mode.addAction(Shoot());
return mode;
}

public static ActionGroup rightSide() {
String actionGroupText = "shoot from right side";
ActionGroup mode = new ActionGroup(actionGroupText);
mode.addActions(move(-12));
mode.addActions(turn(42));
mode.addActions(moveDistanceForwardAndGather(7));
mode.addActions(move(-2));
mode.addActions(turn(-45));
mode.addAction(Shoot());
return mode;
}

}
Loading