In your else block, you don't seem to be stopping the motors. Try changing all those calls to set 0.
Also, I noticed that you're using the command based framework (based on the code you left commented out).
Why not make an autonomous command and use the internal timer of each command to run your autonomous? Something like this:
Code:
class AutoCommand extends Command {
public AutoCommand(){
}
public void init(){
setTimeout(2); // here's where you set the internal timer of commands
}
public void execute(){
Robot.drivetraintank.setLeft_Back(0.5);
Robot.drivetraintank.setLeft(0.5);
Robot.drivetraintank.setRight(0.5);
Robot.drivetraintank.setRight_Back(0.5);
}
public boolean isFinished(){
return isTimedOut(); // this ends the command once the 2 seconds you set in init() are up
}
// in the last 2 methods, clean up by zeroing out the motors
// this ensures that motor output stops once the command ends or is cancelled
public void end(){
Robot.drivetraintank.setLeft_Back(0);
Robot.drivetraintank.setLeft(0);
Robot.drivetraintank.setRight(0);
Robot.drivetraintank.setRight_Back(0);
}
public void interrupted(){
Robot.drivetraintank.setLeft_Back(0);
Robot.drivetraintank.setLeft(0);
Robot.drivetraintank.setRight(0);
Robot.drivetraintank.setRight_Back(0);
}
}
Then in your robot class
Code:
Command autoCommand = new AutoCommand();
public void autonomousInit(){
autoCommand.start();
}
public void autonomousPeriodic(){
Scheduler.getInstance().run();
}
public void teleopInit(){
// it's always good practice to do this, in case you make some change to your auto command that ends up taking too long to run
// we put code in interrupted() to handle this case
autoCommand.cancel();
}