public void driveToBridge() {
System.out.println("Val: " + potentiometer.getVoltage());
if (Autonomousflag) {
if (potentiometer.getVoltage() < 5) { // line
shoot();
Timer.delay(7);
shoot.set(0);
collect.set(0);
driveTrain.tankDrive(-speed, speed);
Timer.delay(1);
driveTrain.tankDrive(speed, speed);
Timer.delay(1.2);
driveTrain.tankDrive(0, 0);
Timer.delay(0.9);
bridge.set(-1);
} else if (potentiometer.getVoltage() > 5) { // left
shoot();
}
Autonomousflag = false;
} else {
driveTrain.tankDrive(0, 0);
}
}
public void storeLoaderAngle(String sPosition) {
float volts = (float) leftAngleCh.getVoltage();
float voltsR = (float) rightAngleCh.getVoltage();
m_preferences.putFloat(sPosition, volts);
m_preferences.putFloat(sPosition + "R", voltsR);
System.out.println(sPosition + " = " + volts);
}
/** This function is called periodically during test mode */
public void testPeriodic() {
Watchdog.getInstance().feed(); // Tell watchdog we are running
drive();
shooterUse();
// shooterLoft();
armUse();
System.out.println(
"loftPot value: " + loftPot.getValue() + " armPot Value: " + armPot.getValue());
if (xboxDriver
.getBtnBACK()) { // when driver controls, hold button BACK to unwind winch, release to stop,
// only for testing
winchRelay.set(Relay.Value.kOn);
winchRelay.setDirection(Relay.Direction.kForward);
}
}
public void shooterLoft() {
// Ajust loft
double operatorLeftY = xboxOperator.getLeftY(); // operator has loft control
int loftPotValue = loftPot.getValue();
if (xboxOperator.getBtnX()) { // hold to ajust for close shot
if (loftPotValue < closeShotValue - focusTolerance) {
if (loftPotValue < closeShotValue - angleTolerance) {
shooterLoft.set(-1);
} else {
shooterLoft.set(-0.001);
}
} else if (loftPotValue > closeShotValue + focusTolerance) {
if (loftPotValue > closeShotValue + angleTolerance) {
shooterLoft.set(1);
} else {
shooterLoft.set(0.001);
}
} else {
shooterLoft.set(0);
}
} else if (xboxOperator.getBtnY()) { // hold to ajust for far shot
if (loftPotValue < farShotValue - focusTolerance) {
if (loftPotValue < farShotValue - angleTolerance) {
shooterLoft.set(-1);
} else {
shooterLoft.set(-0.001);
}
} else if (loftPotValue > farShotValue + focusTolerance) {
if (loftPotValue > farShotValue + angleTolerance) {
shooterLoft.set(1);
} else {
shooterLoft.set(0.001);
}
} else {
shooterLoft.set(0);
}
} else if (xboxOperator.getBtnB()) { // hold to ajust for feeding
if (loftPotValue < shooterFeedingValue - focusTolerance) {
if (loftPotValue < shooterFeedingValue - angleTolerance) {
shooterLoft.set(-1);
} else {
shooterLoft.set(-0.001);
}
} else if (loftPotValue > shooterFeedingValue + focusTolerance) {
if (loftPotValue > shooterFeedingValue + angleTolerance) {
shooterLoft.set(1);
} else {
shooterLoft.set(0.001);
}
} else {
shooterLoft.set(0);
}
} else if (loftPotValue > maxHeightValue
&& loftPotValue < minHeightValue
&& ((operatorLeftY > .3) || operatorLeftY < -.3)) { // move loft with left operator stick
shooterLoft.set(operatorLeftY);
} else {
shooterLoft.set(0); // don't move loft
}
} // end of shooter loft
public boolean isDetensioned() {
double curr_volts = sensor.getAverageVoltage();
boolean on_target =
((curr_volts > (slack_tension_volts - threshold_volts))
&& (curr_volts < (slack_tension_volts + threshold_volts)));
boolean at_min = leftMin.get();
return (on_target || at_min);
}
// basic arcadeDrive: y=forward/backward speed, x=left/right speed
public void arcadeDrive(double y, double x) {
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
robotDrive.arcadeDrive(y, x + 0.05);
SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
SmartDashboard.putNumber("RangefinderVoltage", ultraDist.getVoltage());
} // end arcadeDrive
public void teleopPeriodic() {
smart.putNumber("distance", ultrasonic.getVoltage());
setSuckingLED();
setLEDTeamColour();
// smart dashboard stuff
smart.putBoolean("Fully Pressurized", compressor.getPressureSwitchValue());
smart.putBoolean("fast gear", sol1.get() == true && sol2.get() == false);
smart.putBoolean("slow gear", sol1.get() == false && sol2.get() == true);
if (gyro.getAngle() > 360 || gyro.getAngle() < -360) {
gyro.reset();
}
smart.putBoolean("good angle", gyro.getAngle() < 30 && gyro.getAngle() > -30);
smart.putBoolean(
"too far right", (gyro.getAngle() > 30 && gyro.getAngle() < 180) || gyro.getAngle() < -180);
smart.putBoolean(
"too far left", (gyro.getAngle() < -30 && gyro.getAngle() > -180) || gyro.getAngle() > 180);
if (ultrasonic.getVoltage() < 0.55) {
tooClose = true;
} else {
tooClose = false;
}
smart.putBoolean("Too close", tooClose);
if (ultrasonic.getVoltage() > 0.8) {
tooFar = true;
} else {
tooFar = false;
}
smart.putBoolean("Too far", tooFar);
if (ultrasonic.getVoltage() >= 0.55 && ultrasonic.getVoltage() <= 0.8) {
inRange = true;
} else {
inRange = false;
}
smart.putBoolean("In range", inRange);
}
public void autonomousPeriodic() {
relay.set(Relay.Value.kOn);
smart.putNumber("distance", ultrasonic.getVoltage());
drive.setRun(false);
loadAndShoot.setRun(false);
// relay.set(Relay.Value.kOn);
if (autoTimer.get() < autoMoveForwardTime) {
System.out.println(
"Moving forward, Timer at "
+ autoTimer.get()
+ ", Ultrasonic at "
+ ultrasonic.getAverageVoltage());
}
if (autoTimer.get() >= autoMoveForwardTime && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
stop();
if (!autonomousStopped) {
autonomousStopped = true;
autoTimeAtStop = autoTimer.get();
}
System.out.println("Stopped, Ultrasonic at " + ultrasonic.getAverageVoltage());
}
if (autoTimer.get() >= autoTimeAtStop + 1 && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
autoStop = true;
shoot();
System.out.println("Shooting, Ultrasonic at " + ultrasonic.getAverageVoltage());
}
}
// aligns robot for shooting
public void alignToShoot(double left, double right) {
Preferences p = Preferences.getInstance();
final double kShootLimitVoltage = p.getDouble("DriveTrainShootLimitVoltage", 0.0);
double maxVoltage = ultraDist.getVoltage();
for (int i = 0; i < 10; i++) {
double voltage = ultraDist.getVoltage();
if (voltage > maxVoltage) {
maxVoltage = voltage;
}
}
SmartDashboard.putNumber(
"RangefinderVoltage", maxVoltage); // want to see on competition dashboard!!
System.out.println(
"RangefinderVoltage: " + maxVoltage + " kShootLimitVoltage: " + kShootLimitVoltage);
if (maxVoltage <= kShootLimitVoltage) {
hasBeenAchieved = true;
}
if (left <= 0.0 && hasBeenAchieved) {
left = 0.0;
}
arcadeDrive(left, right);
} // end alignToShoot
public void armUse() {
// Arm contols
int armPotValue = armPot.getValue();
if (xboxDriver.getBtnB()) { // arm for slot feeding
if (armPotValue < armSlotFeedingValue - angleTolerance) {
armVictor.set(-.2);
} else if (armPotValue < armSlotFeedingValue - focusTolerance) {
armVictor.set(-.02);
} else if (armPotValue > armSlotFeedingValue + angleTolerance) {
armVictor.set(.5);
} else if (armPotValue > armSlotFeedingValue + focusTolerance) {
armVictor.set(.05);
} else {
armVictor.set(0);
}
} else if (xboxDriver.getBtnY()) { // arm for shovel feeding
if (armPotValue < armShovelFeedingValue - angleTolerance) {
armVictor.set(-.2);
} else if (armPotValue < armShovelFeedingValue - focusTolerance) {
armVictor.set(-.02);
} else if (armPotValue > armShovelFeedingValue + angleTolerance) {
armVictor.set(.5);
} else if (armPotValue > armShovelFeedingValue + focusTolerance) {
armVictor.set(.05);
} else {
armVictor.set(0);
}
} else {
if (xboxDriver.getBtnLB() && armPotValue > minArmValue) {
armVictor.set(-.2); // driver left bumper = down, right bumper = up;
} else if (xboxDriver.getBtnRB() && armPotValue < maxArmValue) {
armVictor.set(.5);
} else {
armVictor.set(0);
}
if (xboxDriver.getBtnA()
&& armPotValue
< climbStopValue) { // when driver controls, hold button A to use winch, release to
// stop
winchRelay.set(Relay.Value.kOn);
winchRelay.setDirection(Relay.Direction.kReverse);
} else {
winchRelay.set(Relay.Value.kOff);
}
}
} // end of arm use
public double getUltrasonic() {
return us.getValue() / 1.9;
}
protected double returnPIDInput() {
return sensor.getAverageVoltage();
}
public double getCurrentVoltage() {
CURRENTVOLTAGE = ac.getVoltage();
return CURRENTVOLTAGE;
}
public double getPotVal() {
return (pot.getValue() - elevatorDownPos) * potScale;
}
public double getTensionLevel() {
return sensor.getAverageVoltage();
}
// The absolute returns a voltage, 0 - 5, thus, any voltage *72 will = deg.
public double getAbsoluteCurrentPosition() {
return analogInputAbsolute.getAverageVoltage() * 72;
}
public double getAbsoluteVoltage() {
return analogInputAbsolute.getAverageVoltage();
}
public double getDistance() {
return voltToIn(ultrasonic.getValue());
}
public double getPosition() {
return pot.getAccumulatorCount();
}
public double getPot() {
return pot.getValue();
}
public void resetPot() {
elevatorDownPos = pot.getValue();
}
public static double voltageToDistance(AnalogChannel Ultrasonic) {
return (Ultrasonic.getVoltage() * 1000.0 / 9.766);
}