Announcement

Collapse
No announcement yet.

robot backs up WAY too far!

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

  • robot backs up WAY too far!

    After our robot presses the correct beacon we want it to reverse .3 seconds but it drives backwards way too far. Any help would be appreciated, I underlined the part that is not working!


    package org.firstinspires.ftc.teamcode;

    import android.graphics.Color;

    import com.qualcomm.robotcore.eventloop.opmode.Autonomous ;
    import com.qualcomm.robotcore.eventloop.opmode.LinearOpMo de;
    import com.qualcomm.robotcore.hardware.ColorSensor;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.OpticalDistanceSen sor;
    import com.qualcomm.robotcore.hardware.TouchSensor;
    import com.qualcomm.robotcore.util.ElapsedTime;

    //import com.qualcomm.robotcore.eventloop.opmode.Disabled;
    //import com.qualcomm.robotcore.hardware.LightSensor;

    /**
    * This file illustrates the concept of driving up to a line and then stopping.
    * It uses the common Pushbot hardware class to define the drive on the robot.
    * The code is structured as a LinearOpMode
    *
    * The code shows using two different light sensors:
    * The Primary sensor shown in this code is a legacy NXT Light sensor (called "light sensor")
    * Alternative "commented out" code uses a MR Optical Distance Sensor (called "sensor_ods")
    * instead of the LEGO sensor. Chose to use one sensor or the other.
    *
    * Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
    * This should be set half way between the light and dark values.
    * These values can be read on the screen once the OpMode has been INIT, but before it is STARTED.
    * Move the senso on asnd off the white line and not the min and max readings.
    * Edit this code to make WHITE_THRESHOLD half way between the min and max.
    *
    * 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
    */

    @Autonomous(name="Pushbot: WHATEVER", group="Pushbot")
    //@Disabled
    public class WHATEVER extends LinearOpMode {

    /* Declare OpMode members. */
    HardwarePushbotCopy robot = new HardwarePushbotCopy(); // Use a Pushbot's hardware
    private ElapsedTime runtime = new ElapsedTime();

    OpticalDistanceSensor lightSensor; // Alternative MR ODS sensor
    TouchSensor touchSensor;
    ColorSensor colorSensor;


    static final double WHITE_THRESHOLD = 0.3; // spans between 0.1 - 0.5 from dark to light
    static final double APPROACH_SPEED = 0.2;
    static final double INITIAL_SPEED = 0.3;

    @Override
    public void runOpMode() throws InterruptedException {

    float hsvValues[] = {0F, 0F, 0F};

    // values is a reference to the hsvValues array.
    final float values[] = hsvValues;

    /* Initialize the drive system variables.
    * The init() method of the hardware class does all the work here
    */
    robot.init(hardwareMap);

    lightSensor = hardwareMap.opticalDistanceSensor.get("ods");
    touchSensor = hardwareMap.touchSensor.get("touch sensor");
    colorSensor = hardwareMap.colorSensor.get("color sensor");
    // turn on LED of light sensor.
    lightSensor.enableLed(true);

    // Send telemetry message to signify robot waiting;
    telemetry.addData("Status", "Ready to run"); //
    telemetry.update();
    // Wait for the game to start (driver presses PLAY)
    while (!isStarted()) {
    // Display the light level while we are waiting to start
    telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    // send the info back to driver station using telemetry function.
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();
    }

    int initLeftCount = robot.leftMotor.getCurrentPosition();
    int tgLeftCount = initLeftCount + 89;
    int initRightCount = robot.rightMotor.getCurrentPosition();
    int tgRightCount = initRightCount + 89;

    robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

    robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

    robot.leftMotor.setPower(0.89);
    robot.rightMotor.setPower(0.89);

    while (opModeIsActive() && (robot.rightMotor.getCurrentPosition() < tgRightCount *34));
    while (opModeIsActive() && (robot.leftMotor.getCurrentPosition() < tgLeftCount *34));

    robot.leftMotor.setPower(0);
    robot.rightMotor.setPower(0);

    robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

    robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
    //turn left
    robot.leftMotor.setPower(0);
    robot.rightMotor.setPower(.75);

    while (opModeIsActive() && (robot.rightMotor.getCurrentPosition() < tgRightCount *14.8));
    //reset motors
    robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

    robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

    robot.leftMotor.setPower(.30);
    robot.rightMotor.setPower(.30);

    while (opModeIsActive() && (robot.rightMotor.getCurrentPosition() < tgRightCount *16));
    //while (opModeIsActive() && (robot.leftMotor.getCurrentPosition() < tgLeftCount *16));

    robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

    robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

    // Start the robot moving forward, and then begin looking for a white line.
    robot.leftMotor.setPower(APPROACH_SPEED);
    robot.rightMotor.setPower(APPROACH_SPEED);

    // run until the white line is seen OR the driver presses STOP;
    while (opModeIsActive() && (lightSensor.getLightDetected() < WHITE_THRESHOLD)) {

    // Display the light level while we are looking for the line
    telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();}
    while (opModeIsActive() && (!touchSensor.isPressed())){
    {telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();}

    if (lightSensor.getLightDetected() < WHITE_THRESHOLD) {
    robot.leftMotor.setPower(0.2);
    robot.rightMotor.setPower(0.0);}
    else
    {robot.leftMotor.setPower(0.0);
    robot.rightMotor.setPower(0.2);}
    }
    // Stop all motors
    robot.leftMotor.setPower(0);
    robot.rightMotor.setPower(0);

    while (opModeIsActive() && (touchSensor.isPressed())) {
    {telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();

    sleep(2000);
    if (colorSensor.blue() >= 4)
    {robot.rightClaw.setPosition(.0);}
    else
    {robot.leftClaw.setPosition(.9);}
    }
    sleep(500);

    robot.leftClaw.setPosition(.1);
    robot.rightClaw.setPosition(0.9);

    robot.leftMotor.setPower(-0.5);
    robot.rightMotor.setPower(-0.5);}
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < 1)) {
    }
    robot.leftMotor.setPower(0);
    robot.rightMotor.setPower(0);
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < .3)) {
    }


    sleep(450);

    robot.leftMotor.setPower(APPROACH_SPEED);
    robot.rightMotor.setPower(-APPROACH_SPEED);
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < 1.3)) {
    }

    robot.leftMotor.setPower(0.6);
    robot.rightMotor.setPower(0.6);
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < 1.0)) {
    }

    robot.leftMotor.setPower(0.6);
    robot.rightMotor.setPower(0.6);
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < 0.3)) {
    }

    robot.leftMotor.setPower(-INITIAL_SPEED);
    robot.rightMotor.setPower(INITIAL_SPEED);
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < 0.66)) {
    }

    robot.leftMotor.setPower(APPROACH_SPEED);
    robot.rightMotor.setPower(APPROACH_SPEED);
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < 0.6)) {
    }

    while (opModeIsActive() && (lightSensor.getLightDetected() < WHITE_THRESHOLD)) {

    // Display the light level while we are looking for the line
    telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();}
    while (opModeIsActive() && (!touchSensor.isPressed())){
    {telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();}

    if (lightSensor.getLightDetected() < WHITE_THRESHOLD) {
    robot.leftMotor.setPower(0.2);
    robot.rightMotor.setPower(0.0);}
    else
    {robot.leftMotor.setPower(0.0);
    robot.rightMotor.setPower(0.2);}
    }
    // Stop all motors
    robot.leftMotor.setPower(0);
    robot.rightMotor.setPower(0);

    while (opModeIsActive() && (touchSensor.isPressed())) {
    {telemetry.addData("Light Level", lightSensor.getLightDetected());
    Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
    telemetry.addData("Red ", colorSensor.red());
    telemetry.addData("Blue ", colorSensor.blue());
    telemetry.addData("Hue", hsvValues[0]);
    telemetry.update();}
    sleep(2000);
    if (colorSensor.blue() >= 4)
    {robot.rightClaw.setPosition(.0);}
    else
    {robot.leftClaw.setPosition(.9);}

    sleep(500);

    robot.leftClaw.setPosition(0.5);
    robot.rightClaw.setPosition(0.1);

    robot.leftMotor.setPower(-0.45);
    robot.rightMotor.setPower(-0.45);}
    runtime.reset();
    while (opModeIsActive() && (runtime.seconds() < .5)) {
    }

    robot.leftMotor.setPower(0);
    robot.rightMotor.setPower(0);}

    {}}

  • #2
    In your bold code, you appear to be backing up for 1 second at speed -0.5, then setting power 0, then sitting for 0.3s.

    There also seems to be a potential issue with how you set your target counts.
    You do:

    int initLeftCount = robot.leftMotor.getCurrentPosition();
    int tgLeftCount = initLeftCount + 89;
    int initRightCount = robot.rightMotor.getCurrentPosition();
    int tgRightCount = initRightCount + 89;

    robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

    robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
    robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

    robot.leftMotor.setPower(0.89);
    robot.rightMotor.setPower(0.89);

    while (opModeIsActive() && (robot.rightMotor.getCurrentPosition() < tgRightCount *34));
    while (opModeIsActive() && (robot.leftMotor.getCurrentPosition() < tgLeftCount *34));

    The counts are not guaranteed to be zero when you enter the opmode - especially if you have run one opmode already during this power cycle.
    So, tgLeftCount = initLeftCount + 89 could be a large number, and tgRightCount *34 would be a very large number.
    You are getting the current count and adding an offset (89). Say current was 911 - tgtLeftCount will be 1000. You then reset the encoders, so the current will be set to 0.
    Then you run to encoder with a target of tgLeftCount *34 (34000). That will take a while!
    After this, you reset encoders and run to target without ever setting a new target - also potentially problematic.
    If 89 is really your counts per inch, then you should not set target to current + 89. You can reset encoders each time, and then set target to desiredInches * 89 for each segment.

    Comment


    • #3
      That makes perfect sense, but what would that look like? We are VERY inexperienced with Java. Any help would be great!

      Does this look like it would work?
      int initLeftCount = robot.leftMotor.getCurrentPosition();
      int tgLeftCount = initLeftCount;
      int initRightCount = robot.rightMotor.getCurrentPosition();
      int tgRightCount = initRightCount;

      robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
      robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

      robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
      robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

      robot.leftMotor.setPower(0.89);
      robot.rightMotor.setPower(0.89);

      while (opModeIsActive() && (robot.rightMotor.getCurrentPosition() <89 * 34));
      while (opModeIsActive() && (robot.leftMotor.getCurrentPosition() < 89 * 34));
      Last edited by thensley; 02-15-2017, 09:34 AM.

      Comment


      • #4
        A little better, but you still need some changes.
        For starters, getting the counts before resetting is probably rather pointless. You get the counts, then immediately tell the motors/controllers to set the counts to zero.
        Unless you are going to try to drive back to the pre-RESET_ENCODER position, this is probably not what you want.
        Set a variable somewhere to represent your counts per inch.
        private static final int CPI = 89;
        Then for each target/segment, a simple approach is to reset the encoders, set the mode to RUN_TO_POSITION, set target position to segmentDistance * CPI, and then set a power - start slow until you know things are working. Finally, have a while loop that checks if the motors are busy - while(opModeIsActive && robot.leftMotor.isBusy() && robot.rightMotor.isBusy()){ //print telemetry of counts or just do nothing}. Follow this with setPower(0) calls. The motor controller PID will take care of to the specified target, and compensating for overshoot.
        Alternatively, you can do a similar thing with RUN_USING_ENCODERS, where you control things instead of the motor controller. This looks more like what you currently have. For each segment, you can reset you encoders, set the mode to RUN_USING_ENCODER, set the power, then do your while loops on getCurrentPosition < distance * CPI, and finally setPower to 0 for both motors. One problem here, is that you will be running at speed (in your case very high speed of 0.89!!!) and then hitting the brakes once you reach the target. Naturally, you will fly right past your target point as you decelerate - you've waited to hit the brakes until you are at the stop line - you are certainly going through the red light.
        Final point - you don't have to reset the encoders each segment. When you use distance*CPI as your target check, it assumes the start count was 0 - which is the case when you have called STOP_AND_RESET_ENCODER. But resetting the encoders takes some time. Instead, you could start each segment with target=getCurrentPosition + desiredDistance*CPI, and then use that as your value to compare against in the while condition. If you do this, you don't call reset encoders (except perhaps once in init). If you are using your current approach, you could just stay in RUN_USING_ENCODER the whole time.

        Comment


        • #5
          How does this look?

          int initLeftCount = robot.leftMotor.getCurrentPosition();
          int tgLeftCount = initLeftCount + 89;
          int initRightCount = robot.rightMotor.getCurrentPosition();
          int tgRightCount = initRightCount + 89;
          private static final int CPI = 89
          //CPI=counts per inch

          robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
          robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

          robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
          robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

          robot.leftMotor.setPower(0.5);
          robot.rightMotor.setPower(0.5);

          while (opModeIsActive() && (robot.rightMotor.getCurrentPosition() < 34*CPI));
          while (opModeIsActive() && (robot.leftMotor.getCurrentPosition() < 34*CPI));

          robot.leftMotor.setPower(0);
          robot.rightMotor.setPower(0);

          robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
          robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
          target = getCurrentPosition + 34 * CPI

          //turn left
          robot.leftMotor.setPower(0);
          robot.rightMotor.setPower(.75);

          while (opModeIsActive() && (target () < 14.8 * CPI));
          robot.leftMotor.setPower(0);
          robot.rightMotor.setPower(0);

          robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
          robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

          Target = getCurrentPosition +14.8 * CPI

          robot.leftMotor.setPower(.30);
          robot.rightMotor.setPower(.30);

          while (opModeIsActive() && (target() < 16 * CPI));
          //while (opModeIsActive() && (target() < 16 * CPI));
          robot.leftMotor.setPower(0);
          robot.rightMotor.setPower(0);

          robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
          robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
          Last edited by thensley; 02-15-2017, 11:20 AM.

          Comment


          • #6
            Closer. Some ordering issues.

            Try this. NOTE: There are better ways to do this (some mentioned in prior post), and this was modeled off of your existing flow, and done in the forum reply block while I did not have access to Android Studio. It is completely off the top of my head, so you will likely have to make corrections and adjustments.
            To avoid the repeated code, it would be good to move the relevant pieces into a separate method, and simply call it for each segment.
            The method signature could be something like:
            public void move(double lDist, double rDist, double power);
            The method would calculate the target position from the current position and the conversion of lDist and rDist to counts. It would set the mode (if needed), set the power, do the while loop with the target position conditional, and set the power to 0.

            private static final int CPI = 89; //CPI=counts per inch
            private static final int THRESH = 10;

            //Put the next four lines before waitForStart();
            robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
            robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
            robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
            robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

            int lTarget = robot.leftMotor.getCurrentPosition() + 34 * CPI;
            int rTarget = robot.rightMotor.getCurrentPosition() + 34 * CPI;

            robot.leftMotor.setPower(0.3);
            robot.rightMotor.setPower(0.3);

            while (opModeIsActive() &&
            (Math.abs(lTarget - robot.leftMotor.getCurrentPosition()) > THRESH) &&
            (Math.abs(rTarget - robot.rightMotor.getCurrentPosition()) > THRESH) {}

            robot.leftMotor.setPower(0);
            robot.rightMotor.setPower(0);

            //turn left
            lTarget = robot.leftMotor.getCurrentPosition - 14.8 * CPI; //do a center left turn by making the left wheel go bkwds and right wheel fwd
            rTarget = robot.rightMotor.getCurrentPosition + 14.8 * CPI;
            robot.leftMotor.setPower(-0.3);
            robot.rightMotor.setPower(0.3);

            while (opModeIsActive() &&
            (Math.abs(lTarget - robot.leftMotor.getCurrentPosition()) > THRESH) &&
            (Math.abs(rTarget - robot.rightMotor.getCurrentPosition()) > THRESH) {}

            robot.leftMotor.setPower(0);
            robot.rightMotor.setPower(0);

            lTarget = robot.leftMotor.getCurrentPosition() + 16 * CPI;
            rTarget = robot.rightMotor.getCurrentPosition() + 16 * CPI;

            robot.leftMotor.setPower(.30);
            robot.rightMotor.setPower(.30);

            while (opModeIsActive() &&
            (Math.abs(lTarget - robot.leftMotor.getCurrentPosition()) > THRESH) &&
            (Math.abs(rTarget - robot.rightMotor.getCurrentPosition()) > THRESH) {}

            robot.leftMotor.setPower(0);
            robot.rightMotor.setPower(0);

            Comment


            • #7
              Thank You So Much, We will give this a shot this afternoon when we meet after school!

              Comment


              • #8
                You really should try the RUN_TO_POSITION approach. That way you don't have to worry about overshoot - let the controller PID take care of it for you.

                Comment


                • #9
                  ok I added the code and the only error is underlined, it says it cannot resolve "get current position" but only on the turn, the next one is fine?

                  package org.firstinspires.ftc.teamcode;

                  import android.graphics.Color;

                  import com.qualcomm.robotcore.eventloop.opmode.Autonomous ;
                  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMo de;
                  import com.qualcomm.robotcore.hardware.ColorSensor;
                  import com.qualcomm.robotcore.hardware.DcMotor;
                  import com.qualcomm.robotcore.hardware.OpticalDistanceSen sor;
                  import com.qualcomm.robotcore.hardware.TouchSensor;
                  import com.qualcomm.robotcore.util.ElapsedTime;

                  //import com.qualcomm.robotcore.eventloop.opmode.Disabled;
                  //import com.qualcomm.robotcore.hardware.LightSensor;

                  /**
                  * This file illustrates the concept of driving up to a line and then stopping.
                  * It uses the common Pushbot hardware class to define the drive on the robot.
                  * The code is structured as a LinearOpMode
                  *
                  * The code shows using two different light sensors:
                  * The Primary sensor shown in this code is a legacy NXT Light sensor (called "light sensor")
                  * Alternative "commented out" code uses a MR Optical Distance Sensor (called "sensor_ods")
                  * instead of the LEGO sensor. Chose to use one sensor or the other.
                  *
                  * Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
                  * This should be set half way between the light and dark values.
                  * These values can be read on the screen once the OpMode has been INIT, but before it is STARTED.
                  * Move the senso on asnd off the white line and not the min and max readings.
                  * Edit this code to make WHITE_THRESHOLD half way between the min and max.
                  *
                  * 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
                  */

                  @Autonomous(name="Pushbot: test", group="Pushbot")
                  //@Disabled
                  public class test extends LinearOpMode {

                  /* Declare OpMode members. */
                  HardwarePushbotCopy robot = new HardwarePushbotCopy(); // Use a Pushbot's hardware
                  private ElapsedTime runtime = new ElapsedTime();

                  OpticalDistanceSensor lightSensor; // Alternative MR ODS sensor
                  TouchSensor touchSensor;
                  ColorSensor colorSensor;


                  static final double WHITE_THRESHOLD = 0.3; // spans between 0.1 - 0.5 from dark to light
                  static final double APPROACH_SPEED = 0.2;
                  static final double INITIAL_SPEED = 0.3;
                  private static final int CPI = 89; //CPI=counts per inch
                  private static final int THRESH = 10;

                  @Override
                  public void runOpMode() throws InterruptedException {

                  float hsvValues[] = {0F, 0F, 0F};

                  // values is a reference to the hsvValues array.
                  final float values[] = hsvValues;

                  /* Initialize the drive system variables.
                  * The init() method of the hardware class does all the work here
                  */
                  robot.init(hardwareMap);

                  lightSensor = hardwareMap.opticalDistanceSensor.get("ods");
                  touchSensor = hardwareMap.touchSensor.get("touch sensor");
                  colorSensor = hardwareMap.colorSensor.get("color sensor");
                  // turn on LED of light sensor.
                  lightSensor.enableLed(true);

                  // Send telemetry message to signify robot waiting;
                  telemetry.addData("Status", "Ready to run"); //
                  telemetry.update();
                  robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
                  robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
                  robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
                  robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
                  // Wait for the game to start (driver presses PLAY)
                  while (!isStarted()) {
                  // Display the light level while we are waiting to start
                  telemetry.addData("Light Level", lightSensor.getLightDetected());
                  Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
                  // send the info back to driver station using telemetry function.
                  telemetry.addData("Red ", colorSensor.red());
                  telemetry.addData("Blue ", colorSensor.blue());
                  telemetry.addData("Hue", hsvValues[0]);
                  telemetry.update();
                  }

                  // int initLeftCount = robot.leftMotor.getCurrentPosition();
                  //int tgLeftCount = initLeftCount + 89;
                  //int initRightCount = robot.rightMotor.getCurrentPosition();
                  //int tgRightCount = initRightCount + 89;

                  //robot.leftMotor.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER);
                  //robot.rightMotor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);

                  //robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ ENCODER);
                  //robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);

                  //robot.leftMotor.setPower(0.7);
                  //robot.rightMotor.setPower(0.7);

                  int lTarget = robot.leftMotor.getCurrentPosition() + 34 * CPI;
                  int rTarget = robot.rightMotor.getCurrentPosition() + 34 * CPI;

                  robot.leftMotor.setPower(0.3);
                  robot.rightMotor.setPower(0.3);

                  while (opModeIsActive() &&
                  (Math.abs(lTarget - robot.leftMotor.getCurrentPosition()) > THRESH) &&
                  (Math.abs(rTarget - robot.rightMotor.getCurrentPosition()) > THRESH)) {}

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);
                  //turn left

                  lTarget =robot.leftMotor.getCurrentPosition - 14.8 * CPI;
                  rTarget = robot.rightMotor.getCurrentPosition + 14.8 * CPI;

                  robot.leftMotor.setPower(-0.3);
                  robot.rightMotor.setPower(0.3);

                  while (opModeIsActive() &&
                  (Math.abs(lTarget - robot.leftMotor.getCurrentPosition()) > THRESH) &&
                  (Math.abs(rTarget - robot.rightMotor.getCurrentPosition()) > THRESH)) {}

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  lTarget = robot.leftMotor.getCurrentPosition() + 16 * CPI;
                  rTarget = robot.rightMotor.getCurrentPosition() + 16 * CPI;

                  robot.leftMotor.setPower(.30);
                  robot.rightMotor.setPower(.30);

                  while (opModeIsActive() &&
                  (Math.abs(lTarget - robot.leftMotor.getCurrentPosition()) > THRESH) &&
                  (Math.abs(rTarget - robot.rightMotor.getCurrentPosition()) > THRESH)) {}

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  // Start the robot moving forward, and then begin looking for a white line.
                  robot.leftMotor.setPower(APPROACH_SPEED);
                  robot.rightMotor.setPower(APPROACH_SPEED);

                  // run until the white line is seen OR the driver presses STOP;
                  while (opModeIsActive() && (lightSensor.getLightDetected() < WHITE_THRESHOLD)) {

                  // Display the light level while we are looking for the line
                  telemetry.addData("Light Level", lightSensor.getLightDetected());
                  Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
                  telemetry.addData("Red ", colorSensor.red());
                  telemetry.addData("Blue ", colorSensor.blue());
                  telemetry.addData("Hue", hsvValues[0]);
                  telemetry.update();}
                  while (opModeIsActive() && (!touchSensor.isPressed())){
                  {telemetry.addData("Light Level", lightSensor.getLightDetected());
                  Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
                  telemetry.addData("Red ", colorSensor.red());
                  telemetry.addData("Blue ", colorSensor.blue());
                  telemetry.addData("Hue", hsvValues[0]);
                  telemetry.update();}

                  if (lightSensor.getLightDetected() < WHITE_THRESHOLD) {
                  robot.leftMotor.setPower(0.2);
                  robot.rightMotor.setPower(0.0);}
                  else
                  {robot.leftMotor.setPower(0.0);
                  robot.rightMotor.setPower(0.2);}
                  }
                  // Stop all motors
                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  while (opModeIsActive() && (touchSensor.isPressed())) {
                  {telemetry.addData("Light Level", lightSensor.getLightDetected());
                  Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
                  telemetry.addData("Red ", colorSensor.red());
                  telemetry.addData("Blue ", colorSensor.blue());
                  telemetry.addData("Hue", hsvValues[0]);
                  telemetry.update();
                  sleep(1000);
                  if (colorSensor.blue() >= 4)
                  {robot.rightClaw.setPosition(.0);}
                  else
                  {robot.leftClaw.setPosition(1.0);}
                  }
                  sleep(1000);

                  //robot.leftClaw.setPosition(.1);
                  //robot.rightClaw.setPosition(0.9);

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  robot.leftMotor.setPower(-0.45);
                  robot.rightMotor.setPower(-0.45);
                  runtime.reset();
                  while (opModeIsActive() && (runtime.seconds() < .25)) {
                  }

                  sleep(1000);

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  robot.leftMotor.setPower(-0.5);
                  robot.rightMotor.setPower(0.5);
                  runtime.reset();
                  while (opModeIsActive() && (runtime.seconds() < 1.35)) {
                  }

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  sleep(1000);

                  robot.leftMotor.setPower(-APPROACH_SPEED);
                  robot.rightMotor.setPower(-APPROACH_SPEED);
                  runtime.reset();
                  while (opModeIsActive() && (runtime.seconds() < 2.2)) {
                  }

                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);}

                  sleep(1000);

                  robot.TomMotor.setPower(1.0);
                  runtime.reset();
                  while (opModeIsActive() && (runtime.seconds() < .5)) {
                  }

                  robot.TomMotor.setPower(0.0);

                  sleep(1000);

                  robot.leftMotor.setPower(0.5);
                  robot.rightMotor.setPower(-0.5);
                  runtime.reset();
                  while (opModeIsActive() && (runtime.seconds() < 0.1)) {
                  }
                  robot.leftMotor.setPower(0);
                  robot.rightMotor.setPower(0);

                  sleep(1000);

                  robot.armMotor.setPower(1.0);
                  runtime.reset();
                  while (opModeIsActive() && (runtime.seconds() < 0.5)) {
                  }
                  robot.armMotor.setPower(0.0);

                  }}

                  Comment


                  • #10
                    getCurrentPosition is a method call - on a method that takes no parameters, so you need the open/close parens.
                    getCurrentPosition()

                    Comment


                    • #11
                      Thank You that worked great. We also go the red light bulb and had to make something "double". Not really sure what it was but all the red went away and the program works as advertised! Thank You guys so much for your help!

                      Comment

                      Working...
                      X