Esempio n. 1
0
    void FixedUpdate()
    {
        initialise += 1;

        // Enable the gyro and compass data
        Input.gyro.enabled    = true;
        Input.compass.enabled = true;

        // Wait till all the sensors have warmed up before using any readings
        if ((initialise > 2 / 0.02) && (startStopPedometer.text == "Stop"))
        {
            //Graphing update values
            // tempList = pedometer.value_history.OfType<float>().ToList();
            // window.updateValues(tempList);
            // filtered_graph.updateValues(tempList);


            // Log the time and acceleration data
            string dl_data   = System.DateTime.Now.ToString();
            string disp_data = "";

            this.updateAllClasses();

            // Display the data to GUI
            //  Pedometer data
            disp_data = "Currently at " + pedometer.numSteps + " steps \n" +
                        // "V1: Dist = " + 0.8* pedometer.numSteps + " meters \n" +
                        // "V2: Dist = " + pedometer.distance_travelled + " meters \n" +
                        "Distance travelled:" + pedometer.distance_travelled + " meters \n";
            // "max_pulse_magnitude = " + pedometer.max_pulse_magnitude + " \n\n";

            // Orientation data raw from sensors
            // disp_data += string.Format("Gyro data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", Input.gyro.rotationRate.x, Input.gyro.rotationRate.y, Input.gyro.rotationRate.z);
            // disp_data += string.Format("Magneto data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", Input.compass.rawVector.x, Input.compass.rawVector.y, Input.compass.rawVector.z);
            // // Continuous angles data
            // disp_data += string.Format("Angle about xyz GYRO:\n x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", gyroIntegrator.orientationX, gyroIntegrator.orientationY, gyroIntegrator.orientationZ);
            // disp_data += string.Format("Angle about xyz MAGNETOMETER:\n x: {0:F4}, y: {1:F4}, z: {2:F4}. \n", magnetoAxisRotationAngles.aboutX, magnetoAxisRotationAngles.aboutY, magnetoAxisRotationAngles.aboutZ);

            // disp_data += string.Format("Gravity data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", findGX.outputHistory[0], findGY.outputHistory[0], findGZ.outputHistory[0]);
            // disp_data += string.Format("Kalman data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", kalmanX.xhat, kalmanY.xhat, kalmanZ.xhat);

            // Heading data
            disp_data += string.Format("Heading at: {0:F4}, degrees, filteredHeading @{1:F4}\n", (float)(heading.current_heading * (180 / Math.PI)), (float)(filteredHeading.outputHistory[0] * (180 / Math.PI)));
            // Update compass arrow
            compass_arrow.Rotate(new Vector3(0, 0, (float)(heading.current_heading * (180 / Math.PI) - compass_arrow.eulerAngles.z)));



            // Update text on the screen
            stepCountViewer.GetComponent <UnityEngine.UI.Text>().text = disp_data;



            // data logging data
            // dl_data += "," + pedometer.dist_travelled_this_step + "," + heading.current_heading + ",";
            dl_data += "," + pedometer.dist_travelled_this_step + "," + heading.current_heading + "," + filteredHeading.outputHistory[0] + "," + Input.gyro.rotationRate.z + "," + gyroIntegrator.orientationZ + "," + Input.compass.rawVector.z + "," + kalmanZ.xhat + ",";

            dl.AppendData(dl_data);
        }
    }
Esempio n. 2
0
        /// <summary>
        /// Logs the data from the round of calculations.
        /// </summary>
        /// <param name="time">Seconds since start of trial</param>
        /// <param name="currentMotion">Current motion</param>
        /// <param name="calculatedMotion">New motion, calculated by dynamics</param>
        /// <param name="recording">The c</param>
        /// <param name="controlInput">Current joystick position</param>
        internal void LogData(double time, MotionCommand currentMotion, MotionCommand calculatedMotion, RecordingTimepoint recording, ControlInput controlInput)
        {
            StringBuilder builder;

            builder = new StringBuilder();

            if (Hulk.InnerAxis == Hulk.Axis.Roll)
            {
                builder.AppendFormat(
                    "{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
                    time,
                    recording.TrialNumber,
                    recording.TrialPhase,
                    recording.DirectionOfBalance.roll,
                    recording.DirectionOfBalance.pitch,
                    recording.DirectionOfBalance.yaw,
                    recording.MovingDirectionOfBalance.roll,
                    recording.MovingDirectionOfBalance.pitch,
                    recording.MovingDirectionOfBalance.yaw,
                    currentMotion.innerPosition.ToString("F6"),
                    currentMotion.outerPosition.ToString("F6"),
                    0,
                    currentMotion.innerVelocity.ToString("F6"),
                    currentMotion.outerVelocity.ToString("F6"),
                    0,
                    calculatedMotion.innerPosition.ToString("F6"),
                    calculatedMotion.outerPosition.ToString("F6"),
                    0,
                    (controlInput.trigger ? "1" : "0")
                    );
            }
            else
            {
                builder.AppendFormat(
                    "{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
                    time,
                    recording.TrialNumber,
                    recording.TrialPhase,
                    recording.DirectionOfBalance.roll,
                    recording.DirectionOfBalance.pitch,
                    recording.DirectionOfBalance.yaw,
                    recording.MovingDirectionOfBalance.roll,
                    recording.MovingDirectionOfBalance.pitch,
                    recording.MovingDirectionOfBalance.yaw,
                    0,
                    currentMotion.outerPosition.ToString("F6"),
                    currentMotion.innerPosition.ToString("F6"),
                    0,
                    currentMotion.outerVelocity.ToString("F6"),
                    currentMotion.innerVelocity.ToString("F6"),
                    0,
                    calculatedMotion.outerPosition.ToString("F6"),
                    calculatedMotion.innerPosition.ToString("F6"),
                    (controlInput.trigger ? "1" : "0")
                    );
            }

            DataLogger.AppendData(builder.ToString());
        }
        /// <summary>
        /// Logs the data from the round of calculations.
        /// </summary>
        /// <param name="time">Seconds since start of trial</param>
        /// <param name="trial">The trial being run</param>
        /// <param name="currentMotion">Current motion</param>
        /// <param name="calculatedMotion">New motion, calculated by dynamics</param>
        /// <param name="controlInput">Current joystick position</param>
        /// <param name="noise">Noise input to position</param>
        internal void LogData(double time, Trial trial, MotionCommand currentMotion, MotionCommand calculatedMotion,
                              ControlInput controlInput, RotationAngles noise)
        {
            StringBuilder  builder;
            RotationAngles position;
            RotationAngles calcPosition;
            int            phase;

            position     = new RotationAngles();
            calcPosition = new RotationAngles();

            builder = new StringBuilder();
            switch (trial.TrialStatus)
            {
            case Trial.Status.Initializing:
                phase = 0;
                break;

            case Trial.Status.Moving:
                phase = 1;
                if (Hulk.InnerAxis == Hulk.Axis.Roll)
                {
                    position.roll = currentMotion.innerPosition;
                }
                else
                {
                    position.yaw = currentMotion.innerPosition;
                }
                position.pitch = currentMotion.outerPosition;
                break;

            case Trial.Status.BalancingDOBChanging:
                phase = 2;
                if (Hulk.InnerAxis == Hulk.Axis.Roll)
                {
                    position.roll     = currentMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
                    calcPosition.roll = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
                }
                else
                {
                    position.yaw     = currentMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
                    calcPosition.yaw = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
                }
                position.pitch     = currentMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
                calcPosition.pitch = calculatedMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
                break;

            case Trial.Status.BalancingDOBStable:
                phase = 3;
                if (Hulk.InnerAxis == Hulk.Axis.Roll)
                {
                    position.roll     = currentMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
                    calcPosition.roll = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
                }
                else
                {
                    position.yaw     = currentMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
                    calcPosition.yaw = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
                }
                position.pitch     = currentMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
                calcPosition.pitch = calculatedMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
                break;

            case Trial.Status.Resetting:
                phase = 4;
                if (Hulk.InnerAxis == Hulk.Axis.Roll)
                {
                    position.roll = currentMotion.innerPosition;
                }
                else
                {
                    position.yaw = currentMotion.innerPosition;
                }
                position.pitch = currentMotion.outerPosition;
                break;

            case Trial.Status.Complete:
                phase = 5;
                break;

            default:
                phase = -1;
                break;
            }

            if (Hulk.InnerAxis == Hulk.Axis.Roll)
            {
                builder.AppendFormat("{0},{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}\n",
                                     time,
                                     trial.TrialNumber,
                                     phase,
                                     trial.DirectionOfBalance.roll,
                                     trial.DirectionOfBalance.pitch,
                                     trial.DirectionOfBalance.yaw,
                                     trial.MovingDirectionOfBalance.roll,
                                     trial.MovingDirectionOfBalance.pitch,
                                     trial.MovingDirectionOfBalance.yaw,
                                     position.roll,
                                     position.pitch,
                                     0,
                                     currentMotion.innerVelocity.ToString("F6"),
                                     currentMotion.outerVelocity.ToString("F6"),
                                     0,
                                     ((calculatedMotion != null) ? calcPosition.roll.ToString("F6") : "0"),
                                     ((calculatedMotion != null) ? calcPosition.pitch.ToString("F6") : "0"),
                                     0,
                                     ((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
                                     ((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
                                     0,
                                     ((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
                                     ((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
                                     0,
                                     controlInput.x.ToString("F6"),
                                     controlInput.y.ToString("F6"),
                                     (controlInput.blanked ? "1" : "0"),
                                     (controlInput.trigger ? "1" : "0"),
                                     noise.roll,
                                     noise.pitch,
                                     noise.yaw
                                     );
            }
            else
            {
                builder.AppendFormat("{0},{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}\n",
                                     time,
                                     trial.TrialNumber,
                                     phase,
                                     trial.DirectionOfBalance.roll,
                                     trial.DirectionOfBalance.pitch,
                                     trial.DirectionOfBalance.yaw,
                                     trial.MovingDirectionOfBalance.roll,
                                     trial.MovingDirectionOfBalance.pitch,
                                     trial.MovingDirectionOfBalance.yaw,
                                     0,
                                     position.pitch,
                                     position.yaw,
                                     0,
                                     currentMotion.outerVelocity.ToString("F6"),
                                     currentMotion.innerVelocity.ToString("F6"),
                                     0,
                                     ((calculatedMotion != null) ? calcPosition.pitch.ToString("F6") : "0"),
                                     ((calculatedMotion != null) ? calcPosition.yaw.ToString("F6") : "0"),
                                     0,
                                     ((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
                                     ((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
                                     0,
                                     ((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
                                     ((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
                                     controlInput.x.ToString("F6"),
                                     controlInput.y.ToString("F6"),
                                     (controlInput.blanked ? "1" : "0"),
                                     (controlInput.trigger ? "1" : "0"),
                                     noise.roll,
                                     noise.pitch,
                                     noise.yaw
                                     );
            }


            DataLogger.AppendData(builder.ToString());
        }
        /// <summary>
        /// Logs the data from the round of calculations.
        /// </summary>
        /// <param name="time">Seconds since start of trial</param>
        /// <param name="currentMotion">Current motion</param>
        /// <param name="controlInput">Current forcing function</param>
        private void LogData(double time, MotionCommand currentMotion, MotionCommand calculatedMotion, ControlInput controlInput)
        {
            StringBuilder builder;
            int           phase;

            builder = new StringBuilder();

            phase = 0;

            if (Hulk.InnerAxis == Hulk.Axis.Roll)
            {
                builder.AppendFormat(
                    "{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
                    time,
                    phase,
                    currentMotion.innerPosition.ToString("F6"),
                    currentMotion.outerPosition.ToString("F6"),
                    0,
                    currentMotion.innerVelocity.ToString("F6"),
                    currentMotion.outerVelocity.ToString("F6"),
                    0,
                    ((calculatedMotion != null) ? calculatedMotion.innerPosition.ToString("F6") : "0"),
                    ((calculatedMotion != null) ? calculatedMotion.outerPosition.ToString("F6") : "0"),
                    0,
                    ((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
                    ((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
                    0,
                    ((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
                    ((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
                    0,
                    controlInput.x.ToString("F6"),
                    controlInput.y.ToString("F6")
                    );
            }
            else
            {
                builder.AppendFormat(
                    "{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
                    time,
                    phase,
                    0,
                    currentMotion.outerPosition.ToString("F6"),
                    currentMotion.innerPosition.ToString("F6"),
                    0,
                    currentMotion.outerVelocity.ToString("F6"),
                    currentMotion.innerVelocity.ToString("F6"),
                    0,
                    ((calculatedMotion != null) ? calculatedMotion.outerPosition.ToString("F6") : "0"),
                    ((calculatedMotion != null) ? calculatedMotion.innerPosition.ToString("F6") : "0"),
                    0,
                    ((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
                    ((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
                    0,
                    ((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
                    ((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
                    controlInput.x.ToString("F6"),
                    controlInput.y.ToString("F6")
                    );
            }

            DataLogger.AppendData(builder.ToString());
        }