from machine import Pin, PWM,ADC,time_pulse_us
'''
_____________________________
|frequency scaling|S0  |S1  |
|-----------------|----|----|
|2%               |LOW |HIGH|
|20%              |HIGH|LOW |
|100%             |HIGH|HIGH|
-----------------------------
'''
#defining the pin for sensor
S0=Pin(9, Pin.OUT)
S1=Pin(8, Pin.OUT)
S2=Pin(11, Pin.OUT)
S3=Pin(10, Pin.OUT)
ssout=Pin(12, Pin.IN)
#defining pins for motors 
IN1=Pin(1,Pin.OUT)
IN2=Pin(2,Pin.OUT)
EnA = PWM(Pin(3))
#for potentiometer
potPin=ADC(Pin(4))
#defining pins for solenoid
SP1=Pin(6,Pin.OUT)
SP2=Pin(4,Pin.OUT)
#setting values of sensor frequencies as 0 to avoid garbage values
rf=0
gf=0
bf=0
# Setting the sensor frequency scaling to 20% for better speed
S0.value(1)
S1.value(0)
def readcolor():
    '''
 _____________________________
|color fitlers     |S2  |S3  |
|------------------|----|----|
|RED               |LOW |LOW |
|BLUE              |LOW |HIGH|
|GREEN             |HIGH|HIGH|
------------------------------
    '''
    
    S2.off()
    S3.off()
    redfreq=time_pulse_us(senout,0)

    S2.on()
    S3.on()
    greenfreq=time_pulse_us(senout,0)

    S2.off()
    S3.on()
    bluefreq=time_pulse_us(senout,0)

    return redfreq,greenfreq,bluefreq

def map_value(x,in_min,in_max,out_min,out_max):
    return int((x-in_min)*(out_max-out_min)/(in_max-in_min)+out_min)
def motor():
    pot_value = potPin.read_u16()  # Read potentiometer value (0-65535)

    IN1.value(1)  # Set motor direction
    IN2.value(0)

    EnA.duty_u16(pot_value)  # Set motor speed

while True:
    red,green,blue=readcolor()
    #map the frequencyvalues
    red_mapped=map_value(red,25,70,255,0)
    green_mapped=map_value(green,30,80,250,0)
    blue_mapped=map_value(blue,25,70,255,0)
    #print rgb values
    print("r=",red_mapped)
    print("g=",green_mapped)  
    print("b=",blue_mapped)
    
    # Run motor control function
    motor()
    
    #delay for one sec for stability
    time.sleep(1)