Propeller Basic Servocontroller
FriedV
Posts: 77
Hi,
here is a servo controller for the propeller written in propbasic. Comments/critics/suggestion are always very welcome. Of course the easy way would be to use pulsout, but this way you can only control up to 9 servos with one cog, if you wish to renew the pulse train ever 20 msec. Thats some wasting of ressources. Here is a program that controls 24 servos (I need 18+ for a hexapod), could also be expanded to32 servos. The output was checked with the Logicport LA, one of the best instruments I have ever used, excellent price/perf ratio.
During development of the software I would have liked to be able to have indexed PINs, but PIN(i)=x is not possible (yet?). Will there be an object oriented PropBasic in the (near?) future? Calling TASKs in newly started cogs with parameters via a stack would be a nice first step.
Regards, Fried
here is a servo controller for the propeller written in propbasic. Comments/critics/suggestion are always very welcome. Of course the easy way would be to use pulsout, but this way you can only control up to 9 servos with one cog, if you wish to renew the pulse train ever 20 msec. Thats some wasting of ressources. Here is a program that controls 24 servos (I need 18+ for a hexapod), could also be expanded to32 servos. The output was checked with the Logicport LA, one of the best instruments I have ever used, excellent price/perf ratio.
During development of the software I would have liked to be able to have indexed PINs, but PIN(i)=x is not possible (yet?). Will there be an object oriented PropBasic in the (near?) future? Calling TASKs in newly started cogs with parameters via a stack would be a nice first step.
Regards, Fried
' ----------------------------------------------------------------------
' File...... ServoPuls.pbas
' Author.... fgvissel 24.08.2011
' ----------------------------------------------------------------------
' ----- Device Settings ------------------------------------------------
DEVICE P8X32A, XTAL1, PLL16X
XIN 5_000_000
' ----- Conditional Compilation Symbols --------------------------------
' ----- Constants ------------------------------------------------------
HIGHCNT CON 444 'Dummy Constant for inner Servo loops, can never
'reach 0 whithin 400 loop cycles
' ----- IO Pins --------------------------------------------------------
ServoGrp1 PIN 0..7 Low
ServoGrp2 PIN 8..15 Low
ServoGRp3 PIN 16..23 Low
P0 PIN 0
P1 PIN 1
P2 PIN 2
P3 PIN 3
P4 PIN 4
P5 PIN 5
P6 PIN 6
P7 PIN 7
P8 PIN 8
P9 PIN 9
P10 PIN 10
P11 PIN 11
P12 PIN 12
P13 PIN 13
P14 PIN 14
P15 PIN 15
P16 PIN 16
P17 PIN 17
P18 PIN 18
P19 PIN 19
P20 PIN 20
P21 PIN 21
P22 PIN 22
P23 PIN 23
PTest PIN 24 '<--------------- Test only --------------------------
' ----- Shared (HUB) Variables (Byte, Word, Long) ----------------------
Servo_set HUB LONG (24)
' ----- User Data (DATA, WDATA, LDATA, FILE) ---------------------------
' ----- TASK Definitions -----------------------------------------------
' ----- Local Variables (LONG only) ------------------------------------
loopcnt VAR LONG
i VAR LONG
k VAR long
delta VAR LONG
smdelta VAR LONG
target VAR LONG
smtarget VAR LONG
pos VAR LONG (24) 'position for each servo
' ----- SUB and FUNC Definitions ---------------------------------------
PROGRAM Start
Start:
wrlong Servo_set(0), 400,360,320,300,280,220,210,200
wrlong Servo_set(8), 200,210,220,230,240,250,260,270
wrlong Servo_set(16), 400,390,380,370,360,350,340,320
Main:
ServoGrp1 = $00 'all pins low
ServoGrp2 = $00
ServoGrp3 = $00
delta = _FREQ / 50 'For MainLoop, run every 20 msec
target = cnt + delta
MainLoop: 'every 20 msec
pulsout PTest, 1000 '<----- Trigger Pulse for LA, Test only -------
for i = 0 to 23 'Get target servo positions from HUB Ram
rdlong Servo_set(i),pos(i)
next
smdelta = _Freq / 200_000 'small delta for inner loops, 5 usecs,
Loopcnt = 400 'enough time for all loop instructions (~280usec)
'----- This is for the 2. Group of 8 Servos-----
ServoGrp1 = $FF 'all servos in group1 high
smtarget = cnt + smdelta 'time stamp
Do 'This is for the 1. Group of 8 Servos
djnz pos(7), cont6 'Pulse resolution is 400 for full scale,
P7 = 0 'so we have 100/400 = 0.25%, or if the total angle is 180°
Pos(7) = HIGHCNT 'this means 180/400 = 0,45°, better than most servos will do
cont6:
djnz pos(6), cont5
P6 = 0
Pos(6) = HIGHCNT
cont5:
djnz pos(5), cont4
P5 = 0
Pos(5) = HIGHCNT
cont4:
djnz pos(4), cont3
P4 = 0
Pos(4) = HIGHCNT
cont3:
djnz pos(3), cont2
P3 = 0
Pos(3) = HIGHCNT
cont2:
djnz pos(2), cont1
P2 = 0
Pos(2) = HIGHCNT
cont1:
djnz pos(1), cont0
P1 = 0
Pos(1) = HIGHCNT
cont0:
djnz pos(0), conti1
P0 = 0
Pos(1) = HIGHCNT
conti1:
waitcnt smtarget, smdelta
loop Loopcnt
'----- This is for the 2. Group of 8 Servos-----
ServoGrp2 = $FF 'all servos in group2 high
Loopcnt = 400
smtarget = cnt + smdelta 'time stamp
Do
djnz pos(15), cont14
P15 = 0
Pos(15) = HIGHCNT
cont14:
djnz pos(14), cont13
P14 = 0
Pos(14) = HIGHCNT
cont13:
djnz pos(13), cont12
P13 = 0
Pos(13) = HIGHCNT
cont12:
djnz pos(12), cont11
P12 = 0
Pos(12) = HIGHCNT
cont11:
djnz pos(11), cont10
P11 = 0
Pos(11) = HIGHCNT
cont10:
djnz pos(10), cont9
P10 = 0
Pos(10) = HIGHCNT
cont9:
djnz pos(9), cont8
P9 = 0
Pos(9) = HIGHCNT
cont8:
djnz pos(8), conti2
P8 = 0
Pos(8) = HIGHCNT
conti2:
waitcnt smtarget, smdelta
loop Loopcnt
'----- This is for the 3. Group of 8 Servos-----
ServoGrp3 = $FF 'all servos in group3 high
Loopcnt = 400
smtarget = cnt + smdelta 'time stamp
Do
djnz pos(23), cont22
P23 = 0
Pos(23) = HIGHCNT
cont22:
djnz pos(22), cont21
P22 = 0
Pos(22) = HIGHCNT
cont21:
djnz pos(21), cont20
P21 = 0
Pos(21) = HIGHCNT
cont20:
djnz pos(20), cont19
P20 = 0
Pos(20) = HIGHCNT
cont19:
djnz pos(19), cont18
P19 = 0
Pos(19) = HIGHCNT
cont18:
djnz pos(18), cont17
P18 = 0
Pos(10) = HIGHCNT
cont17:
djnz pos(17), cont16
P17 = 0
Pos(17) = HIGHCNT
cont16:
djnz pos(16), conti3
P16 = 0
Pos(16) = HIGHCNT
conti3:
waitcnt smtarget, smdelta
loop Loopcnt
waitcnt target, delta
goto MainLoop
goto Main
END
' ----- SUB and FUNC Code ----------------------------------------------
' ----- TASK Code ------------------------------------------------------
' ----------------------------------------------------------------------
800 x 500 - 80K