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.switch_servo()
sleep(1)
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew1")
sleep(3)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
sleep(3)
manager.switch_servo()
manager.move_current_servo_up()
sleep(1)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew2")
sleep(4)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.switch_servo()
manager.move_current_servo_up()
sleep(3)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew3")
sleep(3)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.switch_servo()
manager.move_current_servo_up()
sleep(1)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew4")
sleep(4)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.switch_servo()
manager.move_current_servo_up()
sleep(1)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew5")
sleep(4)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.switch_servo()
manager.move_current_servo_up()
sleep(1)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew6")
sleep(4)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.switch_servo()
manager.move_current_servo_up()
sleep(1)
manager.switch_servo()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew7")
sleep(3)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.switch_servo()
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()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
manager.move_current_servo_down()
print("wew8")
sleep(4)
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.move_current_servo_up()
sleep(3)
manager.record_offset()
manager.print_offset_json()

