from flask import Flask, render_template, request, jsonify
from pypot.dynamixel.io import DxlIO
import time

app = Flask(__name__)

# Initialize the motor connection
motor_IDs = [1, 2, 3, 4]
dxl_io = DxlIO('/dev/tty.usbserial-AH01FPVO', baudrate=1000000)

# Configure motors on startup
dxl_io.set_moving_speed(dict(zip(motor_IDs, ([0, 26, 33, 133]))))
time.sleep(2)
dxl_io.set_angle_limit(dict(zip(motor_IDs, ([(-150, 150), (-110, 110), (-128, 128), (-128, 128)]))))
time.sleep(2)
dxl_io.set_torque_limit(dict(zip(motor_IDs, ([15, 100, 100, 100]))))
time.sleep(5)

@app.route('/')
def index():
    return render_template('index.html', motor_IDs=motor_IDs)

@app.route('/set_position', methods=['POST'])
def set_position():
    data = request.json
    motor_id = data['motor_id']
    position = data['position']
    speed = data['speed']
    torque = data['torque']

    # Send the end position, speed, and torque to the motor
    dxl_io.set_goal_position({motor_id: position})
    # dxl_io.set_moving_speed({motor_id: speed})
    # dxl_io.set_torque_limit({motor_id: torque})

    return jsonify(success=True)

if __name__ == '__main__':
    app.run(debug=True)