def __init__(self):
print '* Fault tolerant line-follower'
print '* Author: Piotr Polak'
print '* Date: May 28th 2009\n\n'
Debugger.printAction('Initializing RobotLogic')
print ''
Debugger.printConfig('KHEPERA_ADDRESS', Config.KHEPERA_ADDRESS)
Debugger.printConfig('KHEPERA_PORT', Config.KHEPERA_PORT)
Debugger.printConfig('ENABLE_COMM', Config.ENABLE_COMM)
Debugger.printConfig('MAXIMUM_MOTOR_SPEED', Config.MAXIMUM_MOTOR_SPEED)
Debugger.printConfig('MINIMUM_MOTOR_SPEED', Config.MINIMUM_MOTOR_SPEED)
Debugger.printConfig('MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE',
Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE)
Debugger.printConfig('GROUND_PROXIMITY_SENSOR_THRESHOLD',
Config.GROUND_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('FRONT_PROXIMITY_SENSOR_THRESHOLD',
Config.FRONT_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('MAIN_LOOP_SLEEP_STEP',
Config.MAIN_LOOP_SLEEP_STEP)
Debugger.printConfig('BROKEN_LINE_STEPS_NUMBER',
Config.BROKEN_LINE_STEPS_NUMBER)
Debugger.printConfig('TURN_90_DEGREES_TIME',
Config.TURN_90_DEGREES_TIME)
Debugger.printConfig('GO_PARALLEL_TIME', Config.GO_PARALLEL_TIME)
print ''
self.__doloop = True
self.__state = 0
self.__khepera = Khepera(Config.KHEPERA_ADDRESS, Config.KHEPERA_PORT)
self.__broken_line_current_steps = 0
def __init__(self):
print '* Fault tolerant line-follower'
print '* Author: Piotr Polak'
print '* Date: May 28th 2009\n\n'
Debugger.printAction( 'Initializing RobotLogic' )
print ''
Debugger.printConfig('KHEPERA_ADDRESS', Config.KHEPERA_ADDRESS)
Debugger.printConfig('KHEPERA_PORT', Config.KHEPERA_PORT)
Debugger.printConfig('ENABLE_COMM', Config.ENABLE_COMM)
Debugger.printConfig('MAXIMUM_MOTOR_SPEED', Config.MAXIMUM_MOTOR_SPEED)
Debugger.printConfig('MINIMUM_MOTOR_SPEED', Config.MINIMUM_MOTOR_SPEED)
Debugger.printConfig('MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE', Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE)
Debugger.printConfig('GROUND_PROXIMITY_SENSOR_THRESHOLD', Config.GROUND_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('FRONT_PROXIMITY_SENSOR_THRESHOLD', Config.FRONT_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('MAIN_LOOP_SLEEP_STEP', Config.MAIN_LOOP_SLEEP_STEP)
Debugger.printConfig('BROKEN_LINE_STEPS_NUMBER', Config.BROKEN_LINE_STEPS_NUMBER)
Debugger.printConfig('TURN_90_DEGREES_TIME', Config.TURN_90_DEGREES_TIME)
Debugger.printConfig('GO_PARALLEL_TIME', Config.GO_PARALLEL_TIME)
print ''
self.__doloop = True;
self.__state = 0;
self.__khepera = Khepera( Config.KHEPERA_ADDRESS, Config.KHEPERA_PORT )
self.__broken_line_current_steps = 0;
class RobotLogic:
def __init__(self):
print '* Fault tolerant line-follower'
print '* Author: Piotr Polak'
print '* Date: May 28th 2009\n\n'
Debugger.printAction('Initializing RobotLogic')
print ''
Debugger.printConfig('KHEPERA_ADDRESS', Config.KHEPERA_ADDRESS)
Debugger.printConfig('KHEPERA_PORT', Config.KHEPERA_PORT)
Debugger.printConfig('ENABLE_COMM', Config.ENABLE_COMM)
Debugger.printConfig('MAXIMUM_MOTOR_SPEED', Config.MAXIMUM_MOTOR_SPEED)
Debugger.printConfig('MINIMUM_MOTOR_SPEED', Config.MINIMUM_MOTOR_SPEED)
Debugger.printConfig('MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE',
Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE)
Debugger.printConfig('GROUND_PROXIMITY_SENSOR_THRESHOLD',
Config.GROUND_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('FRONT_PROXIMITY_SENSOR_THRESHOLD',
Config.FRONT_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('MAIN_LOOP_SLEEP_STEP',
Config.MAIN_LOOP_SLEEP_STEP)
Debugger.printConfig('BROKEN_LINE_STEPS_NUMBER',
Config.BROKEN_LINE_STEPS_NUMBER)
Debugger.printConfig('TURN_90_DEGREES_TIME',
Config.TURN_90_DEGREES_TIME)
Debugger.printConfig('GO_PARALLEL_TIME', Config.GO_PARALLEL_TIME)
print ''
self.__doloop = True
self.__state = 0
self.__khepera = Khepera(Config.KHEPERA_ADDRESS, Config.KHEPERA_PORT)
self.__broken_line_current_steps = 0
def run(self):
while (self.__doloop):
if self.__state == 0:
self.__followLineRoutine()
elif self.__state == 1:
self.__followObstacleRoutine()
elif self.__state == 2:
self.__brokenLineRoutine()
time.sleep(Config.MAIN_LOOP_SLEEP_STEP)
def __followLineRoutine(self):
self.__state = 0
self.__khepera.enableDiode(True)
# Check ProximitySensors
s = self.__khepera.getFrontProximitySensors()
leftFrontProximitySensor = int(s[0])
rightFrontProximitySensor = int(s[1])
#print leftFrontProximitySensor
if leftFrontProximitySensor > Config.FRONT_PROXIMITY_SENSOR_THRESHOLD or rightFrontProximitySensor > Config.FRONT_PROXIMITY_SENSOR_THRESHOLD:
Debugger.printAction('RobotLogic: followObstacleRoutine')
self.__state = 1
return
# Check AmbientLightSensors
s = self.__khepera.getGroundSensors()
leftGroundSensor = int(s[1])
rightGroundSensor = int(s[0])
broken_trashhole = Config.GROUND_PROXIMITY_SENSOR_THRESHOLD
#print "Ambient", leftGroundSensor, rightGroundSensor
if rightGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD and leftGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
# Continue going straight (following line)
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
elif rightGroundSensor > broken_trashhole and leftGroundSensor > broken_trashhole:
# Continue going straight (broken line)
Debugger.printAction('RobotLogic: brokenLineRoutine')
self.__state = 2
return
elif rightGroundSensor > Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
# Adjust speed, turn left
light_proportion = float(
rightGroundSensor) / Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE
light_proportion = light_proportion / 2
slow_motor_speed = Config.MAXIMUM_MOTOR_SPEED - light_proportion * Config.MAXIMUM_MOTOR_SPEED
self.__khepera.setMotorSpeeds(int(slow_motor_speed),
Config.MAXIMUM_MOTOR_SPEED)
elif leftGroundSensor > Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
# Adjust speed, turn right
light_proportion = float(
leftGroundSensor) / Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE
light_proportion = light_proportion / 2
slow_motor_speed = Config.MAXIMUM_MOTOR_SPEED - light_proportion * 5 * Config.MAXIMUM_MOTOR_SPEED
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
int(slow_motor_speed))
def __followObstacleRoutine(self):
self.__state = 1
scenario = 1
# Turn left
Debugger.printAction('RobotLogic: followLineRoutine - Turn left')
self.__khepera.setMotorSpeeds(Config.MINIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
time.sleep(Config.TURN_90_DEGREES_TIME)
# Go parallel
Debugger.printAction('RobotLogic: followLineRoutine - Go parallel')
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
time.sleep(Config.GO_PARALLEL_TIME)
# Turn right
Debugger.printAction('RobotLogic: followLineRoutine - Turn right')
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MINIMUM_MOTOR_SPEED)
time.sleep(Config.TURN_90_DEGREES_TIME)
# Go parallel
Debugger.printAction('RobotLogic: followLineRoutine - Go parallel')
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
time.sleep(Config.GO_PARALLEL_ALONG_TIME)
# Go right
Debugger.printAction('RobotLogic: followLineRoutine - Turn right')
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MINIMUM_MOTOR_SPEED)
time.sleep(Config.TURN_90_DEGREES_TIME)
if scenario != 0:
# Go parallel
Debugger.printAction('RobotLogic: followLineRoutine - Go parallel')
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
time.sleep(Config.GO_PARALLEL_TIME)
# Turn left
Debugger.printAction('RobotLogic: followLineRoutine - Turn left')
self.__khepera.setMotorSpeeds(Config.MINIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
time.sleep(Config.TURN_90_DEGREES_TIME)
Debugger.printAction('RobotLogic: followLineRoutine')
self.__state = 0
return
else:
# Go parallel using broken line routine
Debugger.printAction('RobotLogic: brokenLineRoutine')
self.__state = 2
return
def __brokenLineRoutine(self):
self.__state = 2
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED,
Config.MAXIMUM_MOTOR_SPEED)
self.__khepera.enableDiode(False)
s = self.__khepera.getGroundSensors()
leftGroundSensor = int(s[1])
rightGroundSensor = int(s[0])
if leftGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD or rightGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
Debugger.printAction('RobotLogic: followLineRoutine')
self.__broken_line_current_steps = 0
self.__state = 0
return
self.__broken_line_current_steps = self.__broken_line_current_steps + 1
if self.__broken_line_current_steps > Config.BROKEN_LINE_STEPS_NUMBER:
Debugger.printAction('RobotLogic: Found no line')
self.__broken_line_current_steps = 0
return self.stop()
def stop(self):
Debugger.printAction('RobotLogic: Stop')
self.__doloop = 0
self.__khepera.setMotorSpeeds(0, 0)
class RobotLogic:
def __init__(self):
print '* Fault tolerant line-follower'
print '* Author: Piotr Polak'
print '* Date: May 28th 2009\n\n'
Debugger.printAction( 'Initializing RobotLogic' )
print ''
Debugger.printConfig('KHEPERA_ADDRESS', Config.KHEPERA_ADDRESS)
Debugger.printConfig('KHEPERA_PORT', Config.KHEPERA_PORT)
Debugger.printConfig('ENABLE_COMM', Config.ENABLE_COMM)
Debugger.printConfig('MAXIMUM_MOTOR_SPEED', Config.MAXIMUM_MOTOR_SPEED)
Debugger.printConfig('MINIMUM_MOTOR_SPEED', Config.MINIMUM_MOTOR_SPEED)
Debugger.printConfig('MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE', Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE)
Debugger.printConfig('GROUND_PROXIMITY_SENSOR_THRESHOLD', Config.GROUND_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('FRONT_PROXIMITY_SENSOR_THRESHOLD', Config.FRONT_PROXIMITY_SENSOR_THRESHOLD)
Debugger.printConfig('MAIN_LOOP_SLEEP_STEP', Config.MAIN_LOOP_SLEEP_STEP)
Debugger.printConfig('BROKEN_LINE_STEPS_NUMBER', Config.BROKEN_LINE_STEPS_NUMBER)
Debugger.printConfig('TURN_90_DEGREES_TIME', Config.TURN_90_DEGREES_TIME)
Debugger.printConfig('GO_PARALLEL_TIME', Config.GO_PARALLEL_TIME)
print ''
self.__doloop = True;
self.__state = 0;
self.__khepera = Khepera( Config.KHEPERA_ADDRESS, Config.KHEPERA_PORT )
self.__broken_line_current_steps = 0;
def run(self):
while( self.__doloop ):
if self.__state == 0:
self.__followLineRoutine()
elif self.__state == 1:
self.__followObstacleRoutine()
elif self.__state == 2:
self.__brokenLineRoutine()
time.sleep(Config.MAIN_LOOP_SLEEP_STEP)
def __followLineRoutine(self):
self.__state = 0
self.__khepera.enableDiode(True)
# Check ProximitySensors
s = self.__khepera.getFrontProximitySensors()
leftFrontProximitySensor = int(s[0])
rightFrontProximitySensor = int(s[1])
#print leftFrontProximitySensor
if leftFrontProximitySensor > Config.FRONT_PROXIMITY_SENSOR_THRESHOLD or rightFrontProximitySensor > Config.FRONT_PROXIMITY_SENSOR_THRESHOLD:
Debugger.printAction( 'RobotLogic: followObstacleRoutine' )
self.__state = 1
return
# Check AmbientLightSensors
s = self.__khepera.getGroundSensors()
leftGroundSensor = int(s[1])
rightGroundSensor = int(s[0])
broken_trashhole = Config.GROUND_PROXIMITY_SENSOR_THRESHOLD
#print "Ambient", leftGroundSensor, rightGroundSensor
if rightGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD and leftGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
# Continue going straight (following line)
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED)
elif rightGroundSensor > broken_trashhole and leftGroundSensor > broken_trashhole:
# Continue going straight (broken line)
Debugger.printAction( 'RobotLogic: brokenLineRoutine' )
self.__state = 2
return
elif rightGroundSensor > Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
# Adjust speed, turn left
light_proportion = float(rightGroundSensor)/Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE
light_proportion = light_proportion/2
slow_motor_speed = Config.MAXIMUM_MOTOR_SPEED - light_proportion*Config.MAXIMUM_MOTOR_SPEED
self.__khepera.setMotorSpeeds( int(slow_motor_speed), Config.MAXIMUM_MOTOR_SPEED )
elif leftGroundSensor > Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
# Adjust speed, turn right
light_proportion = float(leftGroundSensor)/Config.MAXIMUM_AMBIENT_LIGHT_SENSOR_VALUE
light_proportion = light_proportion/2
slow_motor_speed = Config.MAXIMUM_MOTOR_SPEED - light_proportion*5*Config.MAXIMUM_MOTOR_SPEED
self.__khepera.setMotorSpeeds( Config.MAXIMUM_MOTOR_SPEED, int(slow_motor_speed) )
def __followObstacleRoutine(self):
self.__state = 1
scenario = 1
# Turn left
Debugger.printAction( 'RobotLogic: followLineRoutine - Turn left' )
self.__khepera.setMotorSpeeds( Config.MINIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED )
time.sleep( Config.TURN_90_DEGREES_TIME )
# Go parallel
Debugger.printAction( 'RobotLogic: followLineRoutine - Go parallel' )
self.__khepera.setMotorSpeeds( Config.MAXIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED )
time.sleep( Config.GO_PARALLEL_TIME )
# Turn right
Debugger.printAction( 'RobotLogic: followLineRoutine - Turn right' )
self.__khepera.setMotorSpeeds( Config.MAXIMUM_MOTOR_SPEED, Config.MINIMUM_MOTOR_SPEED )
time.sleep( Config.TURN_90_DEGREES_TIME )
# Go parallel
Debugger.printAction( 'RobotLogic: followLineRoutine - Go parallel' )
self.__khepera.setMotorSpeeds( Config.MAXIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED )
time.sleep( Config.GO_PARALLEL_ALONG_TIME )
# Go right
Debugger.printAction( 'RobotLogic: followLineRoutine - Turn right' )
self.__khepera.setMotorSpeeds( Config.MAXIMUM_MOTOR_SPEED, Config.MINIMUM_MOTOR_SPEED )
time.sleep( Config.TURN_90_DEGREES_TIME )
if scenario != 0:
# Go parallel
Debugger.printAction( 'RobotLogic: followLineRoutine - Go parallel' )
self.__khepera.setMotorSpeeds( Config.MAXIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED )
time.sleep( Config.GO_PARALLEL_TIME )
# Turn left
Debugger.printAction( 'RobotLogic: followLineRoutine - Turn left' )
self.__khepera.setMotorSpeeds( Config.MINIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED )
time.sleep( Config.TURN_90_DEGREES_TIME )
Debugger.printAction( 'RobotLogic: followLineRoutine' )
self.__state = 0
return
else:
# Go parallel using broken line routine
Debugger.printAction( 'RobotLogic: brokenLineRoutine' )
self.__state = 2
return
def __brokenLineRoutine(self):
self.__state = 2
self.__khepera.setMotorSpeeds(Config.MAXIMUM_MOTOR_SPEED, Config.MAXIMUM_MOTOR_SPEED)
self.__khepera.enableDiode(False)
s = self.__khepera.getGroundSensors()
leftGroundSensor = int(s[1])
rightGroundSensor = int(s[0])
if leftGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD or rightGroundSensor < Config.GROUND_PROXIMITY_SENSOR_THRESHOLD:
Debugger.printAction( 'RobotLogic: followLineRoutine' )
self.__broken_line_current_steps = 0
self.__state = 0
return
self.__broken_line_current_steps = self.__broken_line_current_steps+1;
if self.__broken_line_current_steps > Config.BROKEN_LINE_STEPS_NUMBER:
Debugger.printAction( 'RobotLogic: Found no line' )
self.__broken_line_current_steps = 0
return self.stop();
def stop(self):
Debugger.printAction( 'RobotLogic: Stop' )
self.__doloop = 0;
self.__khepera.setMotorSpeeds(0, 0)
