Help with quadrature encoder controlled speed correction

I’d like to use two optical shafts (one on a left wheel and one on a right) to dynamically alter the speeds of the wheels so they spin at the same rate. As you can see in the attached easyC code, the method I tried was to compare the values of each encoder and slow down the motor which corresponded to the higher quad shaft value. This was supposed to slow down the faster motor repeatedly until the speed of this motor balanced with the other.
I was aware that there was a possibility for the motor which was originally faster to slow down beyond the other motor, thus causing the originally slower motor to slow down. I was worried this cycle would repeat until both values were near 127, so I added the motor function to bring the slower motor back up to full.
I received several different erroneous results after executing the program. The first was motor 2 returning a value of -1 (which caused motor 2 not to turn at all) while motor 1 began at maximum speed and quickly dropped down to a stop.
After some manipulation I managed to get both motors returning values of -1 (at least it was consistent :mad:).
After changing more I managed to get both motors spinning at full speed, then switching direction for a few milliseconds, then switching back. This switching alternated on each motor 180 degrees out of phase. They kept perfect time with each other, but failed to show any sort of speed control.
After further manipulation I got both motors to switch at the same time, but they switched four times each cycle.
I gave up at this point due to pure frustration. I wish I had saved the different stages of this code, but all I have is the original. I would really like to know the correct way to do this. Someone please help.
Dynamic Speed (2.08 KB)

I wasn’t able to open the attached file.

It works just fine for me? I use IZArc to open archive files if that helps at all? It’s a free program:

Saving all the different stages of the code is called revision control. Since EasyC doesn’t do this for you automatically, it takes good discipline to do it on your own. You might search on topics like “revision control”.

I was not able to load the code, and the strings view of it was not clear. Since there are generic C programmers here without EasyC, if you post in clear-text, you might find more help.

Without viewing the code in much detail, here are a couple ideas that might help.

  • Did you correct for the fact that the left wheel full speed is 255, while right wheel full speed is 0?
  • Did you mount the encoders the same way, so that full forward on one is full forward on the other?
  • Debugging with some print statements might help.
  • Did you search the forum for similar questions? In previous years, I think someone has already posted something called “drive straight”. Looking at that might help.

The encoders are mounted correctly. The value 255 corresponds to forward for both motors. I’ve used PrintToScreen functions all throughout this program, but received no output anywhere. I looked through older posts and found a lot about slowing down a motor manually, but I’m not happy with the results of this. The closest I’ve seen to integrated speed control is a brief mention of how optical shafts COULD be used for this task.

Here is my code. EasyC doesn’t have an export text tool, so I had to copy this by hand. Please forgive any syntax errors. I have no idea what could be causing the errors I’m getting. Obviously this code is far from perfect, but the idea seems simple enough…

#include “Main.h”

#define M11 255 // left wheel forward
#define M10 0 // left wheel backward
#define M21 255 // right wheel forward
#define M20 0 // right wheel backward

char step = 1;
char bump;
int quad1;
int quad2;
char m1;
char m2;

void main (void)
// quad1: interrupt 1, digital output 1, left wheel
// quad2: interrupt 2, digital output 2, right wheel
// bumper: digital output 3

StartQuadEncoder (1,1,0);
StartQuadEncoder (2,2,0);
PresetQuadEncoder (1,1,0);
PresetQuadEncoder (2,2,0);

m1 = M11;
m2 = M21;

// drive forward until bumper is pressed
// constantly balance speeds to prevent drift

while ( step = 1 )

  bump = GetDigitalInput (1);

  if (bump == 0)

  quad1 = GetQuadEncoder (1,1);
  quad2 = GetQuadEncoder (2,2);

  // left wheel faster than right

  if (quad1 > quad2)

  // bring m1 closer to 127

  if (m1 > 127)
     m1 = m1 - 1;

  if (m1 < 127)
     m1 = m1 + 1;

  // max out m2

  m2 = M21;


  // right wheel faster than left

  if (quad2 > quad1)

  // bring m2 closer to 127

  if (m2 > 127)
     m2 = m2 - 1;

  if (m2 < 127)
     m2 = m2 + 1;

  // max out m1

  m1 = M11;


  SetMotor (1,m1);
  SetMotor (2,m2);


while (step == 2)
SetMotor (1,127);
SetMotor (2,127);
StopQuadEncoder (1,1);
StopQuadEncoder (2,2);

Thanks for posting code even if hand-typed:
I’ll try to get some EasyC running.
I’d like to be able to duplicate your setup.

The (non quad) example here:
shows that PRESET should come before START. That makes sense to me.

RobotC recommends using any of INT3…6 not INT0…3 for quad encoders, not sure if EasyC cares. You might try that also.

Your code above does not start with the motors at forward position.
I recommend starting like this pseudo code:
SetMotor (1,127); SetMotor (2,127); wait (100); // wait 100ms stopped
SetMotor (1,250); SetMotor (2,250); wait (500); // drive forward 500ms
SetMotor (1,127); SetMotor (2,127); wait (500); // wait stopped again
STOP encoders
PRESET encoders
START encoders
Start both motors forward
while(step==1){ loop starts here.
(your read encoder and adjust motor code here)

Can you diagram your motors like line below, or post a picture?
Lmotor-L1frame-Lwheel-L2frame-Lencoder … Rmotor-R1frame-Rwheel-Rframe2-Rencoder

The above code should make robot move forward for 500ms, to show that motor direction is correct.

With EasyC 1.x and 2.x, try this method in post, Copying code, output. This is “broken” in EasyCPro, and I have not tried EasyC 4.x yet…

ive used preset after the start command and even between different steps throughout programs that just track distances with the encoders and i havent had any problems. to be sure it worked correctly i used print functions to to observe the status of both cases, with start first and preset first, and didnt see anything different.
I tried switching start and preset in the speed control program and got the same errors as before.

what do u mean by INT3…6 and INT0…3?

i dont understand the purpose of the bit of pseudo code uve added. is this to get some data stored in the encoders before they try to balance their speeds?

ive attached a diagram of my setup. its just a simple square bot to test this speed control setup.

the 250 motor value makes both motors move the robot forward and the encoders record positive numbers while moving forward.

Old Quad encoders have one wire, I think they only count up.
New Quad encoders have two wires, and can tell the difference between forward and backwards by counting up or down. One of the wires goes to the “interrupt” port INT#, the other wire to a “digital In” port.

With a standard squarebot, one motor will need to be defined as “reversed” or “inverted” in drivemodule code blocks.
When using just SetMotor(1,250); SetMotor(2,250); Squarebot will spin in a circle. The purpose of the SetMotor/Wait sequence of pseudocode at the start was to demonstrate this fact. If you tried it, what happened?

ok when you said int i thought of the variable, not the interrupt port. i dont think it matter which one is used over any other one? i know that before the vexnet setup was introduced, a jumper in interrupts 5 and 6 would disable autonomous or driver control for testing purposes, so i always start from int 1.

i didnt show it in my robot map because i didnt think it would be relevant, but i geared the right motor 1:1 to invert its direction so i could diagnose the programming issue easier. i thought if 255 was the forward command for both motors things would work out simpler. i also mounted the right encoder identically to the left (not mirrored) so that feedback from both would be the same.

so just to recap:

  • 255 is the forward command for both motors
  • pseudo code waits 100ms, drives forward 500ms, then stops
  • both encoders report increasing numbers as the robot drives forward without needing to be inverted
  • ive never seen any recommendation in easyC to use the higher int ports, nor do i think there is a difference between the higher and lower
  • the second encoder wire is in the digital output port specified in the code i posted
  • print functions work in everything else i try them in, but i dont get any feedback whenever i try this code

i dont expect my code to be able to be fixed at this point. all i would like to know is if there is a way to dynamically alter the speed of one wheel to match that of its opposing wheel.
ive heard of velocity PID, which sets a target speed below the maximum speed of both motors, but thats not exactly what im looking for.
first, id like the maximum speed of my slower motor to be the top speed of the robot. and second, id like the speed of this slower motor to determine the speed of the other. this way, when motor 1 slows down, motor 2 also slows down to match. and if motor 2 were to slow down instead, so would motor 1.