def transit_test():
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
x_pos = float(input("Enter x_pos of destination"))
y_pos = float(input("Enter y_pos of destination"))
print(currentState.getCurrentPos())
input('start?')
dest = pp.Position(x_pos, y_pos)
path = create_path(currentState.currentPos, dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
commands = converToCommands(path)
print(str(commands))
done = False
while not done:
for command in commands:
print(str(command[2]))
direction = True
if command[0] < 0:
direction = False
okay = turn(math.fabs(command[0]), direction, ROBOT_SPEED_TURN)
if not okay:
break
distance = command[1]
next_x = currentState.getCurrentPos().getX(
) + math.fabs(distance) * math.cos(
currentState.getCurrentPos().getOrientation())
next_y = currentState.getCurrentPos().getY(
) + math.fabs(distance) * math.sin(
currentState.getCurrentPos().getOrientation())
next_pos = pp.Position(next_x, next_y)
direction = True
if distance < 0:
direction = False
okay = rpmdrive(next_pos, direction, distance, ROBOT_SPEED_DRIVE)
if not okay:
break
rospy.sleep(0.01)
if currentState.getCurrentPos().distanceTo(command[2]) > 0.2:
break
if currentState.getCurrentPos(
) == dest or currentState.getCurrentPos().getDistanceTo(dest) < 0.2:
done = True
else:
print("modifying path")
path = create_path(currentState.getCurrentPos(), dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
commands = converToCommands(path)
print(str(commands))
if rospy.is_shutdown():
exit(-1)
def updateObstacle(msg):
global currentState
obs = pp.Obstacle(msg.x, msg.y, msg.diameter / 2)
if currentState.addObstacle(msg.obsID, obs):
print('added obstacle')
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
def task_8():
# Need to check the actual angle depending on the incline of the bucket
angleStrength = 225
mc.bucket_angle_actuator(angleStrength)
rospy.sleep(7)
mc.bucket_angle_actuator(-angleStrength)
rospy.sleep(7)
def looky_turn_2(currentPos, next_pos):
looky_angle = (currentPos.angleToFace(next_pos) -
currentPos.getOrientation() + 2 * math.pi) % (2 * math.pi)
if looky_angle >= toRadian(360 - 150) or looky_angle <= toRadian(150):
if looky_angle <= toRadian(150):
mc.star_looky(motor_pub, toDegree(looky_angle))
else:
mc.star_looky(motor_pub, toDegree(looky_angle - 2 * math.pi))
elif looky_angle >= ((toRadian(-150) + math.pi) % (2 * math.pi)) \
and looky_angle <= ((toRadian(150) + math.pi) % (2 * math.pi)):
mc.port_looky(motor_pub, toDegree(looky_angle - math.pi))
else:
mc.star_looky(motor_pub, 150)
mc.port_looky(motor_pub, -150)
def looky_turn_2(currentPos, next_pos):
looky_angle = (currentPos.angleToFace(pp.Position(0, 0,
0))) % (2 * math.pi)
print(str(looky_angle))
if looky_angle >= (toRadian(-150) %
(2 * math.pi)) or looky_angle <= toRadian(150):
if looky_angle <= toRadian(150):
mc.star_looky(motor_pub, toDegree(looky_angle))
else:
mc.star_looky(motor_pub, toDegree(looky_angle - 2 * math.pi))
elif looky_angle >= ((toRadian(-150) + math.pi) % (2 * math.pi)) \
and looky_angle <= ((toRadian(150) + math.pi) % (2 * math.pi)):
mc.port_looky(motor_pub, toDegree(looky_angle - math.pi))
else:
mc.star_looky(motor_pub, 150)
mc.port_looky(motor_pub, -150)
def looky_turn(currentPos, next_distance):
nextX = currentPos.getX() + next_distance * math.cos(
currentPos.getOrientation())
nextY = currentPos.getY() + next_distance * math.sin(
currentPos.getOrientation())
nextPos = pp.Position(nextX, nextY, currentPos.getOrientation())
looky_angle = (nextPos.angleToFace(pp.Position(0, 0, 0))) % (2 * math.pi)
if looky_angle >= (toRadian(-150) % 2 *
math.pi) or looky_angle <= toRadian(150):
if looky_angle <= toRadian(150):
mc.star_looky(motor_pub, toDegree(looky_angle))
else:
mc.star_looky(motor_pub, toDegree(looky_angle - 2 * math.pi))
elif looky_angle >= ((toRadian(-150) + math.pi) % (2 * math.pi)) \
and looky_angle <= ((toRadian(150) + math.pi) % (2 * math.pi)):
mc.port_looky(motor_pub, toDegree(looky_angle - math.pi))
else:
mc.star_looky(motor_pub, 150)
mc.port_looky(motor_pub, -150)
def updateObstacle(msg):
global currentState
if currentState.currentPos is None or currentState.ignore_obs:
return
robot_orient = currentState.getCurrentPos().getOrientation()
true_dx = msg.y * math.cos(robot_orient) + msg.x * math.cos(robot_orient -
math.pi / 2)
true_dy = msg.y * math.sin(robot_orient) + msg.x * math.sin(robot_orient -
math.pi / 2)
rad_clear = min(msg.diameter / 2 + 0.75,
math.sqrt(true_dx**2 + true_dy**2) - 0.25)
obs = pp.Obstacle(currentState.getCurrentPos().getX() + true_dx,
currentState.getCurrentPos().getY() + true_dy, rad_clear)
if obs.getCenter()[0] < 0.25 or obs.getCenter(
)[0] > ARENA_WIDTH - 0.25 or obs.getCenter(
)[1] < 2.74 + 0.25 or obs.getCenter()[1] > 1.9 + 2.74 - 0.25:
return
if currentState.addObstacle(msg.obsID, obs):
print('saw obstacle')
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
def waitForLocalization():
global motor_pub
pos = -150
mc.port_looky(motor_pub, pos)
mc.port_looky(motor_pub, pos)
rospy.sleep(1)
while currentState.getCurrentPos() is None and pos < 150:
print('move tag')
pos += 15
mc.port_looky(motor_pub, pos)
mc.star_looky(motor_pub, pos)
rospy.sleep(1) # sleep for 10 milliseconds
if currentState.getCurrentPos() is None:
print("Cannot locate the tag")
exit(-1)
def shutdownRoutine():
global motor_pub
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
def turn(goal, counter, speed):
offset = currentState.getGyroZ()
cum_angle = 0
stop_angle = 0
flag = False
done = False
lastTime = None
if counter:
mc.drive_left_motor(motor_pub, -speed)
mc.drive_right_motor(motor_pub, speed)
else:
mc.drive_left_motor(motor_pub, speed)
mc.drive_right_motor(motor_pub, -speed)
result = True
while not done:
w = currentState.getGyroZ() - offset
if not flag:
if math.fabs((math.fabs(currentState.getStarRPM()) +
math.fabs(currentState.getPortRPM())) / 2 -
speed) < 1:
stop_angle = goal - cum_angle
flag = True
elif cum_angle >= goal / 2:
stop_angle = cum_angle
flag = True
if lastTime is None:
lastTime = time.time()
else:
currentTime = time.time()
deltaT = currentTime - lastTime
cum_angle += math.fabs(w) * deltaT
updateRelativePos(0, toRadian(w * deltaT))
lastTime = currentTime
if flag and cum_angle >= stop_angle:
mc.drive_right_motor(motor_pub, 0)
mc.drive_left_motor(motor_pub, 0)
done = True
if rospy.is_shutdown():
exit(-1)
if currentState.obstacle_found:
mc.drive_right_motor(motor_pub, 0)
mc.drive_left_motor(motor_pub, 0)
done = True
result = False
data = 0
rpm = (math.fabs(currentState.getPortRPM()) +
math.fabs(currentState.getStarRPM())) / 2
if counter:
data = math.fabs(goal)
else:
data = -math.fabs(goal)
logData('t', rpm, currentState.getAcceX(), currentState.getGyroZ(), 0,
data, currentState.getCurrentPos())
data = 0
rpm = (math.fabs(currentState.getPortRPM()) +
math.fabs(currentState.getStarRPM())) / 2
if counter:
data = math.fabs(goal)
else:
data = -math.fabs(goal)
logData('t', rpm, currentState.getAcceX(), currentState.getGyroZ(), 0,
data, currentState.getCurrentPos())
rospy.sleep(0.005)
rpm = (math.fabs(currentState.getPortRPM()) +
math.fabs(currentState.getStarRPM())) / 2
while math.fabs(rpm) > 1:
w = currentState.getGyroZ() - offset
currentTime = time.time()
deltaT = currentTime - lastTime
updateRelativePos(0, toRadian(w * deltaT))
lastTime = currentTime
data = 0
if counter:
data = math.fabs(goal)
else:
data = -math.fabs(goal)
logData('t', rpm, currentState.getAcceX(), currentState.getGyroZ(), 0,
data, currentState.getCurrentPos())
rpm = (math.fabs(currentState.getPortRPM()) +
math.fabs(currentState.getStarRPM())) / 2
looky_turn_2(currentState.getCurrentPos(), COLLECTION_BIN)
rospy.sleep(1)
return result
def rpmdrive(dest, forward, distance, speed):
global motor_pub, currentState
offset = math.fabs(currentState.getStarRPM() +
currentState.getPortRPM()) / 2
done = False
flag = False
cum_distance = 0
stop_dist = 0
lastTime = None
currentState.setDisp(True)
if forward:
mc.drive_left_motor(motor_pub, speed)
mc.drive_right_motor(motor_pub, speed)
else:
mc.drive_right_motor(motor_pub, -speed)
mc.drive_left_motor(motor_pub, -speed)
while not done:
rpm = math.fabs(currentState.getPortRPM() +
currentState.getStarRPM()) / 2 - offset
if not flag:
if rpm - speed < 2.5:
stop_dist = distance - cum_distance - 0.3
flag = True
elif cum_distance >= distance:
stop_dist = cum_distance
flag = True
if lastTime is None:
lastTime = time.time()
else:
currentTime = time.time()
delta = currentTime - lastTime
cum_distance += distance_moved(rpm, 0, delta)
updateRelativePos(distance_moved(rpm, 0, delta), 0)
lastTime = currentTime
if flag and cum_distance >= stop_dist:
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
done = True
if rospy.is_shutdown():
exit(-1)
if currentState.obstacle_found:
print('found')
return False
data = 0
if forward:
data = math.fabs(distance)
else:
data = -math.fabs(distance)
logData('d', rpm, currentState.getAcceX(), currentState.getGyroZ(),
data, 0, currentState.getCurrentPos())
rospy.sleep(0.005)
rpm = (currentState.getPortRPM() + currentState.getStarRPM()) / 2
while math.fabs(rpm) > 1:
currentTime = time.time()
delta = currentTime - lastTime
updateRelativePos(distance_moved(rpm, 0, delta), 0)
lastTime = currentTime
data = 0
if forward:
data = math.fabs(distance)
else:
data = -math.fabs(distance)
logData('d', rpm, currentState.getAcceX(), currentState.getGyroZ(),
data, 0, currentState.getCurrentPos())
rpm = (currentState.getPortRPM() + currentState.getStarRPM()) / 2
rospy.loginfo("optical displacement: " + str(currentState.getDist()))
currentState.setDisp(False)
looky_turn_2(dest, COLLECTION_BIN)
rospy.sleep(1)
return True
def task_15():
angleStrength = 225
mc.bucket_angle_actuator(angleStrength)
rospy.sleep(7)
mc.bucket_angle_actuator(-angleStrength)
rospy.sleep(7)
def conservative_drive(dest, forward, distance):
global motor_pub
done = False
initPos = currentState.getCurrentPos()
if distance > 1.85:
speed = ROBOT_SPEED_DRIVE
else:
speed = math.sqrt(distance * 0.5 / 0.0010265)
stop_distance = distance - 0.001265 * speed**2
while not done:
if rospy.is_shutdown():
exit(-1)
rospy.loginfo('current pos ' + str(currentState.getCurrentPos()))
rospy.loginfo('distance to destination ' +
str(currentState.getCurrentPos().distanceTo(dest)))
if currentState.getCurrentPos() == dest or\
initPos.distanceTo(currentState.getCurrentPos()) > stop_distance:
print('stopping')
done = True
elif math.fabs(initPos.distanceTo(
currentState.getCurrentPos())) > distance:
rospy.loginfo(
'Did not arrive at destination, but moved far enough')
rospy.loginfo('deviation: ' +
str(dest.distanceTo(currentState.getCurrentPos())))
done = True
else:
if forward:
mc.drive_left_motor(motor_pub, speed)
mc.drive_right_motor(motor_pub, speed)
logDriveData(True, speed)
else:
mc.drive_left_motor(motor_pub, -speed)
mc.drive_right_motor(motor_pub, -speed)
logDriveData(False, speed)
if not done:
next_distance = distance_moved(
(currentState.getStarRPM() + currentState.getPortRPM()) / 2,
currentState.getAcceX(), 0.005)
if forward:
looky_turn(currentState.getCurrentPos(), next_distance)
else:
looky_turn(currentState.getCurrentPos(), -next_distance)
rospy.sleep(0.005)
mc.drive_right_motor(motor_pub, 0)
mc.drive_left_motor(motor_pub, 0)
def testShutdown():
global logfile, motor_pub
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
logfile.close()
def transit_dig_test():
transit_test()
t = float(input('How long do you want to dig?'))
mc.bucket_angle_actuator(motor_pub, 60)
rospy.sleep(5)
mc.bucket_drive_motor(motor_pub, 150)
rospy.sleep(1)
mc.bucket_translation_motor(motor_pub, 780)
rospy.sleep(t)
mc.bucket_drive_motor(motor_pub, 0)
mc.bucket_translation_motor(motor_pub, 100)
rospy.sleep(2)
mc.bucket_angle_actuator(motor_pub, 15)
rospy.sleep(5)
def drive(dest, forward, distance, speed):
global motor_pub
offset = math.fabs(currentState.getStarRPM() +
currentState.getPortRPM()) / 2
done = False
flag = False
cum_distance = 0
stop_dist = 0
lastTime = None
result = True
currentState.setDisp(True)
if forward:
mc.drive_left_motor(motor_pub, speed)
mc.drive_right_motor(motor_pub, speed)
else:
mc.drive_right_motor(motor_pub, -speed)
mc.drive_left_motor(motor_pub, -speed)
while not done:
rpm = math.fabs(currentState.getPortRPM() +
currentState.getStarRPM()) / 2 - offset
if not flag:
if rpm - speed < 2.5:
stop_dist = distance - cum_distance - 0.3
flag = True
elif cum_distance >= distance:
stop_dist = cum_distance
flag = True
if lastTime is None:
lastTime = time.time()
else:
currentTime = time.time()
delta = currentTime - lastTime
cum_distance += distance_moved(rpm, 0, delta)
lastTime = currentTime
if flag and cum_distance >= stop_dist:
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
done = True
if rospy.is_shutdown():
exit(-1)
if currentState.obstacle_found:
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
done = True
result = False
rospy.sleep(0.005)
rospy.loginfo("optical displacement: " + str(currentState.getDist()))
currentState.setDisp(False)
looky_turn_2(dest, COLLECTION_BIN)
rospy.sleep(1)
return result
def task_4():
driveStrength = 40
translationStrength = 10
angleStrength = 10
# Is there a way to know if it's fully angled down?
mc.bucket_angle_actuator(angleStrength)
rospy.sleep(15)
mc.bucket_angle_actuator(0)
mc.bucket_drive_motor(driveStrength)
mc.bucket_translation_motor(translationStrength)
rospy.sleep(60)
mc.bucket_drive_motor(0)
mc.bucket_translation_motor(-1 * translationStrength)
rospy.sleep(10)
mc.bucket_translation_motor(0)
mc.bucket_angle_actuator(-angleStrength)
rospy.sleep(15)
mc.bucket_angle_actuator(0)
def turn_algo_2(goal, counter):
offset = currentState.getGyroZ()
cum_angle = 0
stop_angle = 0
flag = False
done = False
initTime = time.time()
lastTime = None
goal_orientation = (currentState.getCurrentPos().getOrientation() +
toRadian(goal)) % (2 * math.pi)
goal_pos = pp.Position(currentState.getCurrentPos().getX(),
currentState.getCurrentPos().getY(),
goal_orientation)
looky_turn_2(currentState.getCurrentPos(), goal_pos)
if counter:
mc.drive_left_motor(motor_pub, -ROBOT_SPEED_TURN)
mc.drive_right_motor(motor_pub, ROBOT_SPEED_TURN)
else:
mc.drive_left_motor(motor_pub, ROBOT_SPEED_TURN)
mc.drive_right_motor(motor_pub, -ROBOT_SPEED_TURN)
while not done:
w = currentState.getGyroZ() - offset
if not flag:
if math.fabs((math.fabs(currentState.getStarRPM()) +
math.fabs(currentState.getPortRPM())) / 2 -
ROBOT_SPEED_TURN) < 1:
print('hi')
stop_angle = goal - cum_angle
print(str(stop_angle))
flag = True
elif cum_angle >= goal / 2:
stop_angle = cum_angle
flag = True
if lastTime is None:
lastTime = time.time()
else:
currentTime = time.time()
deltaT = currentTime - lastTime
cum_angle += math.fabs(w) * deltaT
lastTime = currentTime
logGyroData(currentState.getGyroZ() + offset,
currentState.getPortRPM(), currentState.getStarRPM(),
cum_angle, currentTime - initTime)
if flag and cum_angle >= stop_angle:
mc.drive_right_motor(motor_pub, 0)
mc.drive_left_motor(motor_pub, 0)
done = True
if rospy.is_shutdown():
exit(-1)
while currentState.getGyroZ() > 1:
w = currentState.getGyroZ() - offset
currentTime = time.time()
cum_angle += math.fabs(w * (currentTime - lastTime))
lastTime = currentTime
rospy.sleep(0.005)
measure = input('How much did it turn? (in deg): ')
logTurnData(counter, ROBOT_SPEED_TURN, goal, cum_angle, float(measure),
lastTime - initTime)