Hi,
I am moving robot forward then backward then angular position all using encoders. So far so good. Then I reset encoder and move backward. I notice using telemetry that current position of left and right motor were not set to 0.
Can someone pls help what is wrong?
I’m afraid we likely cannot tell what’s going wrong without an example of your code. Can you point us to a github repo of where your code lives, or give us relevant snippets of your code?
Please find attached file for block mode.
OpMode converted Java code:
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@Autonomous(name = “AutonoumousRobot (Blocks to Java)”)
public class AutonoumousRobot extends LinearOpMode {
private DistanceSensor DistanceSensor_DistanceSensor;
private DcMotor LeftMotor;
private DcMotor RightMotor;
private Servo GrabberServo;
private DcMotor ArmMotor;
boolean l_robot_distance;
double INCHES_TO_MM;
double COUNTS_PER_MM;
double WHEEL_CIRCUMFERENCE_MM;
String COUNTS_PER_ARMMOTOR_DEGREE;
double ROBOT_LENGTH_MM;
double SENSOR_ROBOT_DISTANCE_MM;
double SUBSTRC_DISTANCE_TO_ROBOT_MM;
/**
ROBOT_DISTANCE_FROM_OBJECT = DistanceSensor_DistanceSensor.getDistance(DistanceUnit.MM);
telemetry2("Measured distance by sensor was", (int) ROBOT_DISTANCE_FROM_OBJECT);
l_robot_distance_count_mm = (ROBOT_DISTANCE_FROM_OBJECT - (SENSOR_ROBOT_DISTANCE_MM + SUBSTRC_DISTANCE_TO_ROBOT_MM)) * COUNTS_PER_MM;
LeftMotor.setTargetPosition((int) (LeftMotor.getCurrentPosition() - l_robot_distance_count_mm));
RightMotor.setTargetPosition((int) (RightMotor.getCurrentPosition() + l_robot_distance_count_mm));
LeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
RightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
LeftMotor.setPower(0 - l_chasismotor_power);
RightMotor.setPower(l_chasismotor_power);
while (LeftMotor.isBusy() && RightMotor.isBusy()) {
telemetry2("Measured distance by sensor was", (int) ROBOT_DISTANCE_FROM_OBJECT);
}
}
/**
DistanceSensor_DistanceSensor = hardwareMap.get(DistanceSensor.class, "DistanceSensor");
LeftMotor = hardwareMap.get(DcMotor.class, "LeftMotor");
RightMotor = hardwareMap.get(DcMotor.class, "RightMotor");
GrabberServo = hardwareMap.get(Servo.class, "GrabberServo");
ArmMotor = hardwareMap.get(DcMotor.class, "ArmMotor");
// Put initialization blocks here.
COUNTS_PER_MOTOR_REV = 28;
DRIVE_GEAR_REDUCTION = 15;
WHEEL_CIRCUMFERENCE_MM = 90 * Math.PI;
COUNTS_PER_WHEEL_REV = COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION;
COUNTS_PER_MM = COUNTS_PER_WHEEL_REV / WHEEL_CIRCUMFERENCE_MM;
INCHES_TO_MM = 25.4;
ROBOT_LENGTH_MM = 425.5;
ARMGEAR_REDUCTION = 2 * (125 / 45);
COUNTS_PER_ARMMOTOR_REV = 28 * 60;
COUNTS_PER_ARMGEAR_REV = COUNTS_PER_ARMMOTOR_REV * ARMGEAR_REDUCTION;
COUNTS_PER_ARMMOTOR_DEGREE = COUNTS_PER_ARMGEAR_REV / 360;
SENSOR_ROBOT_DISTANCE_MM = INCHES_TO_MM * 8.6;
SUBSTRC_DISTANCE_TO_ROBOT_MM = INCHES_TO_MM * 4.5;
GrabberServo.setPosition(1);
resetMotorEncoder();
resetArmMotorEncoder();
OriginalArmPos = ArmMotor.getCurrentPosition();
waitForStart();
if (opModeIsActive()) {
telemetry.addLine(OriginalArmPos);
telemetry.update();
moveArmMotor(1, "UP", 90, 1);
telemetry.addLine(OriginalArmPos);
telemetry.update();
telemetry.addLine("Code Started");
TPS = (75 / 60) * COUNTS_PER_WHEEL_REV;
moveRobotToDistanceFD(2, 43, "F", 0.6);
moveArmMotor(3, "UP", 65, 0.3);
telemetry.addLine("Specimen scored");
telemetry.update();
operateGrabberAndWait(-1);
telemetry2("Moving Robot Back", LeftMotor.getCurrentPosition());
moveRobotToDistanceFD(4, 23, "B", 0.3);
rotateDegree(5, "L", 0.5, 125);
resetMotorEncoder();
moveRobotToDistanceFD(6, 58, "B", 0.6);
telemetry2("Ready to reset motor encoder", RightMotor.getCurrentPosition());
resetMotorEncoder();
telemetry2("Reset motor encoder is done", RightMotor.getCurrentPosition());
telemetry2("Reseting Motor Encoder", LeftMotor.getCurrentPosition());
LeftMotor.setTargetPosition(0);
RightMotor.setTargetPosition(0);
LeftMotor.setPower(0);
RightMotor.setPower(0);
LeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
RightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
for (int count = 0; count < 2000; count++) {
telemetry2("Reseting Motor Encoder Done", LeftMotor.getCurrentPosition());
}
LeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
RightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
telemetry2("Reseting Motor Encoder Done", LeftMotor.getCurrentPosition());
LeftMotor.setTargetPosition(0);
RightMotor.setTargetPosition(0);
moveRobotToDistanceFD(7, 10, "F", 0.1);
telemetry2("FInal moved robot forward", LeftMotor.getCurrentPosition());
}
}
/**
/**
/**
/**
/**
/**
GrabberServo.setPosition(l_grabberPos);
GRABBER_POS = GrabberServo.getPosition();
while (l_robot_distance) {
GRABBER_POS = GrabberServo.getPosition();
telemetry2("Grabber Pos", (int) GRABBER_POS);
telemetry.addLine(GRABBER_POS);
telemetry.addLine("Grabber should open");
telemetry.update();
if (GRABBER_POS == l_grabberPos || GRABBER_POS == 0) {
break;
}
}
}
/**
telemetry2("Started rotating robot: step no, direction, degree, left mot pos, right mot pos:", "" + l_stepNo + l_rot_direction + l_deg + LeftMotor.getCurrentPosition() + RightMotor.getCurrentPosition());
DistanceTraveledPerWheel = INCHES_TO_MM * Math.PI * (14.1732 / (360 / l_deg));
OutputShaftRotations = DistanceTraveledPerWheel / WHEEL_CIRCUMFERENCE_MM;
EncodersTicksForDegree = OutputShaftRotations * 420;
if (l_rot_direction.equals("L")) {
LeftMotor.setTargetPosition((int) (LeftMotor.getCurrentPosition() + EncodersTicksForDegree));
RightMotor.setTargetPosition((int) (RightMotor.getCurrentPosition() + EncodersTicksForDegree));
} else {
LeftMotor.setTargetPosition((int) (LeftMotor.getCurrentPosition() - EncodersTicksForDegree));
RightMotor.setTargetPosition((int) (RightMotor.getCurrentPosition() - EncodersTicksForDegree));
}
LeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
RightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
LeftMotor.setPower(l_rotatepower);
RightMotor.setPower(l_rotatepower);
telemetry.addLine("Robot turnig");
telemetry.update();
while (LeftMotor.isBusy() && RightMotor.isBusy()) {
}
telemetry2("Done rotating", LeftMotor.getCurrentPosition());
}
/**
/**
I tried all possible options which we could find from internet like wait for some 100 ms, so we put loop for 2000 but no luck. We tried stopping power of motor before call stop_reset_encoder but no help.
We cannot move forward without resetting the encoder as we rely on encoder to move or rotate.
This is a pretty complex Blocks program. It is difficult to tell just from the screenshot image exactly what is happening.
So the first thing to do is simplify, and unit test.
The assertion is that STOP_AND_RESET_ENCODER is not doing what you expect it to do. Write a trivial Blocks opmode that does only that. Set the RunMode to RUN_USING_ENCODER. Run the motor for a short period of time, then set the RunMode to STOP_AND_RESET_ENCODER, and then drop the value of the encoder into the log, or telemetry.
I am sorry but if you notice I have lot of functions. There is one function called resetMotorEncoder() that I am calling from runOpMode method.
I am going forward then backward then rotating robot then going back then move forward. My issue is after rotating robot when I call reset encoder, it doesn’t work.
I can try simple program to know if it is motor encoder problem or not, i will do that but my problem remains same.
I have uploaded code to GitHub - nicejack79/FTC2024: FTC2024.
Pls help.
Hello,
I tried basic program with just forward move (with encoder) and tried to reset encoder after move and left motor current position didn’t come to 0. So something is missing big time? Pls advise.
Java code.
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
@Autonomous(name = “TestEncoderReset (Blocks to Java)”)
public class TestEncoderReset extends LinearOpMode {
private DcMotor LeftMotor;
private DcMotor RightMotor;
/**
// Put initialization blocks here.
waitForStart();
LeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
RightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
for (int count = 0; count < 2000; count++) {
telemetry.addData("Left Motor Pos after 1st reset", LeftMotor.getCurrentPosition());
telemetry.update();
}
if (opModeIsActive()) {
LeftMotor.setTargetPosition(-144);
RightMotor.setTargetPosition(144);
RightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
LeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
LeftMotor.setPower(0.5);
RightMotor.setPower(0.5);
// Put run blocks here.
while (RightMotor.isBusy() && LeftMotor.isBusy()) {
// Put loop blocks here.
}
LeftMotor.setPower(0);
RightMotor.setPower(0);
LeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
RightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
for (int count2 = 0; count2 < 2000; count2++) {
telemetry.addData("Left Motor Pos after 2nd reset", LeftMotor.getCurrentPosition());
telemetry.update();
}
}
}
}
Block Image:
You did a good job following the previous advice to make a simple OpMode to focus on the problem.
There are a few improvements that could be made to your test OpMode, but overall it’s OK for what you are doing.
===========
Your last post stated the “left motor current position didn’t come to 0”. Did it come close to zero, but not exactly?
If so, here’s a theory, assuming your motor and encoder are not defective: your left motor may not have come to a complete stop (zero movement) when you reset the encoder.
How could this happen?
If the right motor reached its target value first, the while loop will end while the left motor is still trying to reach its target. This is because you used the logical AND.
You then apply zero power to both motors. Both motors will start to coast, because the default ZeroPowerBehavior is FLOAT, not BRAKE. In other words, you are not really forcing or allowing both motors to instantly stop rotating. Note that only the left motor was still trying to rotate, if the right motor had reached its target.
You then apply Stop_And_Reset_Encoder, possibly still with some angular momentum at the motor axle. This could be caused by the momentum of the entire robot, backdriving one or more of the drive motors (causing slight additional rotation).
At this point you read the encoder value, which has not reached zero.
==========
Some suggestions:
in the INIT section, set ZeroPowerBehavior to BRAKE for both motors
sleep approximately 200 milliseconds after stopping, before resetting the encoder
add telemetry inside the while loop, to confirm proper encoder response (magnitude and direction)
increase the magnitude of the target positions. 144 encoder counts is a very short amount, essentially a burst of torque that might limit the PID RunMode’s ability to ramp up/down or stop with precision.
for safety, always add opModeIsActive to your loop conditions
==============
The choice of logical AND or OR for ‘isBusy’ looping, is based on your goals for the current and subsequent driving actions. There are subtle differences, such as:
OR sometimes allows still-running motors to backdrive a non-running motor
OR might allow a laggard motor to stall or surge, trying to reach its target against resistance from the other motor(s)
etc.
Testing may reveal your preferred choice.
============
Please feel free to share your final resolution here, to help other teams.
No, it didn’t come close to 0, it was -220 or any number where motor was.
OK! Still, my suggestions should help you debug the problem.
Confirm that the LeftMotor encoder starts at zero; might help to move the initial reset and telemetry before waitForStart.
Then observe the LeftMotor encoder behavior during the loop.
Could also try Disabling all RightMotor Blocks.
It should be noted that most “DRIVE” motors in FTC use 28 or so ticks per revolution on the internal shaft (pre-gearbox). -220 is about 10 rotations of the internal motor shaft, which depending on your motor transmission gearing is completely explained by the use of the “AND” relationship in the isbusy logic that @Westside mentioned (it really should be “OR”). -220 may seem like a big number, but it represents just over 72 degrees of rotation on an output shaft on a 40:1 motor (not quite a quarter of a turn). Set the motor zero power behavior to BRAKE after each time you change the mode back to “RUN_WITHOUT_ENCODER”, and use the “OR” relationship in the isbusy logic to see if you can reduce that further.
