legoEv3/save/home/robot/motorIR2.py

#EV3 demo code to demonstrate basic robot functions and use of ultrasonic sensor
#Copyright Berend Rah 2016
#as shown on Youtube
#https://www.youtube.com/watch?v=pe8zZEnMTGA


from ev3dev.auto import *
from ev3dev2.led import Leds
from ev3dev2.button import *
import time
from datetime import datetime, timedelta
import random

def stop():
        motors[0].stop(stop_action='brake')
        motors[1].stop(stop_action='brake')

motors = [LargeMotor(address) for address in (OUTPUT_B, OUTPUT_C)]
print("motors", motors)
print("conn", [m.connected for m in motors])
assert all([m.connected for m in motors]), \
    "Two medium motors should be connected to ports A and B"
ir=InfraredSensor()
ir.mode='IR-PROX'
print("ir", ir, 'modes', ir.modes, 'mode', ir.mode, 'nul_values', ir.num_values, 'bin_data_format', ir.bin_data_format)

lft = 1
rgt = 1 - lft
leds = Leds()

but = Button()
Button.evdev_device_name = 'EV3 Brick Buttons' # Constant in Button has wrong case, still aber updgrade ... !!!!


def onePoint(moL, moR, lim, sps):
    while True:
        if but.backspace:
            print('backspace pressed')
            stop()
            leds.set_color("LEFT", "BLACK")
            exit()
        p = ir.value()
        print('prox', p, datetime.now())
        if p < lim: # turn fast away from wall
            leds.set_color("LEFT", "RED")
            sx = 0
        else: # turn slowly to wall
            leds.set_color("LEFT", "GREEN")
            sx = 2    
        moL.run_timed(time_sp=1000, speed_sp=sps[sx])
        moR.run_timed(time_sp=1000, speed_sp=sps[sx+1])
        time.sleep(0.3)

def twoPoint(moL, moR, li1, li2, sps):
    while True:
        if but.any():
            print('buttons pressed', but.buttons_pressed)
            stop()
            leds.set_color("LEFT", "BLACK")
            time.sleep(5)
            exit()
        p = ir.value()
        print('prox', p, datetime.now())
        if p < li1: # turn fast away from wall
            leds.set_color("LEFT", "RED")
            w0 = 1
        elif p >= li2: # turn slowly to wall
            leds.set_color("LEFT", "GREEN")
            w0 = 0
        else: # turn slowly to wall
            leds.set_color("LEFT", "YELLOW")
            w0 = (li2-p) / (li2 - li1)
        w1 = 1 - w0    
        print('prox', p, 'w0', w0,  datetime.now())
        moL.run_timed(time_sp=1000, speed_sp=sps[0] * w0 + sps[2] * w1)
        moR.run_timed(time_sp=1000, speed_sp=sps[1] * w0 + sps[3] * w1)
        time.sleep(0.3)

leds.set_color("RIGHT", "BLACK")

#onePoint(motors[0], motors[1], 70, [600, -300, 500, 600])
twoPoint(motors[0], motors[1], 65, 75, [600, -300, 500, 600])

while True:
    if but.backspace:
        print('backspace pressed')
        stop()
        leds.set_color("LEFT", "BLACK")
        exit()
    p = ir.value()
    if p < 70: # turn fast away from wall
        motors[0].run_timed(time_sp=1000, speed_sp=-300 + lft * 900)
        motors[1].run_timed(time_sp=1000, speed_sp= -300 +rgt * 900)
        leds.set_color("LEFT", "RED")
    else: # turn slowly to wall
        motors[0].run_timed(time_sp=1000, speed_sp=500 + 100 * rgt)
        motors[1].run_timed(time_sp=1000, speed_sp=500 + 100 * lft)
        leds.set_color("LEFT", "GREEN")
    time.sleep(0.3)
def turnRight():
        motors[0].run_timed(time_sp=1000, speed_sp=600)
        motors[1].run_timed(time_sp=1000, speed_sp=-600)
def forward():
        motors[0].run_timed(time_sp=10000, speed_sp=700)
        motors[1].run_timed(time_sp=10000, speed_sp=1000)



# turnRight()

print('forward 10')
forward()
print('forwarded 10')
time.sleep(3)
print('stopping after 1')
stop()
print('stopped')
exit()



btn=Button()

r=random


ir=InfraredSensor()
print("ir", ir, 'modes', ir.modes, 'mode', ir.mode, 'nul_values', ir.num_values, 'bin_data_format', ir.bin_data_format)
ir.mode='IR-SEEK'
ir.mode='IR-PROX'
print("ir", ir, 'modes', ir.modes, 'mode', ir.mode, 'nul_values', ir.num_values, 'bin_data_format', ir.bin_data_format)
#while 1:
#    n = ir.proximity
#    print(str(datetime.now()) + " ir proximity " + repr(n))
#    time.sleep(0.0001)

cnt=0
old=-99
while True:
    n = ir.value()
    cnt += 1
    if old != n:
        print(str(repr(n)), 'cnt', cnt, datetime.now())
        old = n
    time.sleep(0.0001)

while 1:
    nn = []
    nL = 7
    for i in range(nL - 1):
        nn.append(ir.proximity)
        time.sleep(0.01)
    nn.append(ir.proximity)
    # print('nn', nn)
    ns = sorted(nn)
    # print('ns', ns)
    n = ns[nL //2]
    print(n, 'middle of', ns[0], ns[nL-1], str(datetime.now()))
    if n > 50:
        forward()
    else:
        stop()
    time.sleep(0.8)
while 1:
    n = ir.proximity
    if old != n:
        print(str(datetime.now()) + " ir proximity " + repr(n))
        old = n
    time.sleep(1)




IS.mode='US-DC-CM'                  #this is continuous mode USSmodes will display all available modes
exit();
#distance is in mm NOT cm

def mDistance():
    
    try:
        return USS.value()
        time.sleep(0.3)
    
    except IOerror:   #catch connection errors
        
        pass
        
def moveThreeSecs():

        for m in motors:
                m.run_timed(time_sp=3000, speed_sp=600) # run for 3 seconds

def turnRight():
        motors[0].run_timed(time_sp=1000, speed_sp=-600)
        motors[1].run_timed(time_sp=1000, speed_sp=600)

def turnLeft():
        motors[0].run_timed(time_sp=1000, speed_sp=600)
        motors[1].run_timed(time_sp=1000, speed_sp=-600)
        


while not btn.any():

    dist=mDistance()

    if dist <1000:  #distance less than 1 meter
                choice=r.randint(0,2)
                if choice==0:
                        moveThreeSecs()
                elif choice==1:
                        turnRight()
                else:
                        turnLeft()

                time.sleep(0.3)