diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 02b2b0c..6af5c4e 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -13,6 +13,7 @@ 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.visionhelper.AlignHorizontal; import org.usfirst.frc.team2059.robot.RobotMap; /** * This class is the glue that binds the controls on the physical operator @@ -34,7 +35,8 @@ public class OI { } // Print log when button 1 pressed //joystickButtons[0][0].whenPressed(new LogEncoder()); - joystickButtons[0][0].whileHeld(new SetShooterState(true)); +// joystickButtons[0][0].whileHeld(new SetShooterState(true)); + joystickButtons[0][0].whileHeld(new AlignHorizontal()); joystickButtons[0][2].whileHeld(new SpinRollers(-0.5, false)); joystickButtons[0][3].whileHeld(new SpinRollers(1, false)); joystickButtons[0][6].whileHeld(new SpinRollers(1, true)); diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index a542436..b81fe82 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -20,6 +20,11 @@ public class Robot extends IterativeRobot { // cameraServer = CameraServer.getInstance(); // cameraServer.setQuality(50); // cameraServer.startAutomaticCapture("cam0"); + try{ + Process grip = Runtime.getRuntime().exec(new String[]{"/usr/local/frc/JRE/bin/java", "-jar", "grip.jar", "project.grip"}); + }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()); @@ -67,7 +72,10 @@ public class Robot extends IterativeRobot { 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("horizontalError",CommandBase.visionHelper.getHorizontalError()); + SmartDashboard.putNumber("verticalError",CommandBase.visionHelper.getVerticalError()); + SmartDashboard.putNumber("horizontalErrorCorrected",CommandBase.visionHelper.getHorizontalError()+5); + SmartDashboard.putNumber("goalDistance",72/Math.tan(0.0175*(35+CommandBase.visionHelper.getVerticalError()))); if (Robot.oi.getJoysticks()[1].getRawButton(3)) { CommandBase.pneumatics.setArmStopState(true); } else { diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java index 8c46fc6..69ca31e 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java @@ -4,17 +4,17 @@ 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)); - addParallel(new AutoSetArmPosition(90)); - addSequential(new AutoRotate(45)); - addSequential(new AutoDriveStraightTime(0.4, 1.5)); - addSequential(new AutoSpinRollers(1, 2)); - addSequential(new AutoSetShooterState(true)); - addSequential(new AutoSetArmStopState(false)); - addParallel(new AutoSetArmPosition(10)); - addSequential(new AutoRotate(180)); - addSequential(new AutoSetShooterState(false)); - } +// addSequential(new AutoSetArmStopState(true)); +// addSequential(new AutoResetLower(-1)); +// addSequential(new AutoDriveStraightTime(0.5, 1)); +// addParallel(new AutoSetArmPosition(90)); +// addSequential(new AutoRotate(45)); +// addSequential(new AutoDriveStraightTime(0.4, 1.5)); +// addSequential(new AutoSpinRollers(1, 2)); +// addSequential(new AutoSetShooterState(true)); +// addSequential(new AutoSetArmStopState(false)); +// addParallel(new AutoSetArmPosition(10)); +// addSequential(new AutoRotate(180)); +// addSequential(new AutoSetShooterState(false)); + addSequential(new VisionAlignHorizontal()); } } diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java new file mode 100644 index 0000000..8643bb5 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java @@ -0,0 +1,24 @@ +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() { + driveBase.rotateAngle(error); + } + protected void end() { + driveBase.getGyroController().disable(); + } + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java b/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java new file mode 100644 index 0000000..d60edd2 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java @@ -0,0 +1,24 @@ +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.resetGyro(); + } + protected boolean isFinished() { + return false; + } + protected void execute() { + driveBase.rotateAngle(error); + } + protected void end() { + driveBase.getGyroController().disable(); + } + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java index 2fef2b5..030bc14 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java @@ -21,7 +21,7 @@ public class DriveBase extends Subsystem { PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput()); double encoderGyroCorrection = 1; public void initDefaultCommand() { - setDefaultCommand(new Drive()); + //setDefaultCommand(new Drive()); } public void stopDriving() { leftMotorOne.set(0); diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java index 2753419..0417a14 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java @@ -6,7 +6,11 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.networktables.NetworkTable; public class VisionHelper { - NetworkTable contoursTable; + NetworkTable contoursTable; + double hfieldOfView=67; + double vfieldOfView=51; + double imageWidth=640; + double imageHeight=480; public VisionHelper(){ contoursTable=NetworkTable.getTable("GRIP/contours"); } @@ -16,6 +20,22 @@ public class VisionHelper { }catch(Exception e){ return 0; } - + } + public double getCenterY(){ + try{ + return contoursTable.getNumberArray("centerY",new double[0])[0]; + }catch(Exception e){ + return 0; + } + } + public double getHorizontalError(){ + double degreesPerPixel=hfieldOfView/imageWidth; + double centerColumn=imageWidth/2; + return (getCenterX()-centerColumn)*degreesPerPixel; + } + public double getVerticalError(){ + double degreesPerPixel=vfieldOfView/imageHeight; + double centerRow=imageHeight/2; + return (getCenterY()-centerRow)*degreesPerPixel; } }