From 56e2f58e05c4a144eda10cdbae1c8b84be2b7a1a Mon Sep 17 00:00:00 2001
From: Sam Horvath <sam.horvath@kitware.com>
Date: Thu, 11 May 2017 16:53:03 -0400
Subject: [PATCH] ENH: Updated arduino sketch parameters
---
.../imstkVRPNArduino_roll_only.ino | 20 ++++++++++---------
1 file changed, 11 insertions(+), 9 deletions(-)
diff --git a/Arduino/imstkVRPNArduino_roll_only/imstkVRPNArduino_roll_only.ino b/Arduino/imstkVRPNArduino_roll_only/imstkVRPNArduino_roll_only.ino
index fbc656512..74f798cc9 100644
--- a/Arduino/imstkVRPNArduino_roll_only/imstkVRPNArduino_roll_only.ino
+++ b/Arduino/imstkVRPNArduino_roll_only/imstkVRPNArduino_roll_only.ino
@@ -127,6 +127,7 @@ void setup() {
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(57600);
+ Serial.println("setup");
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
@@ -249,11 +250,12 @@ void loop() {
accel[0] = aa.x;
accel[1] = aa.y;
accel[2] = aa.z;
- ComplementaryFilter(accel, gyro, &m_pitch, &m_roll);
+ ComplementaryFilter(accel, gyro);
Serial.print(m_roll);
-
+ Serial.print(",");
+ Serial.print(m_pitch);
//dummy data to fill out 6 vrpn analog channels
- Serial.println(",0,0,0,0,0");
+ Serial.println(",0,0,0,0");
#endif
@@ -263,25 +265,25 @@ void loop() {
}
}
-void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
+void ComplementaryFilter(short accData[3], short gyrData[3])
{
float pitchAcc, rollAcc;
// Integrate the gyroscope data -> int(angularSpeed) = angle
- *pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
- *roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis
+ m_pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
+ m_roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis
// Compensate for drift with accelerometer data if !bullshit
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
int forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]);
- if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
+ if (forceMagnitudeApprox > ACCELEROMETER_SENSITIVITY && forceMagnitudeApprox < 32768)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)accData[1], (float)accData[2]) * 180 / M_PI;
- *pitch = *pitch * 0.5 + pitchAcc * 0.5;
+ m_pitch = m_pitch * 0.98 + pitchAcc * 0.02;
// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f((float)accData[0], (float)accData[2]) * 180 / M_PI;
- *roll = *roll * 0.5 + rollAcc * 0.5;
+ m_roll = m_roll * 0.95 + rollAcc * 0.05;
}
}
--
GitLab