import hiwonder
import time
import csv

# 初始化JetMax机械臂
jetmax = hiwonder.JetMax()

# 定义记录频率(Hz)，即每秒记录次数
record_freq = 5

# 打开 CSV 文件用于写入
current_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())
step = 0
with open(f'servo_positions-{current_time}.csv', 'w', newline='') as csvfile:
    csv_writer = csv.writer(csvfile)
    # 写入CSV表头，定义数据列
    csv_writer.writerow(['Step', 'Servo 1', 'Servo 2', 'Servo 3'])

    while True:
        step += 1
        # 读取三个舵机的当前位置
        servo_1 = hiwonder.serial_servo.read_position(1)  # 读取舵机1位置
        servo_2 = hiwonder.serial_servo.read_position(2)  # 读取舵机2位置
        servo_3 = hiwonder.serial_servo.read_position(3)  # 读取舵机3位置
    
        # 打印当前步骤和舵机位置信息到控制台
        print(f'Step: {step}, servo_1: {servo_1}, servo_2: {servo_2}, servo_3: {servo_3}')
        
        # 将数据写入CSV文件，步骤时间通过step/record_freq计算
        csv_writer.writerow([step / record_freq, servo_1, servo_2, servo_3])
        csvfile.flush()  # 强制将缓冲区数据写入文件，确保数据实时保存
        
        # 根据记录频率设置延时
        time.sleep(1 / record_freq)