I am trying to use One Bot Board II to operate two servos with following program. I wish each of the two servos can move among 7 positions but initiate at different time. The following program just allows these two servos move once from position 1 to position 2 but does not continue to move to the remaining positions. Can any one help me to figure out what wrong in my program and teach me to correct it?
Thank you in advance.
[code]; 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
WTIMERTICSPERMSMUL con 64 ; 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_SWEEP_3 con 3
STATE_SWEEP_4 con 4
STATE_SWEEP_5 con 5
STATE_SWEEP_6 con 6
STATE_SWEEP_7 con 7
STATE_LONG_WAIT con 8
ServoState var byte(NUMSERVOS)
ServoTimeNext var long(NUMSERVOS)
ServoLoopCnt var byte(NUMSERVOS)
iServo var byte
SERVO_TABLE bytetable P4, P5
POSITION1_TABLE swordtable -8700, -8700 ; What position to use for each servo for position 1
POSITION2_TABLE swordtable -5000, -5000 ; … Position 2
POSITION3_TABLE swordtable -3000, -3000 ; What position to use for each servo for position 3
POSITION4_TABLE swordtable 0, 0 ; … Position 4
POSITION5_TABLE swordtable 3000, 3000 ; What position to use for each servo for position 5
POSITION6_TABLE swordtable 5000, 5000 ; … Position 6
POSITION7_TABLE swordtable 8700, 8700 ; What position to use for each servo for position 7
MOVETIME1_TABLE wordtable 5000, 5000 ; how much time to go to position 1 in ms
MOVETIME2_TABLE wordtable 5000, 5000 ; how much time to go to position 2 in ms
MOVETIME3_TABLE wordtable 5000, 5000 ; how much time to go to position 3 in ms
MOVETIME4_TABLE wordtable 5000, 5000 ; how much time to go to position 4 in ms
MOVETIME5_TABLE wordtable 5000, 5000 ; how much time to go to position 5 in ms
MOVETIME6_TABLE wordtable 5000, 5000 ; how much time to go to position 6 in ms
MOVETIME7_TABLE wordtable 5000, 5000 ; how much time to go to position 7 in ms
PAUSETIME1_TABLE wordtable 3000, 3000 ; how much time to pause at position 1 in ms
PAUSETIME2_TABLE wordtable 3000, 3000 ; how much time to pause at position 2 in ms
PAUSETIME3_TABLE wordtable 3000, 3000 ; how much time to pause at position 3 in ms
PAUSETIME4_TABLE wordtable 3000, 3000 ; how much time to pause at position 4 in ms
PAUSETIME5_TABLE wordtable 3000, 3000 ; how much time to pause at position 5 in ms
PAUSETIME6_TABLE wordtable 3000, 3000 ; how much time to pause at position 6 in ms
PAUSETIME7_TABLE wordtable 3000, 3000 ; how much time to pause at position 7 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 cs64/125
; but table is in cs to convert ms to cs we have cs=ms125/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, (8000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; how many ms to delay each servo from starting
TIME1_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; how long the time 1 should be
PAUSE1_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
TIME2_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; likewise for time 2…
PAUSE2_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
TIME3_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; how long the time 1 should be
PAUSE3_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
TIME4_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; likewise for time 2…
PAUSE4_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
TIME5_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; likewise for time 2…
PAUSE5_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
TIME6_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; how long the time 1 should be
PAUSE6_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
TIME7_TABLE longtable (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (5000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; likewise for time 2…
PAUSE7_TABLE longtable (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (3000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL
LONGWAIT_TABLE longtable (601000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL, (601000WTIMERTICSPERMSDIV)/WTIMERTICSPERMSMUL ; setup long wait time… for me minute…
NUMCYCLES_TABLE bytetable 8, 8
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
ServoState(iServo) = STATE_SWEEP_3 ; set up new state
gosub MoveServo[Servo_Table(iServo), POSITION2_TABLE(iServo), MOVETIME3_TABLE(iServo)]
ServoTimeNext(iServo) = ServoTimeNext(iServo) + TIME3_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 debugging 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 corresponding bit in the interrupt pending mask
mov.l @LTIMERCNT:16,er1 ; Add 256 to our counter
add.l #256,er1
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 consistency. That is it is very possible that
; after we grabbed the timers value it overflows, before we grab the other part
; so we check to make sure it is correct and if necessary regrab things.
;==============================================================================
GetCurrentTime:
lCurrentTime = lTimerCnt + TCA
; handle wrap
if lTimerCnt <> (lCurrentTime & 0xffffff00) then
lCurrentTime = lTimerCnt + TCA
endif
return lCurrentTime[/code]