def gyro_color_test_angled():
shared_all.log_string('Pre-run gyro speed ' + str(shared_all.gyro.speed()) + ' angle:' + str(shared_all.gyro.angle()))
shared_all.move_crane_to_floor(crane_motor)
shared_all.move_crane_up(crane_motor, degrees=135)
shared_all.move_straight(max_distance=800, speed_mm_s=200)
shared_all.log_string('post Run speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
shared_all.turn_to_direction(gyro, target_angle=45)
shared_all.log_string('post +45 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
shared_all.move_to_color(color_sensor=color_sensor_left,
stop_on_color=Color.BLACK, speed_mm_s = 70)
shared_all.turn_to_direction(gyro, target_angle=90)
shared_all.log_string('post +90 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
shared_all.move_straight(max_distance=100)
shared_all.move_crane_to_floor(crane_motor)
shared_all.move_crane_up(crane_motor, degrees=45)
shared_all.move_crane_to_floor(crane_motor)
shared_all.move_reverse(max_distance=100)
shared_all.move_crane_up(crane_motor, degrees=135)
shared_all.turn_to_direction(gyro, target_angle=210)
shared_all.log_string('post +210 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
shared_all.move_straight(max_distance=1300)
shared_all.log_string('post Run speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
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 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 runold ():
shared_all.move_rack_to_floor ()
shared_all.move_straight(distance_mm=60, speed_mm_s=120)
while shared_all.did_motor_stall(motor =rack_motor , max_degrees =50 , speed = 320):
# log_string('stalled - step back')
shared_all.move_reverse(max_distance=6, speed_mm_s=20)
shared_all.move_crane_to_angle(motor=rack_motor, target_angle=90)
shared_all.drive_raising_crane (duration_ms=900, robot_distance_mm=80, robot_turn_angle=0,
motor=rack_motor, crane_angle=-20)
shared_all.move_crane_to_angle(motor=rack_motor, target_angle=120)
shared_all.move_reverse(max_distance=20, speed_mm_s=100)
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 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 weight_to_cell():
shared_all.move_reverse( max_distance=25 , speed_mm_s= 100)
shared_all.turn(angle=-90, speed_deg_s=100)
shared_all.move_reverse(max_distance=55, speed_mm_s=100)
def phone_to_bigt(adjust_for_mission=0):
shared_all.move_reverse(max_distance=130, speed_mm_s=160)
def align():
shared_all.move_straight(distance_mm=220, speed_mm_s=100)
shared_all.turn(angle=79, speed_deg_s=100)
shared_all.turn(angle=-10, speed_deg_s=100)
shared_all.move_reverse(max_distance=20, speed_mm_s=100)
shared_all.move_straight(distance_mm=23, speed_mm_s=50)
