Announcement

Collapse
No announcement yet.

Encoders current position running opposite of target position in autonomous

Collapse
X
  • Filter
  • Time
  • Show
Clear All
new posts

  • Encoders current position running opposite of target position in autonomous

    Hello, my team this year decided to use encoders to help make autonomous a bit easier but after I set up the code, I found that the program wasn't doing anything I was telling it to do. I tested and found that the problem lies in the target position and the current position. Originally the target position was 33 and the current position would go negative to the point where the integer became so large it would reset to zero so I tried setting the target position negative and now the current position goes into positive infinity. Our robot uses AndyMark NevRest 60 Motors and 2 Rev Expansion hubs to control it. Thanks!
    Code:
    @Autonomous(name="AutonomousMain", group="Pushbot")
    public class AutonomousMain extends LinearOpMode { 
          private ElapsedTime     runtime = new ElapsedTime();      static final double     COUNTS_PER_MOTOR_REV    = 7 ;    // eg: TETRIX Motor Encoder     static final double     DRIVE_GEAR_REDUCTION    = 60.0 ;     // This is < 1.0 if geared UP     static final double     WHEEL_DIAMETER_INCHES   = 4.0 ;     // For figuring circumference     static final double     COUNTS_PER_INCH         = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /             (WHEEL_DIAMETER_INCHES * 3.1415);     static final double     DRIVE_SPEED             = 0.6;     static final double     TURN_SPEED              = 0.5;      public DcMotor frontLeftW = null;     public DcMotor frontRightW = null;     public DcMotor backLeftW = null;     public DcMotor backRightW = null;     public DcMotor arm = null;     public DcMotor lifter = null;     public CRServo wristL = null;     public CRServo wristR = null;      @Override     public void runOpMode() {           frontLeftW = hardwareMap.get(DcMotor.class, "FL");         frontRightW = hardwareMap.get(DcMotor.class, "FR");         backLeftW = hardwareMap.get(DcMotor.class, "BL");         backRightW = hardwareMap.get(DcMotor.class, "BR");         arm = hardwareMap.get(DcMotor.class, "arm");         lifter = hardwareMap.get(DcMotor.class, "L");         wristL = hardwareMap.crservo.get("WL");         wristR = hardwareMap.crservo.get("WR");          // Send telemetry message to signify robot waiting;         telemetry.addData("Status", "Resetting Encoders");    //         telemetry.update();          frontLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);         frontRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);         backLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);         backRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);          frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);         frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);         backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);         backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);          // Send telemetry message to indicate successful Encoder reset         telemetry.addData("Path0",  "Starting at %7d :%7d",                 frontLeftW.getCurrentPosition(),                 frontRightW.getCurrentPosition(),                 backRightW.getCurrentPosition(),                 backRightW.getCurrentPosition());         telemetry.update();          // Wait for the game to start (driver presses PLAY)         waitForStart();          // Step through each leg of the path,         // Note: Reverse movement is obtained by setting a negative distance (not speed)         encoderDrive(DRIVE_SPEED,  47,  47, 47, 47, 5.0);  // S1: Forward 47 Inches with 5 Sec timeout         encoderDrive(TURN_SPEED,   12, 12, -12, -12,  4.0);  // S2: Turn Right 12 Inches with 4 Sec timeout         encoderDrive(DRIVE_SPEED, 24, 24, 24, 24, 4.0);  // S3: Reverse 24 Inches with 4 Sec timeout          //robot.leftClaw.setPosition(1.0);            // S4: Stop and close the claw.         //robot.rightClaw.setPosition(0.0);         sleep(1000);     // pause for servos to move          telemetry.addData("Path", "Complete");         telemetry.update();     }      /*      *  Method to perfmorm a relative move, based on encoder counts.      *  Encoders are not reset as the move is based on the current position.      *  Move will stop if any of three conditions occur:      *  1) Move gets to the desired position      *  2) Move runs out of time      *  3) Driver stops the opmode running.      */      public void encoderDrive(double speed,                              double frontLeftInches, double backLeftInches,                              double frontRightInches, double backRightInches,                              double timeoutS) {          int newFrontLeftTarget = 0;         int newBackLeftTarget = 0;         int newFrontRightTarget = 0;         int newBackRightTarget = 0;          // Ensure that the opmode is still active         if (opModeIsActive()) {              // Determine new target position, and pass to motor controller             newFrontLeftTarget = (int)(frontLeftInches * COUNTS_PER_INCH);             newBackLeftTarget = (int)(backLeftInches * COUNTS_PER_INCH);             newFrontRightTarget = (int)(frontRightInches * COUNTS_PER_INCH);             newBackRightTarget = (int)(backRightInches * COUNTS_PER_INCH);             frontLeftW.setTargetPosition(newFrontLeftTarget);             backLeftW.setTargetPosition(newBackLeftTarget);             frontRightW.setTargetPosition(newFrontRightTarget);             backRightW.setTargetPosition(newBackRightTarget);              // Turn On RUN_TO_POSITION             frontLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION);             backLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION);             frontRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION);             backRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION);              // reset the timeout time and start motion.             runtime.reset();             frontLeftW.setPower(Math.abs(speed));             backLeftW.setPower(Math.abs(speed));             frontRightW.setPower(Math.abs(speed));             backRightW.setPower(Math.abs(speed));              // keep looping while we are still active, and there is time left, and both motors are running.             // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits             // its target position, the motion will stop.  This is "safer" in the event that the robot will             // always end the motion as soon as possible.             // However, if you require that BOTH motors have finished their moves before the robot continues             // onto the next step, use (isBusy() || isBusy()) in the loop test.             while (opModeIsActive() &&                     (runtime.seconds() < timeoutS) &&                     (frontLeftW.isBusy() || frontRightW.isBusy() || backLeftW.isBusy() || backRightW.isBusy())) {                  // Display it for the driver.                 telemetry.addData("Path1",  "Running to %7d :%7d", newFrontLeftTarget,  newBackLeftTarget, newFrontRightTarget, newBackRightTarget);                 telemetry.addData("Path2",  "Running at %7d :%7d",                         frontLeftW.getCurrentPosition(),                         backLeftW.getCurrentPosition(),                         frontRightW.getCurrentPosition(),                         backRightW.getCurrentPosition());                 telemetry.update();             }              // Stop all motion;             frontLeftW.setPower(0);             backLeftW.setPower(0);             frontRightW.setPower(0);             backRightW.setPower(0);              // Turn off RUN_TO_POSITION             frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);             backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);             frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);             backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);          }     } }

  • markdmatthews
    replied
    Hi,

    For our programming, we've defined a "native" rotation direction for our motors - what direction do they rotate when plugged in normally (red-red, black-black). Note that this is not always the direction desired when programming them for your robot. For example, to move forward, motors on one side of the robot will need to rotate in the opposite direction to the motors on the other side of the robot. Some teams "fix" this by reversing the wiring for the motors on the side of the robot that needs to rotate in the opposite direction. However, if you're using encoders in your code with those motors, this will often fail to work correctly, because the encoders will "count" in the wrong direction compared to the power input to the motor as far as your code is concerned - positive power sent to the motor should result in a corresponding positive increase in encoder counts for most P(ID) controllers. The best thing to do is call DcMotor.setDirection() to change the direction the motors rotate when given a positive power input, see the documentation at setDirection. This will ensure that the encoders change with a sign that is compatible with what your code expects.

    (We actually keep track of what directions motors natively rotate in our code, and use that information when we set the motors up at init time, so that it's easy to swap between motor types without changing too much of the assumptions in the code. Later versions of the FTC SDK return the data you need to figure out all of these key points such as orientation (rotation direction), number of encoder ticks per revolution, etc. with .getMotorType()).

    Leave a comment:


  • Endersky
    replied
    Originally posted by FTC7253 View Post
    I suggest you keep the power wires in the "normal" order. Check the bottom of page 19 of the Rev expansion hub guide (http://www.revrobotics.com/content/d...31-1153-GS.pdf). It shows how the Neverest motor needs to be connected - you need to use a level shifter and the encoder cable between the motor and the shifter needs to swap wires. At the motor side, it should be RBYG, but RGBY at the shifter side. There are encoder cables that come without this crossover, and they will cause problems similar to what you discuss.
    Also make sure you have the correct DC motor type selected in the config on the RC phone.
    Okay but now the current position value just remains at zero

    Leave a comment:


  • FTC7253
    replied
    I suggest you keep the power wires in the "normal" order. Check the bottom of page 19 of the Rev expansion hub guide (http://www.revrobotics.com/content/d...31-1153-GS.pdf). It shows how the Neverest motor needs to be connected - you need to use a level shifter and the encoder cable between the motor and the shifter needs to swap wires. At the motor side, it should be RBYG, but RGBY at the shifter side. There are encoder cables that come without this crossover, and they will cause problems similar to what you discuss.
    Also make sure you have the correct DC motor type selected in the config on the RC phone.

    Leave a comment:


  • Cheer4FTC
    replied
    Swap the "normal" red/black motor wires going into the Rev hub. Then the motor will go in the same direction as the encoder.

    Leave a comment:


  • 3805Mentor
    replied
    Double check that the encoder wires are going to the same port as the motor.

    Leave a comment:


  • Endersky
    replied
    Code:
    @Autonomous(name="AutonomousMain", group="Pushbot")
    public class AutonomousMain extends LinearOpMode {
    
        private ElapsedTime     runtime = new ElapsedTime();
    
        static final double     COUNTS_PER_MOTOR_REV    = 7 ;    // eg: TETRIX Motor Encoder
        static final double     DRIVE_GEAR_REDUCTION    = 60.0 ;     // This is < 1.0 if geared UP
        static final double     WHEEL_DIAMETER_INCHES   = 4.0 ;     // For figuring circumference
        static final double     COUNTS_PER_INCH         = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
                (WHEEL_DIAMETER_INCHES * 3.1415);
        static final double     DRIVE_SPEED             = 0.6;
        static final double     TURN_SPEED              = 0.5;
    
        public DcMotor frontLeftW = null;
        public DcMotor frontRightW = null;
        public DcMotor backLeftW = null;
        public DcMotor backRightW = null;
        public DcMotor arm = null;
        public DcMotor lifter = null;
        public CRServo wristL = null;
        public CRServo wristR = null;
    
        @Override
        public void runOpMode() {
    
    
            frontLeftW = hardwareMap.get(DcMotor.class, "FL");
            frontRightW = hardwareMap.get(DcMotor.class, "FR");
            backLeftW = hardwareMap.get(DcMotor.class, "BL");
            backRightW = hardwareMap.get(DcMotor.class, "BR");
            arm = hardwareMap.get(DcMotor.class, "arm");
            lifter = hardwareMap.get(DcMotor.class, "L");
            wristL = hardwareMap.crservo.get("WL");
            wristR = hardwareMap.crservo.get("WR");
    
            // Send telemetry message to signify robot waiting;
            telemetry.addData("Status", "Resetting Encoders");    //
            telemetry.update();
    
            frontLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            frontRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            backLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            backRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    
            frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    
            // Send telemetry message to indicate successful Encoder reset
            telemetry.addData("Path0",  "Starting at %7d :%7d",
                    frontLeftW.getCurrentPosition(),
                    frontRightW.getCurrentPosition(),
                    backRightW.getCurrentPosition(),
                    backRightW.getCurrentPosition());
            telemetry.update();
    
            // Wait for the game to start (driver presses PLAY)
            waitForStart();
    
            // Step through each leg of the path,
            // Note: Reverse movement is obtained by setting a negative distance (not speed)
            encoderDrive(DRIVE_SPEED,  47,  47, 47, 47, 5.0);  // S1: Forward 47 Inches with 5 Sec timeout
            encoderDrive(TURN_SPEED,   12, 12, -12, -12,  4.0);  // S2: Turn Right 12 Inches with 4 Sec timeout
            encoderDrive(DRIVE_SPEED, 24, 24, 24, 24, 4.0);  // S3: Reverse 24 Inches with 4 Sec timeout
    
            //robot.leftClaw.setPosition(1.0);            // S4: Stop and close the claw.
            //robot.rightClaw.setPosition(0.0);
            sleep(1000);     // pause for servos to move
    
            telemetry.addData("Path", "Complete");
            telemetry.update();
        }
    
        /*
         *  Method to perfmorm a relative move, based on encoder counts.
         *  Encoders are not reset as the move is based on the current position.
         *  Move will stop if any of three conditions occur:
         *  1) Move gets to the desired position
         *  2) Move runs out of time
         *  3) Driver stops the opmode running.
         */
    
        public void encoderDrive(double speed,
                                 double frontLeftInches, double backLeftInches,
                                 double frontRightInches, double backRightInches,
                                 double timeoutS) {
    
            int newFrontLeftTarget = 0;
            int newBackLeftTarget = 0;
            int newFrontRightTarget = 0;
            int newBackRightTarget = 0;
    
            // Ensure that the opmode is still active
            if (opModeIsActive()) {
    
                // Determine new target position, and pass to motor controller
                newFrontLeftTarget = (int)(frontLeftInches * COUNTS_PER_INCH);
                newBackLeftTarget = (int)(backLeftInches * COUNTS_PER_INCH);
                newFrontRightTarget = (int)(frontRightInches * COUNTS_PER_INCH);
                newBackRightTarget = (int)(backRightInches * COUNTS_PER_INCH);
                frontLeftW.setTargetPosition(newFrontLeftTarget);
                backLeftW.setTargetPosition(newBackLeftTarget);
                frontRightW.setTargetPosition(newFrontRightTarget);
                backRightW.setTargetPosition(newBackRightTarget);
    
                // Turn On RUN_TO_POSITION
                frontLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                backLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                frontRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                backRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    
                // reset the timeout time and start motion.
                runtime.reset();
                frontLeftW.setPower(Math.abs(speed));
                backLeftW.setPower(Math.abs(speed));
                frontRightW.setPower(Math.abs(speed));
                backRightW.setPower(Math.abs(speed));
    
                // keep looping while we are still active, and there is time left, and both motors are running.
                // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
                // its target position, the motion will stop.  This is "safer" in the event that the robot will
                // always end the motion as soon as possible.
                // However, if you require that BOTH motors have finished their moves before the robot continues
                // onto the next step, use (isBusy() || isBusy()) in the loop test.
                while (opModeIsActive() &&
                        (runtime.seconds() < timeoutS) &&
                        (frontLeftW.isBusy() || frontRightW.isBusy() || backLeftW.isBusy() || backRightW.isBusy())) {
    
                    // Display it for the driver.
                    telemetry.addData("Path1",  "Running to %7d :%7d", newFrontLeftTarget,  newBackLeftTarget, newFrontRightTarget, newBackRightTarget);
                    telemetry.addData("Path2",  "Running at %7d :%7d",
                            frontLeftW.getCurrentPosition(),
                            backLeftW.getCurrentPosition(),
                            frontRightW.getCurrentPosition(),
                            backRightW.getCurrentPosition());
                    telemetry.update();
                }
    
                // Stop all motion;
                frontLeftW.setPower(0);
                backLeftW.setPower(0);
                frontRightW.setPower(0);
                backRightW.setPower(0);
    
                // Turn off RUN_TO_POSITION
                frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    
            }
        }
    }

    Leave a comment:


  • Endersky
    replied
    Originally posted by Endersky View Post
    Code:
    @Autonomous(name="AutonomousMain", group="Pushbot") public class AutonomousMain extends LinearOpMode { private ElapsedTime runtime = new ElapsedTime(); static final double COUNTS_PER_MOTOR_REV = 7 ; // eg: TETRIX Motor Encoder static final double DRIVE_GEAR_REDUCTION = 60.0 ; // This is < 1.0 if geared UP static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415); static final double DRIVE_SPEED = 0.6; static final double TURN_SPEED = 0.5; public DcMotor frontLeftW = null; public DcMotor frontRightW = null; public DcMotor backLeftW = null; public DcMotor backRightW = null; public DcMotor arm = null; public DcMotor lifter = null; public CRServo wristL = null; public CRServo wristR = null; @Override public void runOpMode() { frontLeftW = hardwareMap.get(DcMotor.class, "FL"); frontRightW = hardwareMap.get(DcMotor.class, "FR"); backLeftW = hardwareMap.get(DcMotor.class, "BL"); backRightW = hardwareMap.get(DcMotor.class, "BR"); arm = hardwareMap.get(DcMotor.class, "arm"); lifter = hardwareMap.get(DcMotor.class, "L"); wristL = hardwareMap.crservo.get("WL"); wristR = hardwareMap.crservo.get("WR"); // Send telemetry message to signify robot waiting; telemetry.addData("Status", "Resetting Encoders"); // telemetry.update(); frontLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // Send telemetry message to indicate successful Encoder reset telemetry.addData("Path0", "Starting at %7d :%7d", frontLeftW.getCurrentPosition(), frontRightW.getCurrentPosition(), backRightW.getCurrentPosition(), backRightW.getCurrentPosition()); telemetry.update(); // Wait for the game to start (driver presses PLAY) waitForStart(); // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) encoderDrive(DRIVE_SPEED, 47, 47, 47, 47, 5.0); // S1: Forward 47 Inches with 5 Sec timeout encoderDrive(TURN_SPEED, 12, 12, -12, -12, 4.0); // S2: Turn Right 12 Inches with 4 Sec timeout encoderDrive(DRIVE_SPEED, 24, 24, 24, 24, 4.0); // S3: Reverse 24 Inches with 4 Sec timeout //robot.leftClaw.setPosition(1.0); // S4: Stop and close the claw. //robot.rightClaw.setPosition(0.0); sleep(1000); // pause for servos to move telemetry.addData("Path", "Complete"); telemetry.update(); } /* * Method to perfmorm a relative move, based on encoder counts. * Encoders are not reset as the move is based on the current position. * Move will stop if any of three conditions occur: * 1) Move gets to the desired position * 2) Move runs out of time * 3) Driver stops the opmode running. */ public void encoderDrive(double speed, double frontLeftInches, double backLeftInches, double frontRightInches, double backRightInches, double timeoutS) { int newFrontLeftTarget = 0; int newBackLeftTarget = 0; int newFrontRightTarget = 0; int newBackRightTarget = 0; // Ensure that the opmode is still active if (opModeIsActive()) { // Determine new target position, and pass to motor controller newFrontLeftTarget = (int)(frontLeftInches * COUNTS_PER_INCH); newBackLeftTarget = (int)(backLeftInches * COUNTS_PER_INCH); newFrontRightTarget = (int)(frontRightInches * COUNTS_PER_INCH); newBackRightTarget = (int)(backRightInches * COUNTS_PER_INCH); frontLeftW.setTargetPosition(newFrontLeftTarget); backLeftW.setTargetPosition(newBackLeftTarget); frontRightW.setTargetPosition(newFrontRightTarget); backRightW.setTargetPosition(newBackRightTarget); // Turn On RUN_TO_POSITION frontLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION); backLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION); frontRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION); backRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION); // reset the timeout time and start motion. runtime.reset(); frontLeftW.setPower(Math.abs(speed)); backLeftW.setPower(Math.abs(speed)); frontRightW.setPower(Math.abs(speed)); backRightW.setPower(Math.abs(speed)); // keep looping while we are still active, and there is time left, and both motors are running. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits // its target position, the motion will stop. This is "safer" in the event that the robot will // always end the motion as soon as possible. // However, if you require that BOTH motors have finished their moves before the robot continues // onto the next step, use (isBusy() || isBusy()) in the loop test. while (opModeIsActive() && (runtime.seconds() < timeoutS) && (frontLeftW.isBusy() || frontRightW.isBusy() || backLeftW.isBusy() || backRightW.isBusy())) { // Display it for the driver. telemetry.addData("Path1", "Running to %7d :%7d", newFrontLeftTarget, newBackLeftTarget, newFrontRightTarget, newBackRightTarget); telemetry.addData("Path2", "Running at %7d :%7d", frontLeftW.getCurrentPosition(), backLeftW.getCurrentPosition(), frontRightW.getCurrentPosition(), backRightW.getCurrentPosition()); telemetry.update(); } // Stop all motion; frontLeftW.setPower(0); backLeftW.setPower(0); frontRightW.setPower(0); backRightW.setPower(0); // Turn off RUN_TO_POSITION frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } } }
    sorry for some reason the code isn't displaying properly, let me try again

    Leave a comment:


  • Endersky
    replied
    Code:
     
     @Autonomous(name="AutonomousMain", group="Pushbot") public class AutonomousMain extends LinearOpMode {      private ElapsedTime     runtime = new ElapsedTime();      static final double     COUNTS_PER_MOTOR_REV    = 7 ;    // eg: TETRIX Motor Encoder     static final double     DRIVE_GEAR_REDUCTION    = 60.0 ;     // This is < 1.0 if geared UP     static final double     WHEEL_DIAMETER_INCHES   = 4.0 ;     // For figuring circumference     static final double     COUNTS_PER_INCH         = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /             (WHEEL_DIAMETER_INCHES * 3.1415);     static final double     DRIVE_SPEED             = 0.6;     static final double     TURN_SPEED              = 0.5;      public DcMotor frontLeftW = null;     public DcMotor frontRightW = null;     public DcMotor backLeftW = null;     public DcMotor backRightW = null;     public DcMotor arm = null;     public DcMotor lifter = null;     public CRServo wristL = null;     public CRServo wristR = null;      @Override     public void runOpMode() {           frontLeftW = hardwareMap.get(DcMotor.class, "FL");         frontRightW = hardwareMap.get(DcMotor.class, "FR");         backLeftW = hardwareMap.get(DcMotor.class, "BL");         backRightW = hardwareMap.get(DcMotor.class, "BR");         arm = hardwareMap.get(DcMotor.class, "arm");         lifter = hardwareMap.get(DcMotor.class, "L");         wristL = hardwareMap.crservo.get("WL");         wristR = hardwareMap.crservo.get("WR");          // Send telemetry message to signify robot waiting;         telemetry.addData("Status", "Resetting Encoders");    //         telemetry.update();          frontLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);         frontRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);         backLeftW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);         backRightW.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);          frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);         frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);         backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);         backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);          // Send telemetry message to indicate successful Encoder reset         telemetry.addData("Path0",  "Starting at %7d :%7d",                 frontLeftW.getCurrentPosition(),                 frontRightW.getCurrentPosition(),                 backRightW.getCurrentPosition(),                 backRightW.getCurrentPosition());         telemetry.update();          // Wait for the game to start (driver presses PLAY)         waitForStart();          // Step through each leg of the path,         // Note: Reverse movement is obtained by setting a negative distance (not speed)         encoderDrive(DRIVE_SPEED,  47,  47, 47, 47, 5.0);  // S1: Forward 47 Inches with 5 Sec timeout         encoderDrive(TURN_SPEED,   12, 12, -12, -12,  4.0);  // S2: Turn Right 12 Inches with 4 Sec timeout         encoderDrive(DRIVE_SPEED, 24, 24, 24, 24, 4.0);  // S3: Reverse 24 Inches with 4 Sec timeout          //robot.leftClaw.setPosition(1.0);            // S4: Stop and close the claw.         //robot.rightClaw.setPosition(0.0);         sleep(1000);     // pause for servos to move          telemetry.addData("Path", "Complete");         telemetry.update();     }      /*      *  Method to perfmorm a relative move, based on encoder counts.      *  Encoders are not reset as the move is based on the current position.      *  Move will stop if any of three conditions occur:      *  1) Move gets to the desired position      *  2) Move runs out of time      *  3) Driver stops the opmode running.      */      public void encoderDrive(double speed,                              double frontLeftInches, double backLeftInches,                              double frontRightInches, double backRightInches,                              double timeoutS) {          int newFrontLeftTarget = 0;         int newBackLeftTarget = 0;         int newFrontRightTarget = 0;         int newBackRightTarget = 0;          // Ensure that the opmode is still active         if (opModeIsActive()) {              // Determine new target position, and pass to motor controller             newFrontLeftTarget = (int)(frontLeftInches * COUNTS_PER_INCH);             newBackLeftTarget = (int)(backLeftInches * COUNTS_PER_INCH);             newFrontRightTarget = (int)(frontRightInches * COUNTS_PER_INCH);             newBackRightTarget = (int)(backRightInches * COUNTS_PER_INCH);             frontLeftW.setTargetPosition(newFrontLeftTarget);             backLeftW.setTargetPosition(newBackLeftTarget);             frontRightW.setTargetPosition(newFrontRightTarget);             backRightW.setTargetPosition(newBackRightTarget);              // Turn On RUN_TO_POSITION             frontLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION);             backLeftW.setMode(DcMotor.RunMode.RUN_TO_POSITION);             frontRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION);             backRightW.setMode(DcMotor.RunMode.RUN_TO_POSITION);              // reset the timeout time and start motion.             runtime.reset();             frontLeftW.setPower(Math.abs(speed));             backLeftW.setPower(Math.abs(speed));             frontRightW.setPower(Math.abs(speed));             backRightW.setPower(Math.abs(speed));              // keep looping while we are still active, and there is time left, and both motors are running.             // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits             // its target position, the motion will stop.  This is "safer" in the event that the robot will             // always end the motion as soon as possible.             // However, if you require that BOTH motors have finished their moves before the robot continues             // onto the next step, use (isBusy() || isBusy()) in the loop test.             while (opModeIsActive() &&                     (runtime.seconds() < timeoutS) &&                     (frontLeftW.isBusy() || frontRightW.isBusy() || backLeftW.isBusy() || backRightW.isBusy())) {                  // Display it for the driver.                 telemetry.addData("Path1",  "Running to %7d :%7d", newFrontLeftTarget,  newBackLeftTarget, newFrontRightTarget, newBackRightTarget);                 telemetry.addData("Path2",  "Running at %7d :%7d",                         frontLeftW.getCurrentPosition(),                         backLeftW.getCurrentPosition(),                         frontRightW.getCurrentPosition(),                         backRightW.getCurrentPosition());                 telemetry.update();             }              // Stop all motion;             frontLeftW.setPower(0);             backLeftW.setPower(0);             frontRightW.setPower(0);             backRightW.setPower(0);              // Turn off RUN_TO_POSITION             frontLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);             backLeftW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);             frontRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);             backRightW.setMode(DcMotor.RunMode.RUN_USING_ENCODER);          }     } }

    Leave a comment:

Working...
X