1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

import pygame
import os
import numpy
# import main
# from main import position
from readIMU import GetPos
import RPi.GPIO as GPIO
##############################################
## Inititlize parameter
#############################################
os.putenv('SDL_VIDEODRIVER','fbcon')
os.putenv('SDL_FBDEV', '/dev/fb1')
os.putenv('SDL_MOUSEDRV', 'TSLIB')
os.putenv('SDL_MOUSEDEV', '/dev/input/touchscreen')

GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)

# exit button
def GPIO17_callback(channel):
    exit()

GPIO.add_event_detect(17, GPIO.FALLING, callback=GPIO17_callback,bouncetime=300)

pygame.init()

pygame.mouse.set_visible(False)
black = 0, 0, 0
size = width, height = 320, 240
screen = pygame.display.set_mode(size)
screen.fill((0,0,0))


try:
    while True:
        points = GetPos()
        points = numpy.array(points)
        points = points[:,[0,1]]
        ranges = numpy.ptp(points, axis=0)
        x_scale = 230/ranges[0]
        y_scale = 180/ranges[1]

        scale = min(x_scale,y_scale)
        points[:,0] = points[:,0]*scale
        points[:,1] = points[:,1]*scale
        means = numpy.mean(points,axis=0)

        points[:,0] = points[:,0] + (160-means[0])
        points[:,1] = points[:,1] + (120-means[1])
        ranges = numpy.ptp(points, axis=0)

        screen.fill(black)
        pygame.draw.lines(screen, (255,255,255), False, points, 2)			#pygame.draw.circle(screen,(255,255,255),[60,250],40)
        pygame.display.flip()

except KeyboardInterrupt:
    print "stop showing the record."

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

import numpy as np

#				axises
#		  |	  X   |   Y   |   Z   |
#	pos1  |  east |  sky  | south | (Y)
#	pos2  |  east | north |  sky  | (-X)
#	pos3  | ground|  east | south | (Y)
#	pos4  |  west | ground| south | (-Y)
#	pos5  |  sky  |  west | south | (Z)
#	pos6  | south |  west | ground| (-Z)

g = 9.80532 # gravity force in Ithaca, NY

# reading calibration data
Cali_Pos1_all = np.loadtxt('calibration_data/calibrate_pos1.txt',delimiter = ',')
Cali_Pos2_all = np.loadtxt('calibration_data/calibrate_pos2.txt',delimiter = ',')
Cali_Pos3_all = np.loadtxt('calibration_data/calibrate_pos3.txt',delimiter = ',')
Cali_Pos4_all = np.loadtxt('calibration_data/calibrate_pos4.txt',delimiter = ',')
Cali_Pos5_all = np.loadtxt('calibration_data/calibrate_pos5.txt',delimiter = ',')
Cali_Pos6_all = np.loadtxt('calibration_data/calibrate_pos6.txt',delimiter = ',')

X_1 = np.mean(Cali_Pos1_all[:,[0]])
X_2 = np.mean(Cali_Pos2_all[:,[0]])
X_3 = np.mean(Cali_Pos3_all[:,[0]])
X_4 = np.mean(Cali_Pos4_all[:,[0]])
X_5 = np.mean(Cali_Pos5_all[:,[0]])
X_6 = np.mean(Cali_Pos6_all[:,[0]])

Y_1 = np.mean(Cali_Pos1_all[:,[1]])
Y_2 = np.mean(Cali_Pos2_all[:,[1]])
Y_3 = np.mean(Cali_Pos3_all[:,[1]])
Y_4 = np.mean(Cali_Pos4_all[:,[1]])
Y_5 = np.mean(Cali_Pos5_all[:,[1]])
Y_6 = np.mean(Cali_Pos6_all[:,[1]])

Z_1 = np.mean(Cali_Pos1_all[:,[2]])
Z_2 = np.mean(Cali_Pos2_all[:,[2]])
Z_3 = np.mean(Cali_Pos3_all[:,[2]])
Z_4 = np.mean(Cali_Pos4_all[:,[2]])
Z_5 = np.mean(Cali_Pos5_all[:,[2]])
Z_6 = np.mean(Cali_Pos6_all[:,[2]])


# calculate calibration matrix
a_x0  = (X_1 + X_2 + X_4 + X_6)/4
S_ax  = (X_3 - X_5)/2
K_ax1 = (X_4 - X_1)/2
K_ax2 = (X_6 - X_2)/2
K_ax3 = (-X_1 - X_2 + 2*X_3 - X_4 + 2 * X_5 - X_6)/4

a_y0  = (Y_2 + Y_3 + Y_5 + Y_6)/4
S_ay  = (Y_4 - Y_1)/2
K_ay1 = (Y_3 - Y_5)/2
K_ay2 = (Y_6 - Y_2)/2
K_ay3 = (2*Y_1 - Y_2 - Y_3 + 2*Y_4 - Y_5 - Y_6)/4

a_z0 = (Z_1 + Z_3 + Z_4 + Z_5)/4
S_az  = (Z_6 - Z_2)/2
K_az1 = (Z_3 - Z_5)/2
K_az2 = (Z_4 - Z_1)/2
K_az3 = (-Z_1 + 2 * Z_2 - Z_3 - Z_4 - Z_5 + 2 * Z_6)/4

Offset = np.array([[a_x0],[a_y0],[a_z0]])/g
Scale = np.array([[S_ax, K_ax1, K_ax2],
				  [K_ay1, S_ay, K_ay2],
				  [K_az1, K_az2, S_az]])/g
Scale_2 = np.array([[K_ax3, 0 , 0],
					[0 ,K_ay3,  0],
					[0,  0, K_ax3]])/g
######################################################
# simpler way of calibration (code commented out)
######################################################
# Bx = (X_pos_g + X_neg_g)/2
# Sx = g * 2/(X_pos_g - X_neg_g)
# By = (Y_pos_g + Y_neg_g)/2
# Sy = g * 2/(Y_pos_g - Y_neg_g)
# Bz = (Z_pos_g + Z_neg_g)/2 
# Sz = g * 2/(Z_pos_g - Z_neg_g)
# S = np.array([[Sx,0,0],[0,Sy,0],[0,0,Sz]])
# B = np.array([[Bx],[By],[Bz]])
print "Offset: ", Offset, '\n'
print "Scale: ",Scale, '\n'
print "scale mcm", Scale_2, '\n'

def return_calibration_matrix():
	# return calibration matrix
	return Offset, Scale, Scale_2

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162

import logging
import sys
import time
import numpy as np 
import RPi.GPIO

from Adafruit_BNO055 import BNO055


# Create and configure the BNO sensor connection.  Make sure only ONE of the
# below 'bno = ...' lines is uncommented:
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
# BeagleBone Black configuration with default I2C connection (SCL=P9_19, SDA=P9_20),
# and RST connected to pin P9_12:
#bno = BNO055.BNO055(rst='P9_12')


# Enable verbose debug logging if -v is passed as a parameter.
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
    logging.basicConfig(level=logging.DEBUG)

# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
    raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')

# Print system status and self test result.
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
    print('System error: {0}'.format(error))
    print('See datasheet section 4.3.59 for the meaning.')

# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version:   {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID:   0x{0:02X}'.format(accel))
print('Magnetometer ID:    0x{0:02X}'.format(mag))
print('Gyroscope ID:       0x{0:02X}\n'.format(gyro))

print('Reading BNO055 data, press Ctrl-C to quit...')

print ("#      |   X   |   Y   |   Z   |" + '\n'
        "pos1  |  east |  sky  | south | (Y)" + '\n'
        "pos2  |  east | north |  sky  | (Z)" + '\n'
        "pos3  | ground|  east | south | (-X)" + '\n'
        "pos4  |  west | ground| south | (-Y)" + '\n'
        "pos5  |  sky  |  west | south | (X)" + '\n'
        "pos6  | south |  west | ground| (-Z)" + '\n')

raw_input("first test z axis, press enter to continue")

t = 35998 ###########!!!!!!!!!!!!!!!!remember to change back
first_data = True
while True:
    # Read the Euler angles for heading, roll, pitch (all in degrees).
    # heading, roll, pitch = bno.read_euler()
    # Read the calibration status, 0=uncalibrated and 3=fully calibrated.
    # sys, gyro, accel, mag = bno.get_calibration_status()
    # Accelerometer data (in meters per second squared):
    # AccX,AccY,AccZ = bno.read_linear_acceleration()

    RawX,RawY,RawZ = bno.read_accelerometer()
    Acc_t = np.array([[RawX, RawY, RawZ]])
    # Orientation as a quaternion:
    # Qx,Qy,Qz,Qw = bno.read_quaternion()
    # Quat = [Qx, Qy, Qz, Qw]

    # Get all data
    # print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}\tSys_cal={3} Gyro_cal={4} Accel_cal={5} Mag_cal={6}'.format(
    #   heading, roll, pitch, sys, gyro, accel, mag))

    # # Convert to strings for display
    strACC = ', '.join(str(num).rjust(5) for num in Acc_t)
    # strQuat = ', '.join(str(num).rjust(5) for num in Quat)
    # Display in a pretty way
    print("t:" + str(t) + "|" + "Accel: " + strACC  +'\n') # + "Quat:" + strQuat

    if not first_data: 
        Acc = np.concatenate((Acc,Acc_t), axis = 0)
    else:
        Acc = Acc_t
        first_data = False
    
    t = t + 1
    time.sleep(1./100)
    if t == 11999:
        file_name = 'calibrate_pos1.txt'
        np.savetxt(file_name ,Acc,delimiter=',')
        first_data = True
        print ("#      |   X   |   Y   |   Z   |" + '\n'
                "pos1  |  east |  sky  | south | (Y)" + '\n'
                "pos2  |  east | north |  sky  | (Z)" + '\n'
                "pos3  | ground|  east | south | (-X)" + '\n'
                "pos4  |  west | ground| south | (-Y)" + '\n'
                "pos5  |  sky  |  west | south | (X)" + '\n'
                "pos6  | south |  west | ground| (-Z)" + '\n')
        raw_input("pos1 finished, press enter to continue for pos2")
    elif t == 23999:
        file_name = 'calibrate_pos2.txt'
        np.savetxt(file_name ,Acc,delimiter=',')
        first_data = True
        print ("#      |   X   |   Y   |   Z   |" + '\n'
                "pos1  |  east |  sky  | south | (Y)" + '\n'
                "pos2  |  east | north |  sky  | (Z)" + '\n'
                "pos3  | ground|  east | south | (-X)" + '\n'
                "pos4  |  west | ground| south | (-Y)" + '\n'
                "pos5  |  sky  |  west | south | (X)" + '\n'
                "pos6  | south |  west | ground| (-Z)" + '\n')
        raw_input("pos2 finished, press enter to continue for pos3")
    elif t == 35999:
        file_name = 'calibrate_pos3.txt'
        np.savetxt(file_name ,Acc,delimiter=',')
        first_data = True
        print ("#      |   X   |   Y   |   Z   |" + '\n'
                "pos1  |  east |  sky  | south | (Y)" + '\n'
                "pos2  |  east | north |  sky  | (Z)" + '\n'
                "pos3  | ground|  east | south | (-X)" + '\n'
                "pos4  |  west | ground| south | (-Y)" + '\n'
                "pos5  |  sky  |  west | south | (X)" + '\n'
                "pos6  | south |  west | ground| (-Z)" + '\n')
        raw_input("pos3 finished, press enter to continue for pos4")
    elif t == 47999:
        file_name = 'calibrate_pos4.txt'
        np.savetxt(file_name ,Acc,delimiter=',')
        first_data = True
        print ("#      |   X   |   Y   |   Z   |" + '\n'
                "pos1  |  east |  sky  | south | (Y)" + '\n'
                "pos2  |  east | north |  sky  | (Z)" + '\n'
                "pos3  | ground|  east | south | (-X)" + '\n'
                "pos4  |  west | ground| south | (-Y)" + '\n'
                "pos5  |  sky  |  west | south | (X)" + '\n'
                "pos6  | south |  west | ground| (-Z)" + '\n')
        raw_input("pos4 finished, press enter to continue for pos5")
    elif t == 59999:
        file_name = 'calibrate_pos5.txt'
        np.savetxt(file_name ,Acc,delimiter=',')
        first_data = True
        print ("#      |   X   |   Y   |   Z   |" + '\n'
                "pos1  |  east |  sky  | south | (Y)" + '\n'
                "pos2  |  east | north |  sky  | (Z)" + '\n'
                "pos3  | ground|  east | south | (-X)" + '\n'
                "pos4  |  west | ground| south | (-Y)" + '\n'
                "pos5  |  sky  |  west | south | (X)" + '\n'
                "pos6  | south |  west | ground| (-Z)" + '\n')
        raw_input("pos5 finished, press enter to continue for pos6 ")
    elif t == 71999:
        file_name = 'calibrate_pos6.txt'
        np.savetxt(file_name ,Acc,delimiter=',')
        first_data = True
        print ("#      |   X   |   Y   |   Z   |" + '\n'
                "pos1  |  east |  sky  | south | (Y)" + '\n'
                "pos2  |  east | north |  sky  | (Z)" + '\n'
                "pos3  | ground|  east | south | (-X)" + '\n'
                "pos4  |  west | ground| south | (-Y)" + '\n'
                "pos5  |  sky  |  west | south | (X)" + '\n'
                "pos6  | south |  west | ground| (-Z)" + '\n')
        raw_input("pos finished, press enter to continue ")
        exit()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

import numpy as np

def quaternProd(a, b):
        # lm=q(1); p1=q(2); p2=q(3); p3=q(4);
        # q1=[lm -p1 -p2 -p3
        #     p1  lm -p3  p2
        #     p2  p3  lm -p1
        #     p3 -p2  p1  lm]*m;
        lm = a[0]
        p1 = a[1]
        p2 = a[2]
        p3 = a[3]
        temp = np.array([[lm, -p1, -p2, -p3],
                         [p1,  lm, -p3,  p2],
                         [p2,  p3,  lm, -p1],
                         [p3, -p2,  p1,  lm]])
        # print temp, b
        q1 = np.dot(temp,np.transpose([b]))
        q1 = np.transpose(q1)
        q1 = q1[0]
        return q1

def quaternInv(a):
    b = [a[0], -a[1], -a[2], -a[3]]

    return b

def quaternRot(Vec,Quat):
    V = [0., Vec[0],Vec[1],Vec[2]]
    # print V
    Ve = quaternProd(quaternProd(quaternInv(Quat),V),Quat)
    Ve = np.array([[Ve[1],Ve[2],Ve[3]]])
    return Ve

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

from __future__ import division
import numpy as np
import warnings

class AHRS(object):

    def __init__(self, SamplePeriod = 1./200, Kp = 2., KpInit = 0., Quaternion = np.array([1, 0, 0, 0]), Ki = 0., InitPeriod = 5. ):
        """docstring for AHRS"""
        self.SamplePeriod = SamplePeriod
        self.Quaternion = Quaternion    # output quaternion describing the sensor relative to the Earth
        self.Kp = Kp                   # proportional gain
        self.Ki = Ki                     # integral gain
        self.KpInit = KpInit               # proportional gain used during initialisation
        self.InitPeriod = InitPeriod             # initialisation period in seconds

        self.q = np.array([1., 0, 0, 0])              #% internal quaternion describing the Earth relative to the sensor
        self.IntError = np.array([[0.], [0.], [0.]])		    #% integral error
        self.KpRamped = None                   #% internal proportional gain used to ramp during initialisation
    	
    def UpdateIMU(self, Gyr, Acc):
        if np.linalg.norm(Acc) == 0:
            warnings.warn("Accelerometer magnitude is zero.  Algorithm update aborted.")
            return
        else:
            Acc = np.array(Acc / np.linalg.norm(Acc))

        # print Acc
        v = np.array([[2*(self.q[1]*self.q[3] - self.q[0]*self.q[2])],
                [2*(self.q[0]*self.q[1] + self.q[2]*self.q[3])],
                [self.q[0]**2 - self.q[1]**2 - self.q[2]**2 + self.q[3]**2]])
        
        # print v
        
        error = np.cross(v,np.transpose([Acc]),axis = 0)

        # print error
        self.IntError = self.IntError + error
        Ref = Gyr - np.transpose(self.Kp*error + self.Ki*self.IntError)
        
        pDot = np.multiply(0.5 , self.quaternProd_single(self.q, [0, Ref[0,0], Ref[0,1], Ref[0,2]]))
        self.q = self.q + pDot * self.SamplePeriod;                               # % integrate rate of change of quaternion
        self.q = self.q / np.linalg.norm(self.q); 
        self.Quaternion = self.quaternConj(self.q);

    def quaternProd_single(self, a, b):

        ab = [None] * 4
        ab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]
        ab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]
        ab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1]
        ab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]
        return ab    

    def quaternProd(self, a, b):
        ab = []
        # print a
        # print b
        ab = np.concatenate((a[:,[0]] * b[:,[0]] - a[:,[1]] * b[:,[1]] - a[:,[2]] * b[:,[2]] - a[:,[3]] * b[:,[3]],
                        a[:,[0]] * b[:,[1]] + a[:,[1]] * b[:,[0]] + a[:,[2]] * b[:,[3]] - a[:,[3]] * b[:,[2]],
                        a[:,[0]] * b[:,[2]] - a[:,[1]] * b[:,[3]] + a[:,[2]] * b[:,[0]] + a[:,[3]] * b[:,[1]],
                        a[:,[0]] * b[:,[3]] + a[:,[1]] * b[:,[2]] - a[:,[2]] * b[:,[1]] + a[:,[3]] * b[:,[0]]),axis = 1)
        return ab

    def quaternConj(self, q):
        if q.ndim == 1:
            qConj = [q[0], -q[1], -q[2], -q[3]]
        else:
            qConj = np.concatenate((q[:,[0]], -q[:,[1]], -q[:,[2]], -q[:,[3]]),axis = 1)
            # print qConj.shape
            # raw_input("wait")
        return qConj

    def quaternRotate(self, v,q):
        row, col = v.shape
        # print row,col
        # print 'shape:', np.zeros((row,1))
        # print 'q: ',  q
        temp = self.quaternProd(q,np.concatenate((np.zeros((row,1)), v),axis = 1))
        v0XYZ = self.quaternProd(temp, self.quaternConj(q))
        v = v0XYZ[:, 1:4]
        # print v.shape
        return v

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391

from __future__ import division
from scipy import signal
from filter import AHRS
import numpy as np
import matplotlib.pyplot as plt
import matplotlib
import math
# import LSM9DS0 as IMU
import csv

import logging
import sys
import time
import numpy as np 
import RPi.GPIO as GPIO
from quatern_func import *
# import draw

from Adafruit_BNO055 import BNO055

###########################
# get data
#############################

GPIO.setmode(GPIO.BCM) 
GPIO.setup(5, GPIO.IN, pull_up_down=GPIO.PUD_UP)

# Create and configure the BNO sensor connection.  Make sure only ONE of the
# below 'bno = ...' lines is uncommented:
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
# BeagleBone Black configuration with default I2C connection (SCL=P9_19, SDA=P9_20),
# and RST connected to pin P9_12:
#bno = BNO055.BNO055(rst='P9_12')


# Enable verbose debug logging if -v is passed as a parameter.
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
    logging.basicConfig(level=logging.DEBUG)

# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
    raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')

# Print system status and self test result.
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
    print('System error: {0}'.format(error))
    print('See datasheet section 4.3.59 for the meaning.')

# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version:   {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID:   0x{0:02X}'.format(accel))
print('Magnetometer ID:    0x{0:02X}'.format(mag))
print('Gyroscope ID:       0x{0:02X}\n'.format(gyro))


print('Reading BNO055 data, press Ctrl-C to quit...')

def GetPos():


    while True:
        sys, gyro, accel, mag = bno.get_calibration_status()
        print "accelerometer calibration status: ", accel 
        if accel == 3:
        # wait until the accelerometer calibration finish
            print "accelerometer calibration finished, press buttom 5 to start and press again to end"
            break

    while True:
        if not GPIO.input(5):
            # wait until the button is pressed
            print "start getting data"
            break

    t = 0 
    first_data = True

    while True:
        # Accelerometer data (in meters per second squared):
        AccX,AccY,AccZ = bno.read_linear_acceleration()

        # RawX,RawY,RawZ = bno.read_accelerometer()

        Acc_t = np.array([[AccX, AccY, AccZ]])
        # Orientation as a quaternion:
        Qx,Qy,Qz,Qw = bno.read_quaternion()
        Quat_t = np.array([[Qw, Qx, Qy, Qz]])

        if not first_data: 
            Acc = np.concatenate((Acc,Acc_t), axis = 0)
            Quat = np.concatenate((Quat,Quat_t), axis = 0)
        else:
            Acc = Acc_t
            Quat = Quat_t
            first_data = False
        
        t = t + 1
        time.sleep(1./100)

        if t > 50 and (not GPIO.input(5)):
            # Button is pressed
            print "end getting data"
            break    




    plt.rcParams['lines.linewidth'] = 0.5

    samplePeriod = 1./100

   #  ################################################
   #  # this commented out part is for the LSM9DS)
   #  ################################################
   #  # ACC = imu.rawAccel() 
   #  # GYR = imu.rawGyro()  
   #  # MAG = imu.rawMag()  
   #  #
   #  # accX = ACC[0] * imu.accelscale
   #  # accY = ACC[1] * imu.accelscale
   #  # accZ = ACC[2] * imu.accelscale
   #  # gryX = GYR[0] * imu.gyroscale
   #  # gryY = GYR[1] * imu.gyroscale
   #  # gryZ = GRY[2] * imu.gyroscale
   #  ################################################

    accX = Acc[:,0]
    accY = Acc[:,1]
    accZ = Acc[:,2]

    t = len(accX)
    runtime = np.array([i*samplePeriod for i in xrange(t)])

    acc_mag = np.sqrt(accX**2 + accY**2 + accZ**2)

    ##########################
    ## butterworth filters
    ############################
    filtCutOff = 0.001
    b, a = signal.butter(1,(2*filtCutOff)/(1/samplePeriod), 'high')
    acc_magFilt = signal.filtfilt(b, a, acc_mag)

    # % Compute absolute value
    acc_magFilt = np.absolute(acc_magFilt)

    # % LP filter accelerometer data
    filtCutOff = 5.
    b, a = signal.butter(1, (2*filtCutOff)/(1/samplePeriod), 'low')
    acc_magFilt = signal.filtfilt(b, a, acc_magFilt)

    accX_filt = signal.filtfilt(b, a, accX)
    accY_filt = signal.filtfilt(b, a, accY)
    accZ_filt = signal.filtfilt(b, a, accZ)

    accX_drift = np.mean(accX)
    accY_drift = np.mean(accY)
    accZ_drift = np.mean(accZ)

    ##############################
    # extract acceleration mean , minimize drift
    ##############################
    accX_fix_drift = accX_filt - accX_drift
    accY_fix_drift = accY_filt - accY_drift
    accZ_fix_drift = accZ_filt - accZ_drift

    # Threshold detection
    stationary = [a < 0.07 for a in acc_magFilt]

    ############################################
    # plots used to debug
    ############################################
    # print t

    # plt.figure('before rotate')
    # plt.subplot(211)
    # plt.plot(runtime, accX_filt,'r')
    # plt.plot(runtime, accY_filt,'g')
    # plt.plot(runtime, accZ_filt,'b')
    # plt.title('Filted_acc')
    # plt.xlabel('runTime (s)')
    # plt.ylabel('Acceleration (g)')


    # plt.subplot(212)
    # plt.plot(runtime, accX, 'r')
    # plt.plot(runtime, accY, 'g')
    # plt.plot(runtime, accZ, 'b')
    # plt.plot(runtime, acc_magFilt, ':k')
    # plt.title('acc')
    # plt.plot(runtime, stationary, 'k',)
    # plt.title('Accelerometer')
    # plt.xlabel('runTime (s)')
    # plt.ylabel('Acceleration (g)')

    # plt.figure('accx')
    # plt.plot(runtime, accX_filt,'g')
    # plt.plot(runtime, accX_fix_drift,'r')

    # plt.figure('accy')
    # plt.plot(runtime, accY_filt,'g')
    # plt.plot(runtime, accY_fix_drift,'r')

    # plt.figure('accz')
    # plt.plot(runtime, accZ_filt,'g')
    # plt.plot(runtime, accZ_fix_drift,'r')

    # quat = np.zeros((t,4))

    ##########################################################################
    # 
    #This commented out part is for LSM9DS0
    #
    ##########################################################################
    #Filter = AHRS(Kp = 1, KpInit = 1)

    #initPeriod = 4
    #index = int(initPeriod/samplePeriod)

    # for i in xrange(2000):

    #     Filter.UpdateIMU([0,0,0],
    #         [np.mean(accX[0:index],dtype = np.float64), 
    #         np.mean(accY[0:index],dtype = np.float64), 
    #         np.mean(accZ[0:index],dtype = np.float64)])

    # for i in xrange(t):
    #     if stationary[i]:
    #         Filter.Kp = 0.5
    #     else:
    #         Filter.Kp = 0

    #     Filter.UpdateIMU([math.radians(gyrX[i]) ,math.radians(gyrY[i]) ,math.radians(gyrZ[i])],[accX[i], accY[i], accZ[i]])
    #     quat[i,:] = Filter.Quaternion
    # print [np.transpose(accX), np.transpose(accY), np.transpose(accZ)]

    # acc = Filter.quaternRotate(np.concatenate((np.transpose([accX_filt]), np.transpose([accY_filt]), np.transpose([accZ_filt])),axis = 1), Filter.quaternConj(Quat))

    # acc = acc * 9.81

    # acc[:,[2]] = acc[:,[2]] - 9.81

    # acc_d = Filter.quaternRotate(np.concatenate((np.transpose([accX_fix_drift]), np.transpose([accY_fix_drift]), np.transpose([accZ_fix_drift])),axis = 1), Filter.quaternConj(Quat))

    # acc[:,[1]] = acc[:,[1]]- np.mean(acc[:,[1]])
    # acc[:,[2]] = acc[:,[2]]- np.mean(acc[:,[2]])
    # acc[:,[0]] = acc[:,[0]]- np.mean(acc[:,[0]])

    ########################################################################
    ####################################################################

    # plt.figure('after rotate with drift')
    # plt.plot(runtime, acc[:,[0]], 'r')
    # plt.plot(runtime, acc[:,[1]], 'g')
    # plt.plot(runtime, acc[:,[2]], 'b')
    # plt.title('Acceleration')
    # plt.xlabel('runTime (s)')
    # plt.ylabel('Acceleration (m/s/s)')

    ######################################################
    ## Using quaternion to do the frame transformation (only for BNO055)
    ######################################################

    l = len(accX)
    acc_fix_drift = np.concatenate((np.transpose([accX_fix_drift]), np.transpose([accY_fix_drift]), np.transpose([accZ_fix_drift])),axis = 1)

    for i in xrange(l):
        # acc = quaternRot([Gx,Gy,Gz], quaternInv([qw,qx,qy,qz]))
        # print acc_fix_drift[i,:],quaternInv(Quat[i,:])
        acc_d_t = quaternRot(acc_fix_drift[i,:],quaternInv(Quat[i,:]))
        if i == 0:
            acc_d = acc_d_t
        else:
            acc_d = np.concatenate((acc_d,acc_d_t),axis = 0)

    # print acc_d.shape, acc.shape
    # plt.figure('after rotate without drift')
    # plt.plot(runtime, acc_d[:,[0]], 'r')
    # plt.plot(runtime, acc_d[:,[1]], 'g')
    # plt.plot(runtime, acc_d[:,[2]], 'b')
    # plt.title('Acceleration')
    # plt.xlabel('runTime (s)')
    # plt.ylabel('Acceleration (m/s/s)')
    # # plt.show()

    vel = np.zeros(acc_d.shape)

    t_total = len(vel) - 2

    for a in xrange(t_total):
        i = a + 1
        vel[[i],:] = vel[[i-1],:] + acc_d[[i],:] * samplePeriod
        if stationary[i]:
            vel[[i],:] = [[0,0,0]]


    #####################################################################
    ## calculate velocity drift
    #####################################################################

    velDrift = np.zeros(vel.shape)
    stationary_temp = [0] + np.diff(1 * np.array(stationary))
    stationaryStart = [i for i,x in enumerate(stationary_temp) if x == -1]
    stationaryEnd = [i for i,x in enumerate(stationary_temp) if x == 1]

    # print stationaryEnd, '\n', stationaryStart
    if len(stationaryEnd) > len(stationaryStart):
        if len(stationaryEnd) > 1:
        	stationaryEnd = stationaryEnd[1:]
        else:
        	stationaryEnd = []

    if len(stationaryStart) > len(stationaryEnd):
            stationaryEnd = stationaryEnd.append(t_total)

    if stationaryStart == [] :
        stationaryStart = [10]
    if stationaryEnd == [] or stationaryEnd == None:
    	stationaryEnd = [t_total]



    for i in xrange(len(stationaryEnd)):
        # print stationaryStart, '\n', stationaryEnd
        # print stationaryEnd[i]
        driftRate =  vel[stationaryEnd[i]-1,:] / (stationaryEnd[i]- stationaryStart[i])
        enum = range(0,(stationaryEnd[i] - stationaryStart[i]))
        drift = np.concatenate((np.transpose([enum])*driftRate[0],
                np.transpose([enum])*driftRate[1],
                np.transpose([enum])*driftRate[2]),axis = 1)
        velDrift[stationaryStart[i]:stationaryEnd[i],:] = drift

    vel = vel-velDrift

    # plt.figure('velocity')
    # plt.plot(runtime, vel[:,0], 'r')
    # plt.plot(runtime, vel[:,1], 'g')
    # plt.plot(runtime, vel[:,2], 'b')
    # plt.title('Velocity')
    # plt.xlabel('runTime (s)')
    # plt.ylabel('Velocity (m/s)')
    # plt.show()

    pos = np.zeros(vel.shape)

    for a in xrange(t_total):
        i = a + 1
        pos[[i],:] = pos[[i-1],:] + vel[[i],:] * samplePeriod
    pos_x_min = np.min(pos[:,0])
    pos_x_max = np.max(pos[:,0])
    pos_y_min = np.min(pos[:,1])
    pos_y_max = np.max(pos[:,1])
    pos_z_min = np.min(pos[:,2])
    pos_z_max = np.max(pos[:,2])

    max_range = np.max([pos_x_max,pos_y_max,pos_z_max])
    min_range = np.min([pos_x_min,pos_y_min,pos_z_min])
    axis_range = max_range - min_range
    max_range = max_range + 0.1 * axis_range
    min_range = min_range - 0.1 * axis_range 

    l = len(pos[:,0])
    pos = pos[0:l-1,:]
    runtime = runtime[:l-1]

    # plt.figure('position')   
    # plt.plot(runtime, pos[:,0], 'r')
    # plt.plot(runtime, pos[:,1], 'g')
    # plt.plot(runtime, pos[:,2], 'b')
    # plt.title('Position')
    # plt.xlabel('runTime (s)')
    # plt.ylabel('Position (m)')

    # plt.figure('trace')

    np.savetxt('position_data' ,pos, delimiter=',')  

    # # print pos[:,0]
    # plt.plot(pos[:,0],pos[:,1],'r')
    # # plt.plot([pos[0,0],pos[l-2,0]],[pos[0,1],pos[l-2,1]],'w')
    # plt.axis([min_range, max_range, min_range, max_range])
    # plt.title('movement')
    # plt.show()
    return pos
    #draw_shape(pos)

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

import pygame
import os
import numpy
import main
from main import position

os.putenv('SDL_VIDEODRIVER','fbcon')
os.putenv('SDL_FBDEV', '/dev/fb1')
os.putenv('SDL_MOUSEDRV', 'TSLIB')
os.putenv('SDL_MOUSEDEV', '/dev/input/touchscreen')

pygame.init()

pygame.mouse.set_visible(False)
size = width, height = 320, 240
screen = pygame.display.set_mode(size)
screen.fill((0,0,0))



def draw_shape(points):
	points = numpy.array(points)[:,[0,1]]
	ranges = numpy.ptp(points, axis=0)
  	x_scale = 260/ranges[0]
  	y_scale = 200/ranges[1]
  	print ranges[0]
	print ranges[1]
  	scale = min(x_scale,y_scale)
 	points[:,0] = points[:,0]*scale
  	points[:,1] = points[:,1]*scale
  	means = numpy.mean(points,axis=0)
  
  	points[:,0] = points[:,0] + (160-means[0])
  	points[:,1] = points[:,1] + (120-means[1])
  	ranges = numpy.ptp(points, axis=0)
	print ranges[0]
	print ranges[1]
	
	try:			
		pygame.draw.lines(screen, (255,255,255), False, points, 2)			#pygame.draw.circle(screen,(255,255,255),[60,250],40)
		pygame.display.flip()
			#print "successful"
	except KeyboardInterrupt:
		print "stop showing the record."


if __name__ == "__main__":
	points = []
	for i in xrange(3):
		points.append([])
		for j in xrange(3):
			points[i].append(i+j)
	print points
	draw_shape(points)