from time import sleep

from src.robot_controllers.dynamixel_robot.calibration.gravity_compensation import Servo2Manager

manager = Servo2Manager(debug=True)
manager.start_no_keyboard()

sleep(2)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(4)
manager.move_current_servo_down()
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(4)
manager.switch_servo()
manager.move_current_servo_down()
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(4)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(4)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(4)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(3)
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
manager.move_current_servo_up()
sleep(4)
manager.switch_servo()
manager.move_current_servo_down()
manager.switch_servo()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
manager.move_current_servo_down()
sleep(3)
manager.record_offset()
sleep(1)
manager.print_offset_json()