from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.sensor.lego import Sensor
from ev3dev2.sensor import INPUT_1, INPUT_2
from ev3dev2.display import Display
lcd = Display()
# Connect Pixy camera
pixy = Sensor()
# Connect TouchSensor
ts = TouchSensor(address=INPUT_1)
# Set mode
pixy.mode = 'ALL'
while not ts.value():
lcd.clear()
if pixy.value(0) != 0: # Object with SIG1 detected
x = pixy.value(1)
y = pixy.value(2)
w = pixy.value(3)
h = pixy.value(4)
dx = int(w / 2) # Half of the width of the rectangle
dy = int(h / 2) # Half of the height of the rectangle
xb = x + int(w / 2) # X-coordinate of bottom-right corner
yb = y - int(h / 2) # Y-coordinate of the bottom-right corner
lcd.draw.rectangle((dx, dy, xb, yb), fill='black')
lcd.update()
for i in range(0, 4):
print(pixy.value(i), file=stderr)
print(
"#######################################################################")
print(
"################ starting in compass mode #########################")
print(
"#######################################################################")
imu.mode = 'COMPASS'
compassVal = 0
prev_compassVal = 0
startTime = time.time()
while (time.time() - startTime < 55):
compassVal = imu.value(0)
if compassVal != prev_compassVal:
print("compassVal = ", compassVal)
prev_compassVal = compassVal
time.sleep(0.05) # Give the CPU a rest
print(
"#######################################################################")
print(
"################# switching to GYRO mode ##########################")
print(
"#######################################################################")
imu.mode = 'GYRO'
gyroVal = 0
if printVerbose > 0:
print("usDistCmVal = ", int(usDistCmVal), " irProxVal = ",
irProxVal, " compassVal = ", compassVal
) # print all sensor vals, regardless of which changed
prev_irProxVal = irProxVal
# prev_irHeadVal = irHeadVal
# if beacon has been found, set beaconLock True
# if irDistVal is not None:
# if irDistVal < 100:
# beaconLock = True
# if irDistVal == 100:
# beaconLock = False
# else:
# beaconLock = False
compassVal = cmp.value(0)
if compassVal != prev_compassVal:
# print("usDistCmVal = ", usDistCmVal, " irDistVal = ", irDistVal, " irHeadVal = ", irHeadVal, " compassVal = ", compassVal) # print all sensor vals, regardless of which changed
if printVerbose > 0:
print("usDistCmVal = ", int(usDistCmVal), " irProxVal = ",
irProxVal, " compassVal = ", compassVal
) # print all sensor vals, regardless of which changed
prev_compassVal = compassVal
# if beaconLock is False and we're outside the compass North range, reset cmpGenHeading to False
if compassVal < (compassGoal - 80) or compassVal > (compassGoal +
80):
cmpGenHeading = False
### if usDistCmVal < 50:
if usDistCmVal < 5: ### changed from '10' to '5' for US data accuracy testing
if obstacleNear == False:
time.sleep(0.5)
print("#######################################################################")
print("################ starting in gyro mode #########################")
print("#######################################################################")
gyro.mode = 'GYRO-ANG'
gyroVal = 0
prev_gyroVal = 0
startTime = time.time()
while (time.time() - startTime < 55):
gyroVal = gyro.value(0)
if gyroVal != prev_gyroVal:
print("gyroVal = ", gyroVal)
prev_gyroVal = gyroVal
time.sleep (0.05) # Give the CPU a rest
print("#######################################################################")
print("################# switching to GYRO-RATE mode #####################")
print("#######################################################################")
gyro.mode = 'GYRO-RATE'
gyroVal = 0
prev_gyroVal = 0
level=logging.INFO,
format='%(asctime)s:%(levelname)s:%(threadName)s:%(message)s')
log = logging.getLogger(__name__)
port1 = INPUT_7
port4 = INPUT_8
legoPort1 = LegoPort(port1)
legoPort1.mode = 'nxt-i2c'
legoPort1.set_device = 'ms-absolute-imu 0x11'
# legoPort4 = LegoPort(port4)
# legoPort4.mode = 'nxt-i2c'
# legoPort4.set_device = 'ms-absolute-imu 0x11'
print('Pausing 2 seconds...')
time.sleep(2)
print('Done, creating sensor class object for {}, {}'.format(port1, port4))
sensor1 = Sensor(address='{}:i2c17'.format(port1))
# sensor4 = Sensor(address='{}:i2c17'.format(INPUT_4))
target_mode = 'ALL'
sensor1.mode = target_mode
# sensor4.mode = target_mode
while True:
# log.info('Joint 3 ({}) X/Y/Z angle = {}/{}/{}'.format(INPUT_4, sensor4.value(0), sensor4.value(1), sensor4.value(2)))
log.info('Joint 4 ({}) X/Y/Z angle = {}/{}/{}'.format(
port1, sensor1.value(0), sensor1.value(1), sensor1.value(2)))
time.sleep(0.125)
log.info('Done')
print("wait 7")
print("")
print("")
# allow for some time to stabilze
time.sleep(3)
miVal = 0
prev_miVal = 18001
### For testing
print("mi driver name is... ", mi.driver_name)
print("mi address is... ", mi.address)
print("mi mode is... ", mi.mode)
print("mi available modes are... ", mi.modes)
print("mi available commands are... ", mi.commands)
print("")
print(
"#######################################################################")
print(
"#################### read mi #############################")
print(
"#######################################################################")
while (1):
miVal = mi.value(0)
print("miVal = ", miVal)
time.sleep(1) # Give the CPU a rest
compass = Sensor(INPUT_1)
# allow for some time to load the new drivers
# time.sleep(0.5)
compassVal = 0
prev_compassVal = 0
print(
"#######################################################################")
print(
"#################### read compass #############################")
print(
"#######################################################################")
startTime = time.time()
while (time.time() - startTime < 55):
compassVal = compass.value(0)
if compassVal != prev_compassVal:
print("compassVal = ", compassVal)
prev_compassVal = compassVal
time.sleep(0.05) # Give the CPU a rest
print(
"#######################################################################")
print(
"#################### time's up - exiting #########################")
print(
"#######################################################################")
def master_function():
cl = ColorSensor()
ts = TouchSensor()
ir = InfraredSensor()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
medium_motor = MediumMotor()
pixy = Sensor(address=INPUT_2)
# assert pixy.connected, "Error while connecting Pixy camera to port 1"
# pixy.mode = 'SIG1'
# lcd = Sensor.Screen()
lcd = Display()
def top_left_channel_1_action(state):
move()
def bottom_left_channel_1_action(state):
move()
def top_right_channel_1_action(state):
move()
def bottom_right_channel_1_action(state):
move()
def move():
buttons = ir.buttons_pressed() # a list
if len(buttons) == 1:
medium_motor.off()
if buttons == ['top_left']:
steer_pair.on(steering=0, speed=40)
elif buttons == ['bottom_left']:
steer_pair.on(steering=0, speed=-40)
elif buttons == ['top_right']:
steer_pair.on(steering=100, speed=30)
elif buttons == ['bottom_right']:
steer_pair.on(steering=-100, speed=30)
elif len(buttons) == 2:
steer_pair.off()
if buttons == ['top_left', 'top_right']:
medium_motor.on(speed_pct=10)
elif buttons == ['bottom_left', 'bottom_right']:
medium_motor.on(speed_pct=-10)
else: # len(buttons)==0
medium_motor.off()
steer_pair.off()
# Associate the event handlers with the functions defined above
ir.on_channel1_top_left = top_left_channel_1_action
ir.on_channel1_bottom_left = bottom_left_channel_1_action
ir.on_channel1_top_right = top_right_channel_1_action
ir.on_channel1_bottom_right = bottom_right_channel_1_action
opts = '-a 200 -s 150 -p 70 -v'
speech_pause = 0
while not ts.is_pressed:
# rgb is a tuple containing three integers
ir.process()
red = cl.rgb[0]
green = cl.rgb[1]
blue = cl.rgb[2]
intensity = cl.reflected_light_intensity
print('{4} Red: {0}\tGreen: {1}\tBlue: {2}\tIntensity: {3}'.format(
str(red), str(green), str(blue), str(intensity), speech_pause),
file=stderr)
lcd.clear()
print(pixy.mode, file=stderr)
if pixy.value(0) != 0: # Object with SIG1 detected
x = pixy.value(1)
y = pixy.value(2)
w = pixy.value(3)
h = pixy.value(4)
dx = int(w / 2) # Half of the width of the rectangle
dy = int(h / 2) # Half of the height of the rectangle
xb = x + int(w / 2) # X-coordinate of bottom-right corner
yb = y - int(h / 2) # Y-coordinate of the bottom-right corner
lcd.draw.rectangle((xa, ya, xb, yb), fill='black')
lcd.update()
speech_pause += 1
if speech_pause == 200:
# try replacing with ir.distance(), ir.heading()
# or ir.heading_and_distance()
distance = ir.heading_and_distance()
if distance == None:
# distance() returns None if no beacon detected
str_en = 'Beacon off?'
else:
str_en = 'Beacon heading {0} and distance {1}'.format(
distance[0], distance[1])
sound.speak(str_en, espeak_opts=opts + 'en+f5')
print(str_en, file=stderr)
speech_pause = 0
str_en = 'Terminating program'
sound.speak(str_en, espeak_opts=opts + 'en+f5')
x = ord(sys.stdin.read(1))
time.sleep(0.05)
if __name__ == "__main__":
myThread = threading.Thread(target=keyboardInput, args=(1, ), daemon=True)
myThread.start()
print("Ready for keyboard commands...")
while (True):
irProxVal = ir.proximity
if irProxVal != prev_irProxVal:
print("irProxVal = ", irProxVal)
prev_irProxVal = irProxVal
compassVal = imu.value(0)
if compassVal != prev_compassVal:
print("compassVal = ", compassVal)
prev_compassVal = compassVal
if irProxVal < 50:
print("Obstacle Detected! Forward motion stopped.")
if spd > 0:
spd = 0
x = 33 # set x to non-zero so we print spd, etc; 33 = ascii !
if x == 32 or x == 120: # space or x key pushed
spd = 0
turnRatio = 0
if x == 119: # w key pushed
if spd < 90: # limit max frwd speed to 90
#Definerer NTC parametere:
a = ntc()
a.beta = 3984
a.maxADC(5000)
#
#tempS = TemperatureSensor(Port.S4)
#Definerer port 4 til å være analog inngang:
p4 = LegoPort(INPUT_4)
p4.mode = 'nxt-analog'
p4.set_device = 'nxt-analog'
#Definerer at temperatursensoren er koblet til port 4:
ntc_sensor = Sensor(INPUT_4)
while True:
#Leser av sensorverdi i volt:
msr = ntc_sensor.value()
#Konverterer til temperatur:
t = a.temp_C(msr)
# Printer temperaturen til EV3 LCD skjerm
#print('The temperature is: %f deg' %t)
# Printer temperaturen til VS Code output panel
print('The temperature is: %f deg' %t, file=stderr)
time.sleep(0.3)