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

more work on vision

Browse Source
This commit is contained in:
Adam Long 2016-10-02 23:32:22 +00:00
parent 40076e6d48
commit 2f8a7e1dfa
7 changed files with 96 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/org/usfirst/frc/team2059/robot/OI.java
View File

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

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

@ -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 {

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

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

24
src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java Normal file
View File

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

24
src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java Normal file
View File

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

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

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

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