from magum import Magum from array import * DT = 0.02 # 20ms axisOffset = array('i', []) i = 0 magum = Magum(250, 1, 2, 1) # first of all calibrate the sensor axisOffset = magum.calibrateSens(1000) while True: try: cFAngleAxis = magum.compFilter( DT, axisOffset ) # Note: it might need a small time amount to get up to speed except IOError: # avoid timeout errors pass if i % 20 == 0: print str(int(round(cFAngleAxis[0], 0))) + ',' + str( int(round(cFAngleAxis[1], 0))) + ',' + str( int(round(cFAngleAxis[2], 0))) i += 1 # it's better to use % operator for printing out data, # time.sleep may cause uncorrect data do to sampling operation
from magum import Magum from array import * import time # enable Gyro, Accelerometer and Magnetometer magum = Magum(2000, 0, 2, 1) # G_scaleRange = 2000 | FsDouble = 0 (disabled) | A_scaleRange = 2 | lnoise = 1 (on) while True: axis = magum.readAData("deg") print str(axis[0]) + "," + str(axis[1]) + "," + str(axis[2]) time.sleep(0.900)
from magum import Magum from array import * DT = 0.02 # 20ms axisOffset = array('i', []) magum = Magum( 250, 1, 2, 1 ) # G_scaleRange = 250 | FsDouble = 1 (enabled) | A_scaleRange = 2 | lnoise = 1 (on) i = 0 # first of all calibrate the sensors axisOffset = magum.calibrateSens(1000) while True: try: kFx = magum.kalmanFilter(DT, 'x', axisOffset) kFy = magum.kalmanFilter(DT, 'y', axisOffset) kFz = magum.kalmanFilter(DT, 'z', axisOffset) except IOError: pass if i % 20 == 0: print str(kFx) + ',' + str(kFy) + ',' + str(kFz) i += 1
from magum import Magum from array import * DT = 0.02 # 20ms axisOffset = array('i',[]) i = 0 magum = Magum(250,1,2,1) # first of all calibrate the sensor axisOffset = magum.calibrateSens(1000) while True: try: cFAngleAxis = magum.compFilter(DT,axisOffset) # Note: it might need a small time amount to get up to speed except IOError: # avoid timeout errors pass if i%20 == 0: print str(int(round(cFAngleAxis[0],0))) + ',' + str(int(round(cFAngleAxis[1],0))) + ',' + str(int(round(cFAngleAxis[2],0))) i += 1 # it's better to use % operator for printing out data, # time.sleep may cause uncorrect data do to sampling operation
from magum import Magum from array import * DT = 0.02 # 20ms axisOffset = array('i',[]) magum = Magum(250,1,2,1) # G_scaleRange = 250 | FsDouble = 1 (enabled) | A_scaleRange = 2 | lnoise = 1 (on) i = 0 # first of all calibrate the sensors axisOffset = magum.calibrateSens(1000) while True: try: kFx = magum.kalmanFilter(DT,'x',axisOffset) kFy = magum.kalmanFilter(DT,'y',axisOffset) kFz = magum.kalmanFilter(DT,'z',axisOffset) except IOError: pass if i%20 == 0: print str(kFx) + ',' + str(kFy) + ',' + str(kFz) i += 1
# Integrate to yield quaternion q1 += qDot1 * deltat q2 += qDot2 * deltat q3 += qDot3 * deltat q4 += qDot4 * deltat norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalize quaternion norm = 1.0/norm q.insert(0,q1 * norm) q.insert(1,q2 * norm) q.insert(2,q3 * norm) q.insert(3,q4 * norm) return q magum = Magum(250,0,2,1) i = 0 ar = array('f',[]) while True: aCompArray = magum.readAData('gcomp') gCompArray = magum.readGData('rads') mCompArray = magum.readMData('ut') ar = madgwickQuaternionFilter(aCompArray,gCompArray,mCompArray) if i%20 == 0: print madgwickQuaternionFilter(aCompArray,gCompArray,mCompArray) i += 1
def constrain(x): if (x < -255): return -255 if (x > 255): return 255 return x ser = serial.Serial('/dev/ttyMCC', 115200, timeout=0) axisOffset = array('i', []) # G_scaleRange = 250, FsDouble = 1 (enabled) # A_scaleRange = 2, lnoise = 1 (on) magum = Magum(250, 1, 2, 1) axisOffset = magum.calibrateSens(1000) DT = 0.1 Kp = 1.0 Ki = 0.0 Kd = 0.0 setpoint = 90.0 pid = PID(Kp, Ki, Kd, setpoint) value = setpoint pid.tunnings = (Kp, Ki, Kd) while True: time.sleep(DT) #readPID(Kp, Ki, Kd)
from magum import Magum from array import * DT = 0.02 # 20ms axisOffset = array('i',[]) i = 0 magum = Magum(250,1,2,1) # first of all calibrate the sensor axisOffset = magum.calibrateSens(1000) while True: try: cFAngleAxis = magum.compFilter(DT,axisOffset) except IOError: # avoid timeout errors pass if i%20 == 0: print str(int(round(cFAngleAxis[0],0))) + ',' + str(int(round(cFAngleAxis[1],0))) + ',' + str(int(round(cFAngleAxis[2],0))) i += 1 # it's better to use % operator for printing out data, # time.sleep may cause uncorrect data do to sampling operation
from magum import Magum from array import * import time import math # enable Gyro, Accelerometer and Magnetometer magum = Magum(2000,0,2,1) # G_scaleRange = 2000 | FsDouble = 0 (disabled) | A_scaleRange = 2 | lnoise = 1 (on) axisOffset = magum.calibrateSens(1000) # calibration (not necessary with read Data functions) if axisOffset[0] >= 32768: axisOffset[0] -= 65536 if axisOffset[1] >= 32768: axisOffset[1] -= 65536 if axisOffset[2] >= 32768: axisOffset[2] -= 65536 aOffsetX = (axisOffset[0]/4) * 0.244 aOffsetY = (axisOffset[1]/4) * 0.244 aOffsetZ = (axisOffset[2]/4) * 0.244 aOffsetX2 = aOffsetX * aOffsetX aOffsetY2 = aOffsetY * aOffsetY aOffsetZ2 = aOffsetZ * aOffsetZ # getting angles aOffsetX = math.atan(aOffsetX/math.sqrt(aOffsetY2+aOffsetZ2))*(180/math.pi) aOffsetY = math.atan(aOffsetY/math.sqrt(aOffsetX2+aOffsetZ2))*(180/math.pi) aOffsetZ = math.atan(aOffsetZ/math.sqrt(aOffsetX2+aOffsetY2))*(180/math.pi)
from flask import Flask, render_template, jsonify from magum import Magum from array import * import logging app = Flask(__name__) magum = Magum() # default configs app.debug = False log = logging.getLogger('werkzeug') log.setLevel(logging.CRITICAL) sensors = [{ 'id': 1, 'name': 'Accelerometer' }, { 'id': 2, 'name': 'Gyroscope' }, { 'id': 3, 'name': 'Magnetometer' }, { 'id': 4, 'name': 'Temperature' }] @app.route("/sensor/<int:sensor_id>", methods=['GET']) def getSens(sensor_id): if sensor_id == 1: data = magum.readAData('gcomp')
from magum import Magum from array import * import time # enable Gyro, Accelerometer and Magnetometer magum = Magum( 2000, 0, 2, 1 ) # G_scaleRange = 2000 | FsDouble = 0 (disabled) | A_scaleRange = 2 | lnoise = 1 (on) while True: axis = magum.readAData('deg') print str(axis[0]) + ',' + str(axis[1]) + ',' + str(axis[2]) time.sleep(.900)