from basicRobot_planner import PathPlanner grid = PathPlanner() grid.set_grid([1, 1]) grid.set_grid([0, 1]) grid.set_grid([2, 1]) grid.set_grid([3, 1]) print grid.optimum_policy() grid.reset_grid([3, 1]) print grid.optimum_policy()
sensor1 = PORT_1
sensor2 = PORT_2
BrickPi.SensorType[sensor1] = TYPE_SENSOR_ULTRASONIC_CONT
BrickPi.SensorType[sensor2] = TYPE_SENSOR_EV3_GYRO_M0
BrickPi.MotorEnable[motor1] = 1 # Enable the Motor A
BrickPi.MotorEnable[motor2] = 1 # Enable the Motor B
BrickPi.MotorEnable[motor3] = 1 # Enable the Motor C
BrickPiSetupSensors() # Send the properties of sensors to BrickPi
step = 0
decision = ""
current_position = [0, 0]
grid_object = PathPlanner()
goal = [0, 0]
grid_object.set_goal(goal)
start_coordinates = [0, 0]
end_coordinates = [0, 0]
count = 0
obstruction_flag = False
fs = os.sep
BrickPi.EncoderOffset[motor1] = BrickPi.Encoder[motor1]
while True:
BrickPiUpdateValues()
event_handler = MyHandler()
observer = Observer()
observer.schedule(event_handler,
fs + "home" + fs + "pi" + fs + "Desktop" + fs +
BrickPi.EncoderOffset[motor1] = BrickPi.Encoder[motor1]
BrickPi.EncoderOffset[motor2] = BrickPi.Encoder[motor2]
BrickPi.EncoderOffset[motor3] = BrickPi.Encoder[motor3]
BrickPiUpdateValues()
ini_coordinates = [
0, 0
] # set initial coordinates to be set to user defined coordinates from java_reply.txt
start_coordinates = [0, 0
] # initialise start coordinates ie. 1st goal coordinates
end_coordinates = [0, 0] # initialise end coordinates ie. 2nd goal coordinates
step = 0 # steps of execution
current_position = [
0, 0
] # initialise current position coordinates to be updated during execution of program
grid_object = PathPlanner() # define a grid object of PathPlanner class
count = 0 # count the execution number of while loop
obstruction_flag = False # set on detection of obstruction or after completion of one step of movement
fs = os.sep # operating system separator
prev_grid = [0, 0] # initialise previous grid location
prev_movement = '' # initialise previous movement of vehicle
gyration_angle = 0 # initialise vehicle orientation, U=0, L=-90, R=90, D=180
# movement_step = 700 # set steps for linear motion
rotation_step = 510 # set steps for rotational motion
observer_flag = True # file system observer flag, detects change in file system
# update the grid-map with the new obstruction coordinate
def obstruction(obst_crd):
BrickPiUpdateValues()
grid_object.set_grid(obst_crd)
sensor3 = PORT_3
BrickPi.SensorType[sensor1] = TYPE_SENSOR_ULTRASONIC_CONT
BrickPi.SensorType[sensor2] = TYPE_SENSOR_ULTRASONIC_CONT
BrickPi.SensorType[sensor3] = TYPE_SENSOR_EV3_GYRO_M0
BrickPi.MotorEnable[motor1] = 1 # Enable the Motor A
BrickPi.MotorEnable[motor2] = 1 # Enable the Motor B
BrickPi.MotorEnable[motor3] = 1 # Enable the Motor C
BrickPiSetupSensors() # Send the properties of sensors to BrickPi
step = 0
decision = ""
current_position = [0, 0]
grid_object = PathPlanner()
goal = [0, 0]
grid_object.set_goal(goal)
count = 0
BrickPi.EncoderOffset[motor1] = BrickPi.Encoder[motor1]
while True:
gyro = BrickPi.Sensor[sensor3]
count += 1
print "counter", count
try:
BrickPiUpdateValues(
) # Ask BrickPi to update values for sensors/motors
obs = obstruction()
if obstruction() == -1: