SHARE
TWEET

TCS3200

a guest Sep 20th, 2019 94 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2.  
  3. # 2015-07-03
  4. # TCS3200.py
  5. # Public Domain
  6.  
  7. import time
  8. import threading
  9.  
  10. import pigpio
  11.  
  12. class sensor(threading.Thread):
  13.    """
  14.    This class reads RGB values from a TCS3200 colour sensor.
  15.  
  16.    GND   Ground.
  17.    VDD   Supply Voltage (2.7-5.5V)
  18.    /OE   Output enable, active low. When OE is high OUT is disabled
  19.          allowing multiple sensors to share the same OUT line.
  20.    OUT   Output frequency square wave.
  21.    S0/S1 Output frequency scale selection.
  22.    S2/S3 Colour filter selection.
  23.  
  24.    OUT is a square wave whose frequency is proprtional to the
  25.    intensity of the selected filter colour.
  26.  
  27.    S2/S3 selects between red, green, blue, and no filter.
  28.  
  29.    S0/S1 scales the frequency at 100%, 20%, 2% or off.
  30.  
  31.    To take a reading the colour filters are selected in turn for a
  32.    fraction of a second and the frequency is read and converted to
  33.    Hz.  
  34.    """
  35.    def __init__(self, pi, OUT, S2, S3, S0=None, S1=None, OE=None):
  36.       """
  37.       The gpios connected to the sensor OUT, S2, and S3 pins must
  38.       be specified.  The S0, S1 (frequency) and OE (output enable)
  39.       gpios are optional.
  40.       """
  41.       threading.Thread.__init__(self)
  42.       self._pi = pi
  43.  
  44.       self._OUT = OUT
  45.       self._S2 = S2
  46.       self._S3 = S3
  47.  
  48.       self._mode_OUT = pi.get_mode(OUT)
  49.       self._mode_S2 = pi.get_mode(S2)
  50.       self._mode_S3 = pi.get_mode(S3)
  51.  
  52.       pi.write(OUT, 0) # Disable frequency output.
  53.       pi.set_mode(S2, pigpio.OUTPUT)
  54.       pi.set_mode(S3, pigpio.OUTPUT)
  55.  
  56.       self._S0 = S0
  57.       self._S1 = S1
  58.       self._OE = OE
  59.  
  60.       if (S0 is not None) and (S1 is not None):
  61.          self._mode_S0 = pi.get_mode(S0)
  62.          self._mode_S1 = pi.get_mode(S1)
  63.          pi.set_mode(S0, pigpio.OUTPUT)
  64.          pi.set_mode(S1, pigpio.OUTPUT)
  65.  
  66.       if OE is not None:
  67.          self._mode_OE = pi.get_mode(OE)
  68.          pi.set_mode(OE, pigpio.OUTPUT)
  69.          pi.write(OE, 0) # Enable device (active low).
  70.  
  71.       self.set_sample_size(20)
  72.  
  73.       self.set_update_interval(1.0) # One reading per second.
  74.  
  75.       self.set_frequency(1) # 2%
  76.  
  77.       self._set_filter(3) # Clear.
  78.  
  79.       self._rgb_black = [0]*3
  80.       self._rgb_white = [10000]*3
  81.  
  82.       self.hertz=[0]*3 # Latest triplet.
  83.       self._hertz=[0]*3 # Current values.
  84.  
  85.       self.tally=[1]*3 # Latest triplet.
  86.       self._tally=[1]*3 # Current values.
  87.  
  88.       self._delay=[0.1]*3 # Tune delay to get _samples pulses.
  89.  
  90.       self._cycle = 0
  91.  
  92.       self._cb_OUT = pi.callback(OUT, pigpio.RISING_EDGE, self._cbf)
  93.       self._cb_S2 = pi.callback(S2, pigpio.EITHER_EDGE, self._cbf)
  94.       self._cb_S3 = pi.callback(S3, pigpio.EITHER_EDGE, self._cbf)
  95.  
  96.       self.daemon = True
  97.  
  98.       self.start()
  99.  
  100.    def cancel(self):
  101.       """
  102.       Cancel the sensor and release resources.
  103.       """
  104.       self._cb_S3.cancel()
  105.       self._cb_S2.cancel()
  106.       self._cb_OUT.cancel()
  107.  
  108.       self.set_frequency(0) # off
  109.  
  110.       self._set_filter(3) # Clear
  111.  
  112.       self._pi.set_mode(self._OUT, self._mode_OUT)
  113.       self._pi.set_mode(self._S2, self._mode_S2)
  114.       self._pi.set_mode(self._S3, self._mode_S3)
  115.  
  116.       if (self._S0 is not None) and (self._S1 is not None):
  117.          self._pi.set_mode(self._S0, self._mode_S0)
  118.          self._pi.set_mode(self._S1, self._mode_S1)
  119.  
  120.       if self._OE is not None:
  121.          self._pi.write(self._OE, 1) # disable device
  122.          self._pi.set_mode(self._OE, self._mode_OE)
  123.  
  124.    def get_rgb(self, top=255):
  125.       """
  126.       Get the latest RGB reading.
  127.  
  128.       The raw colour hertz readings are converted to RGB values
  129.       as follows.
  130.  
  131.       RGB = 255 * (Fv - Fb) / (Fw - Fb)
  132.  
  133.       Where Fv is the sampled hertz, Fw is the calibrated
  134.       white hertz, and Fb is the calibrated black hertz.
  135.  
  136.       By default the RGB values are constrained to be between
  137.       0 and 255.  A different upper limit can be set by using
  138.       the top parameter.
  139.       """
  140.       rgb = [0]*3
  141.       for c in range(3):
  142.          v = self.hertz[c] - self._rgb_black[c]
  143.          s = self._rgb_white[c] - self._rgb_black[c]
  144.          p = top * v / s
  145.          if p < 0:
  146.             p = 0
  147.          elif p > top:
  148.             p = top
  149.          rgb[c] = p
  150.       return rgb[:]
  151.  
  152.    def get_hertz(self):
  153.       """
  154.       Get the latest hertz reading.
  155.       """
  156.       return self.hertz[:]
  157.  
  158.    def set_black_level(self, rgb):
  159.       """
  160.       Set the black level calibration.
  161.       """
  162.       for i in range(3):
  163.          self._rgb_black[i] = rgb[i]
  164.  
  165.    def get_black_level(self):
  166.       """
  167.       Get the black level calibration.
  168.       """
  169.       return self._rgb_black[:]
  170.  
  171.    def set_white_level(self, rgb):
  172.       """
  173.       Set the white level calibration.
  174.       """
  175.       for i in range(3):
  176.          self._rgb_white[i] = rgb[i]
  177.  
  178.    def get_white_level(self):
  179.       """
  180.       Get the white level calibration.
  181.       """
  182.       return self._rgb_white[:]
  183.  
  184.    def set_frequency(self, f):
  185.       """
  186.       Set the frequency scaling.
  187.  
  188.       f  S0  S1  Frequency scaling
  189.       0  L   L   Off
  190.       1  L   H   2%
  191.       2  H   L   20%
  192.       3  H   H   100%
  193.       """
  194.       if f == 0: # off
  195.          S0 = 0; S1 = 0
  196.       elif f == 1: # 2%
  197.          S0 = 0; S1 = 1
  198.       elif f == 2: # 20%
  199.          S0 = 1; S1 = 0
  200.       else: # 100%
  201.          S0 = 1; S1 = 1
  202.  
  203.       if (self._S0 is not None) and (self._S1 is not None):
  204.          self._frequency = f
  205.          self._pi.write(self._S0, S0)
  206.          self._pi.write(self._S1, S1)
  207.       else:
  208.          self._frequency = None
  209.  
  210.    def get_frequency(self):
  211.       """
  212.       Get the current frequency scaling.
  213.       """
  214.       return self._frequency
  215.  
  216.    def set_update_interval(self, t):
  217.       """
  218.       Set the interval between RGB updates.
  219.       """
  220.       if (t >= 0.1) and (t < 2.0):
  221.          self._interval = t
  222.  
  223.    def get_update_interval(self):
  224.       """
  225.       Get the interval between RGB updates.
  226.       """
  227.       return self._interval
  228.  
  229.    def set_sample_size(self, samples):
  230.       """
  231.       Set the sample size (number of frequency cycles to accumulate).
  232.       """
  233.       if samples < 10:
  234.          samples = 10
  235.       elif samples > 100:
  236.          samples = 100
  237.  
  238.       self._samples = samples
  239.  
  240.    def get_sample_size(self):
  241.       """
  242.       Get the sample size.
  243.       """
  244.       return self._samples
  245.  
  246.    def pause(self):
  247.       """
  248.       Pause reading (until a call to resume).
  249.       """
  250.       self._read = False
  251.  
  252.    def resume(self):
  253.       """
  254.       Resume reading (after a call to pause).
  255.       """
  256.       self._read = True
  257.  
  258.    def _set_filter(self, f):
  259.       """
  260.       Set the colour to be sampled.
  261.  
  262.       f  S2  S3  Photodiode
  263.       0  L   L   Red
  264.       1  H   H   Green
  265.       2  L   H   Blue
  266.       3  H   L   Clear (no filter)
  267.       """
  268.       if f == 0: # Red
  269.          S2 = 0; S3 = 0
  270.       elif f == 1: # Green
  271.          S2 = 1; S3 = 1
  272.       elif f == 2: # Blue
  273.          S2 = 0; S3 = 1
  274.       else: # Clear
  275.          S2 = 1; S3 = 0
  276.  
  277.       self._pi.write(self._S2, S2); self._pi.write(self._S3, S3)
  278.  
  279.    def _cbf(self, g, l, t):
  280.  
  281.       if g == self._OUT: # Frequency counter.
  282.          if self._cycle == 0:
  283.             self._start_tick = t
  284.          else:
  285.             self._last_tick = t
  286.          self._cycle += 1
  287.  
  288.       else: # Must be transition between colour samples.
  289.          if g == self._S2:
  290.             if l == 0: # Clear -> Red.
  291.                self._cycle = 0
  292.                return
  293.             else:      # Blue -> Green.
  294.                colour = 2
  295.          else:
  296.             if l == 0: # Green -> Clear.
  297.                colour = 1
  298.             else:      # Red -> Blue.
  299.                colour = 0
  300.  
  301.          if self._cycle > 1:
  302.             self._cycle -= 1
  303.             td = pigpio.tickDiff(self._start_tick, self._last_tick)
  304.             self._hertz[colour] = (1000000 * self._cycle) / td
  305.             self._tally[colour] = self._cycle
  306.          else:
  307.             self._hertz[colour] = 0
  308.             self._tally[colour] = 0
  309.  
  310.          self._cycle = 0
  311.  
  312.          # Have we a new set of RGB?
  313.          if colour == 1:
  314.             for i in range(3):
  315.                self.hertz[i] = self._hertz[i]
  316.                self.tally[i] = self._tally[i]
  317.  
  318.    def run(self):
  319.       self._read = True
  320.       while True:
  321.          if self._read:
  322.  
  323.             next_time = time.time() + self._interval
  324.  
  325.             self._pi.set_mode(self._OUT, pigpio.INPUT) # Enable output gpio.
  326.  
  327.             # The order Red -> Blue -> Green -> Clear is needed by the
  328.             # callback function so that each S2/S3 transition triggers
  329.             # a state change.  The order was chosen so that a single
  330.             # gpio changes state between each colour to be sampled.
  331.  
  332.             self._set_filter(0) # Red
  333.             time.sleep(self._delay[0])
  334.  
  335.             self._set_filter(2) # Blue
  336.             time.sleep(self._delay[2])
  337.  
  338.             self._set_filter(1) # Green
  339.             time.sleep(self._delay[1])
  340.  
  341.             self._pi.write(self._OUT, 0) # Disable output gpio.
  342.  
  343.             self._set_filter(3) # Clear
  344.  
  345.             delay = next_time - time.time()
  346.  
  347.             if delay > 0.0:
  348.                time.sleep(delay)
  349.  
  350.             # Tune the next set of delays to get reasonable results
  351.             # as quickly as possible.
  352.  
  353.             for c in range(3):
  354.  
  355.                # Calculate dly needed to get _samples pulses.
  356.  
  357.                if self.hertz[c]:
  358.                   dly = self._samples / float(self.hertz[c])
  359.                else: # Didn't find any edges, increase sample time.
  360.                   dly = self._delay[c] + 0.1
  361.  
  362.                # Constrain dly to reasonable values.
  363.  
  364.                if dly < 0.001:
  365.                   dly = 0.001
  366.                elif dly > 0.5:
  367.                   dly = 0.5
  368.  
  369.                self._delay[c] = dly
  370.  
  371.          else:
  372.             time.sleep(0.1)
  373.  
  374. if __name__ == "__main__":
  375.  
  376.    import sys
  377.  
  378.    import pigpio
  379.  
  380.    import TCS3200
  381.  
  382.    RED=21
  383.    GREEN=20
  384.    BLUE=16
  385.  
  386.    def wait_for_return(str):
  387.       if sys.hexversion < 0x03000000:
  388.          raw_input(str)
  389.       else:
  390.          input(str)
  391.  
  392.    pi = pigpio.pi()
  393.  
  394.    s = TCS3200.sensor(pi, 24, 22, 23, 4, 17, 18)
  395.  
  396.    s.set_frequency(2) # 20%
  397.  
  398.    interval = 0.2
  399.  
  400.    s.set_update_interval(interval)
  401.  
  402.    wait_for_return("Calibrating black object, press RETURN to start")
  403.  
  404.    for i in range(5):
  405.       time.sleep(interval)
  406.       hz = s.get_hertz()
  407.       print(hz)
  408.    s.set_black_level(hz)
  409.  
  410.    wait_for_return("Calibrating white object, press RETURN to start")
  411.  
  412.    for i in range(5):
  413.       time.sleep(interval)
  414.       hz = s.get_hertz()
  415.       print(hz)
  416.    s.set_white_level(hz)
  417.  
  418.    try:
  419.       while True:
  420.          rgb = s.get_rgb()
  421.  
  422.          pi.set_PWM_dutycycle(RED, rgb[0])
  423.          pi.set_PWM_dutycycle(GREEN, rgb[1])
  424.          pi.set_PWM_dutycycle(BLUE, rgb[2])
  425.  
  426.          print(rgb, s.get_hertz(), s.tally)
  427.  
  428.          time.sleep(interval)
  429.  
  430.    except:
  431.  
  432.       print("cancelling")
  433.       s.cancel()
  434.       pi.stop()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top