feat: servo data plot

servo_positions-2024-11-08 11:31:32.csv

@@ -1,119 +0,0 @@
-Step,Servo 1,Servo 2,Servo 3
-0.2,407,247,623
-0.4,405,247,624
-0.6,405,247,624
-0.8,407,247,623
-1.0,407,247,623
-1.2,405,247,623
-1.4,406,247,624
-1.6,406,247,624
-1.8,405,247,624
-2.0,406,246,624
-2.2,407,247,623
-2.4,406,247,624
-2.6,405,252,623
-2.8,406,247,624
-3.0,407,247,624
-3.2,406,247,624
-3.4,408,247,624
-3.6,411,251,626
-3.8,412,250,627
-4.0,416,251,628
-4.2,416,252,629
-4.4,416,253,629
-4.6,416,252,629
-4.8,417,252,629
-5.0,416,253,629
-5.2,418,252,626
-5.4,418,252,627
-5.6,418,242,621
-5.8,418,242,621
-6.0,418,241,620
-6.2,418,239,617
-6.4,413,236,616
-6.6,413,236,613
-6.8,413,236,613
-7.0,413,240,615
-7.2,413,239,616
-7.4,410,245,623
-7.6,408,245,621
-7.8,409,247,624
-8.0,406,247,624
-8.2,402,246,621
-8.4,402,245,624
-8.6,402,236,618
-8.8,402,228,608
-9.0,402,226,605
-9.2,402,227,606
-9.4,406,227,605
-9.6,409,228,607
-9.8,410,229,606
-10.0,410,236,615
-10.2,411,245,621
-10.4,408,245,622
-10.6,408,247,624
-10.8,413,249,625
-11.0,414,251,628
-11.2,416,253,629
-11.4,415,253,628
-11.6,417,252,628
-11.8,416,251,628
-12.0,418,250,627
-12.2,417,248,626
-12.4,415,248,626
-12.6,415,247,626
-12.8,413,244,624
-13.0,408,240,620
-13.2,408,240,618
-13.4,402,237,618
-13.6,400,232,613
-13.8,397,232,611
-14.0,394,228,608
-14.2,395,228,607
-14.4,392,230,610
-14.6,391,238,617
-14.8,391,242,619
-15.0,389,247,622
-15.2,389,249,627
-15.4,388,252,629
-15.6,395,255,629
-15.8,397,258,634
-16.0,401,261,636
-16.2,402,263,638
-16.4,404,262,638
-16.6,405,253,632
-16.8,407,245,625
-17.0,407,245,622
-17.2,407,240,620
-17.4,405,236,617
-17.6,402,235,614
-17.8,399,232,613
-18.0,397,234,613
-18.2,397,241,619
-18.4,394,248,625
-18.6,393,251,629
-18.8,391,256,627
-19.0,399,258,634
-19.2,402,261,637
-19.4,402,262,637
-19.6,404,262,637
-19.8,406,253,630
-20.0,407,245,625
-20.2,407,244,621
-20.4,407,244,621
-20.6,407,240,619
-20.8,403,237,616
-21.0,397,232,613
-21.2,398,232,611
-21.4,397,232,610
-21.6,397,232,610
-21.8,398,232,610
-22.0,397,232,610
-22.2,397,232,611
-22.4,397,232,610
-22.6,398,232,610
-22.8,398,232,611
-23.0,397,232,610
-23.2,397,233,610
-23.4,397,232,610
-23.6,397,232,610

utils/data_record.py

@@ -2,8 +2,10 @@ import hiwonder
 import time
 import csv
 
+# 初始化JetMax机械臂
 jetmax = hiwonder.JetMax()
 
+# 定义记录频率(Hz)，即每秒记录次数
 record_freq = 5
 
 # 打开 CSV 文件用于写入
@@ -11,20 +13,22 @@ 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)
-        servo_2 = hiwonder.serial_servo.read_position(2)
-        servo_3 = hiwonder.serial_servo.read_position(3)
-    
+        # 读取三个舵机的当前位置
+        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 文件
+        # 将数据写入CSV文件，步骤时间通过step/record_freq计算
         csv_writer.writerow([step / record_freq, servo_1, servo_2, servo_3])
-        csvfile.flush()  # 立即将数据写入文件
+        csvfile.flush()  # 强制将缓冲区数据写入文件，确保数据实时保存
         
+        # 根据记录频率设置延时
         time.sleep(1 / record_freq)

utils/plot_servo.py

@@ -0,0 +1,57 @@
+import pandas as pd
+import seaborn as sns
+import matplotlib.pyplot as plt
+import argparse
+import sys
+
+def plot_servo_data(csv_file):
+    # Read CSV file
+    try:
+        df = pd.read_csv(csv_file)
+    except FileNotFoundError:
+        print(f"Error: File '{csv_file}' not found")
+        sys.exit(1)
+    except Exception as e:
+        print(f"Error reading CSV file: {e}")
+        sys.exit(1)
+
+    # Set plot style
+    sns.set_style("whitegrid")
+    plt.figure(figsize=(12, 6))
+
+    # Plot servo angles
+    sns.lineplot(data=df, x='Step', y='Servo 1', label='Servo 1')
+    sns.lineplot(data=df, x='Step', y='Servo 2', label='Servo 2')
+    sns.lineplot(data=df, x='Step', y='Servo 3', label='Servo 3')
+
+    # Set title and labels
+    plt.title('Servo Angles vs Time')
+    plt.xlabel('Time (seconds)')
+    plt.ylabel('Angle')
+
+    # Add legend
+    plt.legend(title='Servo ID')
+
+    # Optimize display
+    plt.tight_layout()
+
+    # Generate output filename based on input
+    output_file = csv_file.rsplit('.', 1)[0] + '_plot.png'
+    
+    # Save plot
+    plt.savefig(output_file, dpi=300, bbox_inches='tight')
+    print(f"Plot saved as: {output_file}")
+
+def main():
+    # Set up argument parser
+    parser = argparse.ArgumentParser(description='Plot servo angles from CSV file')
+    parser.add_argument('csv_file', help='Path to the CSV file containing servo data')
+    
+    # Parse arguments
+    args = parser.parse_args()
+    
+    # Generate plot
+    plot_servo_data(args.csv_file)
+
+if __name__ == "__main__":
+    main()