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