TeleOp Automation

[CoachZM]

For our capstone we had programmed something similar to this:

Press X:
Raise Capstone Servo
Sleep 1 Second
Open Capstone Holder
Sleep 1 Second
Lower Capstone Servo
Sleep 1 Second
etc...

The problem is obviously that adding a sleep time into TeleOp causes it to have issues. How can we automate this task in auto without using sleep times? We have looked at some timer options but are really drawing a blank. We want it to be as easy for the drive team as possible to where they hit the button once and it just completes the whole task.

Thanks!

[10298Programmer]

When our team need to do something like this in teleOp, we use a state machine.

[Westside]

.
A draft tutorial on FTC Timers is posted here. It's written for Blocks, but may also benefit Java programmers.
.

[FLARE]

Set up an ElapsedTime object at the class level, like:

ElapsedTime timer = new ElapsedTime();

Then every time you want to "sleep", reset your timer and loop for the length of time you want. Then you can check for opModeIsActive while you wait for your hardware to finish up.

double waitTime = 1;
timer.reset();
while (opModeIsActive() && (timer.time() < holdTime)) {
     // probably a good idea to put some telemetry here
}

You can also set the resolution on your timer to milliseconds for shorter wait times.

[mikets]

Here is a sample how it is done one way. I took the sample code BasicOpMode_Iterative.java and made minimum modification to make it do pseudo-multitasking. Basically, the loop method is called periodically and it runs through a number of "tasks". So each task will make progress in turn. The only important is that each task cannot contain any wait/sleep type of statements or any busy loops. In other words, each task must return without blocking. That's why the runCapstoneTask is a state machine.

/* Copyright (c) 2017 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

package org.firstinspires.ftc.robotcontroller.external.sam ples;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

/**
* This file contains an example of an iterative (Non-Linear) "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When an selection is made from the menu, the corresponding OpMode
* class is instantiated on the Robot Controller and executed.
*
* This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot
* It includes all the skeletal structure that all iterative OpModes contain.
*
* Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/

@TeleOp(name="Basic: Iterative OpMode", group="Iterative Opmode")
@Disabled
public class BasicOpMode_Iterative extends OpMode
{
    // Declare OpMode members.
    private ElapsedTime runtime = new ElapsedTime();
    private DcMotor leftDrive = null;
    private DcMotor rightDrive = null;

    /*
     * Code to run ONCE when the driver hits INIT
     */
    @Override
    public void init() {
        telemetry.addData("Status", "Initialized");

        // Initialize the hardware variables. Note that the strings used here as parameters
        // to 'get' must correspond to the names assigned during the robot configuration
        // step (using the FTC Robot Controller app on the phone).
        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
        rightDrive = hardwareMap.get(DcMotor.class, "right_drive");

        // Most robots need the motor on one side to be reversed to drive forward
        // Reverse the motor that runs backwards when     connected directly to the battery
        leftDrive.setDirection(DcMotor.Direction.FORWARD);
        rightDrive.setDirection(DcMotor.Direction.REVERSE) ;

        // Tell the driver that initialization is complete.
        telemetry.addData("Status", "Initialized");
    }

    /*
     * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
     */
    @Override
    public void init_loop() {
    }

    /*
     * Code to run ONCE when the driver hits PLAY
     */
    @Override
    public void start() {
        runtime.reset();
    }

    private void runDriveTask() {
        // Setup a variable for each drive wheel to save power level for telemetry
        double leftPower;
        double rightPower;

        // Choose to drive using either Tank Mode, or POV Mode
        // Comment out the method that's not used. The default below is POV.

        // POV Mode uses left stick to go forward, and right stick to turn.
        // - This uses basic math to combine motions and is easier to drive straight.
        double drive = -gamepad1.left_stick_y;
        double turn = gamepad1.right_stick_x;
        leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
        rightPower = Range.clip(drive - turn, -1.0, 1.0) ;

        // Tank Mode uses one stick to control each wheel.
        // - This requires no math, but it is hard to drive forward slowly and keep straight.
        // leftPower = -gamepad1.left_stick_y ;
        // rightPower = -gamepad1.right_stick_y ;

        // Send calculated power to wheels
        leftDrive.setPower(leftPower);
        rightDrive.setPower(rightPower);
    }

    private boolean buttonAPressed = false;
    private void runOperatorTask() {
        if (!buttonAPressed && gamepad2.a) {
            // button A is pressed
            capstoneTaskStep = 1; //start capstone task
            buttonAPressed = true;
        } else if (buttonAPressed && !gamepad2.a) {
            // button A is released
            buttonAPressed = false;
        }
    }

    private int capstoneTaskStep = 0;
    private void runCapstoneTask() {
        switch (capstoneTaskStep) {
            case 1:
                // raise capstone servo.
                // set elapsed timer to 1 second.
                capstoneTaskStep = 2;
               break;
            case 2:
                if (elapsedTimer expired) {
                    // open capstone holder.
                    // set elapsed timer to 1 second
                    capstoneTaskStep = 3;
                }
                break;
            case 3:
                if (elapsedTimer expired) {
                    // lower capstone servo.
                    //capstoneTaskStep = 4;
                }
                break;
            case 4:
                ... // etc
                break;
            default:
                break;
        }
    }

    /*
     * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
     */
    @Override
    public void loop() {
        runDriveTask();
        runOperatorTask();
        runCapstoneTask();

        // Show the elapsed game time and wheel power.
        telemetry.addData("Status", "Run Time: " + runtime.toString());
        telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);
    }

    /*
     * Code to run ONCE after the driver hits STOP
     */
    @Override
    public void stop() {
    }

}

[FLARE]

Set up an ElapsedTime object at the class level.

So yeah, ignore that - I thought by "causes it to have issues", you meant an emergency Stop might get ignored. But duh on me, this is teleOp... so it would probably be nice if you could actually drive the robot... :-P

State machine is the way to go.

[mikets]

Just realized I did not make it clear on how to use the ElapsedTime class to wait without using a while-loop. Here is the modified runCapstoneTask method that shows how:

private int capstoneTaskStep = 0;
private double expiredTime = 0.0;
private void runCapstoneTask() {
    switch (capstoneTaskStep) {
        case 1:
            // raise capstone servo.
            expiredTime = runtime.time() + 1.0;
            capstoneTaskStep = 2;
            break;
        case 2:
            if (runtime.time() >= expiredTime) {
                // open capstone holder.
                expiredTime = runtime.time() + 1.0;
                capstoneTaskStep = 3;
            }
            break;
        case 3:
            if (runtime.time() >= expiredTime) {
                // lower capstone servo.
                expiredTime = runtime.time() + 1.0;
                capstoneTaskStep = 4;
            }
            break;
        case 4:
            ... // etc
            break;
        default:
            break;
    }
}