From 702df28cb251620a66b9dd5e2790c837d657b35d Mon Sep 17 00:00:00 2001 From: Adam Long Date: Thu, 13 Oct 2016 19:09:30 +0000 Subject: [PATCH] vision almost works --- src/org/usfirst/frc/team2059/robot/OI.java | 7 ++-- src/org/usfirst/frc/team2059/robot/Robot.java | 1 + .../usfirst/frc/team2059/robot/RobotMap.java | 3 +- .../robot/commands/shooter/AlignVertical.java | 36 +++++++++++++++++++ .../commands/vision/AlignHorizontal.java | 5 +++ .../team2059/robot/subsystems/DriveBase.java | 16 ++++++--- .../team2059/robot/subsystems/MainArm.java | 6 ++-- .../robot/subsystems/VisionHelper.java | 15 +++++--- 8 files changed, 74 insertions(+), 15 deletions(-) create mode 100644 src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 5aba211..f9c6527 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -15,6 +15,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.shooter.AlignVertical; import org.usfirst.frc.team2059.robot.commands.visionhelper.AlignHorizontal; import org.usfirst.frc.team2059.robot.RobotMap; /** @@ -39,6 +40,7 @@ public class OI { //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)); @@ -46,13 +48,14 @@ public class OI { joystickButtons[0][6].whileHeld(new SpinRollers(1, true)); joystickButtons[0][9].whileHeld(new Drive()); // joystickButtons[0][2].whileHeld(new PIDDrive(400)); - joystickButtons[1][0].whileHeld(new MoveArm(1)); - joystickButtons[1][1].whileHeld(new MoveArm(-1)); //joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect)); + joystickButtons[1][0].whileHeld(new AlignVertical()); 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 SetArmPosition(RobotMap.mainArmPresetLowShot)); + joystickButtons[1][7].whileHeld(new MoveArm(1)); + joystickButtons[1][8].whileHeld(new MoveArm(-1)); } public Joystick[] getJoysticks() { return joysticks; diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index b7470ed..40e51bc 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -83,6 +83,7 @@ public class Robot extends IterativeRobot { 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); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 2a9f0f0..95f488d 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -37,7 +37,8 @@ public class RobotMap { public static double fWidth = 483.467261958; public static double fHeight = 362.600446468; public static int sonar = 2; - public static int goalHeight = 72; + //public static int goalHeight = 102; + public static int goalHeight = 64; //Misc public static int mainArmPresetCollect = -5; public static int mainArmPresetTraverse = 10; diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java new file mode 100644 index 0000000..cb9e407 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java @@ -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; +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() { + mainArm.setSetpoint(mainArm.getShooterAngleError()+50); + mainArm.enable(); + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + //Move the arm stop + } + // 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 diff --git a/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java b/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java index d60edd2..9f833b4 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java +++ b/src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java @@ -6,7 +6,10 @@ public class AlignHorizontal extends CommandBase { } protected void initialize() { error = visionHelper.getHorizontalError(); + driveBase.setDriveEnabled(false); driveBase.resetGyro(); + mainArm.setSetpoint(90); + mainArm.enable(); } protected boolean isFinished() { return false; @@ -16,6 +19,8 @@ public class AlignHorizontal extends CommandBase { } protected void end() { driveBase.getGyroController().disable(); + driveBase.setDriveEnabled(true); + mainArm.disable(); } protected void interrupted() { end(); diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java index f5f6b18..1a8c5f5 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java @@ -20,8 +20,9 @@ public class DriveBase extends Subsystem { 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()); + setDefaultCommand(new Drive()); } public void stopDriving() { leftMotorOne.set(0); @@ -30,10 +31,12 @@ public class DriveBase extends Subsystem { rightMotorTwo.set(0); } public void driveArcade(double x, double y, double z, double sensitivity) { - leftMotorOne.set((-y + (x + z)) * sensitivity); - leftMotorTwo.set((-y + (x + z)) * sensitivity); - rightMotorOne.set((y + (x + z)) * sensitivity); - rightMotorTwo.set((y + (x + z)) * 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()); @@ -53,6 +56,9 @@ public class DriveBase extends Subsystem { gyroController.enable(); gyroController.setSetpoint(angle); } + public void setDriveEnabled(boolean enabled){ + driveEnabled = enabled; + } public PIDController getLeftController() { return leftEncoderController; } diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 4cc3c0d..bbff6cb 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -16,7 +16,7 @@ public class MainArm extends PIDSubsystem { private double min = RobotMap.zeroDegrees; private double max = RobotMap.ninetyDegrees; public MainArm() { - super("MainArm", 0.1, 0.0, 0.002); + super("MainArm", 0.08, 0.0, 0.002); getPIDController().setContinuous(false); setSetpoint(70); enable(); @@ -90,8 +90,8 @@ public class MainArm extends PIDSubsystem { return false; } public double getShooterAngleError(){ - double correctedHeight = RobotMap.goalHeight - 44; //44 is the distance between the ground and the sonar - return (180/Math.PI) * Math.atan(correctedHeight/getSonarDistance()); + double correctedHeight = (RobotMap.goalHeight + 26) - 44; //44 is the distance between the ground and the sonar + return (180/Math.PI) * Math.atan(correctedHeight/(getSonarDistance()-5)); } public double getSonarDistanceRaw(){ return sonar.getVoltage(); diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java index 2e5c0c5..4f7dbee 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java @@ -15,8 +15,17 @@ public class VisionHelper { 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])[0]; + return contoursTable.getNumberArray("centerX",new double[0])[highestAreaIndex]; }catch(Exception e){ return 0; } @@ -53,7 +62,6 @@ public class VisionHelper { } index++; } - System.out.println("Highest Length Index:"+ highestLengthIndex); try { return (x1s[highestLengthIndex]+x2s[highestLengthIndex])/2; } catch (Exception e) { @@ -72,7 +80,6 @@ public class VisionHelper { } index++; } - System.out.println("Highest Length Index:"+ highestLengthIndex); try { return (y1s[highestLengthIndex]+y2s[highestLengthIndex])/2; } catch (Exception e) { @@ -80,7 +87,7 @@ public class VisionHelper { } } public double getHorizontalError() { - return (180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)); + 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));