15 Phoenix/src/org/usfirst/frc/team2537/robot/auto/vision/VisionRotateCommand.java
View file
@@ -9,9 +9,10 @@
public class VisionRotateCommand extends Command {

/* when the pi cannot see the target, we spin faster to try to find the target */
private static final double DEFAULT_PERCENT_OUTPUT = 0.3;
private static final double CENTER_kP = 1;
private static final double TURNING_TOLERANCE = 0.1;
private static final double DEFAULT_PERCENT_OUTPUT = 1.00;
private static final double CENTER_kP = 8;
private static final double TURNING_TOLERANCE = 0.2;
private static final double PI_MOUNT_OFFSET = -0.25;

private double centerX;
private Side lastSide;
@@ -53,22 +54,22 @@ protected void execute() {
largestTarget = target;
}
}
centerX = largestTarget.getBoundingBoxCenter().x / 320.0 - 1;
centerX = largestTarget.getBoundingBoxCenter().y / 240.0 - 1;// - PI_MOUNT_OFFSET;
lastSide = centerX < 0 ? Side.LEFT : Side.RIGHT;

SmartDashboard.putNumber("center", centerX);

power = Math.min(Math.abs(power), Math.abs(centerX*power*CENTER_kP)) * Math.signum(centerX);
}

// Robot.driveSys.setMotors( power, Motor.LEFT);
// Robot.driveSys.setMotors(-power, Motor.RIGHT);
Robot.driveSys.setMotors(-power, Motor.LEFT);
Robot.driveSys.setMotors(power, Motor.RIGHT);
}

@Override
protected boolean isFinished() {
System.out.println("centerX: " + centerX);
return stopAtTarget && Math.abs(centerX) < TURNING_TOLERANCE;
return false && stopAtTarget && Math.abs(centerX) < TURNING_TOLERANCE;
}

@Override
19 Phoenix/src/org/usfirst/frc/team2537/robot/climb/ClimbSubsystem.java
View file
@@ -1,7 +1,6 @@
package org.usfirst.frc.team2537.robot.climb;

import org.usfirst.frc.team2537.robot.Ports;
import org.usfirst.frc.team2537.robot.Robot;
import org.usfirst.frc.team2537.robot.input.HumanInput;

import edu.wpi.first.wpilibj.Talon;
@@ -40,15 +39,15 @@ public void setClimbMotors(double speed) {
climbMotorThree.set(speed);
}

public double getCurrentClimbMotorOne() {
return Robot.pdp.getCurrent(Ports.CLIMB_MOTOR_ONE_PDP);
}
public double getCurrentClimbMotorTwo() {
return Robot.pdp.getCurrent(Ports.CLIMB_MOTOR_TWO_PDP);
}
public double getCurrentClimbMotorThree() {
return Robot.pdp.getCurrent(Ports.CLIMB_MOTOR_THREE_PDP);
}
// public double getCurrentClimbMotorOne() {
// return Robot.pdp.getCurrent(Ports.CLIMB_MOTOR_ONE_PDP);
// }
// public double getCurrentClimbMotorTwo() {
// return Robot.pdp.getCurrent(Ports.CLIMB_MOTOR_TWO_PDP);
// }
// public double getCurrentClimbMotorThree() {
// return Robot.pdp.getCurrent(Ports.CLIMB_MOTOR_THREE_PDP);
// }

public boolean climberOverridden() {
return HumanInput.climbOverrideKeyOne.get() && HumanInput.climbOverrideKeyTwo.get();
7 Phoenix/src/org/usfirst/frc/team2537/robot/cuber/CuberSubsystem.java
View file
@@ -3,6 +3,7 @@
import org.usfirst.frc.team2537.robot.Ports;
import org.usfirst.frc.team2537.robot.input.HumanInput;
import org.usfirst.frc.team2537.robot.resources.CANTalon;
import org.usfirst.frc.team2537.robot.resources.UltrasonicWrapper;

import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
@@ -30,9 +31,9 @@ public CuberSubsystem() {
flywheelMotorRight = new CANTalon(Ports.FLYWHEEL_MOTOR_RIGHT);
liftMotor = new CANTalon(Ports.FLIPPER_WINDOW_MOTOR);
ultrasonic = new UltrasonicWrapper(Ports.CUBER_ULTRASONIC_TRIGGER, Ports.CUBER_ULTRASONIC_ECHO,
Ports.CUBER_FAKE_ULTRASONIC);
flipperHallEffectOne = new DigitalInput(Ports.FLIPPER_HOLIFAX_ONE);
flipperHallEffectTwo = new DigitalInput(Ports.FLIPPER_HOLIFAX_TW0);
Ports.CUBER_ULTRASONIC_DUMMY);
flipperHallEffectOne = new DigitalInput(Ports.FLIPPER_HALL_EFFECT_TOP);
flipperHallEffectTwo = new DigitalInput(Ports.FLIPPER_HALL_EFFECT_BOTTOM);

liftMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen, 0);

3 Phoenix/src/org/usfirst/frc/team2537/robot/cuber/LowerFlipperCommand.java
View file
@@ -9,6 +9,7 @@ public class LowerFlipperCommand extends Command {

public LowerFlipperCommand() {
requires(Robot.cuberSys); // requires cuberSys variables and methods
setTimeout(1.1);
}

@Override
@@ -28,7 +29,7 @@ protected void execute() {
}

protected boolean isFinished() { // returns true if motor turns over or equal to 90 degrees or when flywheel
return (Robot.cuberSys.getHolifaxTwo());
return (Robot.cuberSys.getHolifaxTwo()) || this.isTimedOut();
}

protected void end() {
4 Phoenix/src/org/usfirst/frc/team2537/robot/cuber/PickUpCommand.java
View file
@@ -14,15 +14,15 @@ public PickUpCommand() {
@Override
protected void initialize() {
Robot.cuberSys.setFlywheelMotors(0);
if (Robot.cuberSys.getUltrasonicInches() > CuberSubsystem.ULTRASONIC_RANGE) {
if (Robot.cuberSys.getUltrasonicInches() > CuberSubsystem.ULTRASONIC_RANGE || Robot.cuberSys.getUltrasonicInches() == 0) {
Robot.cuberSys.setFlywheelMotors(0.8);
}

}

@Override
protected void execute() {
if (Robot.cuberSys.getUltrasonicInches() < CuberSubsystem.ULTRASONIC_RANGE) {
if (Robot.cuberSys.getUltrasonicInches() < CuberSubsystem.ULTRASONIC_RANGE && Robot.cuberSys.getUltrasonicInches() != 0) {
Robot.cuberSys.setFlywheelMotors(0);
}

11 Phoenix/src/org/usfirst/frc/team2537/robot/drive/DriveSubsystem.java
View file
@@ -1,7 +1,7 @@
package org.usfirst.frc.team2537.robot.drive;

import org.usfirst.frc.team2537.robot.Ports;
import org.usfirst.frc.team2537.robot.Robot;
import org.usfirst.frc.team2537.robot.resources.UltrasonicWrapper;
import org.usfirst.frc.team2537.robot.units.Distances;
import org.usfirst.frc.team2537.robot.units.Times;
import org.usfirst.frc.team2537.robot.units.Units;
@@ -58,7 +58,7 @@ public DriveSubsystem() {
talonBackLeft.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0);
talonFrontRight.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0);

ultrasonic = new Ultrasonic(Ports.DRIVE_ULTRASONIC_TRIGGER, Ports.DRIVE_ULTRASONIC_ECHO);
ultrasonic = new UltrasonicWrapper(Ports.DRIVE_ULTRASONIC_TRIGGER, Ports.DRIVE_ULTRASONIC_ECHO, Ports.DRIVE_ULTRASONIC_DUMMY);
}

/******************************************************************************/
@@ -198,11 +198,4 @@ public void setMode(ControlMode controlMode) {
this.controlMode = controlMode;
}

public void fixingDrive() {
System.out.println("Front Right: " + Robot.pdp.getCurrent(Ports.FRONT_RIGHT_PDP));
System.out.println("Front Left: " + Robot.pdp.getCurrent(Ports.FRONT_LEFT_PDP));
System.out.println("Back Right: " + Robot.pdp.getCurrent(Ports.BACK_RIGHT_PDP));
System.out.println("Back Left : " + Robot.pdp.getCurrent(Ports.BACK_LEFT_PDP));
}

}
2 ...am2537/robot/cuber/UltrasonicWrapper.java → ...37/robot/resources/UltrasonicWrapper.java
View file
@@ -1,4 +1,4 @@
package org.usfirst.frc.team2537.robot.cuber;
package org.usfirst.frc.team2537.robot.resources;

import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Ultrasonic;
8 Phoenix/src/org/usfirst/frc/team2537/robot/vert/VertDownCommand.java
View file
@@ -10,11 +10,12 @@ public class VertDownCommand extends Command {
private final double targetDistance;

public VertDownCommand() {
this(-1);
requires(Robot.vertSys);
this.targetDistance = -1;
}

public VertDownCommand(int targetDistance) {
requires(Robot.driveSys);
requires(Robot.vertSys);
this.targetDistance = Math.abs(targetDistance);
}

@@ -25,7 +26,8 @@ protected void initialize() {
}

@Override
protected void execute() {
protected void execute() {
System.out.println("encoder: " + -Robot.vertSys.getEncoderPos() + " target: " + targetDistance);
}

@Override
3 Phoenix/src/org/usfirst/frc/team2537/robot/vert/VertSubsystem.java
View file
@@ -52,6 +52,9 @@ public double getEncoderPos() {

public void resetEncoder() {
vertMotorOne.getSensorCollection().setQuadraturePosition(0, 0);
vertMotorOne.getSensorCollection().setQuadraturePosition(0, 0);
vertMotorOne.getSensorCollection().setQuadraturePosition(0, 0);
vertMotorOne.getSensorCollection().setQuadraturePosition(0, 0);
}

public boolean getLimitSwitch() {
1 Phoenix/src/org/usfirst/frc/team2537/robot/vert/VertUpCommand.java
View file
@@ -27,6 +27,7 @@ protected void initialize() {
protected void execute() {
if (Robot.vertSys.getLimitSwitch())
Robot.vertSys.setVertMotors(0);
System.out.println(Robot.vertSys.getEncoderPos());
}