Announcement

Collapse
No announcement yet.

PID Motor Control In PicBasic - PIC 12F683

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

  • PID Motor Control In PicBasic - PIC 12F683

    Note - See my attachment with PBP file to download; better than copying below

    Code:
    '****************************************************************
    '* Name : Servo Positioner  *
    '* File : Servo Positioner 1.0 *
    '* Author : Marco Middione *
    '* Notes : PIC12F683 *
    '* Date : February 19, 2017 *
    '* Changes : Added PID for control feedback *
    '* : *
    '* : *
    '* : *
    '* : *
    '****************************************************************
    '***PIC 12F683***
    'Pin 1, 5V
    'Pin 2, GP5, CLK
    'Pin 3, GP4, AN3, Pot input to set position of servo 1ms to 2ms
    'Pin 4, GP3, MCLR, Timer reset button, MCLR is off, switch input, digital
    'Pin 5, GP2, AN2, CCP1, Servo out signal 1ms to 2ms
    'Pin 6, GP1, AN1, Vref
    'Pin 7, GP0, AN0, LCD out to read data
    'Pin 8, Ground
    '__config _INTRC_OSC_NOCLKOUT & _WDT_ON & _MCLRE_OFF & _CP_OFF
    
    '******************************************************************************
    '* Pseudocode routine for reference below: *
    '* previous_error = 0 *
    '* integral = 0 *
    '* start: *
    '* error = setpoint - measured_value *
    '* integral = integral + error * dt *
    '* derivative = (error - previous_error)/dt *
    '* output = Kp * error + Ki * integral + Kd * derivative *
    '* previous_error = error *
    '* wait(dt) *
    '* goto start *
    '******************************************************************************
    
    Include "modedefs.bas" 'Needed for serial in, out and other commands
    'Need one blank space before X__config to avoid compilation error
    #CONFIG
    __config _INTRC_OSC_NOCLKOUT & _WDTE_ON & _PWRTE_ON & _MCLRE_OFF & _BOD_OFF & _CP_OFF & _CPD_OFF
    #ENDCONFIG
    Clear 'Clear all memory locations
    
    '******************************************************************************
    'Define Pin Input & Output & Digital versus Analog
    'ADCON0.7 = 0 for left, if using 10 bits, ADCON0.7 = 1 for right & 8 bits
    '%0 left justify 8 bits, %1 right for 10, Vref pin (narrow ADC voltage)
    'ANSEL %X-011-0000, 011=internal clock, AN 3,2,1,0 (1=analog, 0=digital)
    '******************************************************************************
    define OSC 4 '4 Mhz oscillator speed, standard
    TRISIO = %00010100 '1 = input, 0 = output, %XX,(GP=5,4,3,2,1,0)
    CMCON0 = %00000111 '111 = all comparators off
    ADCON0 = %10001100 '%1 = right justified, AN3=11, AN2=10, AN1=01, AN0=00
    ANSEL = %00111000 '%XX11 = internal clock,(AN=3,2,1,0) AN3 = 1 input pot
    
    '***************************************************************
    'Define Constants & Variables
    '***************************************************************
    Beam_POS var word 'Data in from pot, GP4
    Err var word 'Amount of error present
    P var word 'Amount of proportional drive
    I var word 'Amount of integral drive
    D var word 'Amount of derivative drive
    Integral var word 'Accumulate the error integral over time
    Kp var word 'Variable Kp so that it does not overflow
    Servo_PWM var word 'Servo pulse out, GP2
    Last var word 'Holds last ADC_data for derivative drive
    Last = 0 'Clear value
    
    '******************************************************************************
    'PID Control Settings
    '******************************************************************************
    SP con 1500 'Set point, center of servo travel to start
    Kpi con 200 'Initial proportional constant, 200 works good
    Ki con 10 'Integral constant, 5 works good
    Kd con 80 'Derivative constant, 20 works good
    IntThresh con 30 'Limits integration to prevent windup; only near SP
    
    '***************************************************************
    'Set 10-bit A/D conversion and Define ADCIN parameters
    '***************************************************************
    'ADC window comparator pot input GP4, pin3
    Define ADC_BITS 10 ' Set number of bits in result
    Define ADC_CLOCK 3 ' Set clock source (3=rc)
    Define ADC_SAMPLEUS 50 ' Set sampling time in uS
    
    'LCD data review output, normally off in main routine, GP0, pin 7
    DEFINE DEBUG_BIT 0 'Set Debug pin (GP0, pin 7)
    DEFINE DEBUG_BAUD 9600 'Set Debug baud rate
    DEFINE DEBUG_MODE 1 'Set Debug mode: 0 = true, 1 = inverted
    
    '***************************************************************
    'Main
    '***************************************************************
    
    main:
    gosub get_data
    gosub error
    gosub pid
    gosub dead_band
    gosub data_out
    pause 17 'Adjust to 50 Hz (17) cycle, check with oscilloscope
    pauseus (2000 - Servo_PWM)
    'gosub view_data 'Rem out for real time processing, skips 50hz cycle
    '12 hour timer, adjust, once a day
    goto main
    
    get_data:
    adcin 3, Beam_POS 'Input on pin 3, GP4, AN3, direct V from position sensor
    Beam_POS = (Beam_POS/1023*1000) + 1000 max 1000 min 2000 'Set to servo 1ms to 2ms
    'Convert 1023/1023*1000 = 0 to 1000
    return
    
    data_out:
    low 2: High 2 'Servo out GP2, pin 5
    pauseus Servo_PWM 'Servo out, 1 to 2 ms (1000 to 2000) or less to fit paper
    low 2
    Return
    
    '******************************************************************************
    'Error
    '******************************************************************************
    error:
    Err = (SP - Beam_POS) 'Calculate the current error
    Kp = Kpi min (32767/abs(Err)) 'Limit Kp to 32767/Err
    if (abs(Err)) < IntThresh then 'Prevent integral 'windup'
    Integral = Integral + Err 'Accumulate the error integral
    else
    Integral = 0 'Zero integral if out of bounds
    endif
    return
    
    '******************************************************************************
    'PID, currently, this is setup for PWM hobby servo control, pulse 1.0 to 2.0 ms
    'If you want to use a motor / pot then un-rem direction, PORTC.4 and
    'Servo_PWM for output to the H-Bridge of your choice
    '******************************************************************************
    pid:
    P = Err * KP 'Calculate proportional term
    I = Integral * Ki 'Calculate integral term
    D = (Last - Beam_POS) * Kd 'Calculate derivative term
    Servo_PWM = P + I + D 'Total drive = P+I+D
    
    'Direction = Servo_PWM.bit15 'If bit is 0 or 1 then forward, reverse
    'PORTC.4 = Direction 'Motor direction control RC4, pin 6
    'Servo_PWM = (abs(Servo_PWM)) 'Get absolute value of drive
    return
    
    '******************************************************************************
    'Dead Band
    'If deadband is too small, the integration will constantly try to get the
    'position to zero error and the drive will oscillate back and forth trying
    'to get it to zero, if you open the dead band up a little, then this stops
    '******************************************************************************
    dead_band:
    if abs Err < 5 then 'Adds deadband for stability, 3 to 5 works good
    'Sleep 60 'Sleep is low power mode in seconds (~18hrs = 64,800)
    I = 0 'Stops integration when in the dead band
    endif
    return
    
    '******************************************************************************
    'Display output to 4-line Scott Edwards LCD
    '14 backlight, 1 cursor home, 8 backspace, 9 tab, 10 move down, 11 move up
    '12 clear, 13 return, 16 accept cursor position data, 18 right aligned
    '" " clears space after a variable as the digits reduce
    '******************************************************************************
    'view_data:
    ' 'Debug 14,1
    ' debug #z, " ",#upper_window, " "
    'Return
    
    '============================= end of program =================================
    Attached Files
    Last edited by Marco Middione; 07-12-2017, 04:59 PM.
Working...
X