I'm trying to implement a steering mechanism for a segway project. The idea is to sense the tilt of the segway's handlebar with an incremental rotary encoder (omron e6j-cwz 600ppr). This way when I turn the bar to the left or right the segway turns too. I'm using a Teensy 3.2 with the arduino IDE.

To drive the motors I use PWM. The pulse limits are the calibrated values for +/- 100% (+/- 24V) input. The pulse range is 1.0 to 2.0ms, with 1.5ms as the zero value.

Here is a picture of the turning mechanism.

Encoder handlebar coupling

The problem is that, the movement of the handle bar is not very smooth and sometimes after tilting the handlebar and returning it to the straight position, the count never really goes back to 0, but some value around -6 to 6 counts and sometimes even -15 to 15 counts. So the segway tries to turn even though the bar is straight.

The code I'm using to sense the tilt and set the motors is:

//Turn encoder
#define c_TurnEncoderPinA 15
#define c_TurnEncoderPinB 14
#define TurnEncoderIsReversed
volatile bool _TurnEncoderBSet;
volatile int _TurnEncoderTicks = 0;
/** Turn position */
int TurnAngular = 0;

/** Proportional gain */
int kp = 55;

/** voltage to the motor*/
float vol = 0;
float aux;
/** calulated pwm pulse*/
float uy = 0;

/**Left motor control*/
int motorL=9;
/**Right motor control*/
int motorR=10;

//Mapping voltage to pwm (Mc 160 controller)
float  p1 = -25.44, p2 = 0.1894, p3 = 146.6, p4 = -0.2874, p5 =  -480, p6 =  1500; //pwm3

//Discrete time values 
const int samplingTime = 1000*10; // 10000 microseconds = 10ms
unsigned long previousTime, currentTime, timeDifference;
int estimationTimeCount = 1;

void setup() {
  previousTime = micros();
  // Turn encoder
  pinMode(c_TurnEncoderPinA, INPUT_PULLUP);      // sets pin A as input
  pinMode(c_TurnEncoderPinB, INPUT_PULLUP);      // sets pin B as input
  attachInterrupt(digitalPinToInterrupt(c_TurnEncoderPinA), HandleTurnMotorInterruptA, RISING);

  /*Initiate Output*/
  analogWrite(motorR, 1500);
  analogWrite(motorL, 1500);


void loop() {
  currentTime = micros();
  timeDifference = currentTime - previousTime;
  if (timeDifference > samplingTime)
    previousTime = currentTime;

void sampleEncoders() {
  TurnAngular = _TurnEncoderTicks; //update encoder reading every 10ms.

float turn(){
  if(TurnAngular<=6 && TurnAngular>=-6)
    return 0;
   //the voltage is calculated with a proportional gain 
   //times the angle of the encoder in radians.
    return (kp*TurnAngular*(2*pi/(pprT))); 

void setMotors() {
    vol = turn();

    // this two lines map the calculated voltage to the pwm 
    // in a range of 1000 to 2000. 
    aux = (vol+0.01143)/13.13; 
    uy = (int) p1*pow(aux,5) + p2*pow(aux,4) + p3*pow(aux,3) + p4*pow(aux,2) + p5*(aux) + p6;
    uy = constrain(uy, 1000,  2000);

    // this is just and idea I had to counter the problem.
    // if the control is within +/-50 pulses from 1500
    // set the pulse to 1500, and the last saved value of the count to 0.
    // every 2.5 sec if the condition in met, then also reset the actual
    // count of the encoder
    if (uy < 1550 && uy > 1450)
      uy = 1500;
      TurnAngular = 0;
      if ( estimationTimeCount%250 == 0 && _TurnEncoderTicks != 0)
        _TurnEncoderTicks = 0;

 Left wheel (L): forward from 1500 to 1000, backward from 1500 to 2000.
 Right wheel (R): forward from 1500 to 2000, backward from 1500 to 1000.
 //set uy to each wheel to make the segway turn.

void printOutput()
  Serial.print(", :) = [");
  Serial.print(", ");
  Serial.print(", ");

void HandleTurnMotorInterruptA()
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  _TurnEncoderBSet = digitalRead(c_TurnEncoderPinB);   // read the input pin
  // and adjust counter + if A leads B
  #ifdef TurnEncoderIsReversed
    _TurnEncoderTicks -= _TurnEncoderBSet ? -1 : +1;
    _TurnEncoderTicks += _TurnEncoderBSet ? -1 : +1;

The original code for the encoder count can be found here.

In the code I put one of my own but it fails sometimes, I set a 0 voltage to the motors is the count is between +/- 6 counts. A secondary issue is that when the bar tilt's to for example +7 counts the control is to aggressive, so I keep trying to find an ideal proportional gain.

Lastly I did not add the code for the balance action of the segway, mainly because it is not the issue and the control effort is just set to 0 while I try to fix the problem. Also, it would be too long.

What kind of solution would you guys think of?

I've edited the turn function like this:

float turn(){
int8_t TurnVal = 0;
  //deadZoneSize = 6
  if(TurnAngular <= deadZoneSize && TurnAngular >= -deadZoneSize)
    return 0;
    if (TurnAngular<0) 
      TurnVal = TurnAngular + deadZoneSize;
      TurnVal = TurnAngular - deadZoneSize;
    return (kp*TurnVal*(2*pi/(float)(pprT)));

I can't find a way to fix the error between the actual straight position and the measured straight position (0 count). Sometimes after some movement of the bar, the actual straight position lies outside the dead zone, and the segway wants to turn anyways.

Here is an illustration.

enter image description here

  • 1
    I'd suggest using a Absolute rotary encoder, or a simple potentiometer. Those won't suffer from missing steps.
    – Gerben
    Sep 3, 2018 at 16:03
  • I could look into an absolute encoder, but I just want to see if it can fix it by code. The project had a potentiometer before, but the task I'm ask for is to use an encoder, just my luck. Sep 3, 2018 at 18:27
  • What model of Arduino are you using? I don't think there are any that support pins 14 and 15 for RISING edge external interrupts. Could you use pin 2 for c_TurnEncoderPinA instead of 14? Sep 4, 2018 at 0:41
  • I'm using a Teensy 3.2, all digital pins have interrupt capability. Sep 4, 2018 at 0:46

1 Answer 1


What you have come up with on your own is almost what is called a "dead zone", where the angle between -6 and 6 is ignored.

All you need to do now is not reference the actual angle when computing how much to turn, but to reference the distance from the edge of the dead zone:

const uint16_t deadZoneSize = 6;

float turn(){
  unint8_t TurnVal = 0;
  if(TurnAngular<=deadZoneSize && TurnAngular>=-deadZoneSize)
    return 0;
   //the voltage is calculated with a proportional gain 
   //times the angle of the encoder in radians.
    if (TurnAngular<0) {
      TurnVal = TurnAngular + deadZoneSize)
    } else {
      TurnVal = TurnAngular - deadZoneSize)
    return (kp*TurnVal*(2*pi/(pprT))); 
  • Thanks, that's actually an improvement, but it doesn't really solve the problem. Now the whole dead zone moves away from the straight position. A very small tilt on one side gets you out of the zone, and a relatively large tilt on the other side does too. This happens because every time the bar is moved and then returned to the straight position, the counter (_TurnEncoderTicks) never goes to 0, so the error gets worse for every movement of the bar. I'll answer the post with an illustration. Sep 3, 2018 at 18:13
  • What prevents you from setting that variable to zero when you need to? Sep 3, 2018 at 18:15
  • Well, I believe its because I don't have a reference, in code I can never really tell what new number (the count) corresponds to the straight position. Sep 3, 2018 at 18:34
  • Ah, so it seems you are missing counts, otherwise zero would stay zero. What if you increase the sampling time? The line uy = (int) p1*pow(aux,5) + p2*pow(aux,4) + p3*pow(aux,3) + p4*pow(aux,2) + p5*(aux) + p6; takes a whole lot of cycles to use pow() with float types. Is there a way you could simplify that algorithm to integer math? Sep 4, 2018 at 0:14
  • I could increase the sample rate, but it will mess up the whole balance part. Sep 4, 2018 at 0:49

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.