I’m not sure that I can answer your autonomous behavior question, but I do have some suggestions that might help clean up some issues.
If you want to turn the motor safety feature on in teleop and off in autonomous, I would recommend that you move the invocation into the teleopInit() and autonomousInit() methods (and then remove the calls from your periodic methods).
@Override
public void autonomousInit() {
autoStartTime = Timer.getFPGATimestamp();
autoSelected = (String) chooser.getSelected();
System.out.println("Auto selected: " + autoSelected);
Timer timer = new Timer();
timer.start();
arcadeDrive.setSafetyEnabled(false);
}
@Override
public void teleopInit() {
arcadeDrive.setSafetyEnabled(true);
}
You may want to double check some of your if/else logic, I see things like:
if (2.5 < timeElapsed && timeElapsed < 5) {
arcadeDrive.drive(1, 0);
} else if (timeElapsed < 7) {
arcadeDrive.drive(0.75, -.6);
solenoid2.set(DoubleSolenoid.Value.kForward);
} else if (timeElapsed < 9) {
solenoid2.set(DoubleSolenoid.Value.kReverse);
arcadeDrive.drive(-1, -.8);
} else if (timeElapsed < 11) {
arcadeDrive.drive(-.25, 0);
} else if (timeElapsed > 12) {
arcadeDrive.drive(0, 0);
arcadeDrive.setSafetyEnabled(true);
}
I would expect the following behavior from the code above (is this what you want to happen?):
- For the first 2.5 seconds, the first condition would be false and the second condition would be true (arcadeDrive.drive(0.75, -.6) and solenoid set).
- For time 2.5 to 5.0 the first condition would be true (arcadeDrive.drive(1, 0)
- For time 5.0 to 7.0 the second condition would be true again (arcadeDrive.drive(0.75, -.6) and solenoid set)
- For time 7.0 to 9.0 the third condition would be true (solenoid2.set(DoubleSolenoid.Value.kReverse); arcadeDrive.drive(-1, -.8);
- For time 9.0 to 11.0 the forth condition would be true arcadeDrive.drive(-.25, 0);
- For time 11.0 to 12.0 NO condition would be true, motors would be uncontrolled and continue running arcadeDrive.drive(-.25, 0) unless motor safety is enabled and kills them.
- For time 12.0 and later the last condition would be true (motors would be stopped).
Finally, you really want to avoid any use of Timer.delay() in the teleop portion of your robot. If the driver happens to be moving the robot at 10 ft/sec and presses a button that fires off one of your Timer.delay(0.5) calls, the robot will move 5 feet before the operator will have a chance to control it again (hopefully the motor safety will kick in and stop the robot if this condition occurs). You want to avoid features like gamepad rumble if they will disable the ability to drive the robot.
For an iterative base robot, you will need to keep track of the state of your different events. One way to do this is with small helper classes. For example, the following Rumbler class keeps track of the rumbling state of a gamepad and turns it off after the appropriate delay (provided you use the start() method to start it and periodically call its periodic() method):
package org.usfirst.frc.team868.robot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
public class Rumbler {
private final XboxController gamepad;
private boolean rumbling;
private double startTime;
private double duration;
private RumbleType rtype;
public Rumbler(XboxController gamepad, double duration, RumbleType rtype) {
this.gamepad = gamepad;
this.rumbling = false;
this.startTime = 0;
this.duration = 0.5;
this.rtype = RumbleType.kRightRumble;
}
public Rumbler(XboxController gamepad) {
this(gamepad, 0.5, RumbleType.kRightRumble);
}
/**
* Start rumbling the gamepad.
*/
public void start() {
rumbling = true;
startTime = Timer.getFPGATimestamp();
}
/**
* Periodic check to see if we need to turn off rumbling (does not block).
*/
public void periodic() {
if (rumbling) {
double now = Timer.getFPGATimestamp();
if ((now - startTime) > duration) {
rumbling = false;
gamepad.setRumble(rtype, 0);
}
}
}
}
You could make use of this in your code by wrapping a gamepad inside a Rumbler object:
XboxController xbox2 = new XboxController(1); // Manipulator Xbox
// (xboxController)
// Used to trigger rumble feature on Manipulator gamepad
Rumbler rumble2 = new Rumbler(xbox2);
Then in your teleopPeriodic() you could make use of this and get rid of some of your timer delays:
// -----------------------Manipulator
// Controller---------------------------------//
// Always update state of rumble on gamepad
rumble2.periodic();
if (xbox2.getBButton()) {
if (solenoid1_trigger != true) {
solenoid1.set(DoubleSolenoid.Value.kForward);
solenoid1_trigger = true;
// Turn on rumbling (periodic() call above will turn it off at the appropriate time)
rumble2.start();
} else {
solenoid1.set(DoubleSolenoid.Value.kReverse);
solenoid1_trigger = false;
Timer.delay(.5);
}
}
// ...
You would want to do something similar to keep track of your solenoid states so that you remove those timer delay invocations as well.
These kind of situations are why the command based robot framework was designed.
Good luck.
