Feedback Servo (example of setting up PASM2 cog)

tomcrawford

Here is a sample program with a spin program starting a pasm2 program in a separate cog.

I am posting this for two reasons: First, I'm pretty sure there are others with an ES board that might profit from a simple example that includes intra-cog communication. Secondly, I am hoping the experts will dismantle the program and tell me how to do it correctly, that I may profit.

AdThanksVance

CON
            
    oscmode = $010c3f04               'standard stuff
    freq    = 160_000_000
    baud = 115200                     'must configure RUN command to match this

OBJ
    ser: "PrintfSerial"     'access to Propellor output (output only)

var
    long  param             'PASM cog will write results here
    byte  cog

pub main
    clkset(oscmode, freq)
    ser.start(baud)                  'start up serial terminal
    ser.str(string("hello, world"))
    ser.nl

    cog := cognew(@entry, @param)     'send ADDRESS of param
    ser.str(string("cog started: "))
    ser.dec(cog)
    ser.nl
    repeat                              'forever 10 times a second
      ser.str(string("Returned Value is: "))
      ser.dec(Param)
      ser.nl
      waitcnt(clkfreq/10+cnt)


DAT
entry     ORG 0
          mov  hubAdrs, ptra     'boss cog passed an address:  save it
'Monitor feedback pin of 360 Feedback servo and calculate angle in degrees
'have to do double precision to avoid overflow
top       nop
L1        test ina, FBPinMask  wcz
    if_NZ jmp #L1             'wait for a low
L2        test ina, FBPinMask  wcz
    if_Z  jmp #L2             'wait for beginning of high
          getct stime         'save count at very beginning of high pulse
L3        test ina, FBPinMask  wcz
   if_NZ  jmp  #L3            'wait for end of high (beginning of low
          getct ltime         'save count at beginning of low pulse  
L4        test ina, FBPinMask  wcz
    if_Z  jmp  #L4            'wait for end of low
          getct etime         'save count at end of low pulse
          mov  ttime, etime   
          sub  ttime, stime   'compute total time in clock tics
          mov  htime, ltime    
          sub  htime, stime    'compute high time
          Qmul  htime, ##360   'multiply htime by 360
          getqx work           'lower part of product
          getqy work1          'upper part of product 
          setq  work1          'upper part 
          QDiv work, ttime     'divide product*360 by total time
          getqx  work           'ratio in degrees 
          wrlong work, hubAdrs  'save it for display            
          jmp  #top             'forever    

FBPinMask long  2               'feedback pin connected to I/O 1
hubAdrs   res 1                 'hub address of "param"
work      res 1
work1     res 1 
stime     res 1        'CNT at beginning of high time
ltime     res 1        'CNT at beginning of low time 
etime     res 1        'CNT at end of low time        
ttime     res 1        'total period in clock tics
htime     res 1        'high time in clock tics

jmg

I think there is also a smart pin mode-pair for doing this. (measure Period, and Gated-Counter on HI time )

%10011 = For X periods, count time
%10100 = For X periods, count states

FWIR, you can config 2 Smart pins to one mode each, to capture Period and HI-Time in SysCLKs.
Chip said those can start at the same time, and those have finer granularity that a SW polling loop, and do not use a COG to poll for edges.

Mark_T

Another way to do this is store the address of the parameter in a DAT section long that's in the cog's image before
starting the cog. Then the cog doesn't have to waste any instructions getting the address, as its copied into
cog memory/register as the cog starts.

However that isn't thread-safe, so if several Spin cogs try to do it simultaneously they can clash
over the address and corrupt each others setup, so its not the best method for a utility cog/library
that several things might all want to use (such as a comms cog, SPI driver etc) - the PTRA parameter
passing is a safe way, and can be done by PASM cog starting another PASM cog, where a SETQ before
a COGINIT can be used to set the initial PTRA value in the new cog.

PTRA is the Prop2 replacement for the PAR register in Prop1 code.