github.com/simpleiot/simpleiot@v0.18.3/sensors/tof10120.go (about)

     1  package sensors
     2  
     3  import (
     4  	"errors"
     5  	"fmt"
     6  	"io"
     7  	"regexp"
     8  	"strconv"
     9  	"strings"
    10  )
    11  
    12  // The TOF10120 is a Laser distance sensing (TOF) module
    13  // Chinese datasheet and Google translate versions of the datasheet
    14  // can be found: https://github.com/simpleiot/reference/tree/master/sensors
    15  
    16  // Wiring of TOF10120 to FTDI cable:
    17  // TOF10120	  FTDI
    18  // 1 (black, GND) 1 (blk, GND)
    19  // 2 (red, VDD)	  3 (red, VCC)
    20  // 3 (yel, RXD)	  4 (org, TXD)
    21  // 4 (wht, TXD)	  5 (yel, RXD)
    22  
    23  // TOF10120 is a driver for a TOF10120 sensor
    24  type TOF10120 struct {
    25  	port io.ReadWriter
    26  }
    27  
    28  // NewTOF10120 creates a instance to initialize and read the TOF sensor
    29  // port must return an entire packet for each Read().
    30  // github.com/simpleiot/simpleiot/respreader is a good
    31  // way to do this.
    32  func NewTOF10120(port io.ReadWriter) *TOF10120 {
    33  	return &TOF10120{
    34  		port: port,
    35  	}
    36  }
    37  
    38  var re = regexp.MustCompile(`^([0-9]*)mm`)
    39  
    40  // SetSendInterval sets the interval at which sensor sends data
    41  // 10-9999ms, default 100ms
    42  // this should be called before Read() is started
    43  func (tof *TOF10120) SetSendInterval(interval int) error {
    44  	// try to fit this between two send intervals
    45  	buf := make([]byte, 100)
    46  	c, err := tof.port.Read(buf)
    47  
    48  	if err != nil {
    49  		return err
    50  	}
    51  
    52  	if c <= 0 {
    53  		return errors.New("Sensor does not seem to be active")
    54  	}
    55  
    56  	wr := fmt.Sprintf("s2-%v#", interval)
    57  
    58  	_, err = tof.port.Write([]byte(wr))
    59  
    60  	if err != nil {
    61  		return err
    62  	}
    63  
    64  	c, err = tof.port.Read(buf)
    65  	if err != nil {
    66  		return err
    67  	}
    68  
    69  	if c <= 0 {
    70  		return errors.New("Sensor did not return any data after write")
    71  	}
    72  
    73  	buf = buf[:c]
    74  
    75  	if !strings.Contains(string(buf), "ok") {
    76  		return errors.New("Sensor did not return ok")
    77  	}
    78  
    79  	return nil
    80  }
    81  
    82  // Read returns the distance in mm. The sensor continuously ouputs
    83  // readings so the callback is called each time a new reading is
    84  // read.
    85  func (tof *TOF10120) Read(dataCallback func(dist int),
    86  	errCallback func(err error)) error {
    87  	for {
    88  		if tof.port == nil {
    89  			return errors.New("no port")
    90  		}
    91  
    92  		buf := make([]byte, 100)
    93  		c, err := tof.port.Read(buf)
    94  		if err != nil {
    95  			if err != io.EOF {
    96  				return err
    97  			}
    98  		}
    99  
   100  		if c <= 0 {
   101  			continue
   102  		}
   103  
   104  		buf = buf[:c]
   105  
   106  		matches := re.FindSubmatch(buf)
   107  
   108  		if len(matches) < 2 {
   109  			errCallback(errors.New("error parsing data"))
   110  			continue
   111  		}
   112  
   113  		v, err := strconv.Atoi(string(matches[1]))
   114  
   115  		if err != nil {
   116  			errCallback(err)
   117  			continue
   118  		}
   119  
   120  		dataCallback(v)
   121  	}
   122  }