from sys import stderr
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):
time.sleep(0.5)
# Connect sensor to sensor port 1
imu = Sensor(INPUT_1)
# allow for some time to load the new drivers
time.sleep(0.5)
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(
"#######################################################################")
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')
# allow for some time for setup
time.sleep(0.5)
# Connect sensor to sensor port 1
gyro = Sensor(INPUT_4)
# allow for some time to load the new drivers
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("#######################################################################")
# Connect sensor to sensor port 1
imu = Sensor(INPUT_2)
# allow for some time to load the new drivers
time.sleep(0.5)
print("#######################################################################")
print("############### starting compass calibration ######################")
print("#######################################################################")
print("")
print(" Sensor PORT 2 ")
print("")
imu.mode = 'COMPASS'
time.sleep(.5)
imu.command = 'BEGIN-COMP-CAL' # send the command to begin compass calibration
print ("Hit x to end calibration and exit...")
while (doExit == False):
x = ord(sys.stdin.read(1))
if x == 120: # x key pushed
print("x key pushed - command accepted")
imu.command = 'END-COMP-CAL' # send the end cal command
time.sleep(5)
print("calibration cycle complete. Exiting.")
break
time.sleep(.2)
# allow for some time to stabilze
time.sleep(3)
# mi.poll_ms = 0 # didn't work
# set the mi sensor mode
mi.command = 'RESET'
print("wait 6")
# allow for some time to stabilze
time.sleep(3)
# set the mi sensor mode
mi.mode = 'ANGLE'
# mi.mode = 'ALL'
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)