Robomaster SDK紅外線感測器測距 python pyplot.polar畫地圖

 

Python code 如下:

import time
import numpy as np
from robomaster import robot

ep_robot = robot.Robot()
ep_robot.initialize(conn_type="ap")

ep_gimbal = ep_robot.gimbal
ep_sensor = ep_robot.sensor


ep_robot.set_robot_mode(mode='free')



dd=[]
aa=[]
def sub_data_handler(sub_info):
    d = sub_info
    dd.append(d[0])
#    print(d)

def sub_angle_handler(angle_info):
    pitch_angle, yaw_angle, pitch_ground_angle, yaw_ground_angle = angle_info
    a=-yaw_ground_angle/180*np.pi
    aa.append(a)
#    print("gimbal angle: pitch_angle:{0}, yaw_angle:{1}, pitch_ground_angle:{2}, yaw_ground_angle:{3}".format(
#       pitch_angle, yaw_angle, pitch_ground_angle, yaw_ground_angle))

ep_gimbal.moveto(pitch=0, yaw=-180).wait_for_completed() 

ep_sensor.sub_distance(freq=10, callback=sub_data_handler)
ep_gimbal.sub_angle(freq=10, callback=sub_angle_handler)
ep_gimbal.moveto(pitch=0, yaw=180, yaw_speed=10).wait_for_completed()

ep_gimbal.unsub_angle()
ep_sensor.unsub_distance()

ep_gimbal.moveto(pitch=0, yaw=0).wait_for_completed()

ll=min(len(aa), len(dd))
ll


import matplotlib.pyplot as plt
 
plt.polar(aa[:ll], dd[:ll], 'b+')
ax = plt.gca() # gets the active axis 
ax.set_rmax(700)
ax.set_aspect(1) 
plt.savefig('polar.png', dpi=300)
plt.show()