def checkObject():
uValueR = 19
try:
uValueC = BP.get_sensor(BP.PORT_2)
except brickpi3.SensorError as error:
print(error)
try:
uValueR = BP.get_sensor(BP.PORT_4)
except brickpi3.SensorError as error:
print(error)
p = -1
error = (uValueR - 5) * p
BP.set_motor_power(BP.PORT_A, speed + (error * 0.8))
BP.set_motor_power(BP.PORT_D, speed - (error * 0.8))
if (uValueR > 20):
passingTime()
return False
elif (uValueC <= 5):
drive.turnLeft90()
return False
else:
return True
def flagGrab():
drive.moveBackward()
time.sleep(1)
drive.stop()
drive.turnLeft90()
drive.turnLeft90()
release()
drive.moveBackward()
time.sleep(1)
drive.stop()
grab()
def main():
while True:
try:
uValueC = BP.get_sensor(BP.PORT_2)
except brickpi3.SensorError as error:
print(error)
if(uValueC == 0):
drive.stop()
else:
if(uValueC <= 5):
drive.turnLeft90()
checkObject()
else:
drive.moveForward()
time.sleep(0.02)
def avoidance():
uValueC = 70
try:
uValueC = BP.get_sensor(BP.PORT_2)
except brickpi3.SensorError as error:
print(error)
if (uValueC == 0):
drive.stop()
return False
else:
if (uValueC <= 5):
drive.turnLeft90()
return True
else:
drive.moveForward()
return False
def main():
wallfollowing = False
while True:
try:
BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_REFLECTED)
time.sleep(0.02)
reflectedValue = BP.get_sensor(BP.PORT_3)
BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(0.02)
colourValue = BP.get_sensor(BP.PORT_3)
except brickpi3.SensorError as error:
print(error)
try:
uValueR = BP.get_sensor(BP.PORT_4)
except brickpi3.SensorError as error:
print(error)
if (reflectedValue < 27):
linefollowing_ctf.linefollowing()
if (colour[colourValue] == "Yellow"):
clamp.flagGrab()
if (colour[colourValue] == "Red"):
if (uValueR < 70):
drive.turnLeft90()
else:
drive.turnRight90()
if (wallfollowing):
wallfollowing = objectavoidance_ctf.checkObject()
else:
wallfollowing = objectavoidance_ctf.avoidance()
time.sleep(0.02)
def avoidance(self):
uValue = 70
uValue = self.getUltrasonic()
print("Avoidance:" + str(uValue))
if (uValue <= 10):
self.closeToObject = True
if (uValue == 0):
drive.stop()
else:
if (self.closeToObject == True):
if (self.ScanValues[2] == 0):
self.h.returnCentre()
drive.stop()
time.sleep(1)
self.ScanValues[2] = self.getUltrasonic()
print("Centre Scan Value:" + str(uValue))
if (self.i <= 9 and self.ScanValues[1] == 0):
if (self.i == 0):
scanV = self.h.scan(0)
else:
scanV = 3
if (scanV == 2):
self.leftScanArray.append(self.getUltrasonic())
elif (scanV == 3):
if (self.ScanValues[0] == 0):
self.ScanValues[0] = self.getLowest(
self.leftScanArray)
print("Left Scan Value:" + str(self.ScanValues[0]))
self.rightScanArray.insert(self.i,
self.getUltrasonic())
self.i += 1
print("i = " + str(self.i))
# print("Right Scan Value:" + str(self.getUltrasonic()))
elif (self.ScanValues[1] == 0):
self.ScanValues[1] = self.getLowest(self.rightScanArray)
print("Right Scan Value:" + str(self.ScanValues[1]))
self.i = 0
elif (self.positionSet == False):
if (self.ScanValues[1] > 60):
self.h.turnLeft(0.7)
self.direction = 1
self.headAngle = 1
elif (self.ScanValues[0] > 60):
self.h.turnRight(0.7)
self.direction = 0
self.headAngle = 2
else:
self.h.returnCentre()
self.direction = random.randint(0, 1)
self.headAngle = 0
self.positionSet = True
else:
if (uValue < 5):
if (self.headAngle == 0):
if (self.direction == 0):
drive.turnLeft90()
self.h.turnRight(1)
else:
drive.turnRight90()
self.h.turnLeft(1)
elif (self.headAngle == 1):
drive.turnRight45()
self.h.returnCentre()
self.h.turnLeft(1)
else:
drive.turnLeft45()
self.h.returnCentre()
self.h.turnRight(1)
self.switcher = 1
else:
drive.moveForward()
else:
drive.moveForward()
self.h.scan(1)