def main():
try:
#Create your Phidget channels
accelerometer0 = Accelerometer()
gyroscope0 = Gyroscope()
magnetometer0 = Magnetometer()
#Set addressing parameters to specify which channel to open (if any)
#Assign any event handlers you need before calling open so that no events are missed.
accelerometer0.setOnAccelerationChangeHandler(onAccelerationChange)
gyroscope0.setOnAngularRateUpdateHandler(onAngularRateUpdate)
magnetometer0.setOnMagneticFieldChangeHandler(onMagneticFieldChange)
#Open your Phidgets and wait for attachment
accelerometer0.openWaitForAttachment(5000)
gyroscope0.openWaitForAttachment(5000)
magnetometer0.openWaitForAttachment(5000)
#Do stuff with your Phidgets here or in your event handlers.
time.sleep(5)
#Close your Phidgets once the program is done.
accelerometer0.close()
gyroscope0.close()
magnetometer0.close()
except PhidgetException as ex:
#We will catch Phidget Exceptions here, and print the error informaiton.
traceback.print_exc()
print("")
print("PhidgetException " + str(ex.code) + " (" + ex.description +
"): " + ex.details)
def imu_talker():
# Create new phidget22 accelerometer object
accelerometer = Accelerometer()
# Check if imu can be found by OS
accelerometer.openWaitForAttachment(1000)
try:
pub = rospy.Publisher('crashed', Bool, queue_size=10)
rospy.init_node('crash_detector', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Get current acceleration
acceleration = accelerometer.getAcceleration()
# Check if y acceleration has a large negative value in gs
if acceleration[1] < -0.2:
# In this case we have probably crashed
crashed = True
else:
# In this case we probably didn't crash
crashed = False
pub.publish(crashed)
rate.sleep()
except rospy.ROSInterruptException:
accelerometer.close()
def main():
accelerometer0 = Accelerometer()
accelerometer0.setOnAccelerationChangeHandler(onAccelerationChange)
accelerometer0.openWaitForAttachment(5000)
try:
input("Press Enter to Stop\n")
except (Exception, KeyboardInterrupt):
pass
accelerometer0.close()
class PhidgetKalman(object):
#Read the gyro and acceleromater values from MPU6050
def __init__(self):
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.RestrictPitch = True #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
self.radToDeg = 57.2957786
self.kalAngleX = 0
self.kalAngleY = 0
try:
self.accel = Accelerometer() #ch.getAcceleration()
self.gyro = Gyroscope() #ch.getAngularRate()
except Exception as e:
print(
"Cannot initalize, try checking you have the Phidget22 library installed."
)
print("Error:", e)
sys.exit()
# return self.accel, self.gyro
def start(self):
try:
self.accel.openWaitForAttachment(1000)
self.gyro.openWaitForAttachment(1000)
except Exception as e:
print(
"Issue with attaching to IMU. The IMU maybe connected to something else right now.."
)
print("Error:", e)
sys.exit()
def stop(self):
self.accel.close()
self.gyro.close()
print("Stopping")
def get_angles(self, measure_time=5):
#Keep going
time.sleep(1)
accX, accY, accZ = self.accel.getAcceleration()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
# print(roll)
self.kalmanX.setAngle(roll)
self.kalmanX.setAngle(pitch)
gyroXAngle = roll
gyroYAngle = pitch
compAngleX = roll
compAngleY = pitch
timer = time.time()
compare_timer = time.time()
flag = 0
try:
while int(time.time()) < int(measure_time) + int(
compare_timer
): #Should only run for about 5 seconds <- Could make lower
#Read Accelerometer raw value
accX, accY, accZ = self.accel.getAcceleration()
#Read Gyroscope raw value
gyroX, gyroY, gyroZ = self.gyro.getAngularRate()
dt = time.time() - timer
timer = time.time()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(
-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(
accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
gyroXRate = gyroX / 131
gyroYRate = gyroY / 131
if (self.RestrictPitch):
if ((roll < -90 and self.kalAngleX > 90)
or (roll > 90 and self.kalAngleX < -90)):
self.kalmanX.setAngle(roll)
complAngleX = roll
self.kalAngleX = roll
gyroXAngle = roll
else:
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
if (abs(self.kalAngleX) > 90):
gyroYRate = -gyroYRate
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
else:
if ((pitch < -90 and self.kalAngleY > 90)
or (pitch > 90 and self.kalAngleY < -90)):
self.kalmanX.setAngle(pitch)
complAngleY = pitch
self.kalAngleY = pitch
gyroYAngle = pitch
else:
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
if (abs(self.kalAngleY) > 90):
gyroXRate = -gyroXRate
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
#angle = (rate of change of angle) * change in time
gyroXAngle = gyroXRate * dt
gyroYAngle = gyroYAngle * dt
#compAngle = constant * (old_compAngle + angle_obtained_from_gyro) + constant * angle_obtained from accelerometer
compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll
compAngleY = 0.93 * (compAngleY +
gyroYRate * dt) + 0.07 * pitch
if ((gyroXAngle < -180) or (gyroXAngle > 180)):
gyroXAngle = self.kalAngleX
if ((gyroYAngle < -180) or (gyroYAngle > 180)):
gyroYAngle = self.kalAngleY
self.X_angle = self.kalAngleX
self.Y_angle = self.kalAngleY
#print("Angle X: " + str(self.kalAngleX)+" " +"Angle Y: " + str(self.kalAngleY))
#print(str(roll)+" "+str(gyroXAngle)+" "+str(compAngleX)+" "+str(self.kalAngleX)+" "+str(pitch)+" "+str(gyroYAngle)+" "+str(compAngleY)+" "+str(self.kalAngleY))
time.sleep(0.005)
except KeyboardInterrupt:
print("test")
return self.X_angle, self.Y_angle
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
# Definir el tiempo de muestro del acelerometro (En ms)
T_muestreo = 50 # 20 mediciones por segundo
ch.setDataInterval(T_muestreo)
# Esperar 5 segundos por las interrupciones del acelerometro
time.sleep(5)
# Cerrar el acelerometro
try:
ch.close()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
print("Closed Accelerometer device")
# Graficar las aceleraciones en los tres ejes
ac0_np = np.array(ac0) #*9.80665
ac1_np = np.array(ac1) #*9.80665
ac2_np = np.array(ac2) #*9.80665
t_seg = np.array(t) / 1000
plt.figure(1)