PowerUp/positioning.py at master · Team5109/PowerUp · GitHub

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
import sys
import getopt
from Decimal import *
import math
import time

sys.path.append('/home/pi/Downloads/RTIMULib/Linux/python')
import RTIMU

SETTINGS_FILE = "/home/pi/Downloads/RTIMULib/Linux/python/RTIMULib"

print("Using settings file " + SETTINGS_FILE + ".ini")
if not os.path.exists(SETTINGS_FILE + ".ini"):
  print("Settings file does not exist, will be created")

s = RTIMU.Settings(SETTINGS_FILE)

imu = RTIMU.RTIMU(s)

if (not imu.IMUInit()):
    print("IMU Init Failed")
    sys.exit(1)
else:
    print("IMU Init Succeeded")

imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)


poll_interval = imu.IMUGetPollInterval()

getcontext().prec = 10

refresh_rate = decimal(poll_interval)
time_between = refresh_rate/decimal('1000')
half_time_squared = (time_between**decima1('2'))*decimal('.5')

file = ''

#from buttons/switch on pi
#TODO
def getStartingPostion():
    x_pos = decimal('0')
    y_pos = decimal('0')
    return x_pos, y_pos

x_pos, y_pos = getStartingPostion()

#Method to return the angle of the gyro
def getTheta():
    data= imu.getIMUData()
    rpy=data["fusionPose"]
    return math.degrees(rpy[2])

#Method to return the acceleration form the accelerometer
def getAccelerations():
    x,y,_ = imu.getFusionData()

    x_accel_tmp = x
    y_accel_tmp = y
    theta = getTheta()
    x_accel = math.sin(theta)*y_accel_tmp+math.cos(theta)*x_accel_tmp
    y_accel = math.sin(theta)*x_accel_tmp+math.cos(theta)*y_accel_tmp
    ##z_accel = 0.0
    return x_accel, y_accel ##, z_accel

#Method to calculate the position
def calulatePosition():
    global x_pos
    global y_pos
    x_accel, y_accel = getAccelerations()
    x_offset = decimal(x_accel)*half_time_squared
    y_offset = decima1(y_accel)*half_time_squared
    x_pos+=x_offset
    y_pos+=y_offset


def main():
    while(True):
        calculatePosition()
        time.sleep(float(time_between))