def run():
IMU = arduino.IMU()
start = (0, 0)
end = (200, 200)
grid, start, end = mapmaker.read()
mToI = 1
posX = buggyinfo.Position(start[0], metersToIndex=mToI)
posY = buggyinfo.Position(start[1], metersToIndex=mToI)
posZ = buggyinfo.Position(0, initialV=1, metersToIndex=mToI)
drawer = draw.Drawer(300, 300, 700, 700, start, end)
while drawer.done is False:
accel, gyro, dt = IMU.getIMU()
accel.x = 0.01
accel.y = 0.01
dt = 1
posX.update(accel.x, dt)
posY.update(accel.y, dt)
posZ.update(accel.z, dt)
drawer.updateNode(posX.convert(), posY.convert(), posZ.convert(),
False)
drawer.loop()
drawer.drawParticle(int(posX), int(posY))
drawer.update()
def run():
# Init arduino properties, drawer, and map
# while exit is false:
# get accel and gyro
# smooth with kalman
# get position
# update map (any new blocked nodes? Use image processing, ultrasonic, and other sensors)
# plot astar to end
# plot smooth path through next four nodes, extract angles (bezier curve, if angle is greater than allowed turn radius, eliminate node from path, or if its another buggy, slow the hell down!!!)
# command angles to arduino until next node appears
IMU = arduino.IMU()
posX = buggyinfo.Position(0)
posY = buggyinfo.Position(0)
posZ = buggyinfo.Position(0)
# constants
n_time_steps = 10000
n_dim_state = 3
filtered_state_means = np.zeros((n_time_steps, n_dim_state))
filtered_state_covariances = np.zeros(
(n_time_steps, n_dim_state, n_dim_state))
kf = make_filter()
t = 0
#threshhold
th = 1
while True:
accel, gyro, magnet, dt = IMU.getIMU() # get accel and gyro
# if (t == 0):
# filtered_state_means[t] = numpy.array(
# [accel.x, accel.y, accel.z])
# filtered_state_covariances[t] = numpy.eye(n_dim_state)
if t < 200:
pass
elif t == 200:
filtered_state_means[t] = numpy.array(
[accel.x, accel.y, accel.z])
filtered_state_covariances[t] = numpy.eye(n_dim_state)
print "passed"
else:
if abs(accel.x) < th:
accel.x = 0
if abs(accel.y) < th:
accel.y = 0
if abs(accel.z) < th:
accel.z = 0
obs = numpy.array([accel.x, accel.y, accel.z])
results = kf.filter_update(filtered_state_means[t],
filtered_state_covariances[t], obs,
observation_offset = np.array([dt,dt,dt]))
filtered_state_means[t + 1], filtered_state_covariances[
t + 1] = results
x, y, z = filtered_state_means[t + 1]
if (abs(x - filtered_state_means[t][0]) > 3):
x = 0
if (abs(y - filtered_state_means[t][1]) > 3):
y = 0
if (abs(z - filtered_state_means[t][2]) > 3):
z = 0
if abs(x) < th:
x = 0
if abs(y) < th:
y =0
if abs(z) < th:
z = 0
# get position
posX.update(x, dt)
posY.update(y, dt)
posZ.update(z, dt)
# magnet.roll, magnet.pitch, magnet.yaw = \
# buggyinfo.convertToHeading(magnet.x, magnet.y, magnet.z)
print "%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s" % (
accel.x, accel.y, accel.z,
x, y, z, int(posX),
int(posY), int(posZ))
t += 1
arduino.arduino.stop()
selector = raw_input("IMU (i) or stepper (s): ")
goalStep = 0
if selector == 's':
goalStep = convertToBytes(int(raw_input("goal step: ")))
if selector == 's':
writeSteps(goalStep)
else:
board.write(selector)
print "".join(board.readUntil()).split(',')
except KeyboardInterrupt:
pass
elif "oop1" in arguments:
arduino.initBoard(disabled=False, sketchDir="Uno Board Tests/OOP Test")
stepper = arduino.Stepper(1)
IMU = arduino.IMU()
try:
while True:
selector = raw_input("IMU (i) or stepper (s): ")
if selector == 's':
subSelector = int(
raw_input(
"0 = goal reached, 1 = move to, 2 = set speed\n selector: "
))
if subSelector == 0:
print stepper.goalReached()
elif subSelector == 1:
goalStep = int(raw_input("goal step: "))
stepper.moveTo(goalStep)
elif subSelector == 2:
# A simple example that set up an IMU and prints out the raw values received from it
import sys
sys.path.append("../..")
import time
import arduino
ard = arduino.Arduino()
imu = arduino.IMU(ard)
ard.run()
while True:
print imu.getRawValues()
time.sleep(0.05)
def run():
# Init arduino properties, drawer, and map
# while exit is false:
# get accel and gyro
# smooth with kalman
# get position
# update map (any new blocked nodes? Use image processing, ultrasonic, and other sensors)
# plot astar to end
# plot smooth path through next four nodes, extract angles (bezier curve, if angle is greater than allowed turn radius, eliminate node from path, or if its another buggy, slow the hell down!!!)
# command angles to arduino until next node appears
dcmotor = arduino.DCMotor(1)
IMU = arduino.IMU()
button1 = arduino.Button(3)
sonar1 = arduino.Sonar(1)
# grid, start, end = mapmaker.read()
grid, start, end = mapmaker.make(30, 30), (0, 0), (5, 5)
height, width = grid.shape[0:2]
posX = buggyinfo.Position(start[0])
posY = buggyinfo.Position(start[1])
posZ = buggyinfo.Position(0)
drawer = draw.Drawer(width, height, 700, 700, start, end, grid)
dcmotor.driveForward(100)
while drawer.done is False:
# time0 = time.time()
accel, gyro, magnet, dt = IMU.getIMU() # get accel and gyro
# get position
posX.update(accel.x, dt)
posY.update(accel.y, dt)
posZ.update(accel.z, dt)
# magnet.roll, magnet.pitch, magnet.yaw = \
# buggyinfo.convertToHeading(magnet.x, magnet.y, magnet.z)
# print "(%0.3f, %0.3f, %0.3f)\t(%0.3f, %0.3f, %0.3f)" % \
# (gyro.roll, gyro.pitch, gyro.yaw,
# magnet.roll, magnet.pitch, magnet.yaw)
# print "%s\t%s\t%s" % (accel.x, accel.y, accel.z)
# print(sonar1.getDistance())
# if button1.wasPressed():
# dcmotor.reverse()
# update map (any new blocked nodes? Use image processing, ultrasonic, and other sensors)
# ignore until below routines are finished
# plot astar to end
# path, pathInfo, plotTime = astar.search(grid, (posX, posY), end)
# plot smooth path through next four nodes, extract angles
# command angles to arduino until next node appears
drawer.loop()
drawer.drawParticle(int(posX), int(posY))
drawer.update()
# print time.time() - time0
drawer.deinit()
arduino.arduino.stop()