def __init__(self):
self.display = None
print("Booting SpotMicroAI")
self.display = RobotDisplay()
self.gyro = Gyro()
# self.servos=Servos()
self.Kinematic = Kinematic()
def __init__(self):
self.display = None
print("Booting SpotMicroAI")
# self.display=RobotDisplay()
self.gyro = Gyro()
self.servos = Servos()
self.Kinematic = Kinematic()
self.network = Network()
# For testing purposed
self.mode = ModeStandby(self.servos)
class RobotBoot():
def __init__(self):
self.display = None
print("Booting SpotMicroAI")
# self.display=RobotDisplay()
self.gyro = Gyro()
self.servos = Servos()
self.Kinematic = Kinematic()
self.network = Network()
# For testing purposed
self.mode = ModeStandby(self.servos)
def exitHandler(self):
print("Exiting SpotMicroAI")
# if not self.display==None:
# self.display.bye()
sys.exit()
def run(self):
self.mode.init()
while True:
(x, y) = self.gyro.read()
# self.display.setAngles(x,y)
# self.display.setNetworkState(self.network.result())
# self.display.run()
self.mode.update()
time.sleep(0.02)
def cleanup(self):
self.network.cleanup()
def __init__(self, gyroPort='/dev/gyro', gyroBaudrate=921600):
self._gyroPort = gyroPort
self._gyroBaudrate = gyroBaudrate
self._gyro = Gyro(self._gyroPort, self._gyroBaudrate)
self._position = [0] * 3
self._offset = [0] * 3
self._count = 0
queryThd = threading.Thread(target=self.__query)
queryThd.setDaemon(True)
queryThd.start()
def setup():
Safety.setup_terminal_abort()
EventBus.subscribe(BT_REQUEST_SENSOR_DATA, communicator.send_sensor_data)
EventBus.subscribe(BT_REQUEST_SERVO_DATA, communicator.send_servo_data)
EventBus.subscribe(BT_REQUEST_MAP_DATA, communicator.send_map_data)
EventBus.subscribe(BT_DRIVE_FORWARD, communicator.drive_forward)
EventBus.subscribe(BT_DRIVE_BACK, communicator.drive_backward)
EventBus.subscribe(BT_TURN_RIGHT, communicator.turn_right)
EventBus.subscribe(BT_TURN_LEFT, communicator.turn_left)
EventBus.subscribe(BT_DRIVE_FORWARD_RIGHT,
communicator.drive_forward_right)
EventBus.subscribe(BT_DRIVE_FORWARD_LEFT, communicator.drive_forward_left)
EventBus.subscribe(AUTONOMOUS_MODE,
(lambda: navigator.set_mode(Navigator.AUTONOMOUS)))
EventBus.subscribe(MANUAL_MODE,
(lambda: navigator.set_mode(Navigator.MANUAL)))
EventBus.subscribe(CMD_TOGGLE_MODE, navigator.toggle_mode)
EventBus.subscribe(REQUEST_PI_IP, communicator.send_ip)
EventBus.subscribe(CMD_RETURN_SENSOR_DATA, ir.sensor_data_received)
Laser.initialize()
Gyro.initialize()
class RobotBoot():
def __init__(self):
self.display = None
print("Booting SpotMicroAI")
self.display = RobotDisplay()
self.gyro = Gyro()
# self.servos=Servos()
self.Kinematic = Kinematic()
def exitHandler(self):
print("Exiting SpotMicroAI")
if not self.display == None:
self.display.bye()
sys.exit()
def run(self):
while True:
(x, y) = self.gyro.read()
self.display.setAngles(x, y)
self.display.run()
time.sleep(0.02)
def run_simulation(args):
t_end = args.time
dt = args.dt
start_angle = 0.05
accel = Accel((args.accel_stddev_x, args.accel_stddev_z), args.gravity)
gyro = Gyro(args.gyro_stddev, args.gyro_biasdrift_stddev)
filt = ComplementaryFilter(args.filter_acc_weight, args.gravity,
start_angle)
accel_data = []
gyro_data = []
angles = []
true_angle = start_angle
true_angvel = 0.0
ts = np.arange(0, t_end, dt)
true_angles = true_angle * np.ones(shape=ts.shape)
for i, t in enumerate(ts):
acc_obs = accel.get(true_angles[i])
angvel_obs = gyro.get(true_angvel, dt)
accel_data.append(acc_obs)
gyro_data.append(angvel_obs)
ang_est = filt.get(acc_obs, angvel_obs, dt)
angles.append(ang_est)
angles = np.array(angles)
angle_error = angles - true_angles
mean_abs_error = np.mean(np.abs(angle_error))
if not args.no_print:
print("Mean abs error is %f radians (%f degrees)" %
(mean_abs_error, np.degrees(mean_abs_error)))
if not args.no_plot:
import matplotlib.pyplot as plt
plt.figure("Filter Results")
plt.plot(ts, angles, label='Estimated')
plt.plot(ts, true_angles, label='True')
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.title("Estimated vs. True Angle")
plt.legend()
plt.figure("Angle Error")
plt.plot(ts, angle_error)
plt.xlabel('Time (s)')
plt.ylabel('Angle Error (rad)')
plt.title("Angle Error")
accel_data = np.array(accel_data)
accel.plot(ts, accel_data)
gyro_data = np.array(gyro_data)
gyro.plot(ts, gyro_data)
plt.show()
return mean_abs_error
from navigator import Navigator
from driver import Driver
from laser import Laser
from gyro import Gyro
from ir import IR
from communicator import Communicator
from eventbus import EventBus
from position import Position
from protocol import *
from safety import Safety
ir = IR()
laser = Laser()
gyro = Gyro()
driver = Driver(gyro, laser)
navigator = Navigator(Navigator.MANUAL, ir, driver, laser)
position = Position(laser, ir, navigator)
communicator = Communicator(ir, laser, gyro, driver, navigator, position)
def setup():
Safety.setup_terminal_abort()
EventBus.subscribe(BT_REQUEST_SENSOR_DATA, communicator.send_sensor_data)
EventBus.subscribe(BT_REQUEST_SERVO_DATA, communicator.send_servo_data)
EventBus.subscribe(BT_REQUEST_MAP_DATA, communicator.send_map_data)
EventBus.subscribe(BT_DRIVE_FORWARD, communicator.drive_forward)
EventBus.subscribe(BT_DRIVE_BACK, communicator.drive_backward)
EventBus.subscribe(BT_TURN_RIGHT, communicator.turn_right)
EventBus.subscribe(BT_TURN_LEFT, communicator.turn_left)
from time import sleep
from datetime import datetime, timedelta
from gyro import Gyro
from driver import Driver
gyro = Gyro()
gyro.initialize()
previous_time = datetime.now()
degrees_total = 0
GYRO_LOWER_LIMIT = 10
while(True):
data = gyro.read_data()
time_delta = (previous_time - datetime.now()).total_seconds()
delta_degrees = data * time_delta
previous_time = datetime.now()
#Has to be run after previous time is set
if abs(data) <= GYRO_LOWER_LIMIT:
continue
degrees_total += delta_degrees
print(degrees_total)
import time
from gyro import Gyro
g = Gyro()
g.calibrateGyro()
while True:
print(g.getGyroValues())
time.sleep(0.1)
def setup():
Safety.setup_terminal_abort()
EventBus.subscribe(CMD_RETURN_SENSOR_DATA, sensor_data_received)
Laser.initialize()
Gyro.initialize()