def align():
shared_all.move_crane_to_floor( motor=crane_motor)
shared_all.move_crane_up( motor=crane_motor, degrees=60)
shared_all.move_to_color(color_sensor = color_sensor_right,
stop_on_color = Color.GREEN,
alternative_color = Color.GREEN, speed_mm_s=40, max_distance_mm=60)
shared_all.move_straight(distance_mm=17, speed_mm_s=40)
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 run(adjust_for_mission=0):
# shared_all.turn_to_direction( gyro=gyro, target_angle=10)
shared_all.move_rack_to_floor()
shake()
#pull bsck
shared_all.drive_raising_crane(duration_ms=1900, robot_distance_mm=-120, robot_turn_angle=-10,
motor=rack_motor, crane_angle=-40)
#release
shared_all.drive_raising_crane(duration_ms=1900, robot_distance_mm=5, robot_turn_angle=0,
motor=rack_motor, crane_angle=15)
shared_all.move_crane_up(rack_motor, 180)
shared_all.move_rack_to_top()
def level1crane():
shared_all.move_crane_to_floor(motor=crane_motor)
shared_all.move_crane_up(motor=crane_motor, degrees=35)
shared_all.move_straight(distance_mm=60, speed_mm_s=90)
shared_all.move_crane_down(motor=crane_motor, degrees=15)
shared_all.move_straight(distance_mm=8, speed_mm_s=90)
shared_all.drive_raising_crane(duration_ms=700,
robot_distance_mm=-10,
robot_turn_angle=0,
motor=crane_motor,
crane_angle=120)
shared_all.move_crane_down(motor=crane_motor, degrees=30)
shared_all.move_straight(distance_mm=30, speed_mm_s=-90)
def run():
shared_all.move_crane_down(rack_motor, 70)
shared_all.move_hook_to_floor()
shared_all.move_straight(distance_mm=20, speed_mm_s=-50)
shared_all.drive_raising_crane (duration_ms=800, robot_distance_mm=-20, robot_turn_angle=0,
motor=rack_motor, crane_angle=20)
shared_all.drive_raising_crane (duration_ms=1900, robot_distance_mm=-70, robot_turn_angle=0,
motor=rack_motor, crane_angle=30)
shared_all.move_straight(distance_mm=40, speed_mm_s=-130)
shared_all.move_crane_up(rack_motor, 30)
#backup
shared_all.move_straight(distance_mm=600, speed_mm_s=-200)
def all_cranes():
shared_all.log_string('crane ang start ' + str(crane_motor.angle()))
shared_all.move_crane_to_floor(crane_motor)
shared_all.log_string('crane ang flr ' + str(crane_motor.angle()))
shared_all.start_moving_crane_to_top(motor=crane_motor)
shared_all.move_straight(distance_mm=70, speed_mm_s=-50)
shared_all.log_string('crane ang top ' + str(crane_motor.angle()))
shared_all.start_moving_crane_to_angle(motor=crane_motor, target_angle=50)
shared_all.move_straight(distance_mm=70, speed_mm_s=150)
shared_all.log_string('crane ang at 50 ' + str(crane_motor.angle()))
shared_all.log_string('arm ang strt ' + str(rack_motor.angle()))
shared_all.move_rack_to_top()
shared_all.log_string('arm ang top ' + str(rack_motor.angle()))
shared_all.move_rack_to_floor()
shared_all.log_string('arm ang flr ' + str(rack_motor.angle()))
shared_all.move_crane_up(rack_motor, 90)
shared_all.log_string('arm ang flr+90 ' + str(rack_motor.angle()))
shared_all.move_crane_down(rack_motor, 90)
shared_all.log_string('arm ang -90 ' + str(rack_motor.angle()))
shared_all.move_rack_to_top()
shared_all.log_string('arm ang top ' + str(rack_motor.angle()))
shared_all.log_string('crane motor ' + str(crane_motor))
shared_all.log_string('rack motor ' + str(rack_motor))
shared_all.log_string('left motor ' + str(left_motor))
shared_all.log_string('right motor ' + str(right_motor))
shared_all.move_crane_to_floor(crane_motor)
crane_motor.reset_angle(0)
shared_all.log_string('crane ang flr ' + str(crane_motor.angle()))
shared_all.move_crane_to_top(motor=crane_motor)
shared_all.log_string('crane ang top ' + str(crane_motor.angle()))
shared_all.move_rack_to_floor()
rack_motor.reset_angle(0)
shared_all.log_string('rack ang flr ' + str(rack_motor.angle()))
shared_all.move_rack_to_top()
shared_all.log_string('rack ang top ' + str(rack_motor.angle()))
shared_all.log_string('rack top ' + str(rack_motor.angle()) +
'crane top ' + str(crane_motor.angle()))
def runfacing():
shared_all.move_crane_down(rack_motor, 70)
shared_all.move_hook_to_floor()
shared_all.move_straight(distance_mm=20, speed_mm_s=-50)
shared_all.drive_raising_crane (duration_ms=800, robot_distance_mm=-30, robot_turn_angle=0,
motor=rack_motor, crane_angle=20)
shared_all.drive_raising_crane (duration_ms=1900, robot_distance_mm=-90, robot_turn_angle=0,
motor=rack_motor, crane_angle=20)
# shared_all.move_straight(distance_mm=40, speed_mm_s=130)
shared_all.move_crane_up(rack_motor, 70)
shared_all.move_straight(distance_mm=500, speed_mm_s=-200)
shared_all.move_rack_to_top()
# shared_all.calibrate_gyro(new_gyro_angle=-45)
# alignfacing()
# runfacing()
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 run():
##lift
shared_all.move_crane_up(crane_motor, 55)
# shared_all.move_crane_down(crane_motor, 10)
# shared_all.drive_raising_crane(duration_ms=600, robot_distance_mm=0, robot_turn_angle=0,
# motor=crane_motor, crane_angle=110)
# shared_all.move_crane_down(crane_motor, 80)
#knock
# shared_all.move_straight(distance_mm=18, speed_mm_s=-90)
shared_all.move_crane_to_floor(crane_motor)
shared_all.drive_raising_crane(duration_ms=400,
robot_distance_mm=10,
robot_turn_angle=-30,
motor=crane_motor,
crane_angle=-20)
#backup
shared_all.start_moving_crane_to_angle(crane_motor, 70)
shared_all.turn_to_direction(gyro=gyro, target_angle=-85)
shared_all.move_straight(distance_mm=450, speed_mm_s=-220)
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 run():
shared_all.move_crane_to_floor(motor=crane_motor)
shared_all.move_crane_up(motor=crane_motor, degrees=60)
# shared_all.move_crane_down(motor=crane_motor, degrees=30)
shared_all.move_straight(distance_mm=1080, speed_mm_s=-300)
## Below lines only for testing
## Comment out when done testing. Do not upload to Git hub without commenting.
# shared_all.calibrate_gyro(-45)
# align()
# run()
# def move_straight(duration, speed_mm_s):
# robot.drive_time(speed_mm_s, 0, duration)
# robot.stop(stop_type=Stop.BRAKE)
# move_straight(duration=3000, speed_mm_s=300)
# crane_motor=Motor(Port.D, Direction.COUNTERCLOCKWISE, [8,24])
# def move_crane_down( crane_motor, degrees):
# crane_motor.run_angle(90, -1 * degrees, Stop.BRAKE)
# move_crane_down(crane_motor, 30)
