Announcement

Collapse
No announcement yet.

Issue with Autonomous E-Stop not stopping program right away

Collapse
This topic is closed.
X
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • Issue with Autonomous E-Stop not stopping program right away

    Hi all,

    We're having an issue with our autonomous. When the E-Stop button is pressed in the Robot Controller, it takes several seconds to stop. At our league meets, we were told that the reason was because we needed to add another condition to our loops, a function or condition that would check to see if the E-Stop button had been pressed, and that it was in all the example code. Well, I've looked and haven't found anything that looks right.

    We're using a program with a main loop, and a switch case inside it. In each case, it calls a separate function outside of the main loop, which loops until a certain condition is reached. Here's our code:

    Code:
    package org.firstinspires.ftc.teamcode;
    
    import android.os.SystemClock;
    
    import com.kauailabs.navx.ftc.AHRS;
    import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
    import com.qualcomm.robotcore.eventloop.opmode.OpMode;
    import com.qualcomm.robotcore.hardware.ColorSensor;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.LightSensor;
    import com.qualcomm.robotcore.hardware.Servo;
    import com.qualcomm.robotcore.hardware.TouchSensor;
    
    /**
     * Created by Robotics Team 4543 on 11/16/2016.
     */
    
    @Autonomous(name = "Blue",  group = "Autonomous")
    public class BlueBeacon extends OpMode {
    
        //Motor assignments and variables
        DcMotor motorRight; //Assinging the name "motorRight" to the motor
        DcMotor motorLeft;  //same, but with "motorLeft"
        double left;
        double right;
    
        //Sensors
        LightSensor sensorLeft;
        LightSensor sensorRight;
        Servo servoleft;
        Servo servoright;
        ColorSensor colorSensor;
        TouchSensor beaconSensor;
    
        //For the Line follow portion
        boolean line = false;
    
        //Switch Case state variable
        byte state = 0;
    
        //Navx Variables
        private final int NAVX_DIM_I2C_PORT = 1;
        private AHRS navx_device;
    
    
        @Override
        public void init() {
    
            motorLeft = hardwareMap.dcMotor.get("motor_2"); //Sets motorRight in the program to the config file name "motor_1"
            motorRight = hardwareMap.dcMotor.get("motor_1"); //Same, but with motorLeft and "motor_2"
            motorLeft.setDirection(DcMotor.Direction.REVERSE); //Sets the motorRight in reverse
    
            //Servo hardware map
            servoleft = hardwareMap.servo.get("servo_1");
            servoright = hardwareMap.servo.get("servo_2");
    
            //Sensor hardware map
            //Light
            sensorLeft = hardwareMap.lightSensor.get("left");
            sensorRight = hardwareMap.lightSensor.get("right");
            sensorLeft.enableLed(true);
            sensorRight.enableLed(true);
            //Color
            colorSensor = hardwareMap.colorSensor.get("color sensor");
            colorSensor.enableLed(false);
            //NavX
            navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), NAVX_DIM_I2C_PORT, AHRS.DeviceDataType.kProcessedData);
            //Touch sensor
            beaconSensor = hardwareMap.touchSensor.get("beacon");
    
            //Zero the position of the NavX
            navx_device.zeroYaw();
    
            //Set initial servo positions
            servoleft.setPosition(0.52);
            servoright.setPosition(0.52);
    
    
        }
    
    
        @Override
        public void loop() {
    
            switch(state){
                case 0:
                    runToInches(-57);
                    break;
    
                case 1:
                    turnToAngle(0);
                    break;
    
                case 2:
                    turnToAngle(-90);
                    break;
    
                case 3:
                    runToInches(21);
                    break;
    
                case 4:
                    turnToLine("right");
                    break;
    
                case 5:
                    followLine();
                    break;
    
                case 6:
                    pressBeacon();
                    break;
    
                case 7:
                    //Drive forwards half a second, then drive more Leftwards in order to try to press the correct button
                    SystemClock.sleep(1024);
    
                    //Turn off the motors, hopefully the button has been pressed.
                    motorLeft.setPower(0.0);
                    motorRight.setPower(0.0);
    
                    //Retract both servos- If we keep the button pressed more than 5 seconds, it'll change to red!
                    servoleft.setPosition(0.52);
                    servoright.setPosition(0.52);
    
                    state += 1;
                    break;
    
                case 8:
                    telemetry.addData("Beacon Acquired!", "");
                    state += 1;
                    break;
    
                case 9:
                    runToInches(-42);
                    break;
    
                case 10:
                    turnToAngle(23);
                    break;
    
                case 11:
                    runToInches(-21);
                    break;
            }
    
        }
    
    
        @Override
        public void stop() {
            motorLeft.setPower(0.0);
            motorRight.setPower(0.0);
    
            servoleft.setPosition(0.52);
            servoright.setPosition(0.52);
        }
    
    
        //Function that will run the robot forward a certain amount of inches, while autocorrecting using the NavX
        void runToInches(int inches){
    
            //Base motor power variables
            double power = 0.5;
            double correct = 0.0;
            double correctVal = 0.06;
    
            //Variable for storing the yaw reading, so we only need to read it once per loop
            double gotYaw = 0;
    
            // Reset the encoders
            motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    
            //Zero the yaw on the navX to help combat yaw drift
            navx_device.zeroYaw();
    
            // Set Target based on the input (90 is the number of ticks/inch)
            motorLeft.setTargetPosition(inches * 90);
            motorRight.setTargetPosition(inches * 90);
    
            // We're only using the Left encoder for distance measurement
            motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    
            //If the robot is going backwards, then reverse the correctVal value so that it won't mess up the drive straight function
            if(inches < 0){
                correctVal = correctVal - correctVal * 2;
            }
    
            //While the motor is driving to the position:
            while(motorLeft.isBusy()){
    
                //Get the current yaw and store it in a variable
                gotYaw = navx_device.getYaw();
    
                //If it's too far in the right direction, Set correctLeft to 0.25
                if(gotYaw > 1.0){
                    correct = correctVal;
                }
    
                //If it's too far in the left direction, Set correctRight to 0.25
                if(gotYaw < -1.0){
                    correct = -correctVal;
                }
    
                //If it's right where we want it, Set set both to Zero
                if(gotYaw > -1.0 && gotYaw < 1.0){
                    correct = 0.0;
                    correct = 0.0;
                }
    
                //Subtract the correction values from the left and right motor variables
                left = power + correct;
                right = power - correct;
    
                motorRight.setPower(right);
                motorLeft.setPower(left);
    
            }
    
            //Turn off motors, reset encoders, and Advance the state
            right = 0.0;
            left = 0.0;
            motorLeft.setPower(left);
            motorRight.setPower(right);
            state += 1;
    
        }
    
    
        //Function to turn to a specified angle using the navX
        void turnToAngle(double angle){
    
            //Determine which way we are going, then add or subtract 10 to account for overshoot caused by program lag.
            if(angle < 0){
                //Because of lag in the program, it will overshoot by -10 degrees unless we add 10 first.
                angle += 10;
            }
            if(angle > 0){
                //Because of lag in the program, it will overshoot by 10 degrees unless we subtract 10 first.
                angle -= 10;
            }
    
            //Set the motor RunModes
            motorLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            motorRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    
            //Set the power the motors will use.
            double power = 0.25;
    
            //Variable for subtracting the turn value
            double turn = 0.0;
    
            //Sets reachedAngle to false
            boolean reachedAngle = false;
    
            //Variable for storing the yaw reading, so we only need to read it once per loop
            double gotYaw = 0;
    
            //This will loop until it sets reachedAngle to true within itself
            while(!reachedAngle){
    
                //Get the current yaw and store it in a variable
                gotYaw = navx_device.getYaw();
    
                //If it's right where we want it, set power to Zero
                if(gotYaw <= angle + 1.55 && gotYaw >= angle - 1.55){
    
                    //Turn off the motors
                    motorRight.setPower(0.0); //setting the power of the right motor to the variable "right"
                    motorLeft.setPower(0.0); //same, but with variable "left" and the left motor
                    reachedAngle = true;
                    turn = 0.0;
                    power = 0.0;
                }
    
                //If it's getting close, slow the motors down
                if(gotYaw <= angle + 15.0 && gotYaw >= angle - 15.0){
                    power = 0.08;
                }
    
                //If it's to the right, Set turn to power
                if(gotYaw > angle){
                    turn = power;
                }
    
                //If it's too the left, Set turn to -power
                if(gotYaw < angle){
                    turn = -power;
                }
    
                //Set motor powers:
                motorRight.setPower(-turn); //setting the power of the right motor to the variable "right"
                motorLeft.setPower(turn); //same, but with variable "left" and the left motor
    
            }
    
            state += 1;
        }
    
    
        //Function for pressing the correct button on the beacon
        void pressBeacon(){
    
            boolean pressedButton = false;
    
            //Because of the shape of the robot, we can ensure it's lined up by turning on the right motor only. It'll hit the wall before it presses the right button.
            //motorLeft.setPower(0.125);
    
            //Loop until the robot sees a definite color and presses the button
            while(!pressedButton){
                //If the sensor sees Blue then press the blue button
                if (colorSensor.blue() > colorSensor.red()){
                    servoleft.setPosition(0.87);
                    pressedButton = true;
    
                    //Set motor powers to turn slightly towards the button we want to push, ensuring it gets pushed.
                    motorLeft.setPower(0.25);
                    motorRight.setPower(0.126);
                }
    
                //If the sensor sees red then press the other (blue) button
                if (colorSensor.red() > colorSensor.blue()){
                    servoright.setPosition(0.2);
                    pressedButton = true;
    
                    //Set motor powers to turn slightly towards the button we want to push, ensuring it gets pushed.
                    motorLeft.setPower(0.126);
                    motorRight.setPower(0.25);
                }
    
            }
    
            //Advance the state after pressing the correct button
            state += 1;
        }
    
    
        //Function for following the line once found
        void followLine(){
    
            boolean endLine = false;
    
            boolean driveDirection = false; //False means it needs to turn left, true means turn right
            int dumbTimer1 = 0;
            int dumbTimer2 = 0;
    
            while(!endLine){
    
                //Read sensor values, assign directions
                if(sensorRight.getLightDetected() > 0.3) {driveDirection = false;}
                if(sensorLeft.getLightDetected() > 0.3) {driveDirection = true;}
    
                //If we need to go left, go left.
                if(!driveDirection){
                    left = 0.0;
                    right = 0.3;
                }
    
                //If we need to go right, go right.
                if(driveDirection){
                    left = 0.3;
                    right = 0.0;
                }
    
                motorLeft.setPower(left);
                motorRight.setPower(right);
    
                if(beaconSensor.isPressed()){
                    endLine = true;
                    motorRight.setPower(0.0);
                    motorLeft.setPower(0.0);
                }
    
            }
    
            state += 1;
    
        }
    
    
        //Function for finding the line after driving close to it
        void turnToLine(String direct){
    
            //Set the motor RunModes
            motorLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            motorRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    
            boolean lineFound = false;
    
            if(direct == "left" || direct == "Left"){
                while(!lineFound){
                    motorLeft.setPower(0.30);
    
                    if(sensorRight.getLightDetected() > 0.35){
                        lineFound = true;
                    }
                }
            }
    
            if(direct == "Right" || direct == "right"){
                while(!lineFound){
                    motorRight.setPower(0.30);
    
                    if(sensorLeft.getLightDetected() > 0.35){
                        lineFound = true;
                    }
                }
            }
    
            state += 1;
    
        }
    
        void zeroYaw(){
            navx_device.zeroYaw();
        }
    
    
    }
    Has anyone any advice on the matter? Other than that, our autonomous works pretty well, but unless that gets fixed, we're not passing inspection.

    Thanks, All!

  • #2
    I wouldn't expect that to work at all. You aren't supposed to have loops in a regular OpMode.

    They were thinking of LinearOpMode, where a method is available to determine if it's stopped or not.

    You could emulate it by overriding the stop() method and setting a variable which you check.

    Comment


    • #3
      Thanks for the quick reply!
      Yeah, I was wondering if that's what they were thinking of.

      How would I do what you're talking about? Is there something I can check to see if the E-Stop button has been pressed?

      Comment


      • #4
        The stop() method is called when the button is pressed. Given that, you can use that to set a boolean variable which you could then check later on.

        Code:
        boolean isEStopped = false;
        
        @Override
        public void stop(){
           isEStopped = true;
        }

        Comment


        • #5
          Ok, thanks!

          I'll try that and see how it goes. Personally, I see nothing wrong with calling loops outside of the main loop, but I can see why it would mess it up- Hopefully this works.

          I'll get back to this thread after I test it.

          Comment


          • #6
            "Personally, I see nothing wrong with calling loops outside of the main loop"

            Iterative opmode is designed for the loop method to be called ~50 times a second. If you block it in a loop that has a long delay, it brakes this design.
            That is why linearOpmode exists - to allow individuals to choose which model they prefer. Each has its pros and cons.

            Comment


            • #7
              Well. I've tried the code you supplied, FTC4160, but it hasn't worked. Thanks for the suggestion, however! The time it takes to stop seems to have improved, but it still takes several seconds.

              I suppose the only way to go now is to convert it all into a Linear Op mode?

              Comment


              • #8
                Hurrah! I've converted it into a linear op mode and no have no problems! It stops right away as it should, and all my carefully crafted code still works! Thanks for your help, guys!

                Comment

                Working...
                X