1.txt

pic得电机控制程序

-- -----------------------------------------------------------------------------
-- -----------------------------------------------------------------------------
-- Library that can control up to 2 stepper motors.
-- Features
--    * position control (in half-steps) controled by a 4 byte number
--    * both half-stepping and full-stepping implemented
--    * on full-stepping, 1 or 2 windings can be forced
--    * acceleration / deceleration supported


--
--
-- Version: 1.0  24-08-2002  orginal release
-- Author : Stef Mientki
-- uses   : byte3_math, TMR0 as interrupt generator
-- -----------------------------------------------------------------------------
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- example
-- -----------------------------------------------------------------------------
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- automatic include the necessary files
const LOG_ACCURACY = 2  -- necessary for byte3_math
include byte3_math
-- -----------------------------------------------------------------------------



-- motor status constants
const byte _stepper_status_stopped      = 0
const byte _stepper_status_running      = 1
const byte _stepper_status_continuous   = 2


-- variables for motor1
var volatile byte _stepper1_period
var volatile byte _stepper1_period_preset = 40
var volatile byte _stepper1_index = 0
var volatile byte stepper1_status
var volatile bit  stepper1_full_steps
var volatile bit  stepper1_full_force_1_winding
var volatile bit  stepper1_full_force_2_winding
var volatile bit  stepper1_forward
var volatile byte stepper1_pos1
var volatile byte stepper1_pos2
var volatile byte stepper1_pos3
var volatile byte stepper1_pos4
var volatile byte _stepper1_target_pos1
var volatile byte _stepper1_target_pos2
var volatile byte _stepper1_target_pos3
var volatile byte _stepper1_target_pos4
var volatile bit  stepper1_accelerate_on
var volatile byte _stepper1_acc_count
var volatile byte _stepper1_acc_index
var volatile bit  _stepper1_not_decelerating


-- identical variables for motor 2
var volatile byte _stepper2_period
var volatile byte _stepper2_period_preset = 40
var volatile byte _stepper2_index = 0
var volatile byte stepper2_status
var volatile bit  stepper2_full_steps
var volatile bit  stepper2_full_force_1_winding
var volatile bit  stepper2_full_force_2_winding
var volatile bit  stepper2_forward
var volatile byte stepper2_pos1
var volatile byte stepper2_pos2
var volatile byte stepper2_pos3
var volatile byte stepper2_pos4
var volatile byte _stepper2_target_pos1
var volatile byte _stepper2_target_pos2
var volatile byte _stepper2_target_pos3
var volatile byte _stepper2_target_pos4
var volatile bit  stepper2_accelerate_on
var volatile byte _stepper2_acc_count
var volatile byte _stepper2_acc_index
var volatile bit  _stepper2_not_decelerating



-- ----------------------------------------------------------------------------
-- internal routine
-- This table can be used for both half-stepping and full-stepping of 4-phase motors
-- for full-stepping the table indices are incremented or decremented by 2
-- NOTE: depending on the moment of switching form half-stepping to full-stepping
--       the odd or the even indices can be used !!
--
-- If these tables are not used, they will not consume any memory space,
-- because they will be wiped out by the compiler
-- ----------------------------------------------------------------------------
procedure _stepper_table_4_phase_forward is
   pragma jump_table
   assembler
      addwf   PCL,f
      retlw   0b_0001
      retlw   0b_0011
      retlw   0b_0010
      retlw   0b_0110
      retlw   0b_0100
      retlw   0b_1100
      retlw   0b_1000
      retlw   0b_1001
   end assembler
end procedure
-- ----------------------------------------------------------------------------
procedure _get_stepper_value (  byte in  out indx, byte out x ) is
  assembler
    bank  movfw  indx
    page  call   _stepper_table_4_phase_forward
    bank  movwf  x
  end assembler
end procedure
-- ----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- increment the current position counter of motor=1
-- -----------------------------------------------------------------------------
procedure _stepper1_inc_pos is
  assembler
  local non_zero
    incfsz   stepper1_pos1,f
    goto     non_zero
    incfsz   stepper1_pos2,f
    goto     non_zero
    incfsz   stepper1_pos3,f
    goto     non_zero
    incfsz   stepper1_pos4,f
  non_zero:
  end assembler
end procedure
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- increment the current position counter of motor=2
-- -----------------------------------------------------------------------------
procedure _stepper2_inc_pos is
  assembler
  local non_zero
    incfsz   stepper2_pos1,f
    goto     non_zero
    incfsz   stepper2_pos2,f
    goto     non_zero
    incfsz   stepper2_pos3,f
    goto     non_zero
    incfsz   stepper2_pos4,f
  non_zero:
  end assembler
end procedure
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- decrement the current position counter of motor=1
-- -----------------------------------------------------------------------------
procedure _stepper1_dec_pos is
  assembler
  local non_zero
    movlw    1
    subwf    stepper1_pos1,f
    btfsc    C
    goto     non_zero
    subwf    stepper1_pos2,f
    btfsc    C
    goto     non_zero
    subwf    stepper1_pos3,f
    btfsc    C
    goto     non_zero
    subwf    stepper1_pos4,f
  non_zero:
  end assembler
end procedure
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- decrement the current position counter of motor=2
-- -----------------------------------------------------------------------------
procedure _stepper2_dec_pos is
  assembler
  local non_zero
    movlw    1
    subwf    stepper2_pos1,f
    btfsc    C
    goto     non_zero
    subwf    stepper2_pos2,f
    btfsc    C
    goto     non_zero
    subwf    stepper2_pos3,f
    btfsc    C
    goto     non_zero
    subwf    stepper2_pos4,f
  non_zero:
  end assembler
end procedure
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- compare the current postion and the target position of motor=1
-- returns TRUE if both are equal
-- ..?..result stored in gloabl ??
-- -----------------------------------------------------------------------------
function _stepper1_cmp_pos return bit is
  assembler
  local not_equal, finish
    movf     stepper1_pos1,w
    subwf    _stepper1_target_pos1,w
    btfss    Z
    goto     not_equal

    movf     stepper1_pos2,w
    subwf    _stepper1_target_pos2,w
    btfss    Z
    goto     not_equal

    movf     stepper1_pos3,w
    subwf    _stepper1_target_pos3,w
    btfss    Z
    goto     not_equal

    movf     stepper1_pos4,w
    subwf    _stepper1_target_pos4,w
    btfss    Z
    goto     not_equal

;    movlw    0           ;equal
;    goto     finish      ;
  not_equal:             ;
;    movlw    1           ;
  finish:                ;
;    movwf    _stepper1_z          ;
  end assembler
  return Z
end function
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- compare the current postion and the target position of motor=2
-- returns TRUE if both are equal
-- ..?..result stored in gloabl ??
-- -----------------------------------------------------------------------------
function _stepper2_cmp_pos return bit is
  assembler
  local not_equal, finish
    movf     stepper2_pos1,w
    subwf    _stepper2_target_pos1,w
    btfss    Z
    goto     not_equal

    movf     stepper2_pos2,w
    subwf    _stepper2_target_pos2,w
    btfss    Z
    goto     not_equal

    movf     stepper2_pos3,w
    subwf    _stepper2_target_pos3,w
    btfss    Z
    goto     not_equal

    movf     stepper2_pos4,w
    subwf    _stepper2_target_pos4,w
    btfss    Z
    goto     not_equal

;    movlw    0           ;equal
;    goto     finish      ;
  not_equal:             ;
;    movlw    1           ;
  finish:                ;
;    movwf    _stepper1_z          ;
  end assembler
  return Z
end function
-- -----------------------------------------------------------------------------



-- -----------------------------------------------------------------------------
-- internal routine
-- tests in full step mode if forced windings 1/2 are ok for motor=1
-- TRUE if no forced windings or forced windings are wrong
-- -----------------------------------------------------------------------------
function _stepper1_test_forced_windings return bit is
var bit index_lsb at _stepper1_index : 0
  return
    ((! stepper1_full_force_1_winding) & (! stepper1_full_force_2_winding)) |
    (stepper1_full_force_1_winding & index_lsb)|
    (stepper1_full_force_2_winding & (! index_lsb))