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("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() {
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;
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));
addSequential(new AutoSetArmPosition(90));
//addSequential(new AutoRotate(45));
//addSequential(new AutoDriveStraightTime(0.4, 1.5));
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;
}
protected void execute() {
driveBase.rotateAngle(error);
if(visionHelper.getHorizontalError()==420){
driveBase.resetGyro();
driveBase.rotateAngle(5);
}else{
// driveBase.rotateAngle(visionHelper.getHorizontalError());
}
}
protected void end() {
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());
double encoderGyroCorrection = 1;
public void initDefaultCommand() {
//setDefaultCommand(new Drive());
setDefaultCommand(new Drive());
}
public void stopDriving() {
leftMotorOne.set(0);

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

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