import vpython as vp
from robot_imu import RobotImu, ImuFusion
from delta_timer import DeltaTimer
import imu_settings
imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets,
mag_offsets=imu_settings.mag_offsets)
fusion = ImuFusion(imu)
vp.graph(xmin=0, xmax=60, scroll=True)
graph_pitch = vp.gcurve(color=vp.color.red)
graph_roll = vp.gcurve(color=vp.color.green)
graph_yaw = vp.gcurve(color=vp.color.blue)
timer = DeltaTimer()
while True:
vp.rate(100)
dt, elapsed = timer.update()
fusion.update(dt)
graph_pitch.plot(elapsed, fusion.pitch)
graph_roll.plot(elapsed, fusion.roll)
graph_yaw.plot(elapsed, fusion.yaw)
import vpython as vp
from robot_imu import RobotImu
import time
import imu_settings
import virtual_robot
imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
pitch = 0
roll = 0
yaw = 0
model = virtual_robot.make_robot()
virtual_robot.robot_view()
latest = time.time()
while True:
vp.rate(1000)
current = time.time()
dt = current - latest
latest = current
gyro = imu.read_gyroscope()
roll += gyro.x * dt
pitch += gyro.y * dt
yaw += gyro.z * dt
# reset the model
model.up = vp.vector(0, 1, 0)
model.axis = vp.vector(1, 0, 0)
# Reposition it
import vpython as vp
from robot_imu import RobotImu
from delta_timer import DeltaTimer
import imu_settings
imu = RobotImu(mag_offsets=imu_settings.mag_offsets)
vp.graph(xmin=0, xmax=60, scroll=True)
graph_yaw = vp.gcurve(color=vp.color.blue)
timer = DeltaTimer()
while True:
vp.rate(100)
dt, elapsed = timer.update()
mag = imu.read_magnetometer()
yaw = -vp.degrees(vp.atan2(mag.y, mag.x))
graph_yaw.plot(elapsed, yaw)
import vpython as vp
import logging
import time
from robot_imu import RobotImu
logging.basicConfig(level=logging.INFO)
imu = RobotImu()
pr = vp.graph(xmin=0, xmax=60, scroll=True)
graph_pitch = vp.gcurve(color=vp.color.red, graph=pr)
graph_roll = vp.gcurve(color=vp.color.green, graph=pr)
xyz = vp.graph(xmin=0, xmax=60, scroll=True)
graph_x = vp.gcurve(color=vp.color.orange, graph=xyz)
graph_y = vp.gcurve(color=vp.color.cyan, graph=xyz)
graph_z = vp.gcurve(color=vp.color.purple, graph=xyz)
start = time.time()
while True:
vp.rate(100)
elapsed = time.time() - start
pitch, roll = imu.read_accelerometer_pitch_and_roll()
raw_accel = imu.read_accelerometer()
graph_pitch.plot(elapsed, pitch)
graph_roll.plot(elapsed, roll)
print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
graph_x.plot(elapsed, raw_accel.x)
graph_y.plot(elapsed, raw_accel.y)
graph_z.plot(elapsed, raw_accel.z)
import vpython as vp
import time
from robot_imu import RobotImu
imu = RobotImu()
vp.graph(xmin=0, xmax=60, scroll=True)
graph_x = vp.gcurve(color=vp.color.red)
graph_y = vp.gcurve(color=vp.color.green)
graph_z = vp.gcurve(color=vp.color.blue)
start = time.time()
while True:
vp.rate(100)
elapsed = time.time() - start
gyro = imu.read_gyroscope()
print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
graph_x.plot(elapsed, gyro.x)
graph_y.plot(elapsed, gyro.y)
graph_z.plot(elapsed, gyro.z)
import vpython as vp
import logging
from robot_imu import RobotImu
from robot_pose import robot_view
logging.basicConfig(level=logging.INFO)
imu = RobotImu()
robot_view()
mag_arrow = vp.arrow(pos=vp.vector(0, 0, 0))
x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
while True:
vp.rate(100)
mag = imu.read_magnetometer()
mag_arrow.axis = mag.norm()
print(f"Magnetometer: {mag}")
"""When running,
use VPYTHON_PORT=9020 VPYTHON_NOBROWSER=true"""
import vpython as vp
import time
import logging
from robot_imu import RobotImu
logging.basicConfig(level=logging.INFO)
imu = RobotImu()
vp.graph(xmin=0, xmax=60, scroll=True)
temp_graph = vp.gcurve()
start = time.time()
while True:
vp.rate(100)
temperature = imu.read_temperature()
logging.info("Temperature {}".format(temperature))
elapsed = time.time() - start
temp_graph.plot(elapsed, temperature)
import vpython as vp
import logging
from robot_imu import RobotImu
from robot_pose import robot_view
logging.basicConfig(level=logging.INFO)
imu = RobotImu()
robot_view()
accel_arrow = vp.arrow(axis=vp.vector(0, 1, 0))
x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
while True:
vp.rate(100)
accel = imu.read_accelerometer()
print(f"Accelerometer: {accel}")
accel_arrow.axis = accel.norm()
import vpython as vp
from robot_imu import RobotImu
from imu_settings import mag_offsets
imu = RobotImu(mag_offsets=mag_offsets)
mag_min = vp.vector(0, 0, 0)
mag_max = vp.vector(0, 0, 0)
scatter_xy = vp.gdots(color=vp.color.red)
scatter_yz = vp.gdots(color=vp.color.green)
scatter_zx = vp.gdots(color=vp.color.blue)
while True:
vp.rate(100)
mag = imu.read_magnetometer()
mag_min.x = min(mag_min.x, mag.x)
mag_min.y = min(mag_min.y, mag.y)
mag_min.z = min(mag_min.z, mag.z)
mag_max.x = max(mag_max.x, mag.x)
mag_max.y = max(mag_max.y, mag.y)
mag_max.z = max(mag_max.z, mag.z)
offset = (mag_max + mag_min) / 2
print(f"Magnetometer: {mag}. Offsets: {offset}.")
scatter_xy.plot(mag.x, mag.y)
scatter_yz.plot(mag.y, mag.z)
scatter_zx.plot(mag.z, mag.x)
