def alignheadon(adjust_for_mission=0):
shared_all.turn(180, 210)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
shared_all.move_to_color_reverse(color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK)
def inovtobball():
shared_all.move_straight(distance_mm=100, speed_mm_s=-100)
shared_all.turn(90)
shared_all.move_straight(distance_mm=150, speed_mm_s=150)
shared_all.turn(-90)
shared_all.move_straight(distance_mm=350, speed_mm_s=200)
shared.turn(-45)
def alignatangle(adjust_for_mission=0):
shared_all.move_straight_target_direction(gyro = gyro,
distance_mm= 170,
speed_mm_s= 130,
target_angle= -130+ adjust_for_mission)
shared_all.turn(angle=110, speed_deg_s=190)
shared_all.turn_to_direction( gyro=gyro, target_angle=10+ adjust_for_mission)
def treadmill_to_row_withalign(adjust_for_mission=0):
shared_all.move_to_color_reverse(
color_sensor=color_sensor_center,
stop_on_color=Color.WHITE,
alternative_color=Color.BLACK,
speed_mm_s=30,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=50)
shared_all.move_straight(distance_mm=120, speed_mm_s=100)
shared_all.turn(angle=90, speed_deg_s=140)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=100,
speed_mm_s=-160,
target_angle=-90)
shared_all.push_back_reset_gyro(distance_mm=40,
reset_gyro=True,
new_gyro_angle=-90)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=270,
speed_mm_s=180,
target_angle=-90)
shared_all.turn(angle=90, speed_deg_s=120)
shared_all.move_reverse(max_distance=40, speed_mm_s=100)
def runold(adjust_for_mission=0):
shared_all.start_moving_crane_to_angle(motor=crane_motor, target_angle=25)
shared_all.move_to_color(color_sensor=color_sensor_center,
stop_on_color=Color.GREEN,
alternative_color=Color.GREEN,
max_distance_mm=60)
shared_all.move_to_color(color_sensor=color_sensor_center,
stop_on_color=Color.RED,
alternative_color=Color.YELLOW,
max_distance_mm=170)
shared_all.turn_arc(distance=30, angle=15, speed_mm_s=60)
#lift weight and back up
crane_motor.stop()
shared_all.move_crane_down(motor=crane_motor, degrees=10)
crane_motor.run_time(600, 900)
# shared_all.move_crane_to_floor(crane_motor)
# shared_all.move_crane_up( motor = crane_motor, degrees = 40)
shared_all.start_moving_crane_to_angle(motor=crane_motor, target_angle=25)
shared_all.move_straight(distance_mm=70, speed_mm_s=-120)
# turn west and back up
shared_all.turn(-140, speed_deg_s=200)
shared_all.start_moving_crane_to_top(motor=crane_motor)
shared_all.move_straight(distance_mm=70, speed_mm_s=-150)
shared_all.turn_arc(distance=50, angle=60, speed_mm_s=190)
def run_arp():
shared_all.move_crane_to_top(crane_motor)
shared_all.move_crane_to_floor(crane_motor)
shared_all.turn(-10)
shared_all.move_straight(
distance_mm=300,
speed_mm_s=-190,
)
def alignreverse(adjust_for_mission=0):
shared_all.turn(angle=-30)
shared_all.move_to_color(
color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=180)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
def align():
shared_all.move_reverse( max_distance=30 , speed_mm_s= 100)
shared_all.turn(angle=-45, speed_deg_s=100)
shared_all.move_straight(distance_mm=60, speed_mm_s=100)
# Calibrate the gyro point in the direction at the start
# shared_all.calibrate_gyro(0)
# align()
def row_to_weight_right_angleTurns(adjust_for_mission=0):
shared_all.turn(angle=-40)
shared_all.turn_arc(distance=50, angle=-60, speed_mm_s=80)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=70,
speed_mm_s=160,
target_angle=-90 +
adjust_for_mission)
def runold(adjust_for_mission=0):
shared_all.turn_to_direction( gyro=gyro, target_angle=-37+ adjust_for_mission)
shared_all.move_rack_to_floor()
shake()
shared_all.drive_raising_crane(duration_ms=1900, robot_distance_mm=-80, robot_turn_angle=-20,
motor=rack_motor, crane_angle=-5)
shared_all.drive_raising_crane(duration_ms=1900, robot_distance_mm=25, robot_turn_angle=-50,
motor=rack_motor, crane_angle=-5)
shared_all.turn(8)
shared_all.drive_raising_crane(duration_ms=1900, robot_distance_mm=-10, robot_turn_angle=0,
motor=rack_motor, crane_angle=110)
def align():
# shared_all.move_straight(distance_mm=50, speed_mm_s=200)
# shared_all.turn(angle=90, speed_deg_s=200)
# shared_all.move_reverse(max_distance=65, speed_mm_s=75)
# shared_all.move_straight(distance_mm=450, speed_mm_s=175)
# shared_all.turn(angle=15, speed_deg_s=200)
# shared_all.turn_arc(distance=15,angle=15,speed_mm_s=50)
# shared_all.move_straight(distance_mm=138, speed_mm_s=200)
shared_all.start_moving_crane_to_angle(crane_motor, 45)
shared_all.move_rack_to_top()
shared_all.move_straight(distance_mm=110, speed_mm_s=120)
shared_all.turn(35)
def run(adjust_for_mission=0):
shared_all.turn(5)
#
shared_all.move_straight(distance_mm=190, speed_mm_s=-210)
right_motor.run_angle(-90, 50, Stop.BRAKE, True)
left_motor.run_angle(-270, 3 * 360, Stop.BRAKE, True)
shared_all.move_straight(distance_mm=120, speed_mm_s=180)
shared_all.move_straight(distance_mm=20, speed_mm_s=90)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
def level2crane():
shared_all.move_rack_to_floor()
shared_all.log_string('Moved racktotop')
shared_all.move_crane_up(motor=rack_motor, degrees=40)
shared_all.turn(angle=20)
shared_all.drive_raising_crane(duration_ms=500,
robot_distance_mm=20,
robot_turn_angle=0,
motor=rack_motor,
crane_angle=190)
shared_all.move_straight(distance_mm=40, speed_mm_s=-90)
shared_all.move_crane_down(motor=rack_motor, degrees=90)
shared_all.log_string('MOtor is ' + str(crane_motor))
def weightpushdown_to_phone(adjust_for_mission=0):
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=90,
speed_mm_s=110,
target_angle=-90 +
adjust_for_mission)
shared_all.turn(-90)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=60,
speed_mm_s=110,
target_angle=180 +
adjust_for_mission)
def level1():
shared_all.move_crane_to_top(motor=crane_motor)
shared_all.move_rack_to_floor()
shared_all.turn(angle=-10, speed_deg_s=90)
shared_all.move_straight(distance_mm=90, speed_mm_s=90)
shared_all.turn(angle=20, speed_deg_s=90)
# rack_motor.stop(Stop.BRAKE)
# cntr=0
# while cntr < 4 and shared_all.did_motor_stall(motor =rack_motor , max_degrees =80 , speed = 320):
# shared_all.log_string('Mtr stalled')
# shared_all.move_crane_down(rack_motor, 5)
# shared_all.log_string('mved crane down')
# shared_all.move_rack_to_floor()
# shared_all.move_straight(distance_mm=6, speed_mm_s=20)
# cntr +=1
# shared_all.move_rack_to_floor()
# shared_all.move_crane_down(rack_motor, 5)
# rack_motor.run_time(720, 900)
shared_all.drive_raising_crane(duration_ms=1300,
robot_distance_mm=-40,
robot_turn_angle=0,
motor=rack_motor,
crane_angle=80)
shared_all.drive_raising_crane(duration_ms=1300,
robot_distance_mm=0,
robot_turn_angle=0,
motor=rack_motor,
crane_angle=50)
shared_all.drive_raising_crane(duration_ms=900,
robot_distance_mm=20,
robot_turn_angle=0,
motor=rack_motor,
crane_angle=15)
# back up while lowering slightly
shared_all.drive_raising_crane(duration_ms=800,
robot_distance_mm=-70,
robot_turn_angle=0,
motor=rack_motor,
crane_angle=-20)
shared_all.move_rack_to_floor()
shared_all.move_straight(distance_mm=60, speed_mm_s=-90)
shared_all.move_rack_to_top()
def alignold(adjust_for_mission=0):
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=15,
speed_mm_s=110,
target_angle=180 +
adjust_for_mission)
shared_all.move_crane_to_top(motor=crane_motor)
shared_all.turn(90, speed_deg_s=160)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=10,
speed_mm_s=-110,
target_angle=-90 +
adjust_for_mission)
def stepcounter_to_treadmill_reverse(adjust_for_mission=0):
shared_all.move_straight(distance_mm=160, speed_mm_s=150)
shared_all.turn(angle=-10)
shared_all.turn_arc(distance=330, angle=-50, speed_mm_s=-150)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=400,
speed_mm_s=-170,
target_angle=180)
shared_all.move_to_color_reverse(
color_sensor=color_sensor_center,
stop_on_color=Color.WHITE,
alternative_color=Color.WHITE,
min_intensity=robot_setup.WHITE_MIN_INTENSITY[color_sensor_center],
max_distance_mm=180)
def runpushdown():
shared_all.move_rack_to_top()
shared_all.move_straight(distance_mm=110, speed_mm_s=120)
shared_all.turn(35)
shared_all.move_crane_down(rack_motor, 210)
shared_all.turn(-35)
shared_all.move_straight(distance_mm=50, speed_mm_s=-120)
shared_all.start_moving_crane_to_top(rack_motor)
shared_all.move_to_color_reverse(
color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=70)
shared_all.move_rack_to_top()
def basket_to_bench():
shared_all.turn_arc(distance=90, angle=-55, speed_mm_s=-70)
shared_all.move_crane_to_top(motor=rack_motor)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
shared_all.move_to_color_reverse(color_sensor=color_sensor_center,
stop_on_color=Color.WHITE,
alternative_color=Color.WHITE)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=120,
speed_mm_s=140,
target_angle=180 +
adjust_for_mission)
shared_all.move_crane_to_top(motor=crane_motor)
shared_all.turn(-60)
def row_to_weight_pushdown(adjust_for_mission=0):
shared_all.turn(angle=-40)
shared_all.turn_arc(distance=50, angle=-60, speed_mm_s=80)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=70,
speed_mm_s=160,
target_angle=-90 +
adjust_for_mission)
shared_all.move_to_color(
color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=120)
def turn_points_90():
shared_all.move_straight(distance_mm=90, speed_mm_s=140)
shared_all.turn(110, 190)
wait(2000)
shared_all.turn(-110, 190)
wait(2000)
shared_all.turn(90)
wait(3000)
shared_all.turn(-90)
def alignold(adjust_for_mission=0):
shared_all.move_to_color(color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK)
shared_all.move_straight(distance_mm=10, speed_mm_s=70)
shared_all.turn(-90)
shared_all.move_straight(distance_mm=170, speed_mm_s=-170)
shared_all.push_back_reset_gyro(distance_mm=60,
reset_gyro=False,
new_gyro_angle=0)
shared_all.move_straight(distance_mm=160, speed_mm_s=150)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
shared_all.move_to_color_reverse(color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK)
def row_to_weight_curved(adjust_for_mission=0):
shared_all.turn(angle=-55)
shared_all.move_straight(distance_mm=280, speed_mm_s=170)
# shared_all.move_to_color(color_sensor=color_sensor_center,
# stop_on_color=Color.BLACK, alternative_color=Color.BLACK,
# max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
# max_distance_mm=70)
shared_all.turn(angle=-75, speed_deg_s=180)
# shared_all.turn_arc(distance=100 , angle=-70, speed_mm_s=150)
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=230,
speed_mm_s=-160,
target_angle=-170 +
adjust_for_mission)
shared_all.push_back_reset_gyro(distance_mm=50,
reset_gyro=True,
new_gyro_angle=180)
def run(adjust_for_mission=0):
# shared_all.move_straight(distance_mm=190, speed_mm_s=-210)
# left_motor.run_angle( -210, 3*360, Stop.BRAKE, True)
shared_all.turn(5)
#
shared_all.move_straight(distance_mm=190, speed_mm_s=-210)
right_motor.run_angle(-90, 50, Stop.BRAKE, True)
left_motor.run_angle(-270, 3 * 360, Stop.BRAKE, True)
shared_all.move_straight(distance_mm=230, speed_mm_s=180)
# shared_all.move_straight(distance_mm=20, speed_mm_s=90)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
shared_all.turn(90)
shared_all.push_back_reset_gyro(distance_mm=150,
reset_gyro=True,
new_gyro_angle=-90)
def do_crane_push():
shared_all.move_crane_to_floor(crane_motor)
shared_all.move_crane_up(crane_motor, degrees=135)
shared_all.move_straight(max_distance=60)
shared_all.turn(angle=-50)
# shared_all.move_straight(max_distance=100)
shared_all.move_crane_to_floor(crane_motor)
shared_all.turn(angle=50)
crane_motor.run(40)
shared_all.move_straight(max_distance=130, speed_mm_s=50)
crane_motor.stop(Stop.BRAKE)
shared_all.move_crane_down(crane_motor, degrees=30)
shared_all.move_straight(max_distance=60)
"""for x in range(5):
worker.move_crane_up(robot, crane_motor, degrees=20)
move_robot.move_straight(robot=robot, max_distance=40)"""
shared_all.move_reverse(max_distance=350)
"""worker.move_crane_up(robot, crane_motor, degrees=20)
def align(adjust_for_mission=0):
SLIDE_ALIGN_DIRECTION=138
shared_all.turn(-90, speed_deg_s=140)
shared_all.move_straight(distance_mm=50, speed_mm_s=100)
shared_all.move_to_color_reverse(color_sensor=color_sensor_center,
stop_on_color=Color.BLACK, alternative_color=Color.BLACK, speed_mm_s=60,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=80)
shared_all.move_straight(distance_mm=70, speed_mm_s=-100)
shared_all.turn(90, speed_deg_s=140)
shared_all.move_to_color(color_sensor=color_sensor_center,
stop_on_color=Color.BLACK, alternative_color=Color.BLACK, speed_mm_s=60,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=60)
shared_all.move_straight_target_direction(gyro = gyro,
distance_mm= 40,
speed_mm_s= 130,
target_angle= 180+ adjust_for_mission)
shared_all.turn(SLIDE_ALIGN_DIRECTION - 180, speed_deg_s=100)
shared_all.move_straight_target_direction(gyro = gyro,
distance_mm= 60,
speed_mm_s= -110,
target_angle= SLIDE_ALIGN_DIRECTION+ adjust_for_mission)
shared_all.move_rack_to_floor()
def treadmill_to_row_nearrow(adjust_for_mission=0):
shared_all.turn_arc(distance=110, angle=55,
speed_mm_s=100) # turn in an arc
shared_all.turn(angle=-55, speed_deg_s=120)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
# shared_all.turn_to_direction( gyro=gyro, target_angle=-90+ adjust_for_mission)
shared_all.move_straight(180, -180)
shared_all.push_back_reset_gyro(distance_mm=60,
reset_gyro=True,
new_gyro_angle=180)
shared_all.move_straight(distance_mm=70, speed_mm_s=100)
shared_all.move_to_color(
color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=40)
def stepcounter_to_treadmill_forward(adjust_for_mission=0):
# shared_all.turn( angle=-45)
shared_all.turn_arc(distance=200, angle=-65,
speed_mm_s=150) # turn in an arc
shared_all.turn_arc(distance=200, angle=65,
speed_mm_s=150) # turn in an arc
shared_all.move_straight_target_direction(gyro=gyro,
distance_mm=170,
speed_mm_s=150,
target_angle=0)
# back up and move at an angle to spine black line
shared_all.turn(angle=40, speed_deg_s=90)
shared_all.move_reverse(max_distance=50, speed_mm_s=110)
shared_all.move_to_color(color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=15)
shared_all.move_reverse(max_distance=10, speed_mm_s=80)
shared_all.turn(angle=-40, speed_deg_s=90)
shared_all.turn_to_direction(gyro=gyro, target_angle=0)
def align(adjust_for_mission=0):
# shared_all.move_straight(max_distance=700, speed_mm_s=100)
# wait(1000)
# shared_all.move_crane_to_top(crane_motor)
# wait(10)
shared_all.turn(angle=-30)
shared_all.move_to_color(
color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=90)
# shared_all.move_straight(distance_mm=10, speed_mm_s=30)
shared_all.turn_to_direction(gyro=gyro,
target_angle=180 + adjust_for_mission)
shared_all.move_to_color_reverse(
color_sensor=color_sensor_center,
stop_on_color=Color.BLACK,
alternative_color=Color.BLACK,
max_intensity=robot_setup.BLACK_MAX_INTENSITY[color_sensor_center],
max_distance_mm=180)
def row_to_weight():
shared_all.turn(angle=-90, speed_deg_s=100)
shared_all.move_straight(distance_mm=40, speed_mm_s=100)
shared_all.turn(angle=90, speed_deg_s=100)
shared_all.move_straight(distance_mm=50, speed_mm_s=100)
shaared_all.turn(angle=-90, speed_deg_s=100)
