Lynxmotion Tech Support

; Program to sweep different servos back and forth over a given time...
NUMSERVOS   con 2
; variables and constants associated with system timer
lTimerCnt         var long   
lCurrentTime      var long

; Warning on the timings I am going to further divide the clock value by 4, which will give us a precision of about 2ms,
;but will keep our clock from overflowing for over 100 days...
;This requires changes in, our multiplier below, plus in the timer interrupt and in our GetCurrentTime function.
WTIMERTICSPERMSMUL con 64*4   ; BAP28 is 16mhz need a multiplyer and divider to make the conversion with /8192
WTIMERTICSPERMSDIV con 125  ;

STATE_START_CYCLE   con 0
STATE_SWEEP_1   con 1
STATE_SWEEP_2   con 2
STATE_LONG_WAIT con 3

ServoState      var   byte(NUMSERVOS)
ServoTimeNext    var long(NUMSERVOS)
ServoLoopCnt   var byte(NUMSERVOS)

iServo         var byte


SERVO_TABLE      bytetable P4, P5
POSITION1_TABLE swordtable -9000, -9000         ; What position to use for each servo for position 1
POSITION2_TABLE   swordtable 9000, 9000         ; .. Position 2
MOVETIME1_TABLE wordtable 500, 500            ; how much time to go to position 1 in ms
MOVETIME2_TABLE wordtable 500, 500            ; how much time to go to position 2 in ms

; Note the following tables are stored in Timer units.  The timer is init to increment every 8192 clock cycles
; so to convert from Clock Cycles(CS) to Milliseconds(ms) we have ms = (cs * 8192)*1000/16000000 reduced to cs*64/125
; but table is in cs to convert ms to cs we have cs=ms*125/64
; note below using defines from other programs as if you run on Arc32 it runs at 20mhz so constants change
INIT_WAIT_TABLE   longtable 0, (5000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL            ; how many ms to delay each servo from starting
TIME1_TABLE      longtable (2000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (2000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL         ; how long the time 1 should be
TIME2_TABLE      longtable (2000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (2000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL         ; likewise for time 2...
LONGWAIT_TABLE   longtable (60*1000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (60*1000*WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; setup long wait time... for me minute...

NUMCYCLES_TABLE   bytetable 5, 5

for iServo = 0 to NumServos-1
   ServoState(iServo) = STATE_START_CYCLE
   ServoTimeNext(iServo) = INIT_WAIT_TABLE(iServo)
   ServoLoopCnt(iServo) = 0;
next

; Init our system timer
gosub InitTimer

Loop:
   ; check to see if any of our servos need handling
   gosub GetCurrentTime   ; this sets lCurrentTime
   for iServo = 0 to NUMSERVOS-1
      if lCurrentTime > ServoTimeNext(iServo) then
         if ServoState(iServo) = STATE_START_CYCLE then
            ServoState(iServo) = STATE_SWEEP_1      ; set up new state
            gosub MoveServo[Servo_Table(iServo), POSITION1_TABLE(iServo), MOVETIME1_TABLE(iServo)]
            ServoTimeNext(iServo) = ServoTimeNext(iServo) + TIME1_TABLE(iServo)   ; set up for when the next time for something to happen
             
         elseif ServoState(iServo) = STATE_SWEEP_1
            ServoState(iServo) = STATE_SWEEP_2      ; set up new state
            gosub MoveServo[Servo_Table(iServo), POSITION2_TABLE(iServo), MOVETIME2_TABLE(iServo)]
            ServoTimeNext(iServo) = ServoTimeNext(iServo) + TIME2_TABLE(iServo)   ; set up for when the next time for something to happen

         elseif ServoState(iServo) = STATE_SWEEP_2
            ; We completed a complete cycle, lets increment our count of iterations.  If we reach max count we will go into long
            ; wait else we will start the next cycle...
            ServoLoopCnt(iServo) = ServoLoopCnt(iServo) + 1
            if ServoLoopCnt(iServo) >= NUMCYCLES_TABLE(iServo) then
               ; set up for the long wait
               ServoTimeNext(iServo) = ServoTimeNext(iServo) + LONGWAIT_TABLE(iServo)   ; set up for when the next time for something to happen
               ServoLoopCnt(iServo) = 0 ; reset counter
               HSERVO[Servo_Table(iServo)\-24000\0]   ; special number to tell analog servos to power down (send no more signals).
            endif
            ServoState(iServo) = STATE_START_CYCLE
         endif
     
      endif   
   
   next

   goto loop
   
_MS_Servo var byte
_MS_Pos   var   sword
_MS_Time var long
MoveServo[_MS_Servo, _MS_Pos, _MS_Time]:
    HServo [_MS_Servo\_MS_Pos\(ABS(_MS_Pos - (HServoPos _MS_Servo))*20)/_MS_Time]
;    HServoWait[_MS_Servo]
    return

;========================================================================================================
; Note the code below is used to setup a system timer that we use to time maintain the timings in the program
;====================================================================
;--------------------------------------------------------------------
;[TIMER INTERRUPT INIT]
TIMERINT   con   TIMERAINT
ONASMINTERRUPT TIMERAINT, HANDLE_TIMERA_ASM
InitTimer:
;Timer
; Timer A init, used for timing of messages and some times for timing code...
  TIMERINT   con   TIMERAINT

  WTIMERTICSPERMSMUL con 64   ; BAP28 is 16mhz need a multiplyer and divider to make the conversion with /8192
  WTIMERTICSPERMSDIV con 125  ;
  TMA = 0   ; clock / 8192               ; Low resolution clock - used for timeouts...
  lTimerCnt = 0                         ; initialize our timer to 0
  ENABLE TIMERINT

return

;==============================================================================
;[Handle_Timer_asm] - Handle timer A overflow in assembly language.  Currently only
;used for timings for debuging the speed of the code
;Now used to time how long since we received a message from the remote.
;this is important when we are in the NEW message mode, as we could be hung
;out with the robot walking and no new commands coming in.
;==============================================================================
BEGINASMSUB
HANDLE_TIMERA_ASM
   push.l    er1                 ; first save away ER1 as we will mess with it.
   bclr    #6,@IRR1:8          ; clear the cooresponding bit in the interrupt pending mask
   mov.l    @LTIMERCNT:16,er1   ; Add 256 to our counter  - Note changed to 64 as we are changing the clock to overflow
   add.l   #64,er1          ; only after about 100 days instead of 25
   mov.l    er1, @LTIMERCNT:16
   pop.l    er1
   rte
ENDASMSUB

;-------------------------------------------------------------------------------------
;==============================================================================
;[GetCurrentTime] - Gets the Timer value from our overflow counter as well as the TCA counter.  It
;                makes sure of consistancy. That is it is very posible that
;                after we grabed the timers value it overflows, before we grab the other part
;                so we check to make sure it is correct and if necesary regrab things.
; Warning: Changed function to divide clock by 4 to allow a slower overflow...
;==============================================================================
GetCurrentTime:
  lCurrentTime = lTimerCnt + TCA/4
   
  ; handle wrap
  if lTimerCnt <> (lCurrentTime & 0xffffffC0) then   ' Had to modify mask as well as only updating lower 6 bits...
   lCurrentTime = lTimerCnt + TCA/4
  endif

return lCurrentTime

; up top somewhere...
POSITION_ON   con 32000
POSITION_OFF con 32001
;...
_MS_Servo var byte
_MS_Pos   var   sword
_MS_Time var long
MoveServo[_MS_Servo, _MS_Pos, _MS_Time]:
    if _MS_Pos = POSITION_ON  then
        high _MS_Servo
    elseif _MS_Pos = POSITION_OFF
        low _MS_Servo
    else
        HServo [_MS_Servo\_MS_Pos\(ABS(_MS_Pos - (HServoPos _MS_Servo))*20)/_MS_Time]
    return