Results 1 to 6 of 6

Thread: High Resolution Servo Control

  1. #1
    75+ Posting Member
    Join Date
    Sep 2013
    California, USA

    High Resolution Servo Control

    I’ve been converting cockpit instruments to servo control for my P337 and found that for some of the
    Gauges, notably the altitude indicator, I needed better accuracy and resolution than I could achieve directly from a Mega. After a fair amount of effort I have come up with a working solution and want to share it with anyone else attempting to do the same.

    The Mega’s internal PWM has a 10bit D/A resolution and a 60Hz cycle rate. I have measured pulse length jitter in the 3-5Ás range with an oscilloscope when a Mega was operating with a typical L2FS sketch. Attempting to drive an altimeter with this output, the result was a jerky movement and occasional random excursions of up to 300ft from the actual altitude. Using a 16-channel PCA9685 breakout board (Adafruit $15) which has 12bit PWM resolution and a very stable internal clock which I have set for a 240Hz cycle rate, I’ve been able to obtain gauge needle resolution of 20ft with an accuracy of 1% over the entire range. The PCA9685 performs the PWM calculations and maintains the PWM signal without input from the Mega, so the Arduino only has to send a position signal when there has been a change in output, reducing processing time.

    The key to getting this performance, in addition to using the PCA9685 breakout board, is to have an accurate test station which can provide bit-level control of the PWM signal so that a detailed mapping of output signal vs gauge reading can be generated and a multi-point calibration curve can be calculated for use in the actual L2FS control sketch.

    For servo calibration purposes I built a portable calibration station consisting of:
    1 Mega
    1 quadrature encoder (Gray code)
    1 4-digit 7-segment display to provide continuous indication of the 12-bit control signal (0-4095)
    1 Adafruit PCA9685 breakout board
    2 5v power supplies
    2 100 nf ceramic capacitors

    In addition to driving the servo, I included in the Arduino sketch a routine to automatically save the servo position to non-volatile memory at 60 second intervals and retrieve this data during startup so that the PWM output will automatically begin at the current servo position.

    Calibration Station Design

    Here is the configuration of my calibration station:

    Adafruit PCA9685_bb.jpg

    The output from channel A of the encoder is monitored for movement, and provides a normal increment/decrement of 1 bit per detent, or 25 bits if the encoder knob is depressed while turning.

    Here is the Calibration station code:

    /* This sketch is used for my portable servo calibration station.  It uses a 16-channel 12-bit PCA9685 
    breakout board (Adafruit $15) set up for a servo refresh rate of 240Hz. An additional feature of the sketch is 
    an automatic save of servo position to non-volatile memory every 60 seconds so at power up the PWM 
    resumes at the present servo position.
    The lowest and highest portions of 0-4095 control range will drive the servo against its hard stops and damage 
    the gears so I start testing somewhere in the middle and dial the output up and down to find out where the end 
    of travel points are.
    Once these limits are established, I make a list of significant servo positions (gauge readings or hardware locations) 
    and adjust the encoder to each of these positions, making note of the PWM output.  It can be as few as 2 positions or 
    upwards of 100 if I need additional accuracy.  I use a spreadsheet (Excel) to make an
    X/Y line chart of the values, and use a linear regression analysis on each line segment (Y=aX + b).  The calibration 
    curve information is then used in the sketch for my L2FS cockpit.
    // ***********************BEGIN DECLARATIONS **********************
    #include "Wire.h"
    #include "Adafruit_PWMServoDriver.h"         //Adafruit library for the PCA9685
    #include "EEPROM.h" 
    Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);  //i2c address for the Adafruit board
    uint8_t servonum = 0;                                         // servo connected at position 0 on the Adafruit board
    #define DISPLAY_ADDRESS1 0x71                                 //i2c address of the 7-segment display
    int Servo1_position;
    int encdr1A = 18;
    int encdr1B = 19;
    int encdrSwitch = 16;
    boolean encdrSwitch_Value;
    boolean encdr1A_Value;
    boolean old_encdr1A_Value;
    unsigned time_minute;
    unsigned time_oldMinute;
    boolean time_changeMinute;
    int data;
    //--------------- Function - Write to EEPROM ---------------------------
    /*This function will write a 4 byte (32bit) long to the EEPROM starting at
    a specified address (This sketch uses address 0). The read/write uses a long 
    variable to allow storage of larger numbers if needed.
    void EEPROMWritelong(int address, long Servo1_data)
          /*Convert a long to 4 bytes by using bitshift and masking.
          byte one is the most significant byte.
          byte four is the least significant byte.
          byte four = (Servo1_data & 0xFF);
          byte three = ((Servo1_data >> 8) & 0xFF);
          byte two = ((Servo1_data >> 16) & 0xFF);
          byte one = ((Servo1_data >> 24) & 0xFF);
          //Write the 4 bytes into the eeprom memory.
          EEPROM.write(address, four);
          EEPROM.write(address + 1, three);
          EEPROM.write(address + 2, two);
          EEPROM.write(address + 3, one);
    //-------------Function - Read from EEPROM ------------------
    /*This function will return the 4 byte (32bit) long from the eeprom
    at the specified address(0).
    long EEPROMReadlong(long address)
          //Read the 4 bytes from the eeprom memory.
          long four =;
          long three = + 1);
          long two = + 2);
          long one = + 3);
          //Return the long by using bitshift.
          return ((four << 0) & 0xFF) + ((three << 8) & 0xFFFF) + ((two << 16) & 0xFFFFFF) + ((one << 24) & 0xFFFFFFFF);
    // **********************************************************************
    // ***********************BEGIN VOID SETUP*******************************
    void setup()
    Wire.begin();           //Join the bus as master
    // -------- Send a reset command to the 7-digit display ------------------------------
    // --------this forces the cursor to return to the beginning of the display ----------
    // --------Increase servo refresh rate to 240Hz ---------------------------
    pwm.begin();           //Starts communication with PCA9685
    pwm.setPWMFreq(240);   // 240Hz provides higher resolution with servos.
                           // Analog servos are designed to run at 50~60 Hz, it is possible 
                           // that some servos can't handle the high refresh rates but every
                           // one I have tested is ok.
    Servo1_position=(EEPROMReadlong(0));     //initialize PWM to last known servo position
    // ----------------Setup encoder pins as inputs ------------------------
        pinMode(encdr1A, INPUT_PULLUP);
        pinMode(encdr1B, INPUT_PULLUP);
        pinMode(encdrSwitch, INPUT_PULLUP);
    // **************************************************************************************
    // **************BEGIN VOID LOOP ********************************************************
    void loop()
    // --------------- Write servo position to EEPROM ---------------------------------
      if (time_oldMinute!=time_minute) time_changeMinute=true;
      else time_changeMinute=false;
      long address=0;
      if (time_changeMinute==true) {
          EEPROMWritelong(address, Servo1_position);
     /* This section of code would be used if there is a second servo positions to write
          EEPROMWritelong(address, number2);
      } // end if
    // ------------Increment/decrement servo position if encoder is rotated --------------
      encdr1A_Value=digitalRead(encdr1A);     //read encoder, channel A
    /*When encoder channel A goes from 0 to 1, if Channel B is 1 then the rotation is clockwise, 
    otherwise it is counterclockwise.  If the encoder switch is depressed increment by 25, if
    not depressed increment by 1. 
    if ((old_encdr1A_Value==0)&&(encdr1A_Value==1)){       //test to see if encoder has moved.  If not, skip
      if (digitalRead(encdrSwitch)==1){              //encoder is pressed
        if (digitalRead(encdr1B)==1) Servo1_position++;
        else Servo1_position--;
      else if (digitalRead(encdr1B)==1) Servo1_position=Servo1_position+25;
            else Servo1_position=Servo1_position-25;
    encdrSwitch_Value = digitalRead(encdrSwitch);
    // -------------- SEND SERVO POSITION TO 7 SEGMENT DISPLAY -----------------------
    Wire.beginTransmission(DISPLAY_ADDRESS1); // transmit to device #1
    Wire.write(data / 1000);                  //Send the left most digit
    data %= 1000;                             //Now remove the left most digit
    Wire.write(data / 100);
    data %= 100;
    Wire.write(data / 10);
    data %= 10;
    Wire.write(data);                        //Send the right most digit
    Wire.endTransmission();                  //Stop I2C transmission
    // -------------- SEND POSITION TO SERVO ------------------------------------------------
        pwm.setPWM(servonum, 0, Servo1_position);
    // ************** END *********************************************************
    Calibration Technique

    I divide the servo driven gauge needle reading into as many divisions as I think may be necessary to properly associate PWM outputs to gauge indications. For non-critical gauges, such as a turn & bank indicator I use a two point calibration (max & min gauge needle movement). For critical high resolution gauges, such as an altimeter I use a 200 point calibration so that I can be sure of accurate readings at any elevation. Intermediate gauges, such as an airspeed indicator get a 6 point calibration.

    The PWM output is capable of producing a pulse length from a minimum of 0Ás to the full cycle length. With a 240Hz cycle timing, the full cycle length is 1/240th of a second, or 4166Ás. Servos typically respond in the range of 800-2400Ás, but every servo is different. Servos have a physical stop at their maximum and minimum rotations and you need to find out where these stops are by physical testing because if you drive the servo against its physical stop you risk damaging the internal gears. Do this by starting the servo in mid-position, and run the pulse length down slowly until you hit the stop, back off about 10 and record the command output (0-4096) for this position. Do the same with the upper stop. I mark these values as a permanent record directly onto the side of each servo before I install them.

    Once the servo is installed and linked to the gauge I can then proceed with the calibration. The following procedure illustration is for an airspeed indicator with a 6 point calibration.
    Step 1.
    I divide the gauge range into equal steps, 0, 50, 100, 150, 200, and 250 knots. I hook the gauge/servo to my calibration station, adjust the output to read exactly 0, and record the output. This is repeated for each calibration point. The final result will look something like:
    Speed Output

    I enter these numbers into an Excel spreadsheet, and then generate a chart (X-Y scatter).

    Inspecting the chart, the response appears to be linear in the middle section, with some reduced response at either extreme. Accuracy at the higher airspeeds is not critical, but it is at lower speeds, so I expect that a two stage calibration will be satisfactory. From the chart, I pick 80kt as the break point.

    Going back to the calibration station and adjusting the output to read 80kt, I measured the output number as 1108. I will use a two segment calibration, first segment is 0-80kt (890-1108 output), and second segment is 80-250kt (1108-1868. I can then write my gauge operating code using the following map functions:

    If (airspeed <=80) output = map (airspeed, 0, 80, 890, 1108;
    Else output = map (airspeed, 80, 250, 1108, 1868;

    I could obtain a higher degree of accuracy if I used a greater number of calibration segments but for this gauge I am satisfied with two.

    Last edited by Jim NZ; 10-28-2014 at 03:12 PM. Reason: code editor dropped some of the include statments

  2. #2
    25+ Posting Member
    Join Date
    May 2014
    Bulgaria and United Kingdom

    Re: High Resolution Servo Control

    Hi Steve , and thank you for sharing your idea with us.

  3. #3
    500+ This must be a daytime job
    Join Date
    Jan 2007

    Re: High Resolution Servo Control

    Nice one Steve,
    Great to see the creativity going on with these sorts of conversions.

  4. #4
    500+ This must be a daytime job Jim NZ's Avatar
    Join Date
    Dec 2005
    New Zealand

    Re: High Resolution Servo Control

    Great stuff Steve ,, your a wee gold mine of information.

    A great write-up Steve ,, another one added to the "Notable Threads" listing.

    Thanks again ,, Jim
    All this and Liz still loves me ! !

  5. #5
    75+ Posting Member
    Join Date
    Sep 2013
    California, USA

    Re: High Resolution Servo Control

    Thanks for the compliment, Jim. I need to get a little help from you. I noticed that the code shows up with the #include statements truncated. It should read:

    (pound sign)(include) (<)(Wire.h)(>)
    (pound sign)(include) (<)(Adafruit_PWMServoDriver.h)(>)
    (pound sign)(include) (<)(EEPROM.h)(>)

    but I can't get them to display properly in the posting. Can you fix it?


  6. #6
    500+ This must be a daytime job Jim NZ's Avatar
    Join Date
    Dec 2005
    New Zealand

    Re: High Resolution Servo Control

    Yes, all done Steve.
    Thanks again ,,, Jim
    All this and Liz still loves me ! !