def __init__(self, core_file_name):
super(Listener, self).__init__()
self.core_file_name = core_file_name
self.setup_ui()
self.setup_timers()
port = SERIAL_PORT
self.serial = serial.Serial(port, BAUDE_RATE, timeout=0)
self.incoming_data = []
self.imu = IMU(PLATFORM_SPECIFIC_QUOTIENTS['stm'])
self.stroke = Stroke()
self.selector = Selector(self.core_file_name)
self.acceleration_filter = AperiodicFilter(ACCELERATION_TIME_CONST)
self.stroke.widget = self.display
self.stroke.on_done = self.get_stroke
self.previous_time = None
self.data_buffer = ''
self.init_selector()
def __init__(self):
print("Motor thread is created")
self.uavServerTX = UAVServerTX()
self.uavServerRX = UAVServerRX()
self.imu = IMU()
def __init__(self, parent, gui, **options):
""" Initializes TkOrthoManager object
@param parent
@param gui
@param options
"""
super(TkOrthoManager,self).__init__(parent, gui, **options)
self.specification = gui.specification
self.initDependencyManager()
if(hasattr(self.gui, 'scheduler')):
self.scheduler = self.gui.scheduler
else:
self.scheduler = Scheduler.GetInstance()
if(not self.dm.installRequired()):
self.pimg = self.gui.getModule('PIL.Image')
self.tkimg = self.gui.getModule('PIL.ImageTk')
if(hasattr(self.gui, 'imu')):
self.imu = self.gui.imu
else:
self.imu = self.gui.imu = IMU(self.specification, self.scheduler, (not Setting.get('imu_autostart', False)))
self.shapes = {}
self.cache = { 'roll': {}, 'pitch': {}, 'yaw': {} }
self.basepath = AmsEnvironment.AppPath()
self.initImages()
self.scheduler.addTask('ortho', self.updateOrtho, 0.2, True)
def __init__(self, parent, gui, **options):
""" Initializes TkGravityManager object
@param parent
@param gui
@param options
"""
super(TkGravityManager, self).__init__(parent, gui, **options)
self.initDependencyManager()
if (hasattr(self.gui, 'scheduler')):
self.scheduler = self.gui.scheduler
else:
self.scheduler = Scheduler.GetInstance()
if (not self.pm.installRequired()):
if (hasattr(self.gui, 'imu')):
self.imu = self.gui.imu
else:
self.imu = self.gui.imu = IMU(
self.scheduler, (not Setting.get('imu_autostart', False)))
self.groundcoords = [[0, 237], [800, 237], [800, 800], [0, 800]]
self.centre = complex(237, 237)
self.shapes = {}
self.cache = {}
self.basepath = AmsEnvironment.AppPath()
self.pimg = self.gui.getModule('PIL.Image')
self.tkimg = self.gui.getModule('PIL.ImageTk')
self.initImages()
self.scheduler.addTask('gravity_display', self.updateGravity, 0.2,
True)
def __init__(self):
self.odom_pub = rospy.Publisher(PUB_TOPIC, Odometry, queue_size=50)
# self.odom_broadcaster = tf.TransformBroadcaster()
self.imu = IMU()
self.last_distance = self.we.get_distance()
self.last_time = rospy.Time.now()
self.seq = 0
def INIT(self):
print("CURRENT STATE: INIT")
# use Raspberry Pi board pin numbers
GPIO.setmode(GPIO.BCM)
GPIO.setup(mbl_bots.TRIG, GPIO.OUT)
GPIO.setup(mbl_bots.ECHO, GPIO.IN)
self.lastIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.currentIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.camera = Cam()
self.imu = IMU(self.bus)
self.distance = -1
signal.signal(signal.SIGINT, self.close)
self.state = mbl_bots.REST
time.sleep(1)
def __init__(self, file):
super(Robot, self).__init__()
(self.actuators, self.sensors) = utils.file2bot(file, mbl_bots.BOTH)
self.bus = smbus.SMBus(1)
self.pwm = PCA9685(self.bus, 0x40, debug=False)
self.pwm.setPWMFreq(50)
#print("My name is: ", self.actuators[0].name)
self.names_acc = [
self.actuators[i].name for i in range(len(self.actuators))
]
self.names_sen = [
self.sensors[i].name for i in range(len(self.sensors))
]
self.idx_acc = [i for i in range(len(self.actuators))]
self.idx_sen = [i for i in range(len(self.sensors))]
self.acc_dic = utils.genDictionary(self.names_acc, self.idx_acc)
self.sen_dic = utils.genDictionary(self.names_sen, self.idx_sen)
self.state = mbl_bots.INIT
self.exploreState = mbl_bots.GETDATA
self.movesCode = mbl_bots.NONE
self.camera = Cam()
self.imu = IMU(self.bus)
self.vision = Vision()
def main(): pressure = PressureSensor() imu = IMU() telemetry = Telemetry() logger = Logger() pressure.init() imu.init() telemetry.init() tick = 0 while True: tick += 1 p = pressure.read_pressure() i = imu.read_IMU() data = "soemthing" #telemetry.send_data(data) # update all sensors pressure.update(logger, tick) imu.update(logger, tick) # log and send data telemetry.update(logger, tick)
if yf > r180:
yf = yf - r360
elif yf < -r180:
yf = yf + r360
self.C_FL = self.euler2C(pf, rf, yf)
self.att = np.array([pf, rf, yf])
# print("=======================>C_FLTR: {0}".format(np.degrees(self.att)))
self.prevTime = self.elapsedTime
return (self.att)
if (__name__ == "__main__"):
print("Test AHRS Class")
myIMU = IMU()
C_orient = np.zeros((3, 3))
C_orient[0][2] = -1.0
C_orient[1][1] = -1.0
C_orient[2][0] = -1.0
myIMU.setOrientation(C_orient)
myAHRS = AHRS(myIMU)
# imu.setOrientation(np.eye(3))
# C_orient = np.zeros( (3,3) )
# C_orient[0][2] = 1.0
# C_orient[1][1] = 1.0
# C_orient[2][0] = 1.0
# imu.setOrientation(C_orient)
nSamples = 10
ser = serial.Serial("COM11", 9600, timeout=TOUT)
print "Connected"
print "Sent Connection Call"
print "Loading Calibration"
xCal = list()
yCal = list()
zCal = list()
with open('..\Data\CalibrationSignal.csv', 'rb') as csvDataFile:
csvReader = csv.reader(csvDataFile)
for row in csvReader:
xCal.append(float(row[0]))
yCal.append(float(row[1]))
zCal.append(float(row[2]))
imu = IMU()
imu.calcOffsets(xCal, yCal, zCal)
imu.getOffsets()
ser.write("t")
recordedData = []
start = time.time()
while (end - start) <= 30.0:
dataIn = []
val = ser.read()
while val != '_':
dataIn.append(val)
val = ser.read()
if dataIn[0] == '\x00':
MOSI = 24 # Master out Slave in, used to transmit FROM the Master device
CS = 25 # Chip select
SPI = SPI_Master(CLK, MISO, MOSI, CS)
# Output pins
OUT_LED = 37
MOTOR = None # TODO Set up PWM for motor
GPIO.setup(OUT_LED, GPIO.OUT)
# TODO Pins for UART Communication with the IMP
# Setup classes for REXUS, IMU and PiCam
REXUS_Comm = REXUS()
PiCam_1 = PiCam()
PiCam_1.video_cut = 5
IMU_1 = IMU()
IMU_1.setup_default()
def flash_led():
print('Flashing LEDs')
for i in range(0, 5):
GPIO.output(OUT_LED, GPIO.HIGH)
time.sleep(0.1)
GPIO.output(OUT_LED, GPIO.LOW)
time.sleep(0.1)
def start_of_data_storage():
'''Backs up data between both Pi's'''
print('Start of data storage')
def begin():
rospy.init_node('picar_imu', anonymous=True)
e = IMU()
while not rospy.is_shutdown():
e.tick()
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
# for k in range(1):
# self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human
self.cars.append(Car(x=50, y=250, theta=90,
speed=1 * 90)) # [2] ghost of ins estimating [0]
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[0].camview.register_events(self.events)
self.cars[0].controller = self.controller
self.cars[0].collision = False
self.cars[0].imu = IMU(self.cars[0])
self.cars[0].ins = INS(self.cars[0].imu.calibration_noise)
self.cars[0].ins.update_pose(self.cars[0].x,
self.cars[0].y,
self.cars[0].theta / Utils.d2r,
gain=1)
self.insghost = INSGhostController(self.cars[0].ins)
self.cars[1].controller = self.human
self.cars[2].controller = self.insghost
self.cars[2].collision = False
self.cars[2].size *= 1.25
self.cars[2].camview = CamView(self.cars[2],
self.background.arr_dist_rgb,
width=275,
height=275,
offset=(0, 75),
angle_offset=-25)
self.cars[2].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "human"
self.cars[2].name = "estimate"
# this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0]
# self.cars[0].ghost = self.cars[2]
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
self.plot_window_size = 100
self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size,
channels=3)
self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer)
# self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels))
self.std_plot = RingBufferPlot(
RingBuffer(self.plot_window_size,
channels=self.xyt_corr_ring_buffer.channels))
self.velocity_carthesian_history_plot = RingBufferPlot(
self.cars[0].ins.velocity_carthesian_history)
# self.hist_plot = HistogramPlot(10)
self.register_events()
self.spin()
def setIMU(self):
self.imu = IMU()
for row in csvReader:
axOff.append(float(row[0]))
ayOff.append(float(row[1]))
azOff.append(float(row[2]))
with open('IMUData60cm_x.csv', 'rb') as csvDataFile:
csvReader = csv.reader(csvDataFile)
for row in csvReader:
data1.append(float(row[0]))
data2.append(float(row[1]))
data3.append(float(row[2]))
originTap.append(int(row[6]))
##x_vel = integrate.cumtrapz(t, Ax, initial=0)
##x_Trap = integrate.cumtrapz(t,x_vel, initial=0)
mpu = IMU()
mpu.calcOffsets(axOff, ayOff, azOff)
mpu.setTime(30.0)
mpu.setAcceleration(data1, data2, data3)
Ax, Ay, Az = mpu.getAcceleration()
totalSum = 0
dt = len(Ax) / 30.0
t = mpu.time
x_vel = [sum(Ax[:i]) * -dt for i in range(len(Ax))]
x_pos = [sum(x_vel[:i]) * dt for i in range(len(x_vel))]
y_vel = [sum(Ay[:i]) * dt for i in range(len(Ay))]
y_pos = [sum(y_vel[:i]) * dt for i in range(len(y_vel))]
#!/usr/bin/env pybricks-micropython
from pybricks import ev3brick as brick
from pybricks.parameters import (Port, Button)
from pybricks.tools import print, wait
#Fuege den Tools Ordner zum PYTHONPATH hinzu. Nicht notwendig wenn IMU.py im selben Ordner ist
import sys
sys.path.append("/home/robot/LEGORoboticsPython/Tools")
#Importiere IMU.py
from IMU import IMU
#Port und Mode des Seonsors festlegen. Moegliche Modes sind TILT, ACCEL, COMPASS, MAG, GYRO
#TODO: implement ALL mode
#Info zu Modes:http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensor_data.html
imu = IMU(Port.S3, 'GYRO')
while not Button.DOWN in brick.buttons():
result = 'IMU: ' + str(imu.angle())
brick.display.clear()
brick.display.text(result, (0, 50))
print(result)
wait(0.1)
def __init__(self):
imu = IMU()
