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());
}
}
public boolean waitForComplete(double milli) {
System.out.println("time currently: " + timer.get() + " - waiting for: " + milli);
if (timer.get() <= milli) return false;
else {
return true;
}
}
protected void execute() {
if (time.get() < 3.0) {
talonLeft.set(1);
talonRight.set(1);
} else if (time.get() < 5.0) {
talonLeft.set(0);
talonRight.set(0);
soleLiftDown.set(true);
} else if (time.get() < 6.0) {
talonLeft.set(-1);
talonRight.set(-1);
} else talonLeft.set(1);
talonRight.set(1);
}
/*
* reads the value of the digital input of the specified channel
* requires that the readArudiono() method has already been called
* param 1: the channel to be read
* return: value of the specified digital input
*/
public boolean getInput(int channelID) {
byte temp;
if (timer.get() > 2.0) {
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser3, 1, " ");
DriverStationLCD.getInstance().updateLCD();
timer.stop();
timer.reset();
}
if (Math.abs(channelID - (13 / 2)) - (13 / 2)
> 0) // returns true if less than zero or greater than thirteen
{
timer.start();
DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "Invalid ID");
DriverStationLCD.getInstance().updateLCD();
return false;
} else if (channelID < 8) {
temp = (byte) (loByte & (1 << channelID));
return temp != 0;
} else {
temp = (byte) (hiByte & (1 << channelID - 8));
return temp != 0;
}
}
@Override
protected void execute() {
if (!wait
|| (!triggered
&& (shooterArm.isArmAtDesiredAngle()
|| Robot.autoTimer.get() > Robot.autoStartTime + 14))) {
if (shooterArm.getSetpoint() != shooterArm.voltsFromDegrees(0.0)) {
System.out.println(triggered + " " + shooterArm.isArmAtDesiredAngle());
triggered = true;
beginTime = timer1.get();
SmartDashboard.putString(
"Shooting Dat",
"Left RPM: "
+ shooterWheel.currentLeftRPM
+ " Right RPM: "
+ shooterWheel.currentRightRPM
+ " Shooting Angle "
+ shooterArm.degreesFromVolts(RobotMap.shooterLifterEncoder.getVoltage())
+ " Difference to Center: "
+ (visionProcessing.centerX
- CommandBase.preferences.getDouble(PreferenceKeys.CENTER_IMAGE, 160.0)));
System.out.println(
"Bounding Rect Shoot: bottom: "
+ visionProcessing.rectBottom
+ " top: "
+ visionProcessing.rectTop
+ " left: "
+ visionProcessing.rectLeft
+ " right: "
+ visionProcessing.rectRight);
shooterBar.rawBallPush(PUSH_POWER);
System.out.println("Shoot Triggered Time: " + beginTime);
}
}
}
public boolean run() {
if (delayTimer.get() < delaySeconds) {
return false;
}
return true;
}
@Override
protected boolean isFinished() {
if (timer.get() >= timetorun) {
return true;
} else {
return false;
}
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drivetrain.resetGyro();
Robot.drivetrain.drivePID.setSetpoint(STRAIGHT_SET_POINT);
Robot.drivetrain.throttleInt.useJoystick(false);
Robot.drivetrain.throttleInt.setThrottle(thisDriveSpeed);
timer2.start();
startTime2 = timer2.get();
state = State.START_DRIVE; // initializes the state
}
private double m_retract() {
if (encoder.get() <= 0 || retractTime.get() >= 1.0) {
shooting = false;
retracting = false;
return 0;
} else { // encoder > 0 && retractTime < 1.0
return .1;
}
}
@Override
protected boolean isFinished() {
boolean finished = timer1.get() > BALL_PUSH_TIME + time + (wait ? beginTime : 0);
if (finished) {
System.out.println("Shoot Finished");
}
return finished;
// Finishes command if the current time is greater than the Ball Push Time
}
/** This is called constantly called by the task manager. */
@Override
public void execute() {
drive.setPower(power);
drive.execute();
if (timer.get() > endTime) {
isFinished = true;
}
}
protected void execute() {
double time = _timer.get();
if (time >= _period) {
Robot.ledStrip.setColor(
_colors[index].getRed(), _colors[index].getGreen(), _colors[index].getBlue());
index = (index + 1) % _colors.length;
_timer.reset();
}
}
private double m_shoot() {
if (encoder.get() >= desired || shootTime.get() > m_timeToShoot) {
shooting = false;
retracting = true;
shootTime.stop();
retractTime.start();
retractTime.reset();
return .1;
} else { // encoder < desired && shootTime < 0.2
error = desired - encoder.get();
return -1;
}
}
protected void execute() {
double delta = timer.get();
distance += (drivetrain.getAverageForwardVelocity() * delta);
timer.reset();
// PID tuning code
pidControllerDistance.setPID(
pidConfigDistance.getP(P),
pidConfigDistance.getI(I),
pidConfigDistance.getD(D),
pidConfigDistance.getF(F));
pidConfigDistance.setSetpoint(pidControllerDistance.getSetpoint());
pidConfigDistance.setValue(drivetrainRotation.pidGet());
}
/**
* Records bots movements to specified file
*
* @param sFileName
*/
public void record(String sFileName) {
if (!m_bRecStarted) {
m_sFile = sFileName;
m_tmRecorder.start();
m_bRecStarted = true;
}
if (!m_bRecDone) {
m_Index++;
m_botDataAuto = new BotData();
m_botDataAuto.setValues(m_tmRecorder.get(), m_bot);
m_List.addElement(m_botDataAuto);
} else {
Vars.fnDisableDrive();
m_bot.stopRobot();
m_tmRecorder.stop();
m_tmRecorder.reset();
}
}
/** Unjams the ingestor conveyor */
public static void shakeIngest() {
timer.start();
while (timer.get() < 2) {
if (direction) {
try {
ingestConveyor.setX(Constants.CONV_INGEST_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
} else {
try {
ingestConveyor.setX(-Constants.CONV_INGEST_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
}
direction = !direction;
timer.reset();
}
@Override
public boolean isCompleted() {
// If the Controller is completable, then the error will need to be
// between a certain threshold before isCompleted return true
if (completable) {
if (Math.abs(previousError) < THRESHOLD) {
if (timerStarted) {
if (doneTimer.get() > TIME_TO_BE_COMPLETE_MILLISECONDS) {
return true;
}
} else {
doneTimer.start();
timerStarted = true;
}
} else {
timerStarted = false;
}
}
return false;
}
/** Unjams the shooter conveyor */
public static void shakeShooter() {
timer.start();
while (timer.get() < 2) {
if (direction) {
try {
shooterConveyor.setX(Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
} else {
try {
shooterConveyor.setX(-Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
}
direction = !direction;
timer.reset();
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
Watchdog.getInstance().feed(); // Tell watchdog we are running
// activate the robot by pushing start button
if (xboxDriver.getBtnSTART()) {
teleopActive = true;
}
if (!teleopActive) {
return; // only run if teleop is active
}
if (lightsTimer.get() > 90000) {
lightsOutput1.set(true);
lightsOutput2.set(true);
lightsOutput3.set(false);
}
drive();
shooterUse();
// shooterLoft();
armUse();
}
/**
* Gets the time value of the recording, as in how long it has been replaying
*
* @return
*/
public double getRecordTime() {
return Vars.fnSetPrecision(m_tmRecorder.get());
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
System.out.println("state = " + state);
// Robot.drivetrain.drivePID.enable();
switch (state) {
case START_DRIVE:
Robot.drivetrain.drivePID.enable();
currentTime2 = timer2.get();
if (currentTime2 - startTime2
>= thisDriveTime1 /*&& Robot.drivetrain.distance.getRangeInches() <= RobotConstants.autoLowBarShootdistance1*/) {
Robot.drivetrain.drivePID.disable();
startTime2 = timer2.get();
state = State.LOWER_INTAKE_DRIVE;
}
break;
case LOWER_INTAKE_DRIVE:
currentTime2 = timer2.get();
Robot.intakeLifter.setAngle(IntakeLifter.Positions.PORT);
Robot.drivetrain.setL(1);
Robot.drivetrain.setR(1);
if (currentTime2 - startTime2 >= thisLowerDriveTime) {
Robot.drivetrain.setL(0.0);
Robot.drivetrain.setR(0.0);
state = State.RAISE_INTAKE;
}
break;
case RAISE_INTAKE:
currentTime2 = timer2.get();
Robot.intakeLifter.setAngle(IntakeLifter.Positions.DEFAULT);
if (currentTime2 - startTime2 >= thisRaiseTime) {
state = State.TURN;
}
break;
case TURN:
currentTime2 = timer2.get();
Robot.drivetrain.throttleInt.setThrottle(0);
Robot.drivetrain.drivePID.setSetpoint(thisTurnAngle);
Robot.drivetrain.drivePID.enable();
if (currentTime2 - startTime2
>= thisTurnTime /*Robot.drivetrain.distance.getRangeInches() > RobotConstants.autoLowBarShootdistance2*/) {
Robot.drivetrain.drivePID.disable();
startTime2 = timer2.get();
state = State.DRIVE_2;
}
break;
case DRIVE_2:
currentTime2 = timer2.get();
// Robot.drivetrain.setL(RobotConstants.autoLowBarshootDrivespeed2);
// Robot.drivetrain.setR(RobotConstants.autoLowBarshootDrivespeed2);
Robot.drivetrain.throttleInt.setThrottle(thisDriveSpeed);
Robot.drivetrain.drivePID.enable();
if (currentTime2 - startTime2
>= thisDriveTime2 /*Robot.drivetrain.distance.getRangeInches() <= RobotConstants.autoLowBarShootdistance3*/) {
Robot.drivetrain.setL(0);
Robot.drivetrain.setR(0);
Robot.drivetrain.drivePID.disable();
startTime2 = timer2.get();
state = State.TURNING_OFF;
}
break;
case TURNING_OFF:
Robot.intakeRoller.setState(IntakeRoller.Direction.STOP);
Robot.drivetrain.drivePID.disable();
state = State.END;
break;
// default:
}
}
/**
*
* <!-- begin-user-doc -->
* <!-- end-user-doc -->
*
* @generated
* @ordered
*/
@Override
public double getTimeInSeconds() {
return timer.get();
}
protected void routine() {
boolean goLeft = false;
Timer t = new Timer();
t.start();
// Turn on wheel
shooterController.setVelocityGoal(4000);
// Grab balls from ground
pinniped.wantFront = false;
pinniped.wantRear = false;
frontIntake.wantBumperGather = true;
rearIntake.wantBumperGather = true;
waitTime(.5);
// Drive to correct place
if (goLeft) path.goLeft();
else path.goRight();
drivePath(path, 10);
// Hold position
drivebase.resetEncoders();
System.out.println("Drive time: " + t.get());
headingController.setDistance(0);
headingController.setHeading(Math.toDegrees(Math.PI / 6.0) * (goLeft ? 1.0 : -1.0));
drivebase.useController(headingController);
// Wait for 5 seconds in
waitTime(.75);
System.out.println("Shooting 1st ball at: " + t.get());
// Shoot first ball
rearIntake.wantShoot = frontIntake.wantShoot = true;
waitTime(.25);
pinniped.wantShot = true;
waitTime(.5);
pinniped.wantShot = false;
rearIntake.wantShoot = frontIntake.wantShoot = false;
// Speed up for 2nd and 3rd shots
shooterController.setVelocityGoal(4300);
// Queue 2nd ball
rearIntake.wantBumperGather = false;
rearIntake.wantDown = true;
rearIntake.setManualRollerPower(1.0);
waitTime(0.3);
rearIntake.wantDown = false;
waitTime(.4);
rearIntake.setManualRollerPower(0);
// Settle time
waitTime(.75);
// Shoot second ball
frontIntake.wantShoot = true;
waitTime(.25);
rearIntake.setManualRollerPower(1);
pinniped.wantShot = true;
waitTime(.5);
pinniped.wantShot = false;
rearIntake.setManualRollerPower(0);
waitTime(0.3);
frontIntake.wantShoot = false;
// Queue 3rd ball
frontIntake.wantBumperGather = false;
frontIntake.wantDown = true;
frontIntake.setManualRollerPower(0.5);
waitTime(0.3);
frontIntake.wantDown = false;
waitTime(0.5);
frontIntake.setManualRollerPower(0.00);
// Settle time
waitTime(1.0);
// Shoot thirdball
rearIntake.setManualRollerPower(1);
pinniped.wantShot = true;
waitTime(.5);
pinniped.wantShot = false;
// Print out time
System.out.println(t.get());
// Clean up
rearIntake.setManualRollerPower(0);
shooterController.setVelocityGoal(0);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
if (timer.get() > time) {
intake.stop();
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return (timer.get() > time);
}
protected void execute() {
System.out.println(time.get());
Robot.left.set(0.82);
Robot.right.set(0.2);
}
protected boolean isFinished() {
return time.get() > 0.5;
}
protected boolean testComplete() {
return (timer.get() > DELAY_TIME);
}
public double getShootTime() {
return shootTime.get();
}
public double getRetractTime() {
return retractTime.get();
}
