What you’re experiencing here is “it’s doing exactly what you told it to do.”
First, since we cannot see all of your code, we have to assume your OpMode is a LinearOpMode. That’s fine, there are significant benefits to using a LinearOpMode. However, you have to think iteratively.
I’m assuming that your first snippet is within the main loop of your program, like thus:
while (opModeIsActive()) {
// A bunch of code here
if(gamepad2.dpad_up)
robot.setArmPower(robot.ARM_UP_POWER);
else if(gamepad2.dpad_down)
robot.setArmPower(robot.ARM_DOWN_POWER);
else
robot.setArmPower(0);
if( gamepad2.a) {
//Slide presets to height = small junction
robot.setArmPostion(robot.LOW_JUNCTION_A);
}
// Whatever code or function you use for controlling drivetrain motors
ManageDrivetrainCode();
// A bunch more code, whatever
}
This code loops while the program is running. Each time you hit your drivetrain code, the program can then issue new commands to the drivetrain. Remember that this loop must continue to run unimpeded in order for all operations of the robot to work correctly.
Okay, now let’s take a look at your setArmPower function:
public void setArmPower(double power) {
//armMotor.setPower(power);
Arm.setPower(power);
}
This is a good function. It simply sets the power, nothing more and nothing less. The motor will continue doing whatever it was last told to do until it is told to do something else - easy peasy lemon squeezy. But do you really understand the ramifications of this? Probably, but let me reiterate:
- In your main loop, the power will be set each and every main loop iteration.
- If your
DPAD_UP button is CURRENTLY pressed, the motor is told to run robot.ARM_UP_POWER value, which let’s assume is a positive number. Only while that button is being held down will the motor keep being told to do that.
- If your
DPAD_UP button is NOT pressed, and if your DPAD_DOWN button is pressed, the motor is told to run the robot.ARM_DOWN_POWER value, which let’s assume that is a negative value. Only while that button is being held down will the motor keep being told to do that.
- As soon as you release both buttons, no buttons are being pressed, and you’re setting the power to
ZERO, which stops the motor.
This is all being controlled via the if statement. This is only possible because each iteration of the Main Loop the joystick buttons are being re-evaluated, and a new value (or the same value) is being set for the Arm Motor’s power.
So what does this mean for your drivetrain motors? Since your setArmPower() function just sets a value and exits, the Drivetrain code can execute and can be updated very quickly. This is why it seems as if the Drivetrain motors and the Arm motors are working in tandem. They’re NOT working in tandem, however, that’s just an illusion. The Arm Motors are being updated, and THEN the Drivetrain motors are being updated, each loop iteration. It’s just that we’re not waiting for the arm motors to do anything, and we’re not waiting for the drivetrain to do anything, so they’re just updated extremely quickly - quickly enough to make it seem like they’re happening in parallel but they’re actually being run consecutively (in series).
So now let’s look at setArmPosition:
public void setArmPostion(int position) {
Arm.setTargetPosition(position);
Arm.setPower(LIFT_POWER);
Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
while (Arm.isBusy()) {
myOpMode.telemetry.addData("Current Position", Arm.getCurrentPosition());
myOpMode.telemetry.addData("Target Position", Arm.getTargetPosition());
myOpMode.telemetry.update();
}
}
First, you’ve got a logic error in this code. Your code should ALWAYS follow this sequence when performing a RUN_TO_POSITION command:
- Set the motor’s Target Position.
- Set the motor’s Mode to
RUN_TO_POSITION
- Set the motor’s Power.
It may seem dumb, but this will save you from a lot of problems when commanding the firmware.
The what? Yes, the firmware. You see, when you use modes like RUN_TO_POSITION you aren’t actually issuing commands to the motors. Nope. You’re actually asking the firmware to command the motors on your behalf, and execute a pre-planned algorithm to make the motor rotate a certain amount using the encoders - THAT is why the previous sequence of commands is important, as the firmware expects to see this sequence. What you’ve done probably doesn’t crash only because you’re setting the motor’s mode earlier, and it just happens to not conflict with other modes the motors could be using.
So what does the firmware do? Well, it moves the motors around. You know that, because you’ve got a while loop in that code - you’re actually WAITING for the firmware to finish moving the motor to its position. The Arm.isBusy() command is asking the firmware if it’s done moving the arm, and the while loop patiently waits until it’s no longer moving. That means the setArmPosition function is sitting there WAITING for the arm motor to finish moving - while you’re sitting there, you’re not updating the Drivetrain code, so you’re not able to command the robot’s drivetrain (or anything else) while you’re waiting. That’s no fun at all.
So how do we “fix” your code? Well, that depends on your intent for the code. If the intent of the code is to execute the arm code WHILE A BUTTON IS BEING PRESSED, then let’s update the code as thus. This requires updating BOTH the Main Loop as well as your functions:
while (opModeIsActive()) {
// A bunch of code here
if(gamepad2.dpad_up) {
robot.setArmPower(robot.ARM_UP_POWER);
}
else if(gamepad2.dpad_down) {
robot.setArmPower(robot.ARM_DOWN_POWER);
}
else if(gamepad2.a) {
/*
Setting the Arm Position is being done within this if/else
block because we only want ONE if block ever controlling
the same motor resource
*/
//Slide presets to height = small junction
robot.setArmPostion(robot.LOW_JUNCTION_A);
}
else {
robot.setArmPower(0);
}
// Whatever code or function you use for controlling drivetrain motors
ManageDrivetrainCode();
// A bunch more code, whatever
}
Okay, and now we need to correct the setArmPower function:
public void setArmPower(double power) {
/* Set intended motor mode. Since the motor can be set to
multiple modes, we should be specific about what we're
wanting the motor to actually do. It's just good form.
If you want to use RUN_WITH_ENCODER, that's fine too.
*/
Arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
Arm.setPower(power);
}
Okay, and now let’s fix your remaining function:
public void setArmPostion(int position) {
/* Check to see if the arm is busy. If so, only update telemetry.
This way, we don't interrupt the command while the firmware is
still commanding the motor. */
if( Arm.isBusy() )
{
myOpMode.telemetry.addData("Current Position", Arm.getCurrentPosition());
myOpMode.telemetry.addData("Target Position", Arm.getTargetPosition());
myOpMode.telemetry.update();
}
else
{
// Arm isn't busy, so we can issue a new command.
Arm.setTargetPosition(position);
Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Arm.setPower(LIFT_POWER);
}
}
There are two caveats to using this new code:
- You have to hold down the buttons to command the arm. For instance, you already had to hold down the button in order to make the arm go Up and Down using the DPAD, if you didn’t then the motor was told to STOP. Now the same thing is true for the A button - if you hold down the button, then the motor is never commanded to stop, it will stop on its own when it reaches its location. If you release the A button, the motor is told to stop because of the
else which commands the motor to set the power to zero. That’s a good thing for safety anyway, if you release your finger off the button it will stop the motor.
- This makes some assumptions about how you want to drive your robot. If those assumptions aren’t valid, you may want to change the code.
Notice what this code doesn’t do - it doesn’t wait in a while loop watching the firmware like a bump on a log while the firmware is doing its bidding. It trusts the firmware to do what it needs to do, and gives you the immediate ability to keep commanding the drivetrain which allows you to appear to keep doing multiple things at once. And that is a “Good Thing.”
Rock on,
-Danny
However thank you for explaning the nuances - specially setting the motor mode and the order of setting the motor power. We tried all the 3 options that you proposed and they all work. With the regionals this weekend (and we just had 5 days to prepare as League was Sunday), it will come down to driver choice - (they will most likely pick Option 2 ).