CON
_xinfreq=5_000_000
_clkmode=xtal1+pll16x 'The system clock is set at 80MHz (this is recommended for optimal resolution)
VAR
long position1, position2, position3, position4
'The assembly program will read these variables from the main Hub RAM to determine
' the high pulse durations of the three servo signals
PUB Demo
p1:=@position1 'Stores the address of the "position1" variable in the main Hub RAM as "p1"
p2:=@position2 'Stores the address of the "position2" variable in the main Hub RAM as "p2"
p3:=@position3 'Stores the address of the "position3" variable in the main Hub RAM as "p3"
p4:=@position4 'Stores the address of the "position4" variable in the main Hub RAM as "p4"
position1:=110_000 'Start sending 1.375ms servo signal high pulses to servomotor 2 (Center position)
position2:=110_000 'Start sending 1.375ms servo signal high pulses to servomotor 2 (Center position)
position3:=110_000 'Start sending 1.375ms servo signal high pulses to servomotor 2 (Center position)
position4:=110_000 'Start sending 1.375ms servo signal high pulses to servomotor 2 (Center position)
cognew(@Servos,0) 'Start a new cog and run the assembly code starting at the "ThreeServos" cell
DAT
Servos org 'Assembles the next command to the first cell (cell 0) in the new cog's RAM
mov Servo,#1
shl Servo, Servo1
mov Servo1, Servo
mov Servo,#1
shl Servo, Servo2
mov Servo2, Servo
mov Servo,#1
shl Servo, Servo3
mov Servo3, Servo
mov Servo,#1
shl Servo, Servo4
mov Servo4, Servo
mov Servo, Servo1
or Servo, Servo2
or Servo, Servo3
or Servo, Servo4
or dira, Servo
Loop
rdlong HighTime,p1
mov counter,cnt
or outa,Servo1
add counter,HighTime
waitcnt counter,0
andn outa,Servo1
rdlong HighTime,p2
mov counter,cnt
or outa,Servo2
add counter,HighTime
waitcnt counter,0
andn outa,Servo2
rdlong HighTime,p3
mov counter,cnt
or outa,Servo3
add counter,HighTime
waitcnt counter,0
andn outa,Servo3
rdlong HighTime,p4
mov counter,cnt
or outa,Servo4
add counter,HighTime
waitcnt counter,0
andn outa,Servo4
mov counter,cnt
add counter,LowTime
waitcnt counter,0
jmp #Loop
'Constants and Variables:
Servo1 long 4
Servo2 long 5
Servo3 long 6
Servo4 long 7
Servo long 0
p1 long 0 'Used to store the address of the position variable 1 in the main RAM
p2 long 0 'Used to store the address of the position variable 2 in the main RAM
p3 long 0 'Used to store the address of the position variable 3 in the main RAM
p4 long 0 'Used to store the address of the position variable 4 in the main RAM
LowTime long 800_000 'This works out to be a 10ms pause time with an 80MHz system clock. If the
'LowTime long 1_600_00 'servo behaves erratically, this value can be changed to 1_600_000 (20ms pause)
counter res 'Reserve one long of cog RAM for this "counter" variable
HighTime res 'Reserve one long of cog RAM for this "HighTime" variable
fit 'Makes sure the preceding code fits within cells 0-495 of the cog's RAM
