Skip to content

Commit

Permalink
Merge pull request #160 from FRC2706/rcvi_vision3
Browse files Browse the repository at this point in the history 
Rcvi vision3
  • Loading branch information
@robertlucyshyn
robertlucyshyn authored Apr 9, 2019
2 parents d7492f8 + 36a4e1d commit acc5045
Show file tree
Hide file tree
Showing 6 changed files with 163 additions and 142 deletions.
21 changes: 13 additions & 8 deletions src/main/java/ca/team2706/frc/robot/OI.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package ca.team2706.frc.robot;

import ca.team2706.frc.robot.commands.drivebase.AbsoluteRotateWithGyro;
import ca.team2706.frc.robot.commands.drivebase.CurvatureDriveWithJoystick;
import ca.team2706.frc.robot.commands.drivebase.*;
import ca.team2706.frc.robot.commands.intake.AfterEjectConditional;
import ca.team2706.frc.robot.commands.intake.EjectConditional;
import ca.team2706.frc.robot.commands.intake.arms.LowerArmsSafely;
Expand Down Expand Up @@ -132,12 +131,18 @@ public boolean get() {
// The button to use to interrupt the robots current command
new FluidButton(driverStick, Config.INTERRUPT_BUTTON)
.whenPressed(new InstantCommand(Robot::interruptCurrentCommand));
// new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_CARGO_AND_LOADING_BINDING)
// .whenHeld(new DriverAssistVision(DriverAssistVisionTarget.CARGO_AND_LOADING));
// new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_ROCKET_BINDING)
// .whenHeld(new DriverAssistVision(DriverAssistVisionTarget.ROCKET));
// new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_BALL_BINDING)
// .whenHeld(new DriverAssistVision(DriverAssistVisionTarget.BALL));
new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_CARGO_AND_LOADING_INITIAL_OFFSET_BINDING)
.whenHeld(new DriverAssistVision(DriverAssistVisionTarget.CARGO_AND_LOADING, true));
new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_ROCKET_INITIAL_OFFSET_BINDING)
.whenHeld(new DriverAssistVision(DriverAssistVisionTarget.ROCKET, true));
new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_CARGO_AND_LOADING_FINAL_OFFSET_BINDING)
.whenHeld(new DriverAssistVision(DriverAssistVisionTarget.CARGO_AND_LOADING, false));
new FluidButton(driverStick, Config.DRIVER_ASSIST_VISION_ROCKET_FINAL_OFFSET_BINDING)
.whenHeld(new DriverAssistVision(DriverAssistVisionTarget.ROCKET, false));
new FluidButton(driverStick, Config.DRIVER_ASSIST_ABSOLUTE_GYRO_RESET_CARGO_AND_LOADING_BINDING)
.whenHeld(new AbsoluteGyroReset(DriverAssistVisionTarget.CARGO_AND_LOADING));
new FluidButton(driverStick, Config.DRIVER_ASSIST_ABSOLUTE_GYRO_RESET_ROCKET_BINDING)
.whenHeld(new AbsoluteGyroReset(DriverAssistVisionTarget.ROCKET));
new FluidButton(driverStick, Config.FACE_FORWARD_BINDING)
.whenHeld(new AbsoluteRotateWithGyro(0.6, 90, Integer.MAX_VALUE));
new FluidButton(driverStick, Config.FACE_RIGHT_BINDING)
Expand Down
39 changes: 39 additions & 0 deletions src/main/java/ca/team2706/frc/robot/commands/drivebase/AbsoluteGyroReset.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
package ca.team2706.frc.robot.commands.drivebase;

import ca.team2706.frc.robot.logging.Log;
import ca.team2706.frc.robot.subsystems.DriveBase;
import edu.wpi.first.wpilibj.command.InstantCommand;

/**
* Resets the absolute gyro heading of the robot to a target
*/
public class AbsoluteGyroReset extends InstantCommand {

/**
* Type of target that robot is aligned with (CARGO_AND_LOADING or ROCKET)
*/
private DriverAssistVisionTarget target;

/**
* Creates absolute gyro reset object
*
* @param target The type of target that the robot is aligned with
*/
public AbsoluteGyroReset(DriverAssistVisionTarget target) {
this.target = target;
}

@Override
public void initialize() {
Log.d("AGR: initialize() called");
double angRobotHeadingCurrent_Field = DriveBase.getInstance().getAbsoluteHeading() % 360;
if (angRobotHeadingCurrent_Field < 0.0) {
angRobotHeadingCurrent_Field += 360.0;
}
Log.d("AGR: angRobotHeadingCurrent_Field: " + angRobotHeadingCurrent_Field);
double updatedAngle = DriverAssistVision.computeAngRobotHeadingFinal_Field(angRobotHeadingCurrent_Field, target);
DriveBase.getInstance().resetAbsoluteGyro(updatedAngle);
Log.d("AGR: Absolute gyro angle updated to: " + updatedAngle);
}
}

183 changes: 67 additions & 116 deletions src/main/java/ca/team2706/frc/robot/commands/drivebase/DriverAssistVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import ca.team2706.frc.robot.config.Config;
import ca.team2706.frc.robot.logging.Log;
import ca.team2706.frc.robot.subsystems.DriveBase;
import ca.team2706.frc.robot.subsystems.RingLight;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
Expand Down Expand Up @@ -83,6 +82,12 @@ public class DriverAssistVision extends Command {
*/
private boolean generateTrajectoryRequestStageComplete;

/**
* True if destination is the initial position at a relatively large offset to the target,
* false if destination is the final position at small offset directly in front of the target
*/
private boolean initialApproach;

/**
* Network table instance to get data from vision subsystem
*/
Expand Down Expand Up @@ -120,23 +125,29 @@ public class DriverAssistVision extends Command {
*/
private volatile Trajectory trajectory;

/**
* True if trajectory has been generated by Pathfinder, false otherwise. Declared volatile since it is
* shared between different threads.
*/
private volatile boolean trajectoryGenerated;

/**
* Creates driver assist command
*
* @param target: The type of the destination target (CARGO_AND_LOADING, ROCKET, BALL)
*/
public DriverAssistVision(DriverAssistVisionTarget target) {
public DriverAssistVision(DriverAssistVisionTarget target, boolean initialApproach) {
this.target = target;

// Ensure that this command is the only one to run on the ring light subsystem.
requires(RingLight.getInstance());
this.initialApproach = initialApproach;

setupNetworkTables();

commandAborted = false;
generateTrajectoryRequestStageComplete = false;
ringLightOnDelayTime = 0.0;
ringLightOnStageComplete = false;
trajectory = null;
trajectoryGenerated = false;
}

/**
Expand Down Expand Up @@ -176,9 +187,7 @@ else if (visionOffline()) {
ringLightOnDelayTime = 0.0;
ringLightOnStageComplete = false;
trajectory = null;

Log.d("DAV: Turning ring light on");
RingLight.getInstance().enableLight();
trajectoryGenerated = false;

Log.d("DAV: Getting entries for network table");
driverEntry = chickenVisionTable.getEntry("Driver");
Expand All @@ -188,52 +197,6 @@ else if (visionOffline()) {
if ((driverEntry == null) || (findTapeEntry == null) || (findCargoEntry == null) || (tapeDetectedEntry == null)) {
Log.d("DAV: Network table entries not set up, command aborted");
commandAborted = true;
return;
}

// Take vision out of driver mode and into target finding mode
if ((target == DriverAssistVisionTarget.CARGO_AND_LOADING) || (target == DriverAssistVisionTarget.ROCKET)) {
driverEntry.setBoolean(false);
findCargoEntry.setBoolean(false);
findTapeEntry.setBoolean(true);
} else if (target == DriverAssistVisionTarget.BALL) {
driverEntry.setBoolean(false);
findCargoEntry.setBoolean(true);
findTapeEntry.setBoolean(false);
} else {
commandAborted = false;
generateTrajectoryRequestStageComplete = false;
ringLightOnDelayTime = 0.0;
ringLightOnStageComplete = false;

Log.d("DAV: Turning ring light on");
RingLight.getInstance().enableLight();

Log.d("DAV: Getting entries for network table");
driverEntry = chickenVisionTable.getEntry("Driver");
findTapeEntry = chickenVisionTable.getEntry("Tape");
findCargoEntry = chickenVisionTable.getEntry("Cargo");
tapeDetectedEntry = chickenVisionTable.getEntry("tapeDetected");
if ((driverEntry == null) || (findTapeEntry == null) || (findCargoEntry == null) || (tapeDetectedEntry == null)) {
Log.d("DAV: Network table entries not set up, command aborted");
commandAborted = true;
} else {
// Take vision out of driver mode and into target finding mode
if ((target == DriverAssistVisionTarget.CARGO_AND_LOADING) || (target == DriverAssistVisionTarget.ROCKET)) {
driverEntry.setBoolean(false);
findCargoEntry.setBoolean(false);
findTapeEntry.setBoolean(true);
} else if (target == DriverAssistVisionTarget.BALL) {
driverEntry.setBoolean(false);
findCargoEntry.setBoolean(true);
findTapeEntry.setBoolean(false);
} else {
// target has unexpected value so abort command
Log.d("DAV: Target value unexpected, aborting command");
commandAborted = true;
}
Log.d("DAV: Exiting initialize()");
}
}
}
}
Expand Down Expand Up @@ -293,30 +256,37 @@ so another reading can be taken on the next execute() call
*/
if (distanceCameraToTarget_Camera > Config.VISION_DISTANCE_MAX.value()) {
Log.d("DAV: Distance to target too high. Rereading vision data.");
return;
}

// Turn off ring light since we now have the data from vision
Log.d("DAV: Turning ring light off");
RingLight.getInstance().disableLight();
} else {

// Send request to generate trajectory in a separate task to avoid execute() overruns
Log.d("DAV: Generating trajectory");
Runnable task = () ->
generateTrajectoryRobotToTarget(distanceCameraToTarget_Camera, angYawTargetWrtCameraLOSCWpos);
new Thread(task).start();
// Send request to generate trajectory in a separate task to avoid execute() overruns
Log.d("DAV: Generating trajectory");
Runnable task = () ->
generateTrajectoryRobotToTarget(distanceCameraToTarget_Camera, angYawTargetWrtCameraLOSCWpos);
new Thread(task).start();

generateTrajectoryRequestStageComplete = true;
generateTrajectoryRequestStageComplete = true;
}
}
}
}
}

// Stage 3: Command Robot to Follow Trajectory
if (followTrajectory == null && trajectory != null) {
Log.d("DAV: Commanding robot to follow trajectory");
followTrajectory = new FollowTrajectory(1.0, 100, trajectory);
followTrajectory.start();
if (followTrajectory == null && trajectoryGenerated && trajectory != null) {
/*
* If the trajectory is commanding the robot to move backwards, which could be
* and case if the robot was closer to the target than the offset, then abort
* command
*/
if (trajectory.segments[trajectory.length() - 1].y < 0) {
Log.d("DAV: Robot trajectory is in backwards direction, command aborted");
commandAborted = true;
} else {

Log.d("DAV: Commanding robot to follow trajectory");
followTrajectory = new FollowTrajectory(1.0, 100, trajectory);
followTrajectory.start();
}
}
}

Expand All @@ -334,24 +304,6 @@ public void end() {
}
followTrajectory = null;
}

/*
Ring light should already be off, but just make sure in case there were early exits
due to errors
*/
RingLight.getInstance().disableLight();

// Put vision back into driver mode
driverEntry = chickenVisionTable.getEntry("Driver");
findTapeEntry = chickenVisionTable.getEntry("Tape");
findCargoEntry = chickenVisionTable.getEntry("Cargo");
driverEntry.setBoolean(true);
findCargoEntry.setBoolean(false);
findTapeEntry.setBoolean(false);

// Vision group has requested that tapeDetected entry be set to false here
tapeDetectedEntry = chickenVisionTable.getEntry("tapeDetected");
tapeDetectedEntry.setBoolean(false);
}

/**
Expand Down Expand Up @@ -489,10 +441,12 @@ public void generateTrajectoryRobotToTarget(double distanceCameraToTarget_Camera
Get current robot heading relative to field frame from IMU
*/
double angRobotHeadingCurrent_Field = DriveBase.getInstance().getAbsoluteHeading() % 360;
if (angRobotHeadingCurrent_Field < 0.0)
angRobotHeadingCurrent_Field += 360.0;
Log.d("DAV: angRobotHeadingCurrent_Field: " + angRobotHeadingCurrent_Field);

// Compute final desired robot heading relative to field
double angRobotHeadingFinal_Field =
angRobotHeadingFinal_Field =
computeAngRobotHeadingFinal_Field(angRobotHeadingCurrent_Field, target);
Log.d("DAV: angRobotHeadingFinal_Field: " + angRobotHeadingFinal_Field);

Expand All @@ -503,10 +457,12 @@ public void generateTrajectoryRobotToTarget(double distanceCameraToTarget_Camera

// Compute vector from target to final robot position in field frame from unit vector in field frame
double d = 0.0;
if (target == DriverAssistVisionTarget.CARGO_AND_LOADING) {
d = Config.ROBOT_HALF_LENGTH.value() + Config.TARGET_OFFSET_DISTANCE_CARGO_AND_LOADING.value();
if (initialApproach) {
d = Config.ROBOT_HALF_LENGTH.value() + Config.TARGET_OFFSET_DISTANCE_INITIAL.value();
} else if (target == DriverAssistVisionTarget.CARGO_AND_LOADING) {
d = Config.ROBOT_HALF_LENGTH.value() + Config.TARGET_OFFSET_DISTANCE_FINAL_CARGO_AND_LOADING.value();
} else if (target == DriverAssistVisionTarget.ROCKET) {
d = Config.ROBOT_HALF_LENGTH.value() + Config.TARGET_OFFSET_DISTANCE_ROCKET.value();
d = Config.ROBOT_HALF_LENGTH.value() + Config.TARGET_OFFSET_DISTANCE_FINAL_ROCKET.value();
}
double vTargetToFinal_FieldX = -d * vUnitFacingTarget_FieldX;
double vTargetToFinal_FieldY = -d * vUnitFacingTarget_FieldY;
Expand Down Expand Up @@ -538,10 +494,15 @@ public void generateTrajectoryRobotToTarget(double distanceCameraToTarget_Camera
/*
Compute vRobotToFinal_Robot as sum of Vectors 1+2 and 3:
vRobotToFinal_Robot = (vRobotToCamera_Robot + vCameraToTarget_Robot) + vTargetToFinal_Robot
vRobotToTarget_Robot + vTargetToFinal_Robot
vRobotToTarget_Robot + vTargetToFinal_Robot
*/
double vRobotToFinal_RobotX = vRobotToTarget_RobotX + vTargetToFinal_RobotX;
if (!initialApproach) {
// Add tuning adjustment for x coordinate if this is the final approach
vRobotToFinal_RobotX = vRobotToFinal_RobotX + Config.FINAL_POSITION_ADJUSTMENT_X.value();
}
double vRobotToFinal_RobotY = vRobotToTarget_RobotY + vTargetToFinal_RobotY;

Log.d("DAV: vRobotToFinal_RobotX: " + vRobotToFinal_RobotX + ", vRobotToFinal_RobotY: " + vRobotToFinal_RobotY);

// STEP 2: Compute final robot heading in robot frame
Expand Down Expand Up @@ -587,6 +548,16 @@ public void generateTrajectoryRobotToTarget(double distanceCameraToTarget_Camera
trajectory.segments[i].heading = PI_OVER_2 - trajectory.segments[i].heading;
}

// Log trajectory
Log.d("DAV: Trajectory length: " + trajectory.length());
for (int i = 0; i < trajectory.length(); i++) {
String str = trajectory.segments[i].x + "," +
trajectory.segments[i].y + "," +
trajectory.segments[i].heading;
Log.d(str);
}

trajectoryGenerated = true;
Log.d("DAV: Trajectory generated");
}

Expand All @@ -605,7 +576,7 @@ public Trajectory getTrajectory() {
* @return True if the trajectory has been generated, false otherwise.
*/
public boolean isTrajectoryGenerated() {
return (trajectory != null);
return trajectoryGenerated;
}

/**
Expand All @@ -624,12 +595,12 @@ public boolean isVisionTargetDetected() {
* @param target Destination target
* @return Final desired angle of robot heading respect to field frame in degrees
*/
public double computeAngRobotHeadingFinal_Field(double angRobotHeadingCurrent_Field, DriverAssistVisionTarget target) {
public static double computeAngRobotHeadingFinal_Field(double angRobotHeadingCurrent_Field, DriverAssistVisionTarget target) {
/*
Compute final robot angle relative to field based on current angle of robot relative to field.
Robot must be in an angular range such that it is approximately facing the desired target.
*/
angRobotHeadingFinal_Field = 0.0;
double angRobotHeadingFinal_Field = 0.0;
if (target == DriverAssistVisionTarget.CARGO_AND_LOADING) {
// Assist requested for cargo ship or loading bay targets
Log.d("DAV: Assist for cargo ship or loading bay requested");
Expand Down Expand Up @@ -687,25 +658,5 @@ public boolean visionOffline() {
videoTimestampPrev = videoTimestamp;
return (offline);
}

/**
* Enum for the different targets the robot may be request to go to
*/
public enum DriverAssistVisionTarget {
/**
* Cargo ship or loading bay target
*/
CARGO_AND_LOADING,

/**
* Rocket target
*/
ROCKET,

/**
* Ball target
*/
BALL
}
}

Loading

0 comments on commit acc5045

Please sign in to comment.