more vision changes

This commit is contained in:
Adam Long 2016-10-06 23:18:44 +00:00
parent 25736f3bff
commit f0ea150fac
4 changed files with 6 additions and 6 deletions

View File

@ -5,6 +5,7 @@ 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.shooter.MoveArm;
import org.usfirst.frc.team2059.robot.commands.shooter.ResetLower;
import org.usfirst.frc.team2059.robot.commands.shooter.ResetUpper;
@ -40,6 +41,7 @@ public class OI {
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));
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));

View File

@ -78,7 +78,7 @@ public class Robot extends IterativeRobot {
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())));
SmartDashboard.putNumber("goalDistance", 71 / Math.tan((52.7 - CommandBase.visionHelper.getVerticalError())));
if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
CommandBase.pneumatics.setArmStopState(true);
} else {

View File

@ -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);

View File

@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
public class VisionHelper {
NetworkTable contoursTable;
double hfieldOfView = 67;
double vfieldOfView = 51;
double imageWidth = 640;
double imageHeight = 480;
public VisionHelper() {
@ -30,9 +28,9 @@ public class VisionHelper {
}
}
public double getHorizontalError() {
return Math.atan2(getCenterX() - RobotMap.imageWidth / 2, RobotMap.fWidth);
return (180/Math.PI) * (Math.atan((getCenterX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
}
public double getVerticalError() {
return Math.atan2(getCenterY() - RobotMap.imageHeight / 2, RobotMap.fHeight);
return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));
}
}