team2059/ford-java
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

attempt to work on vision more

Browse Source
This commit is contained in:
Adam Long 2016-10-03 00:23:40 +00:00
parent 2f8a7e1dfa
commit c8e2c9697e
5 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/org/usfirst/frc/team2059/robot/Robot.java
View File

@ -59,6 +59,9 @@ public class Robot extends IterativeRobot {
SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw()); SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees()); SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations()); 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() { public void teleopInit() {
if (autonomousCommand != null) { if (autonomousCommand != null) {

18
src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java
View File

@ -4,17 +4,11 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
import org.usfirst.frc.team2059.robot.Robot; import org.usfirst.frc.team2059.robot.Robot;
public class RoutineShoot extends CommandGroup { public class RoutineShoot extends CommandGroup {
public RoutineShoot() { public RoutineShoot() {
// addSequential(new AutoSetArmStopState(true)); addSequential(new AutoSetArmStopState(true));
// addSequential(new AutoResetLower(-1)); addSequential(new AutoResetLower(-1));
// addSequential(new AutoDriveStraightTime(0.5, 1)); addSequential(new AutoDriveStraightTime(0.5, 1));
// addParallel(new AutoSetArmPosition(90)); addSequential(new AutoSetArmPosition(90));
// addSequential(new AutoRotate(45)); //addSequential(new AutoRotate(45));
// addSequential(new AutoDriveStraightTime(0.4, 1.5)); //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()); } addSequential(new VisionAlignHorizontal()); }
} }

7
src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java
View File

@ -12,7 +12,12 @@ public class VisionAlignHorizontal extends CommandBase {
return false; return false;
} }
protected void execute() { protected void execute() {
driveBase.rotateAngle(error); if(visionHelper.getHorizontalError()==420){
driveBase.resetGyro();
driveBase.rotateAngle(5);
}else{
// driveBase.rotateAngle(visionHelper.getHorizontalError());
}
} }
protected void end() { protected void end() {
driveBase.getGyroController().disable(); driveBase.getGyroController().disable();

2
src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
View File

@ -21,7 +21,7 @@ public class DriveBase extends Subsystem {
PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput()); PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
double encoderGyroCorrection = 1; double encoderGyroCorrection = 1;
public void initDefaultCommand() { public void initDefaultCommand() {
//setDefaultCommand(new Drive()); setDefaultCommand(new Drive());
} }
public void stopDriving() { public void stopDriving() {
leftMotorOne.set(0); leftMotorOne.set(0);

10
src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java
View File

@ -18,22 +18,28 @@ public class VisionHelper {
try{ try{
return contoursTable.getNumberArray("centerX",new double[0])[0]; return contoursTable.getNumberArray("centerX",new double[0])[0];
}catch(Exception e){ }catch(Exception e){
return 0; return 420;
} }
} }
public double getCenterY(){ public double getCenterY(){
try{ try{
return contoursTable.getNumberArray("centerY",new double[0])[0]; return contoursTable.getNumberArray("centerY",new double[0])[0];
}catch(Exception e){ }catch(Exception e){
return 0; return 420;
} }
} }
public double getHorizontalError(){ public double getHorizontalError(){
if(getCenterX()==420){
return 420;
}
double degreesPerPixel=hfieldOfView/imageWidth; double degreesPerPixel=hfieldOfView/imageWidth;
double centerColumn=imageWidth/2; double centerColumn=imageWidth/2;
return (getCenterX()-centerColumn)*degreesPerPixel; return (getCenterX()-centerColumn)*degreesPerPixel;
} }
public double getVerticalError(){ public double getVerticalError(){
if(getCenterY()==420){
return 420;
}
double degreesPerPixel=vfieldOfView/imageHeight; double degreesPerPixel=vfieldOfView/imageHeight;
double centerRow=imageHeight/2; double centerRow=imageHeight/2;
return (getCenterY()-centerRow)*degreesPerPixel; return (getCenterY()-centerRow)*degreesPerPixel;