It looks like you forgot to remove the interrupt() invocation and check that the thread was not running before attempting to start it again.
However, it looks like you are now using the Command based framework in your robot project. In this case you should not need any threading to accomplish the automated task.
As a rough guess, it appears that you are automating the task of kicking the boulder and this task involves spinning up some shooter motors and using a servo to feed the boulder into the shooter. My guess is that you are trying to:
- Move the servo back to keep the ball out of the “spinners”.
- Spin up your motors.
- Move the servo into a “kick” position to push the boulder through the spinners.
- Let the boulder pass through the spinners (wait a bit).
- Stop the spinners
- Return the servo to its default position
Using your existing Shooter subsystem, you should be able to create two new simple commands and one command group to accomplish this.
SetShooterSpeed - Simply sets the power on your shooter spinners and then quits (leaves the motors running).
SetKickerPosition - Sets the position of the shooter servo.
AutonKickCommand - Uses the two above commands and the built in WaitCommand to automate the entire sequence.
Here is a start of SetShooterSpeed.java:
package org.usfirst.frc.team2847.robot.commands;
import org.usfirst.frc.team2847.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
/**
* Command that sets power to shooter wheels and leaves them running.
*/
public class SetShooterSpeed extends Command {
double speed;
public SetShooterSpeed(double speed) {
requires(Robot.shooter);
this.speed = speed;
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.shooter.spinShooters(speed);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return true;
}
// Called once after isFinished returns true
protected void end() {
// Leave shooters running
// Robot.shooter.spinShooters(0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
Here is SetKickerPosition:
package org.usfirst.frc.team2847.robot.commands;
import org.usfirst.frc.team2847.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
/**
* Command that sets power to shooter wheels and leaves them running.
*/
public class SetKickerPosition extends Command {
double angle;
public SetKickerPosition(double angle) {
requires(Robot.shooter);
this.angle = angle;
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.shooter.setKickAngle(angle);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return true;
}
// Called once after isFinished returns true
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
Finally, here is the command group that ties it all together:
package org.usfirst.frc.team2847.robot.commands;
import org.usfirst.frc.team2847.robot.RobotMap;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.WaitCommand;
public class AutonKick extends CommandGroup {
public AutonKick(double speed) {
super("AutonKick " + speed);
// Get .25 seconds for kicker to get into default position
addSequential(new SetKickerPosition(RobotMap.kickDefaultAngle));
addSequential(new WaitCommand(0.25));
// Start spinning up shooters and give them 2 seconds to reach speed
addSequential(new SetShooterSpeed(speed));
addSequential(new WaitCommand(2.0));
// Move kicker into kick position and give .5 second for boulder
// to pass through
addSequential(new SetKickerPosition(RobotMap.kickHitAngle));
addSequential(new WaitCommand(0.5));
// Stop shooter and return kicker to default position
addSequential(new SetShooterSpeed(0));
addSequential(new SetKickerPosition(RobotMap.kickDefaultAngle));
}
}
NOTE: All of this is a “best guess”, make sure you refactor it to how your robot is actually designed. Also, thanks for providing the github link. It made it much easier to review your code by being able to pull down a copy in Eclipse.
Hope that helps.