Compare commits
66 Commits
Author | SHA1 | Date | |
---|---|---|---|
e5d5848f2a | |||
54a7f87eb3 | |||
77f44f8c36 | |||
a842fbb4ad | |||
4b5e70730a | |||
5c4b376047 | |||
702df28cb2 | |||
d0c46f6e12 | |||
e5dee6a5ab | |||
7745c589ae | |||
f0ea150fac | |||
25736f3bff | |||
836a4317e5 | |||
c8e2c9697e | |||
2f8a7e1dfa | |||
40076e6d48 | |||
b1f2c54ad2 | |||
a9b18bc58a | |||
4be7508f90 | |||
d92680e9ff | |||
1468a634b3 | |||
8d30a24e89 | |||
54ae1193cd | |||
e0c78e71b4 | |||
4e86a8e6fa | |||
a396a7200b | |||
6055b2a879 | |||
05d435c64c | |||
77bd25c69d | |||
5aa7e83bfa | |||
2d5b3699b6 | |||
fc7c6545a4 | |||
8a5f7f7877 | |||
3755d5f88f | |||
f7b7eb47d2 | |||
73c96c9b9d | |||
247e00ab75 | |||
6f0719e4b6 | |||
94ce2eb3bc | |||
da3c1b24d6 | |||
2fe8a25226 | |||
5199055203 | |||
ca5b340a1a | |||
3b1efebba8 | |||
901aa58894 | |||
877b7aaf8d | |||
670de6a143 | |||
3f55c7b2b8 | |||
e7f3b43157 | |||
0108b984b1 | |||
18ff5daaad | |||
fe6dce9640 | |||
35b756efa4 | |||
1542373e40 | |||
77ae642c9b | |||
b326e64087 | |||
ebc87707e6 | |||
d50d7e8556 | |||
e04f4ba346 | |||
667a528b98 | |||
089bab94d1 | |||
135f031ae5 | |||
8c6e5ec1b2 | |||
a9ec38808f | |||
4717100d78 | |||
ca5ee1198c |
@ -1,7 +1,8 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<classpath>
|
||||
<classpathentry kind="src" path="src"/>
|
||||
<classpathentry kind="var" path="wpilib" sourcepath="wpilib.sources"/>
|
||||
<classpathentry kind="var" path="networktables" sourcepath="networktables.sources"/>
|
||||
<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
|
||||
<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
|
||||
<classpathentry kind="lib" path="~/wpilib/java/current/lib/WPILib.jar"/>
|
||||
<classpathentry kind="lib" path="~/wpilib/java/current/lib/NetworkTables.jar"/>
|
||||
<classpathentry kind="output" path="bin"/>
|
||||
</classpath>
|
||||
|
1
.gitignore
vendored
1
.gitignore
vendored
@ -1,3 +1,4 @@
|
||||
dist/FRCUserProgram.jar
|
||||
build
|
||||
bin
|
||||
sysProps.xml
|
||||
|
@ -17,7 +17,7 @@
|
||||
# -H : Pad header (space after if, for, while)
|
||||
OPTS=( --mode=java -xc --style=google -j -s2 -xG -S -K -N -xn -xl -n -p -H )
|
||||
GLOB=( src/**/*.java )
|
||||
astyle $OPTS $GLOB|\grep -P '^(?!Unchanged)'
|
||||
astyle $OPTS $GLOB|\grep -vE '^Unchanged'
|
||||
# Get rid of newlines
|
||||
sed -i'' '/^\s*$/d' $GLOB
|
||||
perl -ni'' -e '/^\s*$/ || print' $GLOB
|
||||
\cd ->/dev/null
|
||||
|
BIN
src/org/usfirst/frc/team2059/robot/.OI.java.swp
Normal file
BIN
src/org/usfirst/frc/team2059/robot/.OI.java.swp
Normal file
Binary file not shown.
BIN
src/org/usfirst/frc/team2059/robot/.Robot.java.swp
Normal file
BIN
src/org/usfirst/frc/team2059/robot/.Robot.java.swp
Normal file
Binary file not shown.
@ -1,30 +1,73 @@
|
||||
package org.usfirst.frc.team2059.robot;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.buttons.Button;
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
import edu.wpi.first.wpilibj.buttons.JoystickButton;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.LogEncoder;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.PIDDrive;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.DriveStraight;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.MoveArm;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.ResetLower;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.ResetUpper;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.SetArmPosition;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.SetShooterState;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.SetArmStopState;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.ShootAtSpeed;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.SpinRollers;
|
||||
import org.usfirst.frc.team2059.robot.commands.shooter.AlignVertical;
|
||||
import org.usfirst.frc.team2059.robot.commands.visionhelper.AlignHorizontal;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
/**
|
||||
* This class is the glue that binds the controls on the physical operator
|
||||
* interface to the commands and command groups that allow control of the robot.
|
||||
*/
|
||||
public class OI {
|
||||
//// CREATING BUTTONS
|
||||
// One type of button is a joystick button which is any button on a joystick.
|
||||
// You create one by telling it which joystick it's on and which button
|
||||
// number it is.
|
||||
// Joystick stick = new Joystick(port);
|
||||
// Button button = new JoystickButton(stick, buttonNumber);
|
||||
// There are a few additional built in buttons you can use. Additionally,
|
||||
// by subclassing Button you can create custom triggers and bind those to
|
||||
// commands the same as any other Button.
|
||||
//// TRIGGERING COMMANDS WITH BUTTONS
|
||||
// Once you have a button, it's trivial to bind it to a button in one of
|
||||
// three ways:
|
||||
// Start the command when the button is pressed and let it run the command
|
||||
// until it is finished as determined by it's isFinished method.
|
||||
// button.whenPressed(new ExampleCommand());
|
||||
// Run the command while the button is being held down and interrupt it once
|
||||
// the button is released.
|
||||
// button.whileHeld(new ExampleCommand());
|
||||
// Start the command when the button is released and let it run the command
|
||||
// until it is finished as determined by it's isFinished method.
|
||||
// button.whenReleased(new ExampleCommand());
|
||||
JoystickButton[][] joystickButtons;
|
||||
Joystick[] joysticks;
|
||||
public OI() {
|
||||
joysticks = new Joystick[2];
|
||||
joystickButtons = new JoystickButton[2][13];
|
||||
// Create joysticks
|
||||
joysticks[0] = new Joystick(0);
|
||||
joysticks[1] = new Joystick(1);
|
||||
// Create buttons
|
||||
for (int i = 0; i < 12; i++) {
|
||||
joystickButtons[0][i] = new JoystickButton(joysticks[0], i + 1);
|
||||
joystickButtons[1][i] = new JoystickButton(joysticks[1], i + 1);
|
||||
}
|
||||
// Print log when button 1 pressed
|
||||
//joystickButtons[0][0].whenPressed(new LogEncoder());
|
||||
// joystickButtons[0][0].whileHeld(new SetShooterState(true));
|
||||
joystickButtons[0][0].whileHeld(new AlignHorizontal());
|
||||
joystickButtons[0][1].whileHeld(new SetShooterState(true));
|
||||
joystickButtons[0][2].whileHeld(new SpinRollers(-0.5, false));
|
||||
joystickButtons[0][3].whileHeld(new SpinRollers(1, false));
|
||||
joystickButtons[0][4].whileHeld(new DriveStraight(0.25));
|
||||
joystickButtons[0][5].whileHeld(new DriveStraight(-0.25));
|
||||
joystickButtons[0][6].whileHeld(new SpinRollers(1, true));
|
||||
joystickButtons[0][9].whileHeld(new Drive());
|
||||
// joystickButtons[0][2].whileHeld(new PIDDrive(400));
|
||||
//joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect));
|
||||
joystickButtons[1][0].whileHeld(new AlignVertical());
|
||||
joystickButtons[1][1].whileHeld(new SetShooterState(true));
|
||||
joystickButtons[1][2].whileHeld(new ResetLower(-1));
|
||||
joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse));
|
||||
joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot));
|
||||
joystickButtons[1][5].whileHeld(new SpinRollers(1, false));
|
||||
// joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot));
|
||||
joystickButtons[1][7].whileHeld(new MoveArm(1));
|
||||
joystickButtons[1][8].whileHeld(new MoveArm(-1));
|
||||
joystickButtons[1][9].whileHeld(new SetArmPosition(110));
|
||||
}
|
||||
public Joystick[] getJoysticks() {
|
||||
return joysticks;
|
||||
}
|
||||
public Joystick getJoystick(int stick) {
|
||||
return joysticks[stick];
|
||||
}
|
||||
public JoystickButton[][] getJoystickButtons() {
|
||||
return joystickButtons;
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
||||
|
@ -1,93 +1,99 @@
|
||||
package org.usfirst.frc.team2059.robot;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.commands.autonomous.*;
|
||||
import edu.wpi.first.wpilibj.IterativeRobot;
|
||||
import edu.wpi.first.wpilibj.CameraServer;
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
import edu.wpi.first.wpilibj.command.Scheduler;
|
||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
* functions corresponding to each mode, as described in the IterativeRobot
|
||||
* documentation. If you change the name of this class or the package after
|
||||
* creating this project, you must also update the manifest file in the resource
|
||||
* directory.
|
||||
*/
|
||||
public class Robot extends IterativeRobot {
|
||||
public static OI oi;
|
||||
Command autonomousCommand;
|
||||
SendableChooser chooser;
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
* used for any initialization code.
|
||||
*/
|
||||
// CameraServer cameraServer;
|
||||
public void robotInit() {
|
||||
CommandBase.init();
|
||||
oi = new OI();
|
||||
chooser = new SendableChooser();
|
||||
// chooser.addObject("My Auto", new MyAutoCommand());
|
||||
// cameraServer = CameraServer.getInstance();
|
||||
// cameraServer.setQuality(50);
|
||||
// cameraServer.startAutomaticCapture("cam0");
|
||||
try {
|
||||
new ProcessBuilder("/home/lvuser/grip").inheritIO().start();
|
||||
} catch (Exception e) {
|
||||
System.out.println("Error starting GRIP");
|
||||
}
|
||||
chooser.addDefault("Nothing", new RoutineNothing());
|
||||
chooser.addObject("Time based low bar", new RoutineDriveTime());
|
||||
chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime());
|
||||
chooser.addObject("Distance based low bar straight", new RoutineDriveStraightDistance());
|
||||
chooser.addObject("Time based defense", new RoutineDefenseTime());
|
||||
chooser.addObject("Shot Auto", new RoutineShoot());
|
||||
chooser.addObject("Two Ball Drop off", new RoutineTwoBallDropOff());
|
||||
SmartDashboard.putData("Auto mode", chooser);
|
||||
SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
|
||||
SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
|
||||
SmartDashboard.putData("gyroPID", CommandBase.driveBase.getGyroController());
|
||||
SmartDashboard.putNumber("GyroCorrection", 0.13);
|
||||
SmartDashboard.putBoolean("CompressorEnabled", true);
|
||||
//Automatically determine if rolling in or rolling out
|
||||
SmartDashboard.putBoolean("SmartRollers", false);
|
||||
//Use the limit swithces on the shooter
|
||||
SmartDashboard.putBoolean("UseLimitSwitches", true);
|
||||
}
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
* the robot is disabled.
|
||||
*/
|
||||
public void disabledInit() {
|
||||
}
|
||||
public void disabledPeriodic() {
|
||||
Scheduler.getInstance().run();
|
||||
}
|
||||
/**
|
||||
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
|
||||
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
|
||||
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
|
||||
* below the Gyro
|
||||
*
|
||||
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
|
||||
* or additional comparisons to the switch structure below with additional strings & commands.
|
||||
*/
|
||||
public void autonomousInit() {
|
||||
autonomousCommand = (Command) chooser.getSelected();
|
||||
/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
|
||||
switch(autoSelected) {
|
||||
case "My Auto":
|
||||
autonomousCommand = new MyAutoCommand();
|
||||
break;
|
||||
case "Default Auto":
|
||||
default:
|
||||
autonomousCommand = new ExampleCommand();
|
||||
break;
|
||||
} */
|
||||
// schedule the autonomous command (example)
|
||||
if (autonomousCommand != null) {
|
||||
autonomousCommand.start();
|
||||
}
|
||||
}
|
||||
/**
|
||||
* This function is called periodically during autonomous
|
||||
*/
|
||||
public void autonomousPeriodic() {
|
||||
Scheduler.getInstance().run();
|
||||
SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
|
||||
SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
|
||||
SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
|
||||
SmartDashboard.putNumber("horizontalError", CommandBase.visionHelper.getHorizontalError());
|
||||
SmartDashboard.putNumber("verticalError", CommandBase.visionHelper.getVerticalError());
|
||||
SmartDashboard.putNumber("horizontalErrorCorrected", CommandBase.visionHelper.getHorizontalError() + 5);
|
||||
}
|
||||
public void teleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (autonomousCommand != null) {
|
||||
autonomousCommand.cancel();
|
||||
}
|
||||
CommandBase.pneumatics.setCompressorEnabled(true);
|
||||
CommandBase.pneumatics.setArmStopState(false);
|
||||
}
|
||||
/**
|
||||
* This function is called periodically during operator control
|
||||
*/
|
||||
public void teleopPeriodic() {
|
||||
Scheduler.getInstance().run();
|
||||
SmartDashboard.putNumber("GyroAngle", CommandBase.driveBase.getGyro().getAngle());
|
||||
SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
|
||||
SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
|
||||
SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
|
||||
SmartDashboard.putNumber("centerX", CommandBase.visionHelper.getCenterX());
|
||||
SmartDashboard.putNumber("centerContoursX", CommandBase.visionHelper.getCenterContourX());
|
||||
SmartDashboard.putNumber("horizontalError", CommandBase.visionHelper.getHorizontalError());
|
||||
SmartDashboard.putNumber("horizontalErrorCorrected", CommandBase.visionHelper.getHorizontalError() + 5);
|
||||
SmartDashboard.putNumber("sonarDistance", CommandBase.mainArm.getSonarDistance());
|
||||
SmartDashboard.putNumber("sonarDistanceRaw", CommandBase.mainArm.getSonarDistanceRaw());
|
||||
SmartDashboard.putNumber("shooterAngleError", CommandBase.mainArm.getShooterAngleError());
|
||||
SmartDashboard.putNumber("shooterAngleErrorFixed", CommandBase.mainArm.getShooterAngleError()+50);
|
||||
SmartDashboard.putNumber("goalDistance", 54 / Math.tan((Math.PI/180) * (60 - CommandBase.visionHelper.getVerticalError())));
|
||||
if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
|
||||
CommandBase.pneumatics.setArmStopState(true);
|
||||
} else {
|
||||
CommandBase.pneumatics.setArmStopState(false);
|
||||
}
|
||||
CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled"));
|
||||
}
|
||||
/**
|
||||
* This function is called periodically during test mode
|
||||
*/
|
||||
public void testPeriodic() {
|
||||
LiveWindow.run();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
||||
// vim: sw=2:ts=2:sts=2
|
||||
|
@ -3,16 +3,19 @@ public class RobotMap {
|
||||
//Drive
|
||||
public static int driveLeftMotorOne = 1;
|
||||
public static int driveLeftMotorTwo = 2;
|
||||
public static int driveRightMotorOne = 1;
|
||||
public static int driveRightMotorTwo = 2;
|
||||
public static int driveRightEncoder = 0;
|
||||
public static int driveLeftEncoder = 1;
|
||||
public static int driveRightMotorOne = 3;
|
||||
public static int driveRightMotorTwo = 4;
|
||||
public static int driveLeftEncoderA = 0;
|
||||
public static int driveLeftEncoderB = 1;
|
||||
public static int gyro = 1;
|
||||
//Arm
|
||||
public static double zeroDegrees = 0.1;
|
||||
public static double ninetyDegrees = 0.7;
|
||||
public static int armPot = 0;
|
||||
public static double zeroDegrees = 1.28;
|
||||
public static double ninetyDegrees = 3.1;
|
||||
public static int armPot = 3;
|
||||
public static int armLeftMotor = 5;
|
||||
public static int armRightMotor = 6;
|
||||
public static int armBottomLim = 4;
|
||||
public static int armTopLim = 5;
|
||||
//Shooter
|
||||
public static int shooterLeftMotor = 7;
|
||||
public static int shooterRightMotor = 8;
|
||||
@ -24,10 +27,22 @@ public class RobotMap {
|
||||
public static int portcullisSolenoidTwo = 3;
|
||||
public static int armStopSolenoidOne = 4;
|
||||
public static int armStopSolenoidTwo = 5;
|
||||
//Camera
|
||||
//public static double fWidth = 2.8;
|
||||
// Camera: Axis M1013
|
||||
// FOV = 67deg
|
||||
// f = pixelSize/(2*tan(FOV/2))
|
||||
public static double imageWidth = 320;
|
||||
public static double imageHeight = 240;
|
||||
public static double fWidth = 483.467261958;
|
||||
public static double fHeight = 362.600446468;
|
||||
public static int sonar = 2;
|
||||
public static int goalHeight = 85;
|
||||
//Misc
|
||||
public static int mainArmPresetCollect = 0;
|
||||
public static int mainArmPresetTraverse = 5;
|
||||
public static int mainArmPresetCloseShot = 95;
|
||||
public static int mainArmPresetFarShot = 85;
|
||||
public static int mainArmPresetCollect = -5;
|
||||
public static int mainArmPresetTraverse = 10;
|
||||
public static int mainArmPresetLowShot = 18;
|
||||
public static int mainArmPresetCloseShot = 90;
|
||||
public static int mainArmPresetFarShot = 70;
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
||||
// vim: sw=2:ts=2:sts=2
|
||||
|
Binary file not shown.
BIN
src/org/usfirst/frc/team2059/robot/commands/.Drive.java.swp
Normal file
BIN
src/org/usfirst/frc/team2059/robot/commands/.Drive.java.swp
Normal file
Binary file not shown.
BIN
src/org/usfirst/frc/team2059/robot/commands/.MoveArm.java.swp
Normal file
BIN
src/org/usfirst/frc/team2059/robot/commands/.MoveArm.java.swp
Normal file
Binary file not shown.
25
src/org/usfirst/frc/team2059/robot/commands/CommandBase.java
Normal file
25
src/org/usfirst/frc/team2059/robot/commands/CommandBase.java
Normal file
@ -0,0 +1,25 @@
|
||||
package org.usfirst.frc.team2059.robot.commands;
|
||||
import org.usfirst.frc.team2059.robot.subsystems.DriveBase;
|
||||
import org.usfirst.frc.team2059.robot.subsystems.EncoderBase;
|
||||
import org.usfirst.frc.team2059.robot.subsystems.MainArm;
|
||||
import org.usfirst.frc.team2059.robot.subsystems.Pneumatics;
|
||||
import org.usfirst.frc.team2059.robot.subsystems.Shooter;
|
||||
import org.usfirst.frc.team2059.robot.subsystems.VisionHelper;
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
public abstract class CommandBase extends Command {
|
||||
public static EncoderBase encoderBase;
|
||||
public static DriveBase driveBase;
|
||||
public static MainArm mainArm;
|
||||
public static Pneumatics pneumatics;
|
||||
public static Shooter shooter;
|
||||
public static VisionHelper visionHelper;
|
||||
public static void init() {
|
||||
encoderBase = new EncoderBase();
|
||||
driveBase = new DriveBase();
|
||||
mainArm = new MainArm();
|
||||
pneumatics = new Pneumatics();
|
||||
shooter = new Shooter();
|
||||
visionHelper = new VisionHelper();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,40 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
public class AutoDrive extends CommandBase {
|
||||
private double distance;
|
||||
// Determines if we should start driving
|
||||
// Will be false if we already started driving
|
||||
private boolean startDriving = true;
|
||||
public AutoDrive(double distance, double timeout) {
|
||||
requires(driveBase);
|
||||
this.distance = distance;
|
||||
setTimeout(timeout);
|
||||
}
|
||||
public AutoDrive(double distance) {
|
||||
requires(driveBase);
|
||||
this.distance = distance;
|
||||
// Make the default timeout 2s
|
||||
setTimeout(2.0d);
|
||||
}
|
||||
protected void initialize() {
|
||||
}
|
||||
protected void execute() {
|
||||
if (startDriving) {
|
||||
driveBase.pidDrive(distance, SmartDashboard.getNumber("GyroCorrection"));
|
||||
}
|
||||
startDriving = false;
|
||||
}
|
||||
protected void end() {
|
||||
startDriving = true;
|
||||
driveBase.stopDriving();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return isTimedOut();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,30 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
public class AutoDriveStraightDistance extends CommandBase {
|
||||
double distance;
|
||||
public AutoDriveStraightDistance(double distance, double timeout) {
|
||||
requires(driveBase);
|
||||
setTimeout(timeout);
|
||||
this.distance = distance;
|
||||
}
|
||||
protected void initialize() {
|
||||
driveBase.resetGyro();
|
||||
driveBase.getLeftEncoder().reset();
|
||||
}
|
||||
protected void execute() {
|
||||
driveBase.pidDrive(distance, SmartDashboard.getNumber("GyroCorrection"));
|
||||
}
|
||||
protected void end() {
|
||||
driveBase.getLeftController().disable();
|
||||
driveBase.stopDriving();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return isTimedOut() || Math.abs(distance - driveBase.getLeftRotations()) <= 3;
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,28 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
public class AutoDriveStraightTime extends CommandBase {
|
||||
double speed;
|
||||
public AutoDriveStraightTime(double speed, double time) {
|
||||
requires(driveBase);
|
||||
setTimeout(time);
|
||||
this.speed = speed;
|
||||
}
|
||||
protected void initialize() {
|
||||
driveBase.resetGyro();
|
||||
}
|
||||
protected void execute() {
|
||||
driveBase.driveStraight(-speed, SmartDashboard.getNumber("GyroCorrection"));
|
||||
}
|
||||
protected void end() {
|
||||
driveBase.stopDriving();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return isTimedOut();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,26 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class AutoDriveTime extends CommandBase {
|
||||
double power;
|
||||
public AutoDriveTime(double timeout, double power) {
|
||||
requires(driveBase);
|
||||
this.power = power;
|
||||
setTimeout(timeout);
|
||||
}
|
||||
protected void initialize() {
|
||||
}
|
||||
protected void execute() {
|
||||
driveBase.driveArcade(0, -power, 0, 1);
|
||||
}
|
||||
protected void end() {
|
||||
driveBase.stopDriving();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return isTimedOut();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,38 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AutoMoveArm extends CommandBase {
|
||||
double speed;
|
||||
public AutoMoveArm(double s, double timeout) {
|
||||
requires(mainArm);
|
||||
speed = s;
|
||||
setTimeout(timeout);
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
mainArm.disable();
|
||||
mainArm.moveArm(speed);
|
||||
System.out.println(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return isTimedOut();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.moveArm(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,37 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AutoResetLower extends CommandBase {
|
||||
double speed;
|
||||
public AutoResetLower(double s) {
|
||||
requires(mainArm);
|
||||
speed = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
System.out.println("test");
|
||||
mainArm.disable();
|
||||
mainArm.resetLower(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
// Stop when bottom limit switch is hit
|
||||
return mainArm.getBottomPressed();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.moveArm(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,31 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
public class AutoRotate extends CommandBase {
|
||||
double degrees;
|
||||
public AutoRotate(double degrees) {
|
||||
requires(driveBase);
|
||||
this.degrees = degrees;
|
||||
setTimeout(2.5);
|
||||
}
|
||||
protected void initialize() {
|
||||
driveBase.getGyro().reset();
|
||||
}
|
||||
protected void execute() {
|
||||
System.out.println("rotating");
|
||||
driveBase.rotateAngle(degrees);
|
||||
}
|
||||
protected void end() {
|
||||
driveBase.stopDriving();
|
||||
driveBase.getGyroController().disable();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
//return Math.abs(driveBase.getGyro().getAngle()-degrees) <= 0.5;
|
||||
return isTimedOut();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,43 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AutoSetArmPosition extends CommandBase {
|
||||
double pos;
|
||||
public AutoSetArmPosition(double p) {
|
||||
requires(mainArm);
|
||||
pos = p;
|
||||
setTimeout(3);
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
mainArm.enable();
|
||||
mainArm.setSetpoint(pos);
|
||||
System.out.println(pos - mainArm.getDegrees());
|
||||
System.out.println(mainArm.getDegrees());
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
// Stop when either limit switch is hit
|
||||
System.out.println("AutoSetArmPosition.isFinished(): " + (Math.abs(pos - mainArm.getDegrees()) <= 1));
|
||||
System.out.println(" pos : " + pos);
|
||||
System.out.println(" mainArm.getDegrees() : " + mainArm.getDegrees());
|
||||
return (Math.abs(pos - mainArm.getDegrees()) <= 1) || isTimedOut();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.disable();
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,32 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AutoSetArmStopState extends CommandBase {
|
||||
boolean state;
|
||||
public AutoSetArmStopState(boolean s) {
|
||||
state = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
pneumatics.setArmStopState(state);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,32 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AutoSetShooterState extends CommandBase {
|
||||
boolean state;
|
||||
public AutoSetShooterState(boolean s) {
|
||||
state = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
pneumatics.setShooterState(state);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,36 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AutoSpinRollers extends CommandBase {
|
||||
double speed;
|
||||
boolean smartrollers;
|
||||
public AutoSpinRollers(double s, double timeout) {
|
||||
speed = s;
|
||||
setTimeout(timeout);
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
shooter.shootAtSpeed(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return isTimedOut();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
shooter.shootAtSpeed(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,11 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineDefenseTime extends CommandGroup {
|
||||
public RoutineDefenseTime() {
|
||||
addSequential(new AutoSetArmStopState(false));
|
||||
addSequential(new AutoSetArmPosition(6));
|
||||
addSequential(new AutoDriveTime(2.5, 0.8));
|
||||
}
|
||||
}
|
@ -0,0 +1,11 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineDriveStraightDistance extends CommandGroup {
|
||||
public RoutineDriveStraightDistance() {
|
||||
addSequential(new AutoSetArmStopState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoDriveStraightDistance(200, 5));
|
||||
}
|
||||
}
|
@ -0,0 +1,11 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineDriveStraightTime extends CommandGroup {
|
||||
public RoutineDriveStraightTime() {
|
||||
addSequential(new AutoSetArmStopState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoDriveStraightTime(0.5, 1));
|
||||
}
|
||||
}
|
@ -0,0 +1,11 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineDriveTime extends CommandGroup {
|
||||
public RoutineDriveTime() {
|
||||
addSequential(new AutoSetArmStopState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoDriveTime(2.5, 0.75));
|
||||
}
|
||||
}
|
@ -0,0 +1,8 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineNothing extends CommandGroup {
|
||||
public RoutineNothing() {
|
||||
}
|
||||
}
|
@ -0,0 +1,15 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineRaiseArmFire extends CommandGroup {
|
||||
public RoutineRaiseArmFire() {
|
||||
addSequential(new AutoSetArmPosition(90));
|
||||
//Raise the arm a little before spinning rollers
|
||||
try {
|
||||
wait(2);
|
||||
} catch (Exception e) { }
|
||||
addParallel(new AutoSpinRollers(0.5, 5));
|
||||
addSequential(new AutoSetShooterState(true));
|
||||
}
|
||||
}
|
@ -0,0 +1,15 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineShoot extends CommandGroup {
|
||||
public RoutineShoot() {
|
||||
addSequential(new AutoSetArmStopState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoDriveStraightTime(0.5, 1));
|
||||
addSequential(new AutoSetArmPosition(90));
|
||||
//addSequential(new AutoRotate(45));
|
||||
//addSequential(new AutoDriveStraightTime(0.4, 1.5));
|
||||
addSequential(new VisionAlignHorizontal());
|
||||
}
|
||||
}
|
@ -0,0 +1,26 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineTwoBallDropOff extends CommandGroup {
|
||||
public RoutineTwoBallDropOff() {
|
||||
addSequential(new AutoSetArmStopState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoDriveStraightTime(0.5, 2));
|
||||
addSequential(new AutoMoveArm(0.5, 1));
|
||||
addSequential(new AutoSpinRollers(1, 2));
|
||||
addSequential(new AutoSetShooterState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoRotate(180));
|
||||
addSequential(new AutoSetShooterState(false));
|
||||
addParallel(new AutoSpinRollers(-0.5, 3));
|
||||
addSequential(new AutoDriveStraightTime(0.5, 2));
|
||||
addSequential(new AutoRotate(180));
|
||||
addSequential(new AutoDriveStraightTime(0.5, 2));
|
||||
addSequential(new AutoMoveArm(0.5, 1));
|
||||
addSequential(new AutoSpinRollers(1, 2));
|
||||
addSequential(new AutoSetShooterState(true));
|
||||
addSequential(new AutoRotate(180));
|
||||
addSequential(new AutoSetShooterState(false));
|
||||
}
|
||||
}
|
@ -0,0 +1,29 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.autonomous;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
public class VisionAlignHorizontal extends CommandBase {
|
||||
double error;
|
||||
public VisionAlignHorizontal() {
|
||||
}
|
||||
protected void initialize() {
|
||||
error = visionHelper.getHorizontalError();
|
||||
driveBase.resetGyro();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
protected void execute() {
|
||||
if (visionHelper.getHorizontalError() == 420) {
|
||||
driveBase.resetGyro();
|
||||
driveBase.rotateAngle(5);
|
||||
} else {
|
||||
// driveBase.rotateAngle(visionHelper.getHorizontalError());
|
||||
}
|
||||
}
|
||||
protected void end() {
|
||||
driveBase.getGyroController().disable();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,39 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.drivetrain;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class Drive extends CommandBase {
|
||||
double sensitivity = 0.5;
|
||||
public Drive() {
|
||||
requires(driveBase);
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
double x = Robot.oi.getJoysticks()[0].getRawAxis(0);
|
||||
double y = Robot.oi.getJoysticks()[0].getRawAxis(1);
|
||||
double z = Robot.oi.getJoysticks()[0].getRawAxis(2);
|
||||
if (Robot.oi.getJoysticks()[0].getRawButton(2) || Robot.oi.getJoysticks()[0].getRawButton(11)) {
|
||||
sensitivity = 1;
|
||||
} else {
|
||||
sensitivity = 0.7;
|
||||
}
|
||||
driveBase.driveArcade(x, y, z, sensitivity);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,36 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.drivetrain;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class DriveStraight extends CommandBase {
|
||||
double speed;
|
||||
public DriveStraight(double speed) {
|
||||
requires(driveBase);
|
||||
this.speed = speed;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
driveBase.resetGyro();
|
||||
driveBase.driveStraight(speed, SmartDashboard.getNumber("GyroCorrection"));
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
driveBase.driveStraight(0, SmartDashboard.getNumber("GyroCorrection"));
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,28 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.drivetrain;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.structs.EncoderValues;
|
||||
public class LogEncoder extends CommandBase {
|
||||
public LogEncoder() {
|
||||
requires(encoderBase);
|
||||
}
|
||||
protected void initialize() {
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
EncoderValues values = encoderBase.getValues();
|
||||
System.out.println("==== Encoder log ====");
|
||||
System.out.println("Count : " + values.getCount());
|
||||
System.out.println("Distance : " + values.getDistance());
|
||||
System.out.println("Period : " + values.getPeriod());
|
||||
System.out.println("Rate : " + values.getRate());
|
||||
System.out.println("Direction : " + values.getDirection());
|
||||
System.out.println("Stopped : " + values.getStopped());
|
||||
return true;
|
||||
}
|
||||
protected void execute() {
|
||||
}
|
||||
protected void end() {
|
||||
}
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,33 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.drivetrain;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class PIDDrive extends CommandBase {
|
||||
double count;
|
||||
public PIDDrive(double c) {
|
||||
requires(driveBase);
|
||||
count = c;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
driveBase.pidDrive(count, SmartDashboard.getNumber("GyroCorrection"));
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,20 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.drivetrain;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
public class ResetEncoder extends CommandBase {
|
||||
public ResetEncoder() {
|
||||
requires(encoderBase);
|
||||
}
|
||||
protected void initialize() {
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
protected void execute() {
|
||||
encoderBase.resetEncoder();
|
||||
}
|
||||
protected void end() {
|
||||
}
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,35 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class AlignVertical extends CommandBase {
|
||||
public AlignVertical() {
|
||||
requires(mainArm);
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
mainArm.setSetpoint(mainArm.getShooterAngleError()+50);
|
||||
mainArm.enable();
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
// Stop when either limit switch is hit
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.disable();
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,38 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class MoveArm extends CommandBase {
|
||||
double speed;
|
||||
public MoveArm(double s) {
|
||||
requires(mainArm);
|
||||
speed = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
mainArm.disable();
|
||||
mainArm.moveArm(speed);
|
||||
System.out.println(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
//TODO: I don't know if this should go here
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.moveArm(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,36 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ResetLower extends CommandBase {
|
||||
double speed;
|
||||
public ResetLower(double s) {
|
||||
requires(mainArm);
|
||||
speed = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
mainArm.disable();
|
||||
mainArm.resetLower(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
// Stop when either limit switch is hit
|
||||
return mainArm.getBottomPressed();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.moveArm(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,37 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ResetUpper extends CommandBase {
|
||||
double speed;
|
||||
public ResetUpper(double s) {
|
||||
requires(mainArm);
|
||||
speed = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
System.out.println("testUpper");
|
||||
mainArm.disable();
|
||||
mainArm.resetUpper(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
// Stop when either limit switch is hit
|
||||
return mainArm.getBottomPressed() || mainArm.getTopPressed();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.moveArm(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,38 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class SetArmPosition extends CommandBase {
|
||||
double pos;
|
||||
public SetArmPosition(double p) {
|
||||
requires(mainArm);
|
||||
pos = p;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
//Move the arm stop
|
||||
mainArm.enable();
|
||||
mainArm.setSetpoint(pos);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
// Stop when either limit switch is hit
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
mainArm.disable();
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -1,16 +1,20 @@
|
||||
package org.usfirst.frc.team2059.robot.commands;
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class Drive extends Command {
|
||||
public Drive() {
|
||||
public class SetArmStopState extends CommandBase {
|
||||
boolean state;
|
||||
public SetArmStopState(boolean s) {
|
||||
state = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
pneumatics.setArmStopState(state);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
@ -18,10 +22,12 @@ public class Drive extends Command {
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
pneumatics.setArmStopState(!state);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,33 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class SetShooterState extends CommandBase {
|
||||
boolean state;
|
||||
public SetShooterState(boolean s) {
|
||||
state = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
pneumatics.setShooterState(state);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
pneumatics.setShooterState(!state);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,34 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ShootAtSpeed extends CommandBase {
|
||||
double speed;
|
||||
public ShootAtSpeed(double s) {
|
||||
speed = s;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
shooter.shootAtSpeed(speed);
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
shooter.shootAtSpeed(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,40 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.shooter;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
import org.usfirst.frc.team2059.robot.Robot;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class SpinRollers extends CommandBase {
|
||||
double speed;
|
||||
boolean smartrollers;
|
||||
public SpinRollers(double s, boolean sr) {
|
||||
speed = s;
|
||||
smartrollers = sr;
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
}
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
if (smartrollers && (mainArm.getDegrees() < 5 || mainArm.getBottomPressed())) {
|
||||
shooter.shootAtSpeed(-.5);
|
||||
} else {
|
||||
shooter.shootAtSpeed(speed);
|
||||
}
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
shooter.shootAtSpeed(0);
|
||||
}
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,31 @@
|
||||
package org.usfirst.frc.team2059.robot.commands.visionhelper;
|
||||
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||
public class AlignHorizontal extends CommandBase {
|
||||
double error;
|
||||
public AlignHorizontal() {
|
||||
}
|
||||
protected void initialize() {
|
||||
error = visionHelper.getHorizontalError();
|
||||
driveBase.setDriveEnabled(false);
|
||||
driveBase.resetGyro();
|
||||
mainArm.setSetpoint(110);
|
||||
mainArm.enable();
|
||||
}
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
protected void execute() {
|
||||
if(mainArm.getDegrees() >=45){
|
||||
driveBase.rotateAngle(error);
|
||||
}
|
||||
}
|
||||
protected void end() {
|
||||
driveBase.getGyroController().disable();
|
||||
driveBase.setDriveEnabled(true);
|
||||
mainArm.disable();
|
||||
}
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,33 @@
|
||||
package org.usfirst.frc.team2059.robot.structs;
|
||||
public class EncoderValues {
|
||||
private int count;
|
||||
private double distance, period, rate;
|
||||
private boolean direction, stopped;
|
||||
public EncoderValues(int count, double distance, double period, double rate, boolean direction, boolean stopped) {
|
||||
this.count = count;
|
||||
this.distance = distance;
|
||||
this.period = period;
|
||||
this.rate = rate;
|
||||
this.direction = direction;
|
||||
this.stopped = stopped;
|
||||
}
|
||||
public int getCount() {
|
||||
return count;
|
||||
}
|
||||
public double getDistance() {
|
||||
return distance;
|
||||
}
|
||||
public double getPeriod() {
|
||||
return period;
|
||||
}
|
||||
public double getRate() {
|
||||
return rate;
|
||||
}
|
||||
public boolean getDirection() {
|
||||
return direction;
|
||||
}
|
||||
public boolean getStopped() {
|
||||
return stopped;
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
Binary file not shown.
BIN
src/org/usfirst/frc/team2059/robot/subsystems/.MainArm.java.swp
Normal file
BIN
src/org/usfirst/frc/team2059/robot/subsystems/.MainArm.java.swp
Normal file
Binary file not shown.
109
src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
Normal file
109
src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
Normal file
@ -0,0 +1,109 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.PIDController;
|
||||
import edu.wpi.first.wpilibj.PIDOutput;
|
||||
import edu.wpi.first.wpilibj.PIDSource;
|
||||
import edu.wpi.first.wpilibj.PIDSourceType;
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
public class DriveBase extends Subsystem {
|
||||
CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne);
|
||||
CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo);
|
||||
CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne);
|
||||
CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo);
|
||||
Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k4X);
|
||||
AnalogGyro gyro = new AnalogGyro(RobotMap.gyro);
|
||||
PIDController gyroController = new PIDController(0.07, 0.002, 0.08, new GyroPIDSource(), new MotorsPIDOutputReversed());
|
||||
PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
|
||||
double encoderGyroCorrection = 1;
|
||||
boolean driveEnabled = true;
|
||||
public void initDefaultCommand() {
|
||||
setDefaultCommand(new Drive());
|
||||
}
|
||||
public void stopDriving() {
|
||||
leftMotorOne.set(0);
|
||||
leftMotorTwo.set(0);
|
||||
rightMotorOne.set(0);
|
||||
rightMotorTwo.set(0);
|
||||
}
|
||||
public void driveArcade(double x, double y, double z, double sensitivity) {
|
||||
if(driveEnabled){
|
||||
leftMotorOne.set((-y + (x + z)) * sensitivity);
|
||||
leftMotorTwo.set((-y + (x + z)) * sensitivity);
|
||||
rightMotorOne.set((y + (x + z)) * sensitivity);
|
||||
rightMotorTwo.set((y + (x + z)) * sensitivity);
|
||||
}
|
||||
}
|
||||
public void driveStraight(double y, double correction) {
|
||||
SmartDashboard.putNumber("GyroAngle", gyro.getAngle());
|
||||
leftMotorOne.set((-y + -gyro.getAngle() * correction));
|
||||
leftMotorTwo.set((-y + -gyro.getAngle() * correction));
|
||||
rightMotorOne.set((y + -gyro.getAngle() * correction));
|
||||
rightMotorTwo.set((y + -gyro.getAngle() * correction));
|
||||
}
|
||||
public void pidDrive(double setpoint, double correction) {
|
||||
leftEncoder.reset();
|
||||
leftEncoderController.enable();
|
||||
leftEncoderController.setSetpoint(setpoint);
|
||||
encoderGyroCorrection = -gyro.getAngle() * correction;
|
||||
}
|
||||
public void rotateAngle(double angle) {
|
||||
SmartDashboard.putNumber("GyroAngle", gyro.getAngle());
|
||||
gyroController.enable();
|
||||
gyroController.setSetpoint(angle);
|
||||
}
|
||||
public void setDriveEnabled(boolean enabled){
|
||||
driveEnabled = enabled;
|
||||
}
|
||||
public PIDController getLeftController() {
|
||||
return leftEncoderController;
|
||||
}
|
||||
public PIDController getGyroController() {
|
||||
return gyroController;
|
||||
}
|
||||
public double getLeftRotations() {
|
||||
return leftEncoder.get();
|
||||
}
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
}
|
||||
public Encoder getLeftEncoder() {
|
||||
return leftEncoder;
|
||||
}
|
||||
public AnalogGyro getGyro() {
|
||||
return gyro;
|
||||
}
|
||||
public class MotorsPIDOutput implements PIDOutput {
|
||||
@Override
|
||||
public void pidWrite(double output) {
|
||||
leftMotorOne.pidWrite(output + -encoderGyroCorrection);
|
||||
leftMotorTwo.pidWrite(output + -encoderGyroCorrection);
|
||||
rightMotorOne.pidWrite(-output + -encoderGyroCorrection);
|
||||
rightMotorTwo.pidWrite(-output + -encoderGyroCorrection);
|
||||
}
|
||||
}
|
||||
public class MotorsPIDOutputReversed implements PIDOutput {
|
||||
@Override
|
||||
public void pidWrite(double output) {
|
||||
leftMotorOne.pidWrite(output);
|
||||
leftMotorTwo.pidWrite(output);
|
||||
rightMotorOne.pidWrite(output);
|
||||
rightMotorTwo.pidWrite(output);
|
||||
}
|
||||
}
|
||||
public class GyroPIDSource implements PIDSource {
|
||||
public double pidGet() {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
public PIDSourceType getPIDSourceType() {
|
||||
return PIDSourceType.kDisplacement;
|
||||
}
|
||||
public void setPIDSourceType(PIDSourceType type) {
|
||||
}
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -1,21 +0,0 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import org.usfirst.frc.team2059.robot.commands.Drive;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
public class Drivebase extends Subsystem {
|
||||
CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne);
|
||||
CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo);
|
||||
CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne);
|
||||
CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorOne);
|
||||
public void initDefaultCommand() {
|
||||
setDefaultCommand(new Drive());
|
||||
}
|
||||
public void driveArcade(double x, double y, double z, double sensitivity) {
|
||||
leftMotorOne.set(-y + (x + z));
|
||||
leftMotorTwo.set(-y + (x + z));
|
||||
rightMotorOne.set(y + (x + z));
|
||||
rightMotorTwo.set(y + (x + z));
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,33 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import org.usfirst.frc.team2059.robot.structs.EncoderValues;
|
||||
public class EncoderBase extends Subsystem {
|
||||
//TODO: Might not need a constructor
|
||||
public EncoderBase() {
|
||||
}
|
||||
//Encoder enc = new Encoder(0, 1, false, Encoder.EncodingType.k1X);
|
||||
Encoder enc = new Encoder(8, 9);
|
||||
public void initDefaultCommand() {
|
||||
//TODO: Not sure if we need a default command, not setting one
|
||||
//TODO: These are just defaults, they wil lneed to be changed
|
||||
//enc.setMaxPeriod(.1);
|
||||
//enc.setMinRate(10);
|
||||
//enc.setDistancePerPulse(5);
|
||||
//enc.setReverseDirection(false);
|
||||
//enc.setSamplesToAverage(7);
|
||||
}
|
||||
public void resetEncoder() {
|
||||
enc.reset();
|
||||
}
|
||||
public EncoderValues getValues() {
|
||||
//TODO: There are two ways to get distance:
|
||||
//enc.getDistance();
|
||||
//enc.getRaw();
|
||||
//figure out which to use
|
||||
return new EncoderValues(enc.get() , enc.getRaw() , enc.getPeriod() , enc.getRate() , enc.getDirection() , enc.getStopped());
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
104
src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java
Normal file
104
src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java
Normal file
@ -0,0 +1,104 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
|
||||
import edu.wpi.first.wpilibj.command.PIDSubsystem;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
import edu.wpi.first.wpilibj.AnalogInput;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
public class MainArm extends PIDSubsystem {
|
||||
CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor);
|
||||
CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor);
|
||||
AnalogInput pot = new AnalogInput(RobotMap.armPot);
|
||||
AnalogInput sonar = new AnalogInput(RobotMap.sonar);
|
||||
DigitalInput limitSwitchBottom = new DigitalInput(RobotMap.armBottomLim);
|
||||
DigitalInput limitSwitchTop = new DigitalInput(RobotMap.armTopLim);
|
||||
private double min = RobotMap.zeroDegrees;
|
||||
private double max = RobotMap.ninetyDegrees;
|
||||
public MainArm() {
|
||||
super("MainArm", 0.06, 0.0, 0.002);
|
||||
getPIDController().setContinuous(false);
|
||||
setSetpoint(70);
|
||||
// enable();
|
||||
disable();
|
||||
}
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
public void moveArm(double speed) {
|
||||
// Calibrate the arm, but don't do anything about it
|
||||
calibrate();
|
||||
System.out.println(getTopPressed());
|
||||
armMotorLeft.set(-speed);
|
||||
armMotorRight.set(speed);
|
||||
}
|
||||
protected double returnPIDInput() {
|
||||
return getDegrees();
|
||||
}
|
||||
protected void usePIDOutput(double output) {
|
||||
moveArm(output);
|
||||
}
|
||||
public double getRaw() {
|
||||
return pot.getAverageVoltage();
|
||||
}
|
||||
public double getDegrees() {
|
||||
return potToDegrees(getRaw());
|
||||
}
|
||||
public void resetLower(double speed) {
|
||||
if (getBottomPressed()) {
|
||||
moveArm(0);
|
||||
return;
|
||||
} else {
|
||||
moveArm(speed);
|
||||
}
|
||||
}
|
||||
public boolean getBottomPressed() {
|
||||
return !limitSwitchBottom.get();
|
||||
}
|
||||
public void resetUpper(double speed) {
|
||||
if (getTopPressed()) {
|
||||
moveArm(0);
|
||||
return;
|
||||
} else {
|
||||
moveArm(speed);
|
||||
}
|
||||
}
|
||||
public boolean getTopPressed() {
|
||||
return !limitSwitchTop.get();
|
||||
}
|
||||
private double potToDegrees(double pot) {
|
||||
//System.out.println((pot - min) / (Math.abs(min - max) / 90));
|
||||
return (pot - min) / (Math.abs(min - max) / 90);
|
||||
}
|
||||
/**
|
||||
* Calibrates the arm if it hits the upper or lower limit switch
|
||||
* @return True if it was calibrated (if either limit switch was pressed)
|
||||
*/
|
||||
private boolean calibrate() {
|
||||
// It can't be calibrated if the limit swithces are disabled
|
||||
//if (!SmartDashboard.getBoolean("UseLimitSwitches")) {
|
||||
// return false;
|
||||
//}
|
||||
if (getBottomPressed()) {
|
||||
System.out.println("Calibrating bottom to: " + getRaw());
|
||||
min = getRaw();
|
||||
return true;
|
||||
}
|
||||
// } else if (getTopPressed()) {
|
||||
// System.out.println("Calibrating top to: " + getRaw());
|
||||
// max = getRaw();
|
||||
// return true;
|
||||
// }
|
||||
return false;
|
||||
}
|
||||
public double getShooterAngleError(){
|
||||
double correctedHeight = (RobotMap.goalHeight + 16) - 44; //44 is the distance between the ground and the sonar
|
||||
return (180/Math.PI) * Math.atan(correctedHeight/(getSonarDistance()-1));
|
||||
}
|
||||
public double getSonarDistanceRaw(){
|
||||
return sonar.getVoltage();
|
||||
}
|
||||
public double getSonarDistance(){
|
||||
return (getSonarDistanceRaw()/0.000976562f)/25.4f;
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,43 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.Compressor;
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
public class Pneumatics extends Subsystem {
|
||||
Compressor compressor = new Compressor(RobotMap.pcmID);
|
||||
DoubleSolenoid shooterSolenoid = new DoubleSolenoid(RobotMap.pcmID, RobotMap.shooterSolenoidOne, RobotMap.shooterSolenoidTwo);
|
||||
DoubleSolenoid armStopSolenoid = new DoubleSolenoid(RobotMap.pcmID, RobotMap.armStopSolenoidOne, RobotMap.armStopSolenoidTwo);
|
||||
boolean shooterState, armStopState;
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
public void setCompressorEnabled(boolean state) {
|
||||
compressor.setClosedLoopControl(state);
|
||||
}
|
||||
public void setShooterState(boolean state) {
|
||||
if (state) {
|
||||
shooterSolenoid.set(DoubleSolenoid.Value.kForward);
|
||||
} else {
|
||||
shooterSolenoid.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
shooterState = state;
|
||||
}
|
||||
public void setArmStopState(boolean state) {
|
||||
if (state) {
|
||||
armStopSolenoid.set(DoubleSolenoid.Value.kForward);
|
||||
} else {
|
||||
armStopSolenoid.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
armStopState = state;
|
||||
}
|
||||
public boolean getShooterState() {
|
||||
return shooterState;
|
||||
}
|
||||
public boolean getArmStopState() {
|
||||
return armStopState;
|
||||
}
|
||||
public boolean getCompressorEnabled() {
|
||||
return compressor.enabled();
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
15
src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java
Normal file
15
src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java
Normal file
@ -0,0 +1,15 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
public class Shooter extends Subsystem {
|
||||
CANTalon shooterMotorLeft = new CANTalon(RobotMap.shooterLeftMotor);
|
||||
CANTalon shooterMotorRight = new CANTalon(RobotMap.shooterRightMotor);
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
public void shootAtSpeed(double speed) {
|
||||
shooterMotorRight.set(speed);
|
||||
shooterMotorLeft.set(speed);
|
||||
}
|
||||
}
|
||||
// vim: sw=2:ts=2:sts=2
|
@ -0,0 +1,95 @@
|
||||
package org.usfirst.frc.team2059.robot.subsystems;
|
||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||
import edu.wpi.first.wpilibj.command.PIDSubsystem;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
import edu.wpi.first.wpilibj.AnalogInput;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj.networktables.NetworkTable;
|
||||
public class VisionHelper {
|
||||
NetworkTable linesTable, contoursTable;
|
||||
double imageWidth = 640;
|
||||
double imageHeight = 480;
|
||||
public VisionHelper() {
|
||||
linesTable = NetworkTable.getTable("GRIP/lines");
|
||||
contoursTable = NetworkTable.getTable("GRIP/contours");
|
||||
}
|
||||
public double getCenterContourX(){
|
||||
int highestAreaIndex = 0;
|
||||
int index = 0;
|
||||
double areas[] = linesTable.getNumberArray("area", new double[0]);
|
||||
for(double area : areas){
|
||||
if(area >= areas[highestAreaIndex]){
|
||||
highestAreaIndex=index;
|
||||
}
|
||||
index++;
|
||||
}
|
||||
try{
|
||||
return contoursTable.getNumberArray("centerX",new double[0])[highestAreaIndex];
|
||||
}catch(Exception e){
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
public double getSlope(){
|
||||
int highestLengthIndex = 0;
|
||||
int index = 0;
|
||||
double lengths[] = linesTable.getNumberArray("length", new double[0]);
|
||||
try{
|
||||
double x1s[] = linesTable.getNumberArray("x1", new double[0]);
|
||||
double y1s[] = linesTable.getNumberArray("y1", new double[0]);
|
||||
double x2s[] = linesTable.getNumberArray("x2", new double[0]);
|
||||
double y2s[] = linesTable.getNumberArray("y2", new double[0]);
|
||||
for(double length : lengths){
|
||||
if(length >= lengths[highestLengthIndex]){
|
||||
highestLengthIndex=index;
|
||||
}
|
||||
index++;
|
||||
}
|
||||
return ((y1s[highestLengthIndex]-y2s[highestLengthIndex])/(x1s[highestLengthIndex]-x2s[highestLengthIndex]));
|
||||
}catch(Exception e){
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
public double getCenterX() {
|
||||
int highestLengthIndex = 0;
|
||||
int index = 0;
|
||||
double lengths[] = linesTable.getNumberArray("length", new double[0]);
|
||||
double x1s[] = linesTable.getNumberArray("x1", new double[0]);
|
||||
double x2s[] = linesTable.getNumberArray("x2", new double[0]);
|
||||
for(double length : lengths){
|
||||
if(length >= lengths[highestLengthIndex]){
|
||||
highestLengthIndex=index;
|
||||
}
|
||||
index++;
|
||||
}
|
||||
try {
|
||||
return (x1s[highestLengthIndex]+x2s[highestLengthIndex])/2;
|
||||
} catch (Exception e) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
public double getCenterY() {
|
||||
int highestLengthIndex = 0;
|
||||
int index = 0;
|
||||
double lengths[] = linesTable.getNumberArray("length", new double[0]);
|
||||
double y1s[] = linesTable.getNumberArray("y1", new double[0]);
|
||||
double y2s[] = linesTable.getNumberArray("y2", new double[0]);
|
||||
for(double length : lengths){
|
||||
if(length >= lengths[highestLengthIndex]){
|
||||
highestLengthIndex=index;
|
||||
}
|
||||
index++;
|
||||
}
|
||||
try {
|
||||
return (y1s[highestLengthIndex]+y2s[highestLengthIndex])/2;
|
||||
} catch (Exception e) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
public double getHorizontalError() {
|
||||
return ((180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)))+3.5;
|
||||
}
|
||||
public double getVerticalError() {
|
||||
return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user