collab/pb-hal

git clone https://git.julia.blue/collab/pb-hal

Rahul ButaniRestructuringce16264

control
2.6 KiB87 linesraw
1#!/usr/bin/python
2import smbus
3import math
4
5# Register
6power_mgmt_1 = 0x6b
7power_mgmt_2 = 0x6c
8
9def read_byte(reg):
10    return bus.read_byte_data(address, reg)
11
12def read_word(reg):
13    h = bus.read_byte_data(address, reg)
14    l = bus.read_byte_data(address, reg+1)
15    value = (h << 8) + l
16    return value
17
18def read_word_2c(reg):
19    val = read_word(reg)
20    if (val >= 0x8000):
21        return -((65535 - val) + 1)
22    else:
23        return val
24
25def dist(a,b):
26    return math.sqrt((a*a)+(b*b))
27
28def get_y_rotation(x,y,z):
29    radians = math.atan2(x, dist(y,z))
30    return -math.degrees(radians)
31
32def get_x_rotation(x,y,z):
33    radians = math.atan2(y, dist(x,z))
34    return math.degrees(radians)
35
36bus = smbus.SMBus(1) # bus = smbus.SMBus(0) fuer Revision 1
37address = 0x68       # via i2cdetect
38
39# Aktivieren, um das Modul ansprechen zu koennen
40bus.write_byte_data(address, power_mgmt_1, 0)
41
42print ("Gyroskop")
43print ("--------")
44
45gyroskop_xout = read_word_2c(0x43)
46gyroskop_yout = read_word_2c(0x45)
47gyroskop_zout = read_word_2c(0x47)
48
49print("gyroskop_xout: ", "%5d" % gyroskop_xout, " scaled: ", gyroskop_xout / 131)
50print("gyroskop_yout: ", "%5d" % gyroskop_yout, " scaled: ", gyroskop_yout / 131)
51print("gyroskop_zout: ", "%5d" % gyroskop_zout, " scaled: ", gyroskop_zout / 131)
52
53print()
54print("Accelerationsensor")
55print("---------------------")
56
57Acceleration_xout = read_word_2c(0x3b)
58Acceleration_yout = read_word_2c(0x3d)
59Acceleration_zout = read_word_2c(0x3f)
60
61Acceleration_xout_scaled = Acceleration_xout / 16384.0
62Acceleration_yout_scaled = Acceleration_yout / 16384.0
63Acceleration_zout_scaled = Acceleration_zout / 16384.0
64
65print("Acceleration_xout: ", "%6d" % Acceleration_xout, " scaled: ", Acceleration_xout_scaled)
66print("Acceleration_yout: ", "%6d" % Acceleration_yout, " scaled: ", Acceleration_yout_scaled)
67print("Acceleration_zout: ", "%6d" % Acceleration_zout, " scaled: ", Acceleration_zout_scaled)
68
69print("X Rotation: ", get_x_rotation(Acceleration_xout_scaled, Acceleration_yout_scaled, Acceleration_zout_scaled))
70print("Y Rotation: ", get_y_rotation(Acceleration_xout_scaled, Acceleration_yout_scaled, Acceleration_zout_scaled))
71
72"""
73Sample Data Output Expected
74Gyroskop
75--------
76gyroskop_xout:   -260  scaled:  -2
77gyroskop_yout:   -154  scaled:  -2
78gyroskop_zout:     78  scaled:  0
79
80Accelerationsensor
81---------------------
82Acceleration_xout:   -1048  scaled:  -0.06396484375
83Acceleration_yout:    -676  scaled:  -0.041259765625
84Acceleration_zout:   16644  scaled:  1.01586914062
85X Rotation:  -2.32121150537
86Y Rotation:  3.59994842011
87"""