Encoded Motor Position Control

I am currently trying to run a simple code to command an angular displacement for an encoded motor and after hours of searching, I stumbled upon the attached code (with some modifications for the arduino board used). The motor is currently just spinning but I want to be able to command to turn a specific angle.

I am using an MKRZero Motor driver: Encoded motor:

#define InA1            7                      // INA motor pin
#define InB1            6                      // INB motor pin
#define PWM1            2                       // PWM motor pin
#define encodPinA1      5                       // encoder A pin
#define encodPinB1      4                       // encoder B pin

#define LOOPTIME        100                     // PID loop time
#define FORWARD         1                       // direction of rotation
#define BACKWARD        2                       // direction of rotation

unsigned long lastMilli = 0;                    // loop timing
unsigned long lastMilliPrint = 0;               // loop timing
long count = 0;                                 // rotation counter
long countInit;
long tickNumber = 0;
boolean run = false;                                     // motor moves

void setup() {
 pinMode(InA1, OUTPUT);
 pinMode(InB1, OUTPUT);
 pinMode(PWM1, OUTPUT);
 pinMode(encodPinA1, INPUT);
 pinMode(encodPinB1, INPUT);
 digitalWrite(encodPinA1, HIGH);                      // turn on pullup resistor
 digitalWrite(encodPinB1, HIGH);
 attachInterrupt(1, rencoder, FALLING);

void loop() {
 moveMotor(FORWARD, 50, 1200*2);                        // direction, PWM, ticks number
 //moveMotor(BACKWARD, 100, 2400*2);                           // 464=360°

void moveMotor(int direction, int PWM_val, long tick)  {
 countInit = count;    // abs(count)
 tickNumber = tick;
 if(direction==FORWARD)          motorForward(PWM_val);
 else if(direction==BACKWARD)    motorBackward(PWM_val);

void rencoder()  {                                    // pulse and direction, direct port reading to save cycles
 if (encodPinB1 & 0b00000001)    count++;                  // if(digitalRead(encodPinB1)==HIGH)   count_r ++;
 else                      count--;                  // if (digitalRead(encodPinB1)==LOW)   count_r --;
   if((abs(abs(count)-abs(countInit))) >= tickNumber)      motorBrake();

void motorForward(int PWM_val)  {
 analogWrite(PWM1, PWM_val);
 digitalWrite(InA1, LOW);
 digitalWrite(InB1, HIGH);
 run = true;

void motorBackward(int PWM_val)  {
 analogWrite(PWM1, PWM_val);
 digitalWrite(InA1, HIGH);
 digitalWrite(InB1, LOW);
 run = true;

void motorBrake()  {
 analogWrite(PWM1, 0);
 digitalWrite(InA1, HIGH);
 digitalWrite(InB1, HIGH);
 run = false;

You will need something a lot more complex if you want a DC motor to stop at a particular encoder count. You will need code that slows the motor as it approaches the target and code that moves it backwards when it exceeds the target.

This is not a trivial project.


The encoder has over 1400 transitions per revolution of the output shaft. That can give you very fine control over position.

The first problem is that there is no zero position provided by that encoder. You need to add some mind of "home switch" to define the zero position.

The second problem is driving the motor towards the commanded position. Simple proportional control will be easy: the further you are away from the desired position the harder you drive the motor. Just play with the hardness setting until it gets to where you want.

You will find that there is some drive value which is too small to start the motor moving but it will keep moving if it is already moving. Maybe it will not start to move with a PWM value less than 20. So if the calculation of motor speed gives you less than 20 then don't even send that to the motor. Wait until the commanded point moves far enough that it is worth starting the motor.

Simple P-term only will give crude control, but the full PID loop (preferably with integral-windup avoidance) will do a better job once tuned.

Error term is commanded position - current position, output is a singled value that needs converting to direction and PWM level parts.