Protobot (My first robot)

Copper · 2013-12-18 18:42

First: I have made a little progress on the block sensor code. In short, I had failed to recognize that my duty signal was sweeping, just very, very slowly. I was having the duty cycle set ~4294967.296 times, which I settled on while experimenting with an LED. It didn't occur to me that when I added my ~6ms detection routine to each possible duty cycle, it would take forever. I have since reduced it to 512 different duty cycles, at ~6ms per duty cycle. Currently, everything appears to be working, unfortunately, the data I'm getting is too variable to be useful. I'll share the code after I've had some more time to think things through.

Second: I tweaked my claw design so that it could be printed on a Uprint, which is what the rest of the 3D printed parts were printed with. My first claw was printed on a Objet printer. The Objet printer can print with a variety of materials (their higher end printers can use multiple build materials on the same model), but the material I could get a sample of was too soft/sticky. The only reason I didn't use the Uprint to begin with is that it takes more work to accommodate for printer tolerances than the Objet which prints down to 12micron layers. The closest clearance I've gotten away with out of the Uprint is 0.01", so basically, I had to come up with geometry that would do what I want that the printer could handle. It would have been easier to print to first claw with a different material on the Objet; but I decided it was worth it to have all the parts come out of the same printer. Plus the Uprint ABS material is great for small assemblies.

I decided it made more sense to waste a little bit of plastic checking that the gears would come out clean, rather than risk printing the whole thing only to find the teeth aren't going to work.

Final Print

Also, the Left remote is the one I've been using; but I discovered the other day, the little remotes HP stuffs in their laptops use the same protocol.

Copper · 2013-12-19 16:36

Well, I think it's time to consider simplifying my block sensor solution. Rather than worry about trying to do a comparison between the two LEDs to determine "how left" or "how right" the block is, I think I'll just try a Left==Left, Both==Center, and Right==Right approach.

I think my code is doing exactly what I think/want it should/to, but the distance readings on both LEDs is really unreliable. Sometimes it detects objects that aren't there, and vice versa. It could be there is a/an flaw/error in my code, or that a small modification to the circuit might smooth things out; but for now, I should probably keep it simple.

{>>Object DEMO:


CON
   
  _clkmode = xtal1 + pll16x                  ' System clock &#8594; 80 MHz
  _xinfreq = 5_000_000




OBJ
  pst : "Parallax Serial Terminal"
  BS  : "02_BlockSensor_PASM"


PUB main


  'test_sample
  test_align
  
PUB test_align | Lalign  
  
  pst.Start(115200)                      
  waitcnt(clkfreq*2+cnt)


  pst.str(string(pst#CS))


  BS.Start_BlockSensor(9, 7, 8, 6)
                                                   
  repeat


    pst.str(string(pst#NL))
      
    Lalign := BS.BS_align
    
    pst.dec(Lalign)




PUB test_sample | Lsample  
  
  pst.Start(115200)                      
  waitcnt(clkfreq*2+cnt)


  pst.str(string(pst#CS))


  BS.Start_BlockSensor(9, 7, 8, 6)


  dira[6]:=1
                                                   
  repeat
      
    Lsample := BS.BS_sample
                  
    pst.str(string(pst#NL))
    
    pst.bin(Lsample, 2)
}


CON
   
  _clkmode = xtal1 + pll16x                  ' System clock &#8594; 80 MHz
  _xinfreq = 5_000_000
  
PUB Start_BlockSensor(leftIR, rightIR, sensorPIN, _DUTYpin)


  Stop
  cmd := 0
  left := leftIR
  right := rightIR
  sensor := sensorPIN
  _DUTY := _DUTYpin


  cog := cognew(@cogstart, @cmd) + 1


PUB Stop


  if cog
    cogstop(cog~-1)


PUB BS_sample | _sample


  cmd := 1
  repeat while cmd == 1
    next


  _sample := left
  _sample := _sample << 1
  _sample := _sample | right
  return(_sample)


PUB BS_align | Ltemp 'under construction


  cmd := 3
  repeat while cmd == 3
    next
                                                    'dinc == 8388608
  
  return(left)   


PUB BS_monitor   'check sensor status with BS_sensor_status *untested


  cmd := 2                                                   


PUB BS_sensor_status | status


  status := left
  status := status << 1
  status := status | right
  return(status)  


PUB BS_cmd_status


  return(cmd)
   
DAT   
                        org 0
cogstart                mov     mem_addr, PAR                                   'retrieve parameters
:pars                   add     mem_addr, #4                                    'add 1 long to mem_addr (skips @cmd)
                        mov     left_addr, mem_addr                             'store address at mem_addr in left_addr
                        add     mem_addr, #4                                    'add 1 long to mem_addr
                        mov     right_addr, mem_addr                            'store address at mem_addr in right_addr
                        add     mem_addr, #4                                    'add 1 long to mem_addr
                        mov     sensor_addr, mem_addr                           'store address at mem_addr in sensor_addr
                        add     mem_addr, #4                                    'add 1 long to mem_addr
                        mov     _DUTY_addr, mem_addr                            'store address at mem_addr in _DUTY_addr
                        rdlong  S_PIN, sensor_addr                              'read value @sensor_addr into S_PIN
                        rdlong  L_IR, left_addr                                 'read value @left_addr into L_IR
                        rdlong  R_IR, right_addr                                'read value @right_addr into R_IR
                        rdlong  D_PIN, _DUTY_addr                               'read value @_DUTY_addr into D_PIN
:confiD_cntr            mov     temp, #1                                        'config pins
                        shl     temp, L_IR                                      ' ...
                        or      dira, temp
                        mov     temp, #1
                        shl     temp, R_IR
                        or      dira, temp
                        mov     S_PINmask, #1
                        shl     S_PINmask, S_PIN
                        andn    dira, S_PINmask                                 'verify sensor set to input (presumes Z==0)                       
                        or      L_cntr, L_IR                                    'set cntr mode apins
                        or      R_cntr, R_IR
                        or      D_cntr, D_PIN
                        mov     frqb, FReQB                                     'set frqb, to FReQB == 2040109 == (frequency*2^32)/clkfreq
       
                        jmp     #cmdwait


DAT                  
cmdwait                 rdlong  cmd, PAR    wz                                  'wait for (cmd <> 0)
              if_z      jmp     #cmdwait
:jump                   max     cmd, #4 
                        add     cmd, #cmdtbl_0                                  'add line#@cmdtbl_0 to cmd
                        jmp     cmd 
cmdtbl_0                jmp     #cmdwait                                        'PAR==0
cmdtbl_1                jmp     #sample                                         'PAR==1
cmdtbl_2                jmp     #monitor                                        'PAR==2
cmdtbl_3                jmp     #align                                          'PAR==3
cmdtbl_4                jmp     #cmd_done                                       'PAR==4
                                                                                'else
cmd_done                wrlong  zero, PAR                                       'signal command done
                        jmp     #cmdwait


DAT


sampleCALL
sample                  mov     Ldetect, #1                                     'reset "detection registers"    
                        mov     Rdetect, #1


                        'L_IR
                        mov     ctrb, L_cntr                                    'L_IR -> ON
                        mov     time, cnt                                       'set up loop
:rollover_1             add     time, _space            wc                                                 
              if_c      jmp     #:rollover_1                                                               
:check_left             and     S_PINmask, ina          wz, nr                  'start loop
              if_z      mov     Ldetect, #0                                     'if detected, set to 0
                        cmp     time, cnt               wc                           
              if_nc     jmp     #:check_left                                    'loop


                        'rest
                        mov     ctrb, #0                                        'L_IR -> OFF
                        mov     time, cnt                                       'rest
                        add     time, _space
                        waitcnt time, #0
              
                        'R_IR
                        mov     ctrb, R_cntr                                    'R_IR -> ON
                        mov     time, cnt                                       'set up loop
:rollover_2             add     time, _space            wc
              if_c      jmp     #:rollover_2                                    
:check_right            and     S_PINmask, ina          wz, nr                  'start loop
              if_z      mov     Rdetect, #0                                     'if detected, set to 0
                        cmp     time, cnt               wc
              if_nc     jmp     #:check_right


                        mov     ctrb, #0                                        'R_IR -> OFF
                        
                        'report    
                        wrlong  Ldetect, left_addr                              
                        wrlong  Rdetect, right_addr
                       
                        rdlong  temp, PAR
                        cmp     temp, one               wz                      'check sample or sampleCALL
              if_z      jmp     #cmd_done                                       'if sample, cmd_done
sampleCALL_ret          ret                                                     'else, ret


DAT
monitor                 mov     Ldetect, #1                                     'reset "detection registers"    
                        mov     Rdetect, #1


                        'L_IR
                        mov     ctrb, L_cntr                                    'L_IR -> ON
                        mov     time, cnt                                       'set up loop
:rollover_1             add     time, _space            wc
              if_c      jmp     #:rollover_1
:check_left             and     S_PINmask, ina          wz, nr                  'start loop
              if_z      mov     Ldetect, #0                                     'if detected, set to 0
                        cmp     time, cnt               wc                           
              if_nc     jmp     #:check_left                                    'loop


                        'rest
                        mov     ctrb, #0                                        'L_IR -> OFF
                        mov     time, cnt                                       'rest
                        add     time, _space  
                        waitcnt time, #0
                        
                        'R_IR
                        mov     ctrb, R_cntr                                    'R_IR -> ON
                        mov     time, cnt                                       'set up loop
:rollover_2             add     time, _space            wc
              if_c      jmp     #:rollover_2                                    
:check_right            and     S_PINmask, ina          wz, nr                  'start loop
              if_z      mov     Rdetect, #2
                        cmp     time, cnt               wc
              if_nc     jmp     #:check_right


                        mov     ctrb, #0                                        'R_IR -> OFF


                        'report
                        wrlong  Ldetect, left_addr
                        wrlong  Rdetect, right_addr
                        jmp     #monitor


DAT
align                   mov     ctra, D_cntr                                    'set ctra for duty signal
                        mov     frqa, #0
                        mov     phsa, #0
                        mov     temp, #1
                        shl     temp, D_PIN
                        or      dira, temp
                        mov     :minLduty, top                                  'max duty      *could add dinc to duty for max range
                        mov     :minRduty, top                                  '       
                        mov     :duty, top                                      '
:loop                   sub     :duty, :dinc                                    'dinc duty signal
                        cmp     :dinc, :duty            wc                      'is duty < dinc
              if_nc     jmp     #:next                                          'if c:=0, next
                        mov     frqa, :duty                                     'set duty cycle              
                        call    #sampleCALL                                     'sample sensor
                        rdlong  temp, left_addr         wz                      'check left  
              if_z      mov     :minLduty, :duty                                'if detected, store current :duty
                        rdlong  temp, right_addr        wz                      'check right
              if_z      mov     :minRduty, :duty                                'if detected, store current :duty                        
                        jmp     #:loop                                          'loop until next
:next                   wrlong  :minLduty, left_addr                            'report min duty for left
                        wrlong  :minRduty, right_addr                           'report min duty for right
                        jmp     #cmd_done                                       'done
            
:duty                   long 0
:minLduty               long 0
:minRduty               long 0
:dinc                   long 8388608  


DAT


'SPIN <-> PASM addr's
cmd                     long 0
left                    long 0        
right                   long 0
sensor                  long 0
_DUTY                   long 0


'PASM -> SPIN cmd_status values
zero                    long 0
one                     long 1


'BS values
FReQB                   long 2040109                                            '2040109.4656 == (frequency*2^32)/clkfreq where frequency==38khz
top                     long 4294967295                                         ' 2³²== 4294967296             
_space                  long 160000                                             '2ms
L_cntr                  long (100 << 26)
R_cntr                  long (100 << 26)
D_cntr                  long (110 << 26)
S_PINmask               long 0
Ldetect                 long 0
Rdetect                 long 0
time                    long 0
temp                    long 0


cog                     byte 0


mem_addr                res 1
left_addr               res 1
right_addr              res 1
sensor_addr             res 1
_DUTY_addr              res 1
L_IR                    res 1
R_IR                    res 1
S_PIN                   res 1
D_PIN                   res 1

01_ProtoBotServos.spin ; 04_IRReceiver_RC6A.spin ; 04_IRReceiver_RC6A_DEMO.spin ; 04_ProtoBot.spin ; 04_QTIdriver.spin ; 04_QTIdriver_DEMO.spin ; 02_BlockSensor_PASM.spin ; 02_BlockSensor_PASM_DEMO.spin

Copper · 2014-01-12 15:23

Just thought I would update this thread real quick.

Slightly more intelligent code than before; but no huge changes there.

The new block sensor.
Unfortunately, I'm having some trouble getting the "distance" reading part or the code working. I say "distance" because I'm only concerned with knowing the block is close enough to pick up. So I don't care if the "distance value" is 3 or 50331648.

To test the "sensor" I basically copied the IR distance code from the PEkitlabs

''IrDetector.spin                                 '???
'NOTE 1.12.14:     max value == 94/256   ,     max detection value == 84/256     ,    min detection value == 3
'                  no block                    block at ~10in                         block at <=0.5in


CON 
 
  _clkmode = xtal1 + pll16x                  ' System clock &#8594; 80 MHz
  _xinfreq = 5_000_000


  scale = 16_777_216 ' 2³²÷ 256 


OBJ 


 pst : "Parallax Serial Terminal"


VAR 
 
 long anode, cathode, recPin, dMax, duty 


PUB MAIN


  TestIR
   
PUB TestIr | dist 


 ' Starts Parallax Serial Terminal; waits 1 s for you to click Enable button.  
 pst.Start(115200) 
 waitcnt(clkfreq*1+cnt)
 


 'Configure IR detectors.
 init(3, 4, 14)
 
 pst.Clear 
 pst.Str(string("Distance = "))


 'Get and display distance. 
 repeat
   pst.Str(string(pst#PX, 11))    
   dist := Distance               
   pst.Dec(dist)                  
   pst.Str(string("/256", pst#CE))
   waitcnt(clkfreq/3 + cnt)
 
PUB init(irLedAnode, irLedCathode, irReceiverPin) 
 
 anode := irLedAnode  
 cathode := irLedCathode 
 recPin := irReceiverPin
 
PUB distance : dist 


{{ Performs a duty sweep response test on the IR LED/receiver and returns dist, a zone 


value from 0 (closest) to 256 (no object detected). }}


 'Start 38 kHz signal.   
 frqb := 2040109
 ctrb := constant(100 << 26) + anode  
 dira[anode] := 1 
 
 'Configure Duty signal.  
 ctra[30..26] := 110 ' Set ctra to DUTY mode 
 ctra[5..0] := cathode ' Set ctra's APIN        
 frqa := phsa := 0 ' Set frqa register          
 dira[cathode] := 1 ' Set P5 to output 
                                                        
 dist := 0                                              
 repeat duty from 0 to 255 ' Sweep duty from 0 to 255 


    frqa := duty * scale ' Update frqa register 
    waitcnt(clkfreq/128000 + cnt) ' Delay for 1/128th s          <- What's this for???
    dist += ina[recPin] ' Object not detected? Add 1 to dist.

Naturally, I'm surprised by the results.

' ???
'max value == 94/256 , max detection value == 84/256 , min detection value == 3
'no block_____________block at ~10in_______________block at <=0.5in

I did find if I covered the IR emitter with a black Lego, the max value with no block jumped up to 256.
So I suppose I need to consider positioning the receiver further back from the edge of the bumper, or having the center LED stick out from the edge of the bumper. Otherwise, the coponents appear functional,

The problem I'm having however, does not seem like a stray light problem. Basically I would expect the distance value URL="http://forums.parallax.com/attachment.php?attachmentid=106124&d=1389566189"]05_BlockSensor_PASM_DEMO.spin[/URL to be equal to 4294967295 - 16777216*X where X == 1 through 255, but instead get numbers like

block stationary at ~1in

738197503
721420287
704643071
687865855
671088639
654311423
...
... after a while
...
-872415233
-889192449
-905969665
-922746881
-939524097

I think the problem has to be in here somewhere:

{   
        search Theory of Operation:
        ---------------------------
        search- Config cntr module for duty signal
                jmp to :begin


        :begin- call sampleCALL (L/R detection)
                check if block centered
                if centered, jmp to :distance
                if not, repeat (distance := -1)


        :distance- load max duty value and # of dincs
                   run into loop


        :loop- sub 1*dinc from duty
               load frqa with duty
               128th second delay (because there is on in the PElab)
               check sensor for 2ms
               if detected, store current duty (min duty @which block detected)
               sub 1 dinc, if dincs remain, repeat
}
DAT
search                  mov     ctra, DUTY_cntr                                 'set ctra for duty signal
                        mov     frqa, #0
                        mov     phsa, #0
                        mov     temp, #1
                        shl     temp, D_PIN
                        or      dira, temp


                        'detect block
:begin                  call    #sampleCALL                                     'sample sensor
                        rdlong  :on_left, left_addr                             'check left    
                        rdlong  :on_right, right_addr                           'check right  
                        or      :on_left, :on_right     wz, nr                  'check if left/right detected 
              if_z      jmp     #:distance                                      'if centered, 
              if_nz     wrlong  :top, center_addr                               'else, phony distance
              if_nz     jmp     #:begin                                         'repeat


                        'get distance reading
:distance               mov     :duty, :top                                     'load duty
                        mov     :dincs, #255                                    'load dincs (leaves 1*dinc of duty remaining)
                        mov     ctrb, C_cntr                                    'center IR -> ON
:loop                   sub     :duty, :dinc                                    'dinc duty signal 
                        mov     frqa, :duty                                     'set duty cycle
                        mov     :duty_time, :duty_delay       ' 128th second delay
                        add     :duty_time, cnt               ' In PElab.
                        waitcnt :duty_time, #0                ' Why?                             
                        mov     temp, #1                                        'set temp:=1    
                        mov     time, cnt                                       'set up wait loop
                        add     time, _space            wc                      'add _space to time; set C                           
              if_c      jmp     #:rollover                                      'if C, jmp to rollover                         
:check_sensor           and     S_PINmask, ina          wz, nr                  'check sensor
              if_z      mov     temp, #0                                        'if detected, set := 0                                       
                        cmp     time, cnt               wc                      'check time    
              if_nc     jmp     #:check_sensor                                  'if cnt < time, keep checking sensor
              if_c      jmp     #:next                                          'if cnt > time, next
:rollover               and     S_PINmask, ina          wz, nr                  'check sesnor (rollover)
              if_z      mov     temp, #0                                        'if detected, set := 0                                       
                        cmp     time, cnt               wc                      'check time  
              if_c      jmp     #:rollover                                      'if cnt rolled over, jmp to check sensor       
              if_nc     jmp     #:check_sensor                                  'if not, jmp to rollover
:next                   cmp     temp, #0                wz                      'check if detected
              if_z      wrlong  :duty, center_addr                              'if detected, report distance 
                        djnz    :dincs, #:loop                                  'decrement dincs, and loop until dincs==0
                        jmp     #:begin                                         'back to begining 
                        
:duty                   long 0
:top                    long 4294967295                                         ' 2³² == 4294967296 
:dinc                   long 16777216                                           ' :dinc * 256 == 2³²      
:dincs                  long 255
:on_left                long 0
:on_right               long 0


:duty_delay             long 625
:duty_time              long 0

Also , after reading this think I might replace the Panasonic PNA4601M IR receiver with a Vishay TSOP4038.

erco · 2014-01-12 16:34

Copper wrote: »

Also , after reading this think I might replace the Panasonic PNA4601M IR receiver with a Vishay TSOP4038.

The 4038 has a new trick: it can swag intensity/relative distance. I have not seen anyone use this trick yet. From http://www.vishay.com/company/press/releases/2010/100628irsensors :

Many other applications require a reflective sensor that detects not only presence but also proximity by measuring the strength or weakness of the reflected signal. Instead of a fixed detection threshold, analog information from the sensor is needed. This is possible with the TSOP4038, TSOP58038, and TSOP58P38 IR proximity sensors. The length of the sensor’s output pulse in response to the emitter signal varies in proportion to the amount of light reflected from the object being detected. For near objects, the output pulse approaches 100 % of the emitted pulse, for far objects the output pulse becomes shorter.

Protobot (My first robot)

Comments