This repository has been archived by the owner on Nov 6, 2017. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #43 from SouthEugeneRoboticsTeam/align-shooter
Auto
- Loading branch information
Showing
21 changed files
with
399 additions
and
301 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
55 changes: 0 additions & 55 deletions
55
src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java
This file was deleted.
Oops, something went wrong.
67 changes: 67 additions & 0 deletions
67
src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,67 @@ | ||
| package org.usfirst.frc.team2521.robot.commands.automation; | ||
|
|
||
| import org.usfirst.frc.team2521.robot.Robot; | ||
|
|
||
| import edu.wpi.first.wpilibj.command.PIDCommand; | ||
|
|
||
| /** | ||
| * This abstract class provides a template for driving to the gear or the boiler. | ||
| */ | ||
| public abstract class DriveToBlob extends PIDCommand { | ||
| private static final double CAMERA_PROJ_PLANE_DISTANCE = 216.226; | ||
|
|
||
| /** true if we're straight-on */ | ||
| protected boolean oriented = false; | ||
| /** true if we're on the left side of the target */ | ||
| private boolean onLeftSide; | ||
| private boolean hasFoundBlob = false; | ||
|
|
||
| /** | ||
| * @param onLeftSide whether we're on the left side of the target | ||
| */ | ||
| protected DriveToBlob(double p, double i, double d, boolean onLeftSide) { | ||
| super(p, i, d); | ||
| this.onLeftSide = onLeftSide; | ||
| requires(Robot.drivetrain); | ||
| } | ||
|
|
||
| protected abstract double getSlowSpeed(); | ||
|
|
||
| @Override | ||
| protected abstract void initialize(); | ||
|
|
||
| @Override | ||
| protected final void execute() { | ||
| // Angle between camera line of sight and target | ||
| double targetAngle = Math.atan(Robot.sensors.getCVOffsetX() / CAMERA_PROJ_PLANE_DISTANCE); | ||
|
|
||
| // Convert angle to degrees | ||
| targetAngle *= 180 / Math.PI; | ||
|
|
||
| // Consider ourselves oriented when our angle error is less than 20 | ||
| oriented = Math.abs(targetAngle) < 20; | ||
|
|
||
| targetAngle += Robot.sensors.getNavxAngle(); | ||
| setSetpoint(targetAngle); | ||
|
|
||
| if (Robot.sensors.getBlobFound()) { | ||
| hasFoundBlob = true; | ||
| } | ||
| } | ||
|
|
||
| @Override | ||
| protected final double returnPIDInput() { | ||
| return Robot.sensors.getNavxAngle(); | ||
| } | ||
|
|
||
| protected void setCurrentBlobFoundMotorSpeed() { | ||
| if (hasFoundBlob) { | ||
| Robot.drivetrain.setLeft(getSlowSpeed()); | ||
| Robot.drivetrain.setRight(getSlowSpeed()); | ||
| } else { | ||
| // Turn clockwise if we're too far left, counter-clockwise if we're too far right | ||
| Robot.drivetrain.setLeft(onLeftSide ? -getSlowSpeed() : getSlowSpeed()); | ||
| Robot.drivetrain.setRight(onLeftSide ? -getSlowSpeed() : getSlowSpeed()); | ||
| } | ||
| } | ||
| } |
54 changes: 54 additions & 0 deletions
54
src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,54 @@ | ||
| package org.usfirst.frc.team2521.robot.commands.automation; | ||
|
|
||
| import org.usfirst.frc.team2521.robot.Robot; | ||
| import org.usfirst.frc.team2521.robot.subsystems.Sensors; | ||
|
|
||
| /** | ||
| * This command drives to the correct spot to shoot from automatically. | ||
| */ | ||
| public class DriveToBoiler extends DriveToBlob { | ||
| private static final double P = 0.008; | ||
| private static final double I = 0; | ||
| private static final double D = 0; | ||
|
|
||
| private static final double P2 = 0.0125; | ||
| private static final double DISTANCE_SETPOINT = 35; | ||
| private static final double DISTANCE_ERROR_THRESHOLD = 3; | ||
|
|
||
| public DriveToBoiler() { | ||
| super(P, I, D, false); | ||
| } | ||
|
|
||
| @Override | ||
| protected double getSlowSpeed() { | ||
| return P2 * (DISTANCE_SETPOINT - Robot.sensors.getRearUltraInches()); | ||
| } | ||
|
|
||
| @Override | ||
| protected void initialize() { | ||
| Robot.sensors.setCVCamera(Sensors.Camera.REAR); | ||
| } | ||
|
|
||
| @Override | ||
| protected boolean isFinished() { | ||
| return Math.abs(DISTANCE_SETPOINT - Robot.sensors.getRearUltraInches()) < DISTANCE_ERROR_THRESHOLD; | ||
| } | ||
|
|
||
| @Override | ||
| protected final void usePIDOutput(double output) { | ||
| if (Robot.sensors.getBlobFound()) { | ||
| // If we are already oriented, drive straight | ||
| if (oriented) { | ||
| Robot.drivetrain.setLeft(getSlowSpeed()); | ||
| Robot.drivetrain.setRight(getSlowSpeed()); | ||
| } else if (-output > 0) { | ||
| Robot.drivetrain.setLeft(output); | ||
| } else { | ||
| Robot.drivetrain.setRight(-output); | ||
| } | ||
| } else { | ||
| setCurrentBlobFoundMotorSpeed(); | ||
| } | ||
| } | ||
|
|
||
| } |
36 changes: 0 additions & 36 deletions
36
src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java
This file was deleted.
Oops, something went wrong.
Oops, something went wrong.