Driving the Robot Straight Using Encoders and a Joystick

As a team, we decided to drive our robot with the help of encoders. We want to drive our robot straight even though it is controlled by the joystick. We do not know how to achieve this as one motor is obviously slower. Also, we are displaying the encoder values but they seem to stop after an integer around 25, whereas they should increase as they show the count of rotations. Can you guys help us with this?

    public class Robot extends IterativeRobot {
    	Victor Motor0= new Victor(0);
    	Victor Motor1= new Victor(1);
    	DifferentialDrive drive= new DifferentialDrive(Motor0,Motor1);
    	Joystick stick=new Joystick(0);
    	double dist =0.5*3.14/1024;
    	Encoder esit= new Encoder(0,1,true,Encoder.EncodingType.k4X);
    	Encoder esit1 = new Encoder(2,3,true,Encoder.EncodingType.k4X);
    	double m0 = esit.get();
    	double m1 = esit1.get();
    	double fark = m1 - m0;
    	double gidilen_mesafe1 = esit.getDistance();
    	double gidilen_mesafe2 = esit1.getDistance();

    	public void robotInit() {

    	public void autonomousInit() {


    	public void autonomousPeriodic() {


    	public void teleopPeriodic() {
    		drive.arcadeDrive(stick.getRawAxis(0), stick.getRawAxis(1));

    		SmartDashboard.putNumber("Tekerlek1", esit.get());
    		SmartDashboard.putNumber("Tekerlek2", esit1.get());
    		SmartDashboard.putNumber("Aradaki Fark", fark);
    		SmartDashboard.putNumber("Mesafe 1", gidilen_mesafe1);
    		SmartDashboard.putNumber("Mesafe 2", gidilen_mesafe2);

    		boolean m_stop = esit.getStopped();
    		boolean m1_stop = esit1.getStopped();
    		if(m_stop==false && m1_stop==false) {
    	public void testPeriodic() {

I’d suggest using a gyroscope to track the rotational drift, and using that to correct for alignment, as this can also be used to make precise turns in games with auto.

From our 2018 bot:

public double[] getStraightOutput(double l, double r, double target) {
	final double ANGLE_TOLERANCE = 1;
	double k_p = 0.03;
	double l_out = l;
	double r_out = r;
	double current_angle = getAbsoluteAngle() - target;
	if (Math.abs(current_angle) > ANGLE_TOLERANCE) {	// adjust values to compensate for turning drift	
		double modifier = current_angle * k_p;	// make amount of correction proportional to angle
		l_out += modifier;
    	r_out -= modifier;
	l_out = l_out > 1 ? 1 : l_out;	// ensure values are within -1 to 1 range
	l_out = l_out < -1 ? -1 : l_out;
	r_out = r_out > 1 ? 1 : r_out;
	r_out = r_out < -1 ? -1 : r_out;
	return new double[] {l_out, r_out};

This returns the percentages to maintain a straight line.

The rest of the code is at: https://github.com/LakeEffectRobotics/LER_2018_VS

While it would probably be easier to just use a gyro to correct for unwanted turning, you could use the encoders to achieve the same result. You should be able to run a PID loop off of the difference of the two encoder speeds, with a setpoint of 0 and using the output as an adjustment turning value. Assuming the two encoders have identical setups on either side and are functioning correctly, this would be essentially the same as using the gyro.

Your encoder issue sounds strange, though. I’ve got questions: What encoders are you using? Where are they mounted? Which specific values (out of the five you’re sending to the SmartDashboard) are showing weird data? What’s the intended purpose of the section of code below?

boolean m_stop = esit.getStopped();
boolean m1_stop = esit1.getStopped();
if(m_stop==false && m1_stop==false) {

As a side note, you can make this code a little bit cleaner by using more concise boolean logic. Since m_stop and m1_stop are already booleans, m_stop==false is equivalent to !m_stop.

These may help…

  1. Software implem in RIO that does go-straight. It uses our Pigeon-IMU but you could modify to take the different (right-left) sensor. Instead of assign the left and right motors directly, you would update your differential drive.

  2. This one is more advanced (uses our CAN features), so not directly helpful for OP, but demonstrates a similar theory-of-operation.

  3. Same as 2 but uses right-minus-left-encoder-position (does it over CANbus).

Some key points in all the examples

  • Have a button to signal the robot when to maintain heading so driver decides when to assist.
    Shoulder button on gamepad would work well for this.
  • you have to latch/save the current heading on button-transition so your software knows what “straight” is.
  • notice there is no resetting of the sensor(s) after boot up. That’s because the current-heading is saved once the robot decides to transition from turning-to-drive-straight.

We have had software that assumes if the commanded rotation is zero, then straight line driving is requested, and it automatically assists.


double RequestedRotation=getRotationFromJoystick();
double RequestedVelocity=getVelocityFromJoystick();

if (RequestedRotation==0)
double currentAngleError=getCurrentAngleFromGyro()-getTargetAngle();
double correctionFactor=-1 * Kp * curretAngleError;//Make sure you get the sign right, or you’ll go into what we affectionately call “death spiral”
drive.ArcadeDrive(RequestedVelocity,correctionFactor); //Drive straight, but rotating slightly to correct toward the target


 drive.ArcadeDrive(RequestedVelocity, RequestedRotation);

One caution to the above. If you just do that, then if you are rotating the robot, as soon as the driver stops rotating, the requested angle becomes zero. The pseudocode above can, in some circumstances, cause the robot to “kick” backward. It depends on how the target is set. This is caused by the fact that inertia will cause the robot to continue to rotate after the driver has stopped demanding rotation, but the program might decide that the target angle is the one that was requested at the moment the robot stopped receiving the command. In that case, you might have to say if (RequestedRotation==0) && (measuredRotationFromGyro()<THRESHHOLD). then apply the correction.

And if the above paragraph makes no sense, don’t worry about it until you see the robot “kick” after ending a rotation. Then read it again. A full explanation is a bit too long for this post, and whether or not it’s needed depends on the characteristics of your drive train and whether you are using robot centered or field centered driving, and exactly how you implement it.

Also, whenever we have “driver assist” functions, that take control away from the driver, we always implement a “manual override”, to put control back in the hands of the joystick, just in case sensors go crazy or wiring faults cause errors. Unlike the manual override on the Starship Enterprise, ours usually work.

We are using CIMcoders. Our motors are not mounted. Technically we are just trying it out before mounting them. We are displaying the data we got from the encoders. That display is the esit.get() and esit1.get() in the code. Also, we are using this code to reset the data when the motors are not moving. But as it rotates encoders the value is increasing or decreasing with the movement of the joystick. This should always increase as it should display the count of rotations.
//the code below is used to reset the encoder data when motor stops because we want to see //the count of rotations when we are using them
" boolean m_stop = esit.getStopped();
boolean m1_stop = esit1.getStopped();
if(m_stop==false && m1_stop==false) {

I’m don’t think this code is doing what you’re expecting. As I read through it, whenever both esit and esit1 are not stopped (equivalent to m_stop==false and m1_stop==false), both encoders are reset. This means that the encoders are being reset any time both motors are in motion, rather than when they are stopped. I’m not so sure that this would cause the behavior you described in your first post, but it could certainly be part of the issue.