Quote:
Originally Posted by Tom Bottiglieri
If you want to see more I can send you some code snippets.
|
Here is the AutoModeController. You give this guy a list of commands, and then tell it to execute them.
PHP Code:
public class AutoModeController {
// Variables
NutronsQueue cmdList = new NutronsQueue();
AutoModeCommand curCmd = null;
// Add commands
public void addCommand(AutoModeCommand cmd) {
cmdList.push(cmd);
}
public void flush() {
cmdList.removeAllElements();
curCmd = null;
}
// Execute commands
public void handle() {
boolean firstRun = false;
if(curCmd == null) {
curCmd = (AutoModeCommand) cmdList.pop();
if(curCmd != null) {
System.out.println("Popped a command: " + curCmd);
}
firstRun = true;
}
if(curCmd != null) {
if(firstRun) {
curCmd.init();
firstRun = false;
}
if(curCmd.doWork()) {
// this will happen when command is done
// Will grab next command on next call of handle()
curCmd.finish();
curCmd = null;
System.out.println("work done");
}
}
else {
// Maybe add something for do nothing?
}
}
}
Here's an example of an AutoModeCommand (In this case, it raises the elevator)
PHP Code:
public class SignalElevatorPosCommand implements AutoModeCommand {
double wantedPos = 0;
public SignalElevatorPosCommand(double pos) {
wantedPos = pos;
}
public boolean doWork() {
ElevatorController.getInstance().gotoScoringPosition(wantedPos);
return true;
}
public void init() {
// Init code for this command goes here!
}
public void finish() {
// Clean up goes here!
}
}
Here's a way to set it all up:
PHP Code:
public class MyCoolAutonomousRobot extends IterativeRobot {
AutoModeController ac = new AutoModeController();
public void autonomousInit() {
ac.flush();
ac.addCommand(new DriveDistanceCommand(10, 0.75, 10)); // 10 feet, .75 power, 10 sec timeout
ac.addCommand(new SignalElevatorPosCommand(Elevator.POS_TOP_PEG));
ac.addCommand(new CapTubeCommand());
ac.addCommand(new DriveDistanceCommand(-10,0.75,10));
ac.addCommand(new TurnDegreesCommand(-180, 1.0, 3.0); // 180 deg, 1.0 max power, 3s timeout
}
public void autonomousPeriodic() {
ac.handle();
}
}
Hope this helps!