import hiwonder
import time
import csv

jetmax = hiwonder.JetMax()

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_writer.writerow(['Step', 'Servo 1', 'Servo 2', 'Servo 3'])

    while True:
        step += 1
        servo_1 = hiwonder.serial_servo.read_position(1)
        servo_2 = hiwonder.serial_servo.read_position(2)
        servo_3 = hiwonder.serial_servo.read_position(3)
    
        
        print(f'Step: {step}, servo_1: {servo_1}, servo_2: {servo_2}, servo_3: {servo_3}')
        
        # 将数据写入 CSV 文件
        csv_writer.writerow([step / record_freq, servo_1, servo_2, servo_3])
        csvfile.flush()  # 立即将数据写入文件
        
        time.sleep(1 / record_freq)