Elementary Background Task to control Stingray Motors in PropBasic
FriedV
Posts: 77
Something to play with, based on the Stingray Propeller board.
Friedrich
Friedrich
' ----------------------------------------------------------------------
' File...... MotCtl.pbas
' Author.... fgvissel 23.09.2011
' based on the SRMotorControl Object by Nico Schoemaker
' mind the 10 MHz Propeller
' ======================================================================
' ----- Device Settings ------------------------------------------------
DEVICE P8X32A, XTAL1, PLL16X
XIN 6_250_000
' ----- Conditional Compilation Symbols --------------------------------
' ----- Constants ------------------------------------------------------
' ----- IO Pins --------------------------------------------------------
' ----- Shared (HUB) Variables (Byte, Word, Long) ----------------------
hbPeriodTicks hub long = 0
hbCommand hub long = 3 'Sync with PWM cog
hbSpeedL hub long = 0
hbSpeedR hub long = 0
hbRampDelay hub long = 0
' ----- User Data (DATA, WDATA, LDATA, FILE) ---------------------------
' ----- TASK Definitions -----------------------------------------------
PWM TASK AUTO
' ----- Local Variables (LONG only) ------------------------------------
t1 var long 'for intermediate temp results
' ----- SUB and FUNC Definitions ---------------------------------------
MoveForward SUB 1
MoveBackward SUB 1
SetRampDelay SUB 1
' ======================================================================
PROGRAM Start
Start:
t1 = _FREQ / 1000 'PWM frequency is 1 kHz, Period 1 msec
wrlong hbPeriodTicks, t1
wrlong hbSpeedL, 0 'clear command
wrlong hbSpeedR, 0
wrlong hbRampDelay, 0
wrlong hbCommand, 0 'Start Background PWM Task
'------------------Simple Testframe for Stingray Motors ---------------
Main:
SetRampDelay 10
do
MoveBackward 100
pause 3000
MoveForward 100
pause 3000
loop
END
' ----- SUB and FUNC Code ----------------------------------------------
SUB MoveForward 'Speed
do 'Wait forPWM cog to finish command
rdlong hbCommand, t1
loop until t1 = 0
__param1 = __param1 MIN 0 'send new command, forward speed 0 - 100
__param1 = __param1 MAX 100
wrlong hbSpeedL, __param1
wrlong hbSpeedR, __param1
wrlong hbCommand, 1
ENDSUB
SUB MoveBackward 'Speed
do 'Wait forPWM cog to finish command
rdlong hbCommand, t1
loop until t1 = 0
__param1 = __param1 MIN 0 'send new command, backward speed0 - 100
__param1 = __param1 MAX 100
__param1 = - __param1
wrlong hbSpeedL, __param1
wrlong hbSpeedR, __param1
wrlong hbCommand, 1
ENDSUB
SUB SetRampDelay 'RampDelay
__param1 = __param1 MIN 0
__param1 = __param1 MAX 10
wrlong hbRampDelay, __param1
ENDSUB
' ----- TASK Code ------------------------------------------------------
TASK PWM
SetOutputMask SUB
GetCommand SUB
GetHubCommand SUB
t1 var long
delay var long
SlicDur var long
SliceCount var long
OutputMask var long
Command var long
SpeedL var long
curSpeedL var long
curAbsSpeedL var long
SpeedR var long
curSpeedR var long
curAbsSpeedR var long
RampDelay var long
curRampDelay var long
'---Code---
t1 = %1111 'Output Bits 24..27
t1 = t1 << 24 'are set to 0 on cogstart
dira = t1
do
rdlong hbCommand, Command 'Init, Sync with cog 0
loop until Command <> 3
rdlong hbPeriodTicks, SlicDur 'read clockticks for one period (100 Slices)
SlicDur = SlicDur / 100 '1/100 period ticks
SliceCount = 100
SpeedL = 0 'init all Speeds to 0
SpeedR = 0
RampDelay = 0
curSpeedL = 0
curSpeedR = 0
delay = SlicDur 'set delay to the end of 1. period slice
delay = delay + cnt
do 'endless loop, create PWM
do
do 'do forever, main PWM loop
do 'inner loop, run one slice
for SliceCount = 100 to 0 step -1
SetOutputMask 'based on current values
outa = OutputMask << 24 'set pins for current Slice
waitcnt delay, SlicDur 'wait till end of current slice
next
GetHubCommand
loop while RampDelay = 0 'no ramp delay, loop
dec curRampDelay
loop while curRampDelay > 0 'loop for RamDelay
curRampDelay = RampDelay 'set new RampDelay
if curSpeedL < SpeedL then
inc curSpeedL
elseif curSpeedL > SpeedL then
dec curSpeedL
endif
if curSpeedR < SpeedR then
inc curSpeedR
elseif curSpeedR > SpeedR then
dec curSpeedR
endif
loop
'---SUBs---
SUB SetOutputMask
OutputMask = %0000 'OutputMask = 0
if curSpeedL < 0 then
OutputMask = OutputMask | %0001 'LM back, leave RM alone
elseif curSpeedL = 0 then ' bit 24 = 1
OutputMask = OutputMask | %0011 'stop left motor
elseif curSpeedL > 0 then
OutputMask = OutputMask | %0010 'set left motor to forward
endif
curAbsSpeedL = abs curSpeedL
if SliceCount > curAbsSpeedL then
OutputMask = OutputMask | %0011 'Stop left motor
endif
if curSpeedR < 0 then
OutputMask = OutputMask | %1000 'set right motor to backward
elseif curSpeedR = 0 then ' bit 27 = 1
OutputMask = OutputMask | %1100 'stop right motor
elseif curSpeedR > 0 then
OutputMask = OutputMask | %0100 'set right motor to forward
endif
curAbsSpeedR = abs curSpeedR
if SliceCount > curAbsSpeedR then
OutputMask = OutputMask | %1100 'Stop right motor
endif
ENDSUB
SUB GetHubCommand
rdlong hbcommand, command 'get new command
if command <> 0 then
rdlong hbSpeedL, SpeedL
rdlong hbSpeedR, SpeedR
rdlong hbRampDelay, RampDelay
curRampDelay = RampDelay
endif
if Rampdelay = 0 then
curSpeedL = SpeedL
curSpeedR = SpeedR
endif
Command = 0 'set Bit0 = 0
wrlong hbCommand, Command 'sig caller that processing is finished
ENDSUB
ENDTASK
' ----------------------------------------------------------------------
Comments
Fried