No Robot Left Behind 2014

I was thinking this week about an old thread I remembered in 2008. For anyone who doesn’t know or doesn’t remember the 2008 game, robots could gain points by simply driving forward in autonomous. Now, in 2008, there was less space and a longer distance to drive, but it was still very doable for almost any team. That year, there was a thread urging teams to help other, perhaps more inexperienced teams, out there and moving in auto, and I’d like to encourage this for Aerial Assist.

What I would like from this thread are easy ways, in your language/template/architecture of choice, to make a robot drive forward for a certain amount of time, then stop. This will provide teams with extra members or bored programmers with a nice palette of solutions from which to draw from for certain situations to help teams drive forward then stop!

Please list assumptions (I think the team being able to drive and having code for that is a safe assumption, but not a ton else).

I’ll start first, with Command-Based Java.

Assumptions:

  • CommandBase already has a subsystem for the drive, named drive.
  • drive has a method called driveForward(double speed) which uses the elements of the Subsystem to drive the robot forward at the argued speed. This might be something you need to work with the team to implement, or use their existing methods in another way to do the driving.
  • drive has a method called stop() which stops all drive motors. This might be something you need to work with the team to implement, or use their existing methods in another way to do the stopping.

You’ll want a command to do the driving forward and stopping:


package sample;

import edu.wpi.first.wpilibj.Timer;

public class DriveForwardThenStop extends CommandBase {

    Timer timer;
    double time;
    double speed;

    public DriveForwardThenStop(double seconds, double speed) {
        requires(drive);
        timer = new Timer();
        time = seconds;
        this.speed = speed;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        drive.stop();
        timer.start();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        drive.driveForward(speed);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (timer.get() > time);
    }

    // Called once after isFinished returns true
    protected void end() {
        drive.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
    }
}

And you’ll need to run that command during autonomous in the main robot class. Adjust arguments to the command based on robot behavior. Try it out on the practice field!


...

class ExampleRobot extends IterativeRobot{

...

    Command autonomousCommand;

...

    public void robotInit() {
        autonomousCommand = new DriveForwardThenStop(2.0, 0.5);

        // Initialize all subsystems
        CommandBase.init();
    }

    public void autonomousInit() {
        autonomousCommand.start();
    }

    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }
...

}

Anyway, have at it! Here’s a list of common frameworks that we could use examples of solutions in:

  • IterativeRobot Java
  • SimpleRobot Java
  • CommandBased C++
  • IterativeRobot C++
  • SimpleRobot C++
  • LabVIEW (not sure if there are multiple templates/architectures
  • Python?

P.S. Make sure you remember to stop!

Alright, I’ll bite:

(Add this to your current code. Anything already there can remain. Replace any instances of “chassis” with the name of your drivetrain object.)
Java SimpleRobot:

Declaration phase:

import edu.wpi.first.wpilibj.Timer;

Autonomous mode:

public void autonomous ()  {
   chassis.setSafetyEnable(false);
   chassis.drive(-.5,0);
   Timer.delay(2.0);
   chassis.drive(0,0);

Iterative Robot:


public class RobotTemplate extends IterativeRobot {

    /**
     * This function is called at the beginning of autonomous
     */
    private static final long DRIVE_FORWARD_TIME_IN_MILLS = 0;
    private static final double DRIVE_FORWARD_SPEED = .2;
    private long startTime = 0;
    public void autonomousInit() {
        startTime = System.currentTimeMillis();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        //drive forward for the first n milliseconds of autonomous
        if(System.currentTimeMillis() - startTime  < DRIVE_FORWARD_TIME_IN_MILLS){
            drive.driveForward(DRIVE_FORWARD_SPEED);
        }else{
            drive.stop();
        }
    }

}

Simple Robot:


public class RobotTemplate extends SimpleRobot {
/**
     * This function is called once each time the robot enters autonomous mode.
     */
    private static final long DRIVE_FORWARD_TIME_IN_MILLS = 0;
    private static final double DRIVE_FORWARD_SPEED = .2;
    private long startTime = 0;
    public void autonomous() {
        startTime = System.currentTimeMillis();
        
        while(isOperatorControl() && isEnabled()){
            
            //drive forward for the first n milliseconds of autonomous
            if(System.currentTimeMillis() - startTime  < DRIVE_FORWARD_TIME_IN_MILLS){
                drive.driveForward(DRIVE_FORWARD_SPEED);
            }else{
                drive.stop();
            }
            Timer.delay(0.01);
            
        }
        
    }
}

Here’s a Labview autonomous based on a 4CIM tank drive.
Motors can be easily added and removed as needed. Be sure to look at the comments, and configure begin correctly.


Autonomous Independent.vi (48.5 KB)



Autonomous Independent.vi (48.5 KB)

Here’s a python sample for a standard two-motor robot. You can test it using the pyfrc robot simulator.



try:
    import wpilib
except ImportError:
    from pyfrc import wpilib

class MyRobot(wpilib.SimpleRobot):

    def __init__(self):
        super().__init__()
        
        self.stick = wpilib.Joystick(1)
        
        # two wheel drive
        l_motor = wpilib.Jaguar(1)
        r_motor = wpilib.Jaguar(2)
        self.drive = wpilib.RobotDrive(l_motor, r_motor)

    def Autonomous(self):
        '''Called when autonomous mode is enabled'''
        
        timer = wpilib.Timer()
        timer.Start()
        done = False
        
        self.GetWatchdog().SetEnabled(False)
        while self.IsAutonomous() and self.IsEnabled():
            
            if not done and not timer.HasPeriodPassed(5):
                self.drive.ArcadeDrive(0.4, 0)
            else:
                self.drive.ArcadeDrive(0, 0)
                done = True
            
            wpilib.Wait(0.01)

    def OperatorControl(self):
        '''Called when operation control mode is enabled'''
        
        dog = self.GetWatchdog()
        dog.SetEnabled(True)
        dog.SetExpiration(0.25)

        timer = wpilib.Timer()
        timer.Start()

        while self.IsOperatorControl() and self.IsEnabled():
            dog.Feed()
            self.drive.ArcadeDrive(self.stick)
            wpilib.Wait(0.04)

def run():
    '''Called by RobotPy when the robot initializes'''
    
    robot = MyRobot()
    robot.StartCompetition()
    
    return robot


if __name__ == '__main__':
    wpilib.run()


I attached two images. First is the code from the upper disabled structure already in the Autonomous VI. It was given different speeds and delay values. It assumes that you opened a two or four motor drive in Begin and named it “Left and Right Motors”. If you happen to have a six wheel robot without Y-cables, you can also use both a 2 and 4 motor drive in parallel.

I also moved the code to the local computer target and ran it in the simulator. The image shows what that is like. This lets you compare your wiring to the robots wiring and work out motor inversions, positive/negative values, etc. Clearly, I wouldn’t trust the distance the robot will travel will match your real robot, but it is something to practice with.

Greg McKaskle