예제 #1
0
        private Accelerometer()
        {
            OnTrigger("NULL.started", args =>
            {
                U.LoadRecordings();
                V.Navigate("$Start");
                /*
                if (Z.IsConnected)
                {
                    Move("connect");
                    Move("start");
                }
                else
                {
                    Trigger("connect");
                }*/
            });

            OnTrigger("NULL.start_new", args =>
            {
                for (int i = 0; i < 6; i++)
                {
                    G.FilteredReadings[i] = 0;
                    G.FilteredComplete[i] = false;
                    G.FilteredValue[i] = 0;
                }
            });

            OnTrigger("NULL.apply_new", args =>
            {
                G.SaveRecording = true;
                Trigger("connect");
            });

            OnTrigger("NULL.apply_recording", args =>
            {
                if (args.Length != 0 && args[0] is AccelerometerRecording)
                {
                    var recording = (AccelerometerRecording) args[0];
                    G.FilteredReadings[0] = K.MinReadings;
                    G.FilteredValue[0] = recording.Filtered0;
                    G.FilteredComplete[0] = true;

                    G.FilteredReadings[1] = K.MinReadings;
                    G.FilteredValue[1] = recording.Filtered1;
                    G.FilteredComplete[1] = true;

                    G.FilteredReadings[2] = K.MinReadings;
                    G.FilteredValue[2] = recording.Filtered2;
                    G.FilteredComplete[2] = true;

                    G.FilteredReadings[3] = K.MinReadings;
                    G.FilteredValue[3] = recording.Filtered3;
                    G.FilteredComplete[3] = true;

                    G.FilteredReadings[4] = K.MinReadings;
                    G.FilteredValue[4] = recording.Filtered4;
                    G.FilteredComplete[4] = true;

                    G.FilteredReadings[5] = K.MinReadings;
                    G.FilteredValue[5] = recording.Filtered5;
                    G.FilteredComplete[5] = true;

                    G.SaveRecording = false;
                    Trigger("connect", recording.SerialNumber);
                }
            });

            OnTrigger("NULL.connect", args =>
            {
                if (Z.IsConnected == false)
                {
                    A.Trigger("connect", args);
                    Move("connect");
                }
                else
                {
                    Move("connect");
                    Trigger("connected");
                }
            });

            OnTrigger("connect.connected", args =>
            {
                bool completeSet = true;

                for (int i = 0; i < 6; i++)
                {
                    if (G.FilteredComplete[i] == false)
                    {
                        completeSet = false;
                    }
                }

                if (completeSet)
                {
                    Move("axis");
                    Trigger("save");
                }
                else
                {
                    Move("wait_start");
                }

            });

            OnMove("connect>wait_start", args =>
            {
                // Clear
                G.DidSave = false;
                for (int i = 0; i < 6; i++)
                {
                    G.FilteredReadings[i] = 0;
                    G.FilteredComplete[i] = false;
                    G.FilteredValue[i] = 0;
                }

                V.Navigate("$Accelerometer");
            });

            OnTrigger("wait_start.start", args =>
            {
                Move("start");
            });

            OnMove("wait_start>start", args =>
            {
                // Get ZanoHandler to send gyros and raw acceleration frames this way.
                Z.Listen(Symbols.kSensorsGyroGet, "receive_gyro");
                Z.Listen(Symbols.kSensorsRawAccelerationGet, "receive_accelerometer");

                Views.Accelerometer.Instance.Begin();
                Move("axis");
            });

            OnTrigger("axis.retry", args =>
            {
                G.DidSave = false;
                for (int i = 0; i < 6; i++)
                {
                    G.FilteredReadings[i] = 0;
                    G.FilteredComplete[i] = false;
                    G.FilteredValue[i] = 0;
                }
            });

            OnTrigger("axis.save, save.not_sent, connect.save", args =>
            {
                // Compile the readings into maximum and minimum readings
                G.ResultsXMin = U.MinToShort(G.FilteredValue[U.AxisToIndex(1, 0, 0)], G.FilteredValue[U.AxisToIndex(-1, 0, 0)]);
                G.ResultsXMax = U.MaxToShort(G.FilteredValue[U.AxisToIndex(1, 0, 0)], G.FilteredValue[U.AxisToIndex(-1, 0, 0)]);
                G.ResultsYMin = U.MinToShort(G.FilteredValue[U.AxisToIndex(0, 1, 0)], G.FilteredValue[U.AxisToIndex(0, -1, 0)]);
                G.ResultsYMax = U.MaxToShort(G.FilteredValue[U.AxisToIndex(0, 1, 0)], G.FilteredValue[U.AxisToIndex(0, -1, 0)]);
                G.ResultsZMin = U.MinToShort(G.FilteredValue[U.AxisToIndex(0, 0, 1)], G.FilteredValue[U.AxisToIndex(0, 0, -1)]);
                G.ResultsZMax = U.MaxToShort(G.FilteredValue[U.AxisToIndex(0, 0, 1)], G.FilteredValue[U.AxisToIndex(0, 0, -1)]);

                G.ResultsXAvg = (short)(-1 * ((G.ResultsXMin + G.ResultsXMax) / 2));
                G.ResultsYAvg = (short)(-1 * ((G.ResultsYMin + G.ResultsYMax) / 2));
                G.ResultsZAvg = (short)(-1 * ((G.ResultsZMin + G.ResultsZMax) / 2));

                // Copied from ConfigAccelOffsetSet
                Frame sendFrame = new Frame(Z.Handle);
                sendFrame.Reference = Symbols.kConfigAccelOffsetSet;
                sendFrame.Type = Frame.kSendFrame;
                unchecked
                {
                    sendFrame.Names[0] = Symbols.kX;
                    sendFrame.Values[0] = (int)G.ResultsXAvg;
                    sendFrame.Names[1] = Symbols.kY;
                    sendFrame.Values[1] = (int)G.ResultsYAvg;
                    sendFrame.Names[2] = Symbols.kZ;
                    sendFrame.Values[2] = (int)G.ResultsZAvg;
                    sendFrame.Names[3] = Symbols.kXmin;
                    sendFrame.Values[3] = (int)G.ResultsXMin;
                    sendFrame.Names[4] = Symbols.kYmin;
                    sendFrame.Values[4] = (int)G.ResultsYMin;
                    sendFrame.Names[5] = Symbols.kZmin;
                    sendFrame.Values[5] = (int)G.ResultsZMin;
                    sendFrame.Names[6] = Symbols.kXmax;
                    sendFrame.Values[6] = (int)G.ResultsXMax;
                    sendFrame.Names[7] = Symbols.kYmax;
                    sendFrame.Values[7] = (int)G.ResultsYMax;
                    sendFrame.Names[8] = Symbols.kZmax;
                    sendFrame.Values[8] = (int)G.ResultsZMax;
                }

                LibZano.Calibration.Set(ref Z.SerialNumber, ref sendFrame);
                Library.Send(ref sendFrame);

                Frames.ConfigSave(Z.Handle);

                StringBuilder sn = new StringBuilder(16);

                Library.SerialNumber serialNb = new Library.SerialNumber();

                Library.GetSerialNumber(Z.Handle, ref serialNb);
                for (int i = 0; i < 8; i++)
                {
                    sn.AppendFormat("{0:x2}", serialNb.Part[i]);
                }

                G.ResultsSerial = sn.ToString();

                StringBuilder sb = new StringBuilder();
                sb.Append(G.ResultsSerial);
                sb.AppendFormat(" {0:X8} 09", Symbols.kConfigAccelOffsetSet);
                sb.AppendFormat(" {0:X8} {1}", Symbols.kX, U.FormatShort(G.ResultsXAvg));
                sb.AppendFormat(" {0:X8} {1}", Symbols.kY, U.FormatShort(G.ResultsYAvg));
                sb.AppendFormat(" {0:X8} {1}", Symbols.kZ, U.FormatShort(G.ResultsZAvg));

                sb.AppendFormat(" {0:X8} {1}", Symbols.kXmin, U.FormatShort(G.ResultsXMin));
                sb.AppendFormat(" {0:X8} {1}", Symbols.kYmin, U.FormatShort(G.ResultsYMin));
                sb.AppendFormat(" {0:X8} {1}", Symbols.kZmin, U.FormatShort(G.ResultsZMin));

                sb.AppendFormat(" {0:X8} {1}", Symbols.kXmax, U.FormatShort(G.ResultsXMax));
                sb.AppendFormat(" {0:X8} {1}", Symbols.kYmax, U.FormatShort(G.ResultsYMax));
                sb.AppendFormat(" {0:X8} {1}", Symbols.kZmax, U.FormatShort(G.ResultsZMax));

                Console.WriteLine(sb.ToString());

                File.WriteAllText(Core.U.GetApplicationDataPath() + "last_accel_results.txt", sb.ToString());

                if (G.SaveRecording)
                {
                    AccelerometerRecording reading = new AccelerometerRecording();
                    reading.SerialNumber = Z.CopySerialNumber(serialNb);
                    G.HighestRecordingId++;
                    reading.Id = G.HighestRecordingId;
                    reading.Filtered0 = (int) G.FilteredValue[0];
                    reading.Filtered1 = (int) G.FilteredValue[1];
                    reading.Filtered2 = (int) G.FilteredValue[2];
                    reading.Filtered3 = (int) G.FilteredValue[3];
                    reading.Filtered4 = (int) G.FilteredValue[4];
                    reading.Filtered5 = (int) G.FilteredValue[5];

                    G.Recordings.Add(reading);
                    U.SaveRecordings();
                }

                G.ResultsValues = sb.ToString();

                //StringBuilder fc = new StringBuilder();
                //var now = DateTime.Now;
                //fc.AppendFormat("{0} {1}{2}", now.ToLongDateString(), now.ToLongTimeString(), Environment.NewLine);
                // fc.AppendFormat("{0}", sb.)

                //fc.AppendLine(DateTime.Now.)

                // The Zano will automatically reboot, so we should switch state and wait for the connected event which
                // is handled by libZano.
                Move("save_to_zano");
                V.Navigate("$SaveToZano");

            });

            OnTrigger("save_to_zano.disconnected", args =>
            {
                Console.WriteLine("Probably Saved");
                Move("save_to_server");
                V.Navigate("$SaveToServer");
                LibZano.ServerConnectivity.ForceRefresh();
            });

            OnTrigger("save_to_server.no_internet", args =>
            {
                Console.WriteLine("Checking for Internet");
                LibZano.ServerConnectivity.ForceRefresh();
            });

            OnTrigger("save_to_server.configuration_downloaded", args =>
            {
                Console.WriteLine("Got for Internet");
                if (G.DidSave == false)
                {
                    G.DidSave = true;
                    Console.WriteLine("Saving to server...");
                    LibZano.Calibration.ApplyToServer(ref Z.SerialNumber, Symbols.kConfigAccelOffsetSet);
                }
            });

            OnTrigger("save_to_server.config_server_save_complete", args =>
            {
                Console.WriteLine("Saved Config");
                V.Navigate("$Complete");
            });

            OnTrigger("save_to_server.config_server_save_failed", args =>
            {
                Console.WriteLine("Not Saved Config");
            });

            OnTrigger("axis.tick", args =>
            {
                Frames.SensorsGyroGet(Z.Handle);

                if (G.Recording)
                {
                    Frames.SensorsRawAccelerationGet(Z.Handle);
                }
            });

            OnTrigger("axis.receive_gyro", args =>
            {

                Frame frame = (Frame)args[0];

                // Get the gyro vector and calculate the magnitude
                double x = frame.Get(Symbols.kX);
                double y = frame.Get(Symbols.kY);
                double z = frame.Get(Symbols.kZ);
                double mag = Math.Sqrt(x * x + y * y + z * z);

                G.GyroMagnitude = mag;

                Console.WriteLine("Gyro Mag = {0}", mag);
                if (mag >= K.GyroMaxThreshold)
                {
                    // If the magnitude is over the threshold stop asking for accelerometer
                    G.Recording = false;
                    G.RecordingTime = 0;

                    int lastAxis = U.AxisToIndex(G.LastAxisX, G.LastAxisY, G.LastAxisZ);

                    if (G.FilteredReadings[lastAxis] < K.MinReadings)
                    {
                        // Was the number of readings not finished? Then reset them
                        G.FilteredComplete[lastAxis] = false;
                        G.FilteredReadings[lastAxis] = 0;
                        G.FilteredValue[lastAxis] = 0;
                    }
                }
                else
                {
                    // Underneath the current threshold

                    if (G.Recording == false)
                    {
                        // Not recording?
                        // Turn it on!
                        G.Recording = true;
                        G.RecordingTime = Library.Time();
                        G.RecordingStarted = true;
                    }
                }

                // Refresh the view
                if (Views.Accelerometer.Instance != null)
                {
                    Views.Accelerometer.Instance.Refresh();
                }
            });

            OnTrigger("axis.receive_accelerometer", args =>
            {

                Frame frame = (Frame)args[0];

                // Get raw accelerometer values
                double x = frame.Get(Symbols.kX);
                double y = frame.Get(Symbols.kY);
                double z = frame.Get(Symbols.kZ);

                int xDir = 0;
                int yDir = 0;
                int zDir = 0;

                U.GetAxisGravityVector(x, y, z, ref xDir, ref yDir, ref zDir);

                /*

                    // Console.WriteLine("{0} {1} {2}", x, y, z);

                    U.Normalise(ref x, ref y, ref z);

                    // Calculate the best axis that the Zano is pointing up as using
                    // three dot products against each of the unit vectors, from that
                    // you have a ~unit vector that is the best direction.

                    int xDir = U.DotToInt(x, y, z, 1, 0, 0);
                    int yDir = U.DotToInt(x, y, z, 0, 1, 0);
                    int zDir = U.DotToInt(x, y, z, 0, 0, 1);

                */

                // Store old axis and copy new one over.

                G.LastAxisX = G.AxisX;
                G.LastAxisY = G.AxisY;
                G.LastAxisZ = G.AxisZ;

                G.AxisX = xDir;
                G.AxisY = yDir;
                G.AxisZ = zDir;

                // Now on a different axis?

                if (G.AxisX != G.LastAxisX || G.AxisY != G.LastAxisY || G.AxisZ != G.LastAxisZ)
                {
                    int lastAxis = U.AxisToIndex(G.LastAxisX, G.LastAxisY, G.LastAxisZ);
                    if (G.FilteredReadings[lastAxis] >= K.MinReadings)
                    {
                        // Did all readings?
                        // Mark old axis as complete
                        G.FilteredComplete[lastAxis] = true;
                    }
                    else
                    {
                        // Didn't complete.?
                        // Reset readings and value, so they can be done again.
                        G.FilteredReadings[lastAxis] = 0;
                        G.FilteredComplete[lastAxis] = false;
                        G.FilteredValue[lastAxis] = 0;
                    }

                    // Say something
                    Console.WriteLine("Changed Axis from {0} to {1}", U.AxisToString(G.LastAxisX, G.LastAxisY, G.LastAxisZ), U.AxisToString(G.AxisX, G.AxisY, G.AxisZ));
                }

                int axisIndex = U.AxisToIndex(G.AxisX, G.AxisY, G.AxisZ);

                // Read and perform a low pass if the number of readings of the current axis
                // has not been met yet.
                if (G.FilteredReadings[axisIndex] < K.MinReadings)
                {

                    // Set Initial Value, which is +-4096 (based on axis) where 4096 is 1G.
                    // Ideally the accelerometer would be +-4096 but due to manufacturing this
                    // can vary, hence the need for calibration!
                    if (G.RecordingStarted)
                    {
                        G.RecordingStarted = false;
                        G.FilteredValue[axisIndex] = K.LowPassDefaultFilter * U.AxisSign(G.AxisX, G.AxisY, G.AxisZ);
                    }

                    // Perform a low pass filter on the value and store.
                    double filtered = G.FilteredValue[axisIndex];
                    double now = frame.Get(U.AxisToSymbol(G.AxisX, G.AxisY, G.AxisZ));

                    G.FilteredValue[axisIndex] = U.LowPass(filtered, now, K.LowPassD);

                    G.FilteredReadings[axisIndex]++;
                }

                // Exceeded reading count? Then it's complete
                if (G.FilteredReadings[axisIndex] >= K.MinReadings)
                {
                    G.FilteredComplete[axisIndex] = true;
                }

                // Update the view
                // V.RefreshAccelerometerDisplay();
                if (Views.Accelerometer.Instance != null)
                {
                    Views.Accelerometer.Instance.Refresh();
                }

            });

            OnTrigger("save_to_server.menu", args =>
            {
                Move("NULL");
                T.Set("NULL");
            });
        }
예제 #2
0
 public static void LoadRecordings()
 {
     G.Recordings.Clear();
     String path = Core.U.GetApplicationDataPath() + "accelerometer.dat";
     if (File.Exists(path))
     {
         var lines = File.ReadAllLines(path);
         foreach (var line in lines)
         {
             var r = new AccelerometerRecording(line.Trim());
             G.Recordings.Add(r);
             if (G.HighestRecordingId < r.Id)
             {
                 G.HighestRecordingId = r.Id;
             }
         }
     }
 }