Announcement

Collapse
No announcement yet.

Servos

Collapse
X
  • Filter
  • Time
  • Show
Clear All
new posts

  • #16
    Keith, Here is a bit of code I wrote some time ago and it should what you explained.

    ' SERVOPW3.BAS
    '
    ' WRITTEN BY DAVID PUROLA 06/18/2010
    '
    ' WRITTEN FOR USE W/12F1840-I/P @ 32 Mhz.
    '
    ' THE PROGRAMS PURPOSE IS TO GENERATE 2 INDIVIDUAL PWM SIGNALS FROM 2 RATIOMETRICLY
    'CONNECTED POT'S TO THE PIC PROCESSOR. THE HIGH AND LOW SIDE OF EACH OF THE POTS ARE
    'CONNECTED TO VSS and VDD. THE CENTER WIPERS ARE CONNECTED TO A/D CHANNELS
    '0 and 1 WHICH ARE ON RA0 and RA1. THE OUTPUTS ARE ON PINS RA2 and RA4 RESPECTIVLY.
    ' THE PERIOD IS 20 Milliseconds AND THE PULSE WIDTH IS BETWEEN 500uS. and 2500 uS.
    '
    DEFINE OSC 32
    DEFINE NO_CLRWDT 1

    #CONFIG
    ;----- CONFIG1 Options --------------------------------------------------
    __config _CONFIG1, _FOSC_INTOSC & _WDTE_OFF & _PWRTE_ON & _MCLRE_ON & _CP_OFF & _CPD_OFF & _BOREN_ON & _CLKOUTEN_OFF & _IESO_ON & _FCMEN_ON

    ;----- CONFIG2 Options --------------------------------------------------
    __config _CONFIG2, _WRT_OFF & _PLLEN_ON & _STVREN_ON & _BORV_19 & _LVP_OFF
    #ENDCONFIG

    ' ********************************************************************
    ' Declare Port Variables
    ' ********************************************************************
    AZFEEDBK VAR PORTA.0 '1-ANALOG INPUT FROM AZMUNTH POSITION POT
    ELFEEDBK VAR PORTA.1 '1-ANALOG INPUT FROM ELEVATION POSITION POT
    PWMOUT1 VAR LATA.2 '0-PWM OUTPUT TO AZ SERVO
    RST VAR PORTA.3 '1-RESET/MCLR
    PWMOUT2 VAR LATA.4 '0-PWM OUTPUT TO EL SERVO
    RUNLED VAR LATA.5 '0-RUN LED FOR OPERATOR

    ' ********************************************************************
    ' Define Constants
    ' ********************************************************************
    OFF_ CON 0 'DIGITAL 0 NUMONIC
    ON_ CON 1 'DIGITAL 1 NUMONIC
    SINK CON 0 'SINK CURRENT NUMONIC
    SOURCE CON 1 'SOURCE CURRENT NUMONIC

    ADSCALE CON 125'250 'FULL SCALE A/D TO 0-2047 TO 0-2000 X ~.977
    FILTER CON 64 'LOWPASS FILTER CONSTANT
    MAXCHANNELS CON 2 'NUMBER OF A/D CHANNELS

    ADJPERIOD CON 14020 'ADJUSTED PERIOD FOR CODE LATENCY (CHANGE ONLY THIS TO CHANGE PERIOD)
    MINPWM CON 961'461 '500 uS. MINIMUM PWM
    HALFPERIOD CON ADJPERIOD / MAXCHANNELS 'ADJUSTED PERIOD FOR CODE LATENCY DIVIDED BY NUMBER OF CHANNELS
    REMPERIOD CON HALFPERIOD - MINPWM 'ADJUSTED PERIOD - MINIMUM PWM

    ' ********************************************************************
    ' Declare Data Variables
    ' ********************************************************************

    ' ********************************************************************
    ' Declare Variables
    ' ********************************************************************
    PAUSTIME VAR WORD 'SCRATCH VARIABLE FOR PAUSING PROGRAM
    TIMSTAMP VAR WORD 'SCRATCH VARIABLE FOR TIMING COMPARES 10Ms.
    LOOPCNTR VAR WORD 'LOOP TIMER WORD FOR TOGGLING LED
    TIMESEGM VAR WORD 'SCRATCH VARIABLE USED FOR INTERRUPT ROUTINE

    BITSWORD VAR WORD 'BITS FOR STORAGE
    COWS_HOME VAR BITSWORD.0 'LOOP FLAG

    VOLTS VAR WORD[MAXCHANNELS] 'A/D READINGS (N-CHANNELS)
    CHANNEL VAR BYTE 'A/D CHANNEL VARIABLE

    SCRATCH VAR WORD 'SCRATCH VARIABLE

    PASTVAR VAR WORD[MAXCHANNELS] 'A/D READINGS (N-CHANNELS)
    OUTVAR VAR WORD[MAXCHANNELS] 'A/D READINGS (N-CHANNELS)

    ' ********************************************************************
    ' SYSTEM INITIALIZATION
    ' ********************************************************************
    clear
    PORTA = %00000000
    TRISA = %00001011 'INITIALIZE PORT DIRECTIONS
    OSCCON = %11110000 'PLL ENABLED,8Mhz,FSOSC<2:0>
    INTCON = %00000000 'CLEAR GIE,PEIE,TMR0IE,INTE,RBIE,TMR0IF,INTF,RBIF
    PIE1 = %00000000 'CLEAR ALL INTERRUPT ENABLE BITS
    PIR1 = %00000000 'CLEAR ALL INTERRUPT FLAGS
    PIE2 = %00000000 'CLEAR ALL INTERRUPT ENABLE BITS
    PIR2 = %00000000 'CLEAR ALL INTERRUPT FLAGS
    APFCON = %10000100 'ENABLE USART ON PORTA.4:5
    ANSELA = %00000011 'AN0,AN1 ANALOG,:AN2,AN4 DIGITAL I/O
    WPUA = %00000000 'WEAK PULLUP'S OFF
    ADCON0 = %00000000 'CHS2:0,GO/DONE,ADOFF
    ADCON1 = %11100000 'RIGHT JUSTIFIED,FOSC/64,VSS,VDD
    SRCON0 = %00000000
    SRCON1 = %00000000
    CM1CON0 = %00000000 'CLEAR/DISABLE COMPARATOR
    CM1CON1 = %00000000 'CLEAR/DISABLE COMPARATOR
    CMOUT = %00000000 'DISABLE OUTPUT REGISTER
    DACCON0 = %00000000 'CLEAR/DISABLE D/A
    DACCON1 = %00000000 'CLEAR/DISABLE D/A
    FVRCON = %00000000 'DISABLE FIXED VOLTAGE REFERENCE
    OPTION_REG = %10000000 'CLEAR ALL PULL-UPS,INTEDG,T0CS,T0SE,PSA,PS
    T1CON = %00000101 'PRESCALER 1/2,START TIMER 1
    T1GCON = %00000000 'NO GATE CONTROL
    T2CON = %00000110 'POSTSCALER 1/1,START,PRESCALER 16
    TMR2 = 0 'CLEAR TMR2 MODULE REGISTER
    PR2 = 255 'SET PERIOD ((((1 / 40,000,000)*4)*16 PRESCALE)*(1+255 PR2)) = 2.44 KHZ
    CCP1CON = %00000000 'PLACE CCP1 INTO DISABLE MODE
    CCPR1L = $00 'CLEAR CCP1 LOWER 8 BITS
    CCPR1H = $00 'CLEAR CCP1 UPPER 2 BITS

    GOTO MAINLOOP

    ' ********************************************************************
    ' SUBROUTINES
    ' ********************************************************************

    '*********************************************************************
    READAD: 'READ SYSTEM A/D VOLTAGES (AVERAGE 64 READINGS) 12F1840
    '*********************************************************************
    ADCON0 = $01 | (CHANNEL << 2) ' Set A/D to Channel X, On
    PAUSEUS 40
    VOLTS(CHANNEL) = 0 'CLEAR A/D READINGS
    SCRATCH = 0
    WHILE SCRATCH < 64 'SUM 64 READINGS
    ADCON0.1 = 1 ' START CONVERSION
    WHILE ADCON0.1 = 1 'WAIT FOR A/D TO FINISH
    WEND
    VOLTS(CHANNEL) = VOLTS(CHANNEL) + (((ADRESH & $3) << 8) + ADRESL) 'BUILD SENSOR WORD
    SCRATCH = SCRATCH + 1
    WEND
    VOLTS(CHANNEL) = VOLTS(CHANNEL) >> 5 'TURN INTO 11 BIT RESULT
    RETURN

    ' ********************************************************************
    LOWPASS: 'LOW PASS FILTER (256 = NO FILTER)
    ' ********************************************************************
    PASTVAR(CHANNEL) = (PASTVAR(CHANNEL) - (PASTVAR(CHANNEL) */ FILTER)) + VOLTS(CHANNEL)
    OUTVAR(CHANNEL) = PASTVAR(CHANNEL) */ FILTER 'FINAL DRIVE OUTPUT COMPUTED
    RETURN

    ' ********************************************************************
    MAINLOOP:' PROGRAM LOOP
    ' ********************************************************************
    REPEAT

    CHANNEL = 1 'CYCLE THRU ALL CHANNELS
    WHILE CHANNEL < 255
    GOSUB READAD 'READ SYSTEM A/D VOLTAGES (AVERAGE 64 READINGS)
    GOSUB LOWPASS 'LOW PASS FILTER (256 = NO FILTER)
    OUTVAR(CHANNEL) = OUTVAR(CHANNEL) */ ADSCALE 'FULL SCALE A/D TO 0-2000
    LATA.0(2 + (2 * CHANNEL)) = SOURCE 'FIRST, TURN IT ON
    PAUSEUS MINPWM
    PAUSEUS OUTVAR(CHANNEL) '0 TO 2000
    LATA.0(2 + (2 * CHANNEL)) = SINK 'SECOND, TURN IT OFF
    PAUSEUS REMPERIOD - OUTVAR(CHANNEL)
    CHANNEL = CHANNEL - 1
    WEND

    LOOPCNTR = LOOPCNTR + 1 'FLASH RUN LED FOR OPERATOR
    IF LOOPCNTR > 24 THEN
    LOOPCNTR = 0
    TOGGLE RUNLED
    ENDIF

    UNTIL COWS_HOME
    ' ********************************************************************
    END 'SHOULD NEVER GET HERE.........
    STOP
    Dave Purola,
    N8NTA
    EN82fn

    Comment


    • #17
      Thank you David. As much as I read the code many times over, I admit I was a bit lost in concisely applying what you've written to the speed and event commands within controlling the servo and including these desired commands. Also, are you using a hardware PWM to generate the pulses in your code?

      Comment


      • #18
        So in trying to simplify (and comprehend) I'll give you an example of similar code I've posted(I understand that 'pauseus' is desirable but for sake of ignoring that for the moment this just works fine for the time being) and ask 1) where/what would you insert to slow up or speed up this motion? I can only assume it needs to be somewhere between the 20ms frame. and 2)where can I insert a serial out command on some pin that will allow the integrity of the frame for the servo. Here's what it is in a nut shell; I want the servo horn to move at the same rate as a sound that is triggered via a serial out command. The duration is ~4seconds.
        This is the code working for the moment that gives me speed control but it seems as if this is the slowest I can get limited by the pulse size of p0 :

        OSCCON=%01101010

        'define osc

        output porta.2

        porta.2=0



        ANSELA=0

        ANSELC=0

        CM1CON0=0

        CM1CON1=0

        p0 var byte



        Start:for p0=210 to 90 step -1

        pulsout porta.2,p0

        pause 20-p0/100

        next p0



        for p0=90 to 210 step 1

        pulsout porta.2,p0

        pause 20-p0/100

        next p0

        goto Start

        end

        Also interesting is that although I set the proper OSCON bits for 8Mhz, this code does not function as it does when I use the internal 4Mhz oscillator

        Comment


        • #19
          If it is more resolution you need then use "PAUSEUS". At 4 Mhz. oscillator each "PULSEOUT" increment is equal to 10 Microseconds. However using "PAUSEUS" the minimum step is 1 Microsecond. You are correct in that your code will not operate the same at 8 Mhz. and at 4Mhz. The "PULSOUT" statement is based on the actual oscillator frequency. When using the "PAUSEUS" command and the "DEFINE OSC" command, it does not matter what the frequency of the oscillator is, as it will be compensated in software. Read the page 188 in the manual for PAUSEUS command and page 196 for the PULSEOUT command.
          Dave Purola,
          N8NTA
          EN82fn

          Comment

          Working...
          X