def setup():
# Enables servos and sets them to predefined starting positions. This goes before every run
print "Starting setup()"
if c.IS_MAIN_BOT:
print "I am the main bot"
elif c.IS_CLONE_BOT:
print "I am the clone bot"
else:
print "Error in bot determination"
exit(86)
if c.STARTING_CLAW_POS > c.MAX_SERVO_POS or c.STARTING_CLAW_POS < c.MIN_SERVO_POS or c.STARTING_ARM_POS > c.MAX_SERVO_POS or c.STARTING_ARM_POS < c.MIN_SERVO_POS:
print "Invalid desired servo position\n"
exit(86)
graphics_close()
ao()
enable_servo(c.CLAW_SERVO)
enable_servo(c.ARM_SERVO)
enable_servo(c.CUBE_ARM_SERVO)
c.STARTING_ARM_POS = get_servo_position(c.ARM_SERVO)
print "STARTING_ARM_POS now: " + str(c.ARM_DOWN_POS)
m.move_claw(c.STARTING_CLAW_POS)
m.move_arm(c.STARTING_ARM_POS)
m.move_cube_arm(c.STARTING_CUBE_ARM_POS)
msleep(1000)
#print "Set claw to starting position of %d" % c.STARTING_CLAW_POS
#print "Set arm to starting position of %d" % c.STARTING_ARM_POS
#m.move_claw(c.CLAW_CHECKING_POS)
#m.wait()
#m.move_claw(c.STARTING_CLAW_POS)
#m.wait()
console_clear()
print "Setup complete\n\n"
def deliver_ambulance_and_blocks():
w.check_zones_hospital()
if c.SAFE_HOSPITAL == c.NEAR_ZONE:
g.drive_gyro_through_line_right()
s.align_far()
m.open_claw(1, 1, c.CLAW_WAY_OPEN_POS)
msleep(500)
m.move_arm(c.ARM_HIGH_POS)
#m.move_claw(c.CLAW_WAY_OPEN_POS)
#w.close_graphics_window()
g.turn_left_gyro(190)
else:
u.halve_speeds()
g.drive_gyro_through_line_right()
s.align_far()
s.left_forwards_until_white(0)
s.left_forwards_until_black()
m.open_claw(1, 1, c.CLAW_WAY_OPEN_POS)
msleep(500)
m.move_arm(c.ARM_HIGH_POS)
#m.move_claw(c.CLAW_WAY_OPEN_POS)
u.normalize_speeds()
#w.close_graphics_window()
u.sd()
g.turn_left_gyro(190)
g.backwards_gyro_through_line_left(1100)
m.lower_ambulance_arm()
g.backwards_gyro(500)
def deliver_left_coupler():
g.turn_left_gyro(130)
g.forwards_gyro_until_bump()
m.move_wrist(884)
s.wfollow_left_until_black_right_front(0)
s.wfollow_left_until_black_left()
g.backwards_gyro_until_both_white_cliffs()
s.align_close_cliffs()
g.backwards_gyro(1150)
g.turn_right_gyro(25)
m.move_arm(c.ARM_JUST_BARELY_ON_T_POS)
def setup():
print "Starting setup()"
create_disconnect()
print "Boi"
msleep(10)
create_connect()
print "2"
msleep(20)
#g.calibrate_gyro()
enable_servo(c.ARM_SERVO)
print "Servo enabled = %d\n" % get_servo_enabled(c.ARM_SERVO)
m.move_arm(c.ARM_START_POS)
print "Setup complete\n"
def setup():
print "Starting setup()"
reset_roomba()
msleep(20)
g.calibrate_gyro()
enable_servo(c.ARM_SERVO)
enable_servo(c.MAGNET_ARM_SERVO)
enable_servo(c.MICRO_SERVO)
enable_servo(c.WRIST_SERVO)
print "Arm servo enabled = %d\n" % get_servo_enabled(c.ARM_SERVO)
print "Magnet arm servo enabled = %d\n" % get_servo_enabled(
c.MAGNET_ARM_SERVO)
m.move_arm(c.ARM_START_POS)
m.move_magnet_arm(c.MAGNET_ARM_START_POS)
m.move_micro(c.MICRO_START_POS)
m.move_wrist(c.WRIST_START_POS)
print "Setup complete\n"
def setup():
# Enables servos and sets them to predefined starting positions. This goes before every run
print "Starting setup()"
if c.IS_MAIN_BOT:
print "I am the main bot"
elif c.IS_CLONE_BOT:
print "I am the clone bot"
else:
print "Error in bot determination"
exit(86)
START_TIME = seconds()
if c.STARTING_CLAW_POS > c.MAX_SERVO_POS or c.STARTING_CLAW_POS < c.MIN_SERVO_POS or c.STARTING_ARM_POS > c.MAX_SERVO_POS or c.STARTING_ARM_POS < c.MIN_SERVO_POS:
print "Invalid desired servo position\n"
exit(86)
graphics_close()
ao()
msleep(100)
g.calibrate_gyro()
cmpc(c.LEFT_MOTOR)
cmpc(c.RIGHT_MOTOR)
cmpc(c.AMBULANCE_ARM_MOTOR)
enable_servo(c.CLAW_SERVO)
enable_servo(c.ARM_SERVO)
enable_servo(c.MICRO_SERVO)
#c.STARTING_ARM_POS = get_servo_position(c.ARM_SERVO)
#print "STARTING_ARM_POS now: " + str(c.ARM_DOWN_POS)
m.move_claw(c.CLAW_CHECKING_POS, 8, 1)
m.move_micro(c.MICRO_CHECKING_POS, 8, 1)
m.move_claw(c.STARTING_CLAW_POS, 8, 1)
m.move_arm(c.ARM_DOWN_POS, 8, 1)
m.move_micro(c.STARTING_MICRO_POS, 8, 1)
m.lower_ambulance_arm()
msleep(25)
ao()
print "Set claw to starting position of %d" % c.STARTING_CLAW_POS
print "Set arm to starting position of %d" % c.STARTING_ARM_POS
#m.move_claw(c.CLAW_CHECKING_POS)
#msleep(1000)
#m.move_claw(c.STARTING_CLAW_POS)
#msleep(1000)
console_clear()
print "Setup complete\n\n"
def test_servos_extensive(
exit=True
): # Runs all constant servo positions for arm and claw. Not updated.
print "Testing servos extensively\n"
m.move_claw(c.STARTING_CLAW_POS)
m.move_arm(c.STARTING_ARM_POS)
m.stop_for(1000)
m.move_claw(c.CLAW_OPEN_POS)
m.stop_for(1000)
m.move_claw(c.CLAW_CLOSE_POS)
m.stop_for(1000)
m.move_arm(c.ARM_UP_POS)
m.stop_for(1000)
m.move_arm(c.ARM_DOWN_POS)
m.stop_for(1000)
m.move_arm(c.ARM_DOWN_POS)
m.stop_for(1000)
print "Testing complete."
if exit == True:
print "Exiting...\n"
exit(86)
def calibrate():
# Code to calibrate the bw values. This goes before every run. Ends with light sensor calibration.
max_sensor_value_right = 0
min_sensor_value_right = 90000
max_sensor_value_left = 0
min_sensor_value_left = 90000
max_sensor_value_third = 0
min_sensor_value_third = 90000
max_sensor_value_fourth = 0
min_sensor_value_fourth = 90000
cmpc(c.LEFT_MOTOR)
cmpc(c.RIGHT_MOTOR)
if c.IS_MAIN_BOT:
calibrate_tics = 2100
else: # Clone bot
calibrate_tics = 3000
print "Running calibrate()"
angle = 0
error = 0
i = 0
total_left_speed = 0
total_right_speed = 0
total_seconds = 0
m.activate_motors(int(-c.BASE_LM_POWER / 2), int(-c.BASE_RM_POWER / 2))
while abs(gmpc(c.LEFT_MOTOR) - gmpc(c.RIGHT_MOTOR)) / 2 < calibrate_tics:
left_speed = (-c.BASE_LM_POWER + error) / 2
right_speed = (-c.BASE_RM_POWER + error) / 2
total_left_speed += left_speed
total_right_speed += right_speed
i += 1
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (g.get_change_in_angle() - g.bias) * 10
error = 0.034470956 * angle # Positive error means veering left. Negative means veering right.
if analog(c.RIGHT_TOPHAT) > max_sensor_value_right:
max_sensor_value_right = analog(c.RIGHT_TOPHAT)
if analog(c.RIGHT_TOPHAT) < min_sensor_value_right:
min_sensor_value_right = analog(c.RIGHT_TOPHAT)
if analog(c.LEFT_TOPHAT) > max_sensor_value_left:
max_sensor_value_left = analog(c.LEFT_TOPHAT)
if analog(c.LEFT_TOPHAT) < min_sensor_value_left:
min_sensor_value_left = analog(c.LEFT_TOPHAT)
if analog(c.THIRD_TOPHAT) > max_sensor_value_third:
max_sensor_value_third = analog(c.THIRD_TOPHAT)
if analog(c.THIRD_TOPHAT) < min_sensor_value_third:
min_sensor_value_third = analog(c.THIRD_TOPHAT)
if analog(c.FOURTH_TOPHAT) > max_sensor_value_fourth:
max_sensor_value_fourth = analog(c.FOURTH_TOPHAT)
if analog(c.FOURTH_TOPHAT) < min_sensor_value_fourth:
min_sensor_value_fourth = analog(c.FOURTH_TOPHAT)
intermediete_seconds = seconds()
total_seconds += seconds() - intermediete_seconds
msleep(1)
m.deactivate_motors()
# If sensing black when it should be sensing white, increase bias
# If sensing white when it should be sensing black, decrease bias
c.LEFT_TOPHAT_BW = int(
((max_sensor_value_left + min_sensor_value_left) / 2)) - 1000
c.RIGHT_TOPHAT_BW = int(
((max_sensor_value_right + min_sensor_value_right) / 2)) - 1000
if c.IS_MAIN_BOT:
c.THIRD_TOPHAT_BW = int(
((max_sensor_value_third + min_sensor_value_third) / 2)) + 600
else: # Clone bot
c.THIRD_TOPHAT_BW = int(
((max_sensor_value_third + min_sensor_value_third) / 2))
c.FOURTH_TOPHAT_BW = int(
((max_sensor_value_fourth + min_sensor_value_fourth) / 2))
avg_left_speed = total_left_speed / i
avg_right_speed = total_right_speed / i
c.BASE_LM_POWER = int(-avg_left_speed * 2)
c.BASE_RM_POWER = int(-avg_right_speed * 2)
c.FULL_LM_POWER = c.BASE_LM_POWER
c.FULL_RM_POWER = c.BASE_RM_POWER
c.HALF_LM_POWER = int(c.BASE_LM_POWER / 2)
c.HALF_RM_POWER = int(c.BASE_RM_POWER / 2)
c.SECONDS_DELAY = total_seconds / i
print "c.BASE_LM_POWER: " + str(c.BASE_LM_POWER)
print "c.BASE_RM_POWER: " + str(c.BASE_RM_POWER)
print "max_sensor_value_left: " + str(max_sensor_value_left)
print "min_sensor_value_left: " + str(min_sensor_value_left)
print "LEFT_TOPHAT_BW: " + str(c.LEFT_TOPHAT_BW)
print "max_sensor_value_right: " + str(max_sensor_value_right)
print "min_sensor_value_right: " + str(min_sensor_value_right)
print "RIGHT_TOPHAT_BW: " + str(c.RIGHT_TOPHAT_BW)
print "max_sensor_value_third: " + str(max_sensor_value_third)
print "min_sensor_value_third: " + str(min_sensor_value_third)
print "THIRD_TOPHAT_BW: " + str(c.THIRD_TOPHAT_BW)
print "max_sensor_value_fourth: " + str(max_sensor_value_fourth)
print "min_sensor_value_fourth: " + str(min_sensor_value_fourth)
print "FOURTH_TOPHAT_BW: " + str(c.FOURTH_TOPHAT_BW)
print "Seconds delay: " + str(c.SECONDS_DELAY)
c.MAX_TOPHAT_VALUE_RIGHT = max_sensor_value_right
c.MIN_TOPHAT_VALUE_RIGHT = min_sensor_value_right
c.MAX_TOPHAT_VALUE_LEFT = max_sensor_value_left
c.MIN_TOPHAT_VALUE_LEFT = min_sensor_value_left
c.MAX_TOPHAT_VALUE_THIRD = max_sensor_value_third
c.MIN_TOPHAT_VALUE_THIRD = min_sensor_value_third
print "Finished Calibrating. Moving back into starting box...\n"
#g.drive_gyro_through_line_left()
#s.align_far()
m.move_arm(c.STARTING_ARM_POS, 8, 1)
m.lower_ambulance_arm()
off(c.LEFT_MOTOR)
off(c.RIGHT_MOTOR)
wait_for_light(c.LIGHT_SENSOR)
shut_down_in(118) # URGENT: PUT BACK IN BEFORE COMPETITION