g1 = x + l * cos(theta)
g2 = y + l * sin(theta)
g3 = theta
return array([g1, g2, g3])
if __name__ == '__main__':
scanner_displacement = 30.0 # scanner's position related to robot's coordinate
ticks_to_mm = 0.349 # convert ticks from motor to distance
robot_width = 155.0
# initial state
state = array([1850.0, 1897.0, 213.0 / 180 * pi])
# read data from motor's tick sensor
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
states = []
# loop over all motor ticks to generate a states list
for ticks in logfile.motor_ticks:
control = array(ticks) * ticks_to_mm
state = ExtendedKalmanFilter.g(state, control, robot_width)
states.append(state)
f = open("states_from_ticks.txt", "w")
# output the state of scanner instead of the robot
for s in states:
print >> f, "F %f %f %f" % \
tuple(s + [scanner_displacement*cos(s[2]), scanner_displacement*sin(s[2]),0.0])
f.close()
# Plot a scan of the robot using matplotlib.
# 03_a_plot_scan
# XU Shang, 2016-10-13
from pylab import plot,show
from lego_robot import LegoLogfile
# Read the logfile which contains all scans.
logfile = LegoLogfile()
logfile.read("robot4_scan.txt")
# Plot one scan.
plot(logfile.scan_data[10])
show()
# Plot the increments of the left and right motor.
# 01_c_plot_motor_increments.py
# Claus Brenner, 07 NOV 2012
from pylab import *
from lego_robot import LegoLogfile
if __name__ == '__main__':
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
plot(logfile.motor_ticks)
show()