Positron8 - Move a Hobby Servo motor using angle values

[top204]

I created a couple of programs the other day for a friend and thought they may be of use to the users on the forum as well.

The programs move a Hobby Servo motor based upon an angle value from 0 to 180 degrees, and use the compiler's simple Servo command, so the code should be adaptable for many other PIC microcontroller families. With an angle value of 180, the servo motor moves full left, an angle value of 90 centres the motor, and a value of 0 moves the motor fully right.

There are two program listings, one to move the servo motor based upon button presses (Servo_MoveAngle_18F26K22.bas), and one to move the servo motor based upon the position of a potentiometer (Servo_PotToAngle_18F26K22.bas). Both code listings are shown below, and a Proteus simulation project is also attached with the source codes in the zip file below.

I have often found that different hobby servo motors require different pulse timings for their movement, so find out how many pulses it takes for the servo to move left to 180 degrees, then how many to move right to 0 degrees. These values can then be placed in the Map16S procedure call within the ServoAngle procedure for the left and right extremes based upon an angle value of 0 to 180.

Program listing for moving the servo motor based upon button presses for left, right and centre on a PIC18F26K22 device:

Code Select Expand

'
'   /\\\\\\\\\
'  /\\\///////\\\
'  \/\\\     \/\\\                                                 /\\\          /\\\
'   \/\\\\\\\\\\\/        /\\\\\     /\\\\\\\\\\     /\\\\\\\\   /\\\\\\\\\\\  /\\\\\\\\\\\  /\\\\\\\\\
'    \/\\\//////\\\      /\\\///\\\  \/\\\//////    /\\\/////\\\ \////\\\////  \////\\\////  \////////\\\
'     \/\\\    \//\\\    /\\\  \//\\\ \/\\\\\\\\\\  /\\\\\\\\\\\     \/\\\         \/\\\        /\\\\\\\\\\
'      \/\\\     \//\\\  \//\\\  /\\\  \////////\\\ \//\\///////      \/\\\ /\\     \/\\\ /\\   /\\\/////\\\
'       \/\\\      \//\\\  \///\\\\\/    /\\\\\\\\\\  \//\\\\\\\\\\    \//\\\\\      \//\\\\\   \//\\\\\\\\/\\
'        \///        \///     \/////     \//////////    \//////////      \/////        \/////     \////////\//
'                                  Let's find out together what makes a PIC Tick!
'
' Move a hobby servo based on angles using a scaling procedure
' When the buttons are pressed, the angle value will move from 0 to 180 and back, and the servo will move to the correct angle
'
' Written for the Positron BASIC compiler by Les Johnson
'
    Device = 18F26K22                                       ' Tell the compiler what device to compile for
    Declare Xtal = 64                                       ' Tell the compiler what frequency the device is operating at (in MHz)
'
' Setup USART1
'
    Declare Hserial1_Baud = 9600                            ' Set USART1 Baud rate to 9600
    Declare HRSOut1_Pin = PORTC.6                           ' Set USART1 TX pin
'
' Define the pins used in the program
'
$define Servo_Pin           PORTB.0                         ' Connects to the servo's pulse pin
$define Right_Button_Pin    PORTA.0                         ' Connects to a button to move the servo right (active low)
$define Centre_Button_Pin   PORTA.1                         ' Connects to a button to centre the servo (active low)
$define Left_Button_Pin     PORTA.2                         ' Connects to a button to move the servo left (active low)
'
' Create global variables for the demo here
'
    Dim wAngle         As Word                              ' Holds the angle value
    Dim wPreviousAngle As Word = $FFFF                      ' Holds the previous wAngle value

'-------------------------------------------------------------------------------------------------------------
' The main program starts here
' When the buttons are pressed, the angle value will move from 0 to 180 and back, and the servo will move to the correct angle
'
Main:
    Setup()                                                 ' Setup the program and peripherals
    Do
        If Left_Button_Pin = 0 Then                         ' Is the 'move left' button pressed?
            Inc wAngle                                      ' Yes. So increment the angle value
            If wAngle > 180 Then wAngle = 180               ' Make sure the angle does not go over 180

        ElseIf Centre_Button_Pin = 0 Then                   ' Is the 'centre' button pressed?
            wAngle = 90                                     ' Yes. So centre the angle value

        ElseIf Right_Button_Pin = 0 Then                    ' Is the 'move right' button pressed?
            Dec wAngle                                      ' Yes. So decrement the angle value
            If wAngle.SWord < 0 Then wAngle = 0             ' Make sure the angle does not go under 0
        EndIf

        If wAngle <> wPreviousAngle Then                    ' \
            HRsoutLn Dec wAngle, " Degrees"                 ' | Transmit the angle value to a serial terminal if it changes
            wPreviousAngle = wAngle                         ' | This stops the serial terminal becoming full of values
        EndIf                                               ' /

        ServoAngle(wAngle)                                  ' Move the servo motor based upon the angle value
        DelayMs 20                                          ' Create a 20ms delay for the servo motor's timings
    Loop

'-------------------------------------------------------------------------------------------------------------
' Setup the program and peripherals
' Input     : None
' Output    : None
' Notes     : None
'
Proc Setup()
    IntOsc_64Mhz()                                          ' Set the device to operate at 64MHz with its internal oscillator
    PinInput Right_Button_Pin                               ' \
    PinInput Centre_Button_Pin                              ' | Make the button pins inputs (needs pull-up resistors on them)
    PinInput Left_Button_Pin                                ' /

    wAngle = 90                                             ' Start with the servo at 90 degrees (centred)
EndProc

'--------------------------------------------------------------------------
' Move a servo motor based upon an angle value
' Inout     : pAngle holds the angle to move the servo motor too (0 to 180)
' Output    : None
' Notes     : The angle values can be changed from 0 to 180, to any two 16-bit values.
'
Proc ServoAngle(pAngle As Word)
    pAngle = Map16S(pAngle, 0, 180, 994, 2000)              ' Convert the angle value into the servo position value
    Servo Servo_Pin, pAngle                                 ' Move the servo motor
EndProc

'-------------------------------------------------------------------------------------------------------------
' Scale one 16-bit signed integer value range to another 16-bit signed integer value range
' Input     : pValueIn holds the value to scale
'           : pInMin is the lower bound of the value's input range
'           : pInMax is the upper bound of the value's input range
'           : pOutMin is the lower bound of the value's target range
'           : pOutMax is the upper bound of the value's target range
' Output    : Returns the scaled result
' Notes     : None
'
Proc Map16S(pValueIn As SDword, pInMin As SWord, pInMax As SWord, pOutMin As SWord, pOutMax As SWord), SWord
    Result = (((pValueIn - pInMin) * (pOutMax - pOutMin)) / (pInMax - pInMin)) + pOutMin
EndProc

'-----------------------------------------------------------------------------------------
' Setup the device to use its internal oscillator at 64MHz
' Input     : None
' Output    : None
' Notes     : For use with PIC18Fx6K22 devices
'
Proc IntOsc_64Mhz()
    OSCCON  = $70
    OSCCON2 = $04
    OSCTUNE = $40
    Repeat : Until OSCCON2bits_PLLRDY = 1           ' Wait for the PLL to stabilise
EndProc

'-----------------------------------------------------------------------------------------
' Configure the fuses to operate at 64MHz using the internal oscillator, on a PIC18F26K22 device
'
Config_Start
    FOSC     = INTIO67          ' Use the internal oscillator for the device's oscillator
    PLLCFG   = Off              ' Disable PLL
    PRICLKEN = On               ' Primary clock enabled
    FCMEN    = Off              ' Fail-Safe Clock Monitor disabled
    IESO     = Off              ' Internal/External Oscillator Switchover mode disabled
    PWRTEN   = On               ' Power up timer enabled
    BOREN    = SBORDIS          ' Brown-out Reset enabled in hardware only (SBOREN is disabled)
    BORV     = 190              ' Brown Out Reset Voltage set to 1.90 V nominal
    WDTEN    = Off              ' Watch dog timer is always disabled. SWDTEN has no effect.
    WDTPS    = 128              ' Watchdog Timer Postscale 1:128
    CCP2MX   = PORTC1           ' CCP2 input/output is multiplexed with RC1
    PBADEN   = Off              ' PORTB<5:0> pins are configured as digital I/O on Reset
    CCP3MX   = PORTB5           ' P3A/CCP3 input/output is multiplexed with RB5
    HFOFST   = On               ' HFINTOSC output and ready status are not delayed by the oscillator stable status
    T3CMX    = PORTC0           ' Timer3 Clock Input (T3CKI) is on RC0
    P2BMX    = PORTB5           ' ECCP2 B (P2B) is on RB5
    MCLRE    = EXTMCLR          ' MCLR pin enabled, RE3 input pin disabled
    STVREN   = Off              ' Stack full/underflow will not cause Reset
    LVP      = Off              ' Single-Supply ICSP disabled
    XINST    = Off              ' Instruction set extension and Indexed Addressing mode disabled (Legacy mode)
    Debug    = Off              ' Debug Disabled
    Cp0      = Off              ' Block 0 (000800-001FFF) not code-protected
    CP1      = Off              ' Block 1 (002000-003FFF) not code-protected
    CP2      = Off              ' Block 2 (004000-005FFF) not code-protected
    CP3      = Off              ' Block 3 (006000-007FFF) not code-protected
    CPB      = Off              ' Boot block (000000-0007FF) not code-protected
    CPD      = Off              ' Data EEPROM not code-protected
    WRT0     = Off              ' Block 0 (000800-001FFF) not write-protected
    WRT1     = Off              ' Block 1 (002000-003FFF) not write-protected
    WRT2     = Off              ' Block 2 (004000-005FFF) not write-protected
    WRT3     = Off              ' Block 3 (006000-007FFF) not write-protected
    WRTC     = Off              ' Configuration registers (300000-3000FF) not write-protected
    WRTB     = Off              ' Boot Block (000000-0007FF) not write-protected
    WRTD     = Off              ' Data EEPROM not write-protected
    EBTR0    = Off              ' Block 0 (000800-001FFF) not protected from table reads executed in other blocks
    EBTR1    = Off              ' Block 1 (002000-003FFF) not protected from table reads executed in other blocks
    EBTR2    = Off              ' Block 2 (004000-005FFF) not protected from table reads executed in other blocks
    EBTR3    = Off              ' Block 3 (006000-007FFF) not protected from table reads executed in other blocks
    EBTRB    = Off              ' Boot Block (000000-0007FF) not protected from table reads executed in other blocks
Config_End

Program listing for moving the servo motor based upon the position of a potentiometer, with an optimised Map16S procedure. On a PIC18F26K22 device:

Code Select Expand

'
'   /\\\\\\\\\
'  /\\\///////\\\
'  \/\\\     \/\\\                                                 /\\\          /\\\
'   \/\\\\\\\\\\\/        /\\\\\     /\\\\\\\\\\     /\\\\\\\\   /\\\\\\\\\\\  /\\\\\\\\\\\  /\\\\\\\\\
'    \/\\\//////\\\      /\\\///\\\  \/\\\//////    /\\\/////\\\ \////\\\////  \////\\\////  \////////\\\
'     \/\\\    \//\\\    /\\\  \//\\\ \/\\\\\\\\\\  /\\\\\\\\\\\     \/\\\         \/\\\        /\\\\\\\\\\
'      \/\\\     \//\\\  \//\\\  /\\\  \////////\\\ \//\\///////      \/\\\ /\\     \/\\\ /\\   /\\\/////\\\
'       \/\\\      \//\\\  \///\\\\\/    /\\\\\\\\\\  \//\\\\\\\\\\    \//\\\\\      \//\\\\\   \//\\\\\\\\/\\
'        \///        \///     \/////     \//////////    \//////////      \/////        \/////     \////////\//
'                                  Let's find out together what makes a PIC Tick!
'
' Move a hobby servo based on angles using a scaling procedure
' Move a variable resistor connected to pin AN3, and the servo will move to its approximate angle
'
' Written for the Positron BASIC compiler by Les Johnson
'
    Device = 18F26K22                                       ' Tell the compiler what device to compile for
    Declare Xtal = 64                                       ' Tell the compiler what frequency the device is operating at (in MHz)
'
' Setup USART1
'
    Declare Hserial1_Baud = 9600                            ' Set USART1 Baud rate to 9600
    Declare HRSOut1_Pin = PORTC.6                           ' Set USART1 TX pin
'
' Define the pins used in the program
'
$define Servo_Pin PORTB.0                                   ' Connects to the servo's pulse pin
'
' Create global variables for the demo here
'
    Dim wAngle         As Word                              ' Holds the angle value
    Dim wPreviousAngle As Word = $FFFF                      ' Holds the previous wAngle value

'-------------------------------------------------------------------------------------------------------------
' The main program starts here
' Move a variable resistor and the servo will move to its approximate angle
' When the potentiometer has been moved, the angle value will be transmitted to a serial terminal for viewing
'
Main:
    Setup()                                                 ' Setup the program and peripherals

    Do                                                      ' Create a loop
        wAngle = PotToAngle()                               ' Read the potentiometer and convert to its angle value
        If wAngle <> wPreviousAngle Then                    ' \
            HRsoutLn Dec wAngle, " Degrees"                 ' | Transmit the angle value to a serial terminal if it changes
            wPreviousAngle = wAngle                         ' | This stops the serial terminal becoming full of values
        EndIf                                               ' /

        ServoAngle(wAngle)                                  ' Move the servo motor based upon the angle value
        DelayMs 20                                          ' Create a 20ms delay for the servo motor's timings
    Loop                                                    ' Do it forever

'-------------------------------------------------------------------------------------------------------------
' Setup the program and peripherals
' Input     : None
' Output    : None
' Notes     : None
'
Proc Setup()
    IntOsc_64Mhz()                                          ' Set the device to operate at 64MHz with its internal oscillator
    ADC_Init()                                              ' Setup the ADC
EndProc

'--------------------------------------------------------------------------
' Move a servo motor based upon an angle value
' Inout     : pAngle holds the angle to move the servo motor too (0 to 180)
' Output    : None
' Notes     : The angle values can be changed from 0 to 180, to any two 16-bit values.
'
Proc ServoAngle(pAngle As Word)
    pAngle = Map16S(pAngle, 0, 180, 994, 2000)              ' Convert the angle value into the servo position value
    Servo Servo_Pin, pAngle                                 ' Move the servo motor
EndProc

'--------------------------------------------------------------------------
' Read a potentiometer with the ADC and return an angle value from its position
' Input     : None
' Output    : Returns a value from 0 to 180 for the value read from the 10-bit ADC (0 to 1023)
' Notes     : Reads from pin AN3
'
Proc PotToAngle(), Word
    Result = ADC_Read10(3)                                  ' Read the 10-bit ADC
    Result = Map16S(Result, 0, 1023, 0, 180)                ' Convert the ADC value into a degrees value
EndProc

'-------------------------------------------------------------------------------------------------------------
' Scale one 16-bit signed integer value range to another 16-bit signed integer value range
' Input     : pValueIn holds the value to scale
'           : pInMin is the lower bound of the value's input range
'           : pInMax is the upper bound of the value's input range
'           : pOutMin is the lower bound of the value's target range
'           : pOutMax is the upper bound of the value's target range
' Output    : Returns the scaled result
' Notes     : More efficient coding of: Result = (((pValueIn - pInMin) * (pOutMax - pOutMin)) / (pInMax - pInMin)) + pOutMin
'
Proc Map16S(pValueIn As SDword, pInMin As SWord, pInMax As SWord, pOutMin As SWord, pOutMax As SWord), SWord
    Dim dTemp  As pValueIn
    Dim wTemp1 As Result
    Dim wTemp2 As dTemp.SWord1

    wTemp1 = pValueIn - pInMin
    wTemp2 = pOutMax - pOutMin
    dTemp  = wTemp1 * wTemp2
    wTemp1 = pInMax - pInMin
    dTemp  = dTemp / wTemp1
    Result = dTemp + pOutMin
EndProc

'-------------------------------------------------------------------------------------
' Read the 10-bit ADC directly on a PIC18F26K22 device
' Input     : pChan holds the ADC channel to read (0 to X)
' Output    : Returns the 10-bit ADC value
' Notes     : None
'
Proc ADC_Read10(pChan As Byte), Word
    Dim wADRES As ADRESL.Word                       ' Create a 16-bit SFR from ADRESL and ADRESH

    ADCON0 = %00000001                              ' Clear the channel bits and enable the ADC peripheral
    pChan = pChan << 2                              ' Move the channel bits into the correct position for ADCON0
    ADCON0 = ADCON0 | pChan                         ' Or the channel bits into ADCON0
    ADCON0bits_GO_DONE = 1                          ' \ Wait for the ADC to complete its reading
    Repeat : Until ADCON0bits_GO_DONE = 0           ' /
    Result = wADRES                                 ' Load the result with the ADC value
EndProc

'-----------------------------------------------------------------------------------------
' Setup the ADC on a PIC18F26K22 device
' Input     : None
' Output    : None
' Notes     : Set for 10-bit operation
'           : Uses the Gnd and VDD as vrefs
'           : Uses the FRC oscillator
'           : Sets AN3 as an analogue pin
'
Proc ADC_Init()
    ADCON1 = $00
    ADCON2 = %10001111
    ADCON0 = $00
    ANSELAbits_ANSA3 = 1                            ' Setup AN3 as an analogue pin
EndProc

'-----------------------------------------------------------------------------------------
' Setup the device to use its internal oscillator at 64MHz
' Input     : None
' Output    : None
' Notes     : For use with PIC18Fx6K22 devices
'
Proc IntOsc_64Mhz()
    OSCCON  = $70
    OSCCON2 = $04
    OSCTUNE = $40
    Repeat : Until OSCCON2bits_PLLRDY = 1           ' Wait for the PLL to stabilise
EndProc

'-----------------------------------------------------------------------------------------
' Configure the fuses to operate at 64MHz using the internal oscillator, on a PIC18F26K22 device
'
Config_Start
    FOSC     = INTIO67          ' Use the internal oscillator for the device's oscillator
    PLLCFG   = Off              ' Disable PLL
    PRICLKEN = On               ' Primary clock enabled
    FCMEN    = Off              ' Fail-Safe Clock Monitor disabled
    IESO     = Off              ' Internal/External Oscillator Switchover mode disabled
    PWRTEN   = On               ' Power up timer enabled
    BOREN    = SBORDIS          ' Brown-out Reset enabled in hardware only (SBOREN is disabled)
    BORV     = 190              ' Brown Out Reset Voltage set to 1.90 V nominal
    WDTEN    = Off              ' Watch dog timer is always disabled. SWDTEN has no effect.
    WDTPS    = 128              ' Watchdog Timer Postscale 1:128
    CCP2MX   = PORTC1           ' CCP2 input/output is multiplexed with RC1
    PBADEN   = Off              ' PORTB<5:0> pins are configured as digital I/O on Reset
    CCP3MX   = PORTB5           ' P3A/CCP3 input/output is multiplexed with RB5
    HFOFST   = On               ' HFINTOSC output and ready status are not delayed by the oscillator stable status
    T3CMX    = PORTC0           ' Timer3 Clock Input (T3CKI) is on RC0
    P2BMX    = PORTB5           ' ECCP2 B (P2B) is on RB5
    MCLRE    = EXTMCLR          ' MCLR pin enabled, RE3 input pin disabled
    STVREN   = Off              ' Stack full/underflow will not cause Reset
    LVP      = Off              ' Single-Supply ICSP disabled
    XINST    = Off              ' Instruction set extension and Indexed Addressing mode disabled (Legacy mode)
    Debug    = Off              ' Debug Disabled
    Cp0      = Off              ' Block 0 (000800-001FFF) not code-protected
    CP1      = Off              ' Block 1 (002000-003FFF) not code-protected
    CP2      = Off              ' Block 2 (004000-005FFF) not code-protected
    CP3      = Off              ' Block 3 (006000-007FFF) not code-protected
    CPB      = Off              ' Boot block (000000-0007FF) not code-protected
    CPD      = Off              ' Data EEPROM not code-protected
    WRT0     = Off              ' Block 0 (000800-001FFF) not write-protected
    WRT1     = Off              ' Block 1 (002000-003FFF) not write-protected
    WRT2     = Off              ' Block 2 (004000-005FFF) not write-protected
    WRT3     = Off              ' Block 3 (006000-007FFF) not write-protected
    WRTC     = Off              ' Configuration registers (300000-3000FF) not write-protected
    WRTB     = Off              ' Boot Block (000000-0007FF) not write-protected
    WRTD     = Off              ' Data EEPROM not write-protected
    EBTR0    = Off              ' Block 0 (000800-001FFF) not protected from table reads executed in other blocks
    EBTR1    = Off              ' Block 1 (002000-003FFF) not protected from table reads executed in other blocks
    EBTR2    = Off              ' Block 2 (004000-005FFF) not protected from table reads executed in other blocks
    EBTR3    = Off              ' Block 3 (006000-007FFF) not protected from table reads executed in other blocks
    EBTRB    = Off              ' Boot Block (000000-0007FF) not protected from table reads executed in other blocks
Config_End

Below is a screenshot of the proteus simulation:

[Frizie]

Maybe stupid question:
I don't use those high frequencies, but is 64MHz possible with PLL disabled?

[top204]

Yes. Most of the K type 18F devices have a config fuse setting to enable 64MHz internally. On most 18FxxKxx devices the fuse value is named: HFINTOSC_64MHZ, and goes into the RSTOSC position.

It still uses PLL, but invisibly to the user so the oscillator does not need to be setup by the user. I've never tried to drive an 18F device with 64MHz directly into its OSC pin, but I know the 18F devices will operate at over 100MHz using an external crystal and 4xPLL, so it could possibly work. :-)

It is actually better to use the higher frequencies for most programs, especially when operating with the internal oscillator, because any drift in frequency is tiny based upon the higher frequency. It also allows most programs to have lots of "leg room", so timings are not as critical and loop iterations operate much, much faster, so more things can be done within them.