Estimating the Position of the Arm

To estimate the position of the arm using telemetry and testing, lets start with the Gamepad if/else ifcode.

Gamepad if/else if

if(gamepad1.dpad_up){
       arm.setPower(0.2);         
            }
else if (gamepad1.dpad_down){
       arm.setPower(-0.2); 
            }   
else { 
       arm.setPower(0); 
            } 

For now you can comment out the limit switch related code.

Within the while loop add the linestelemetry.addData("Arm Test", arm.getCurrentPosition()); and telemetry.update();

while(opModeIsActive){
    if(gamepad1.dpad_up){
       arm.setPower(0.2);         
            }
     else if (gamepad1.dpad_down){
       arm.setPower(-0.2); 
            }   
     else { 
       arm.setPower(0); 
            } 
     telemetry.addData("Arm Test", arm.getCurrentPosition());
     telemetry.update();
          
 }

Save the op mode and run it. Use the gamepad commands to move the arm to the 90 degree position. Once you have the arm properly positioned read the telemetry off the Driver Station to determine the encoder count relative to the position of the arm.

Recall from the Basic Encoder Concepts section that the encoder position is set to 0 each time the Control Hub is turned on. This means that if your arm is in a position other than the starting position when the Control Hub is turned on, that position becomes zero instead of the starting position.

The number given in the image above is not necessarily an accurate encoder count for the 90 degree position. To get the most accurate encoder reading for your robot make sure that your starting position reads as 0 encoder counts. To further increase accuracy consider doing several testing runs before deciding on the number of counts.

Recall that in order to execute RUN_TO_POSITION the following three lines of cod need to be added to both sections of the Gamepad if/else if block.

arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
arm.setPower(0);

When DpadUp is pressed, the arm should move to the the 90 degree position. WhenDpadDown is pressed the arm should move back to the starting position. To do this set the firstarm.setTargetPosition(0); equal to the number of ticks it took your arm to get to 90 degrees, for this example we will use 83 ticks.

Since we want DpadDown to return the arm to the starting position, keeping the arm.setTargetPosition(0); set to 0 will allow us to accomplish this. Set both arm.setPower(0); equal to 0.5.

Target Position if/else if

if(gamepad1.dpad_up){
     arm.setTargetPosition(83);
     arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
     arm.setPower(0.5);
            }
else if (gamepad1.dpad_down){
      arm.setTargetPosition(0);
      arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
      arm.setPower(0.5);
            } 

Note: the code above was given a file name Target Position if/else if this code will be referenced again.

Recall that the target position dictates which direction the motor moves, taking over the directionality control from arm.setPower(); so both blocks can be set to a positive value since they will control the speed.

If you try running this code you may notice that the arm oscillates in the 90 degree position. When this behavior is present you should also notice the telemetry output for the encoder counts fluctuating. RUN_TO_POSITION is a Closed Loop Control, which means that if the arm does not perfectly reach the target position, the motor will continue to fluctuate until it does. When motors continue to oscillate and never quite reach the target position this may be a sign that the factors determining tolerances and other aspects of the closed loop are not tuned to this particular motor or mechanism. There are ways to tune the motor, but for now we want to focus on working with the arm and expanding on how limits and positions work with regards to the mechanism.