Exemple #1
0
        public static void setScaleRunningSate(bool run)
        {
            if (run)
            {
                // Move to scale cal
                currentState       = 0;
                biasPointsRunning  = false;
                scalePointsRunning = true;
                readPointsRunning  = false;
                prgPointsRunning   = false;
                passCount          = 0;

                // Button color
                uiForm.btStartGyroCal.BackColor = System.Drawing.Color.Green;
            }
            else
            {
                // State
                biasPointsRunning  = false;
                scalePointsRunning = false;
                readPointsRunning  = false;
                prgPointsRunning   = false;

                // Button color
                uiForm.btStartGyroCal.BackColor = System.Drawing.Color.DimGray;

                // Move home
                PosFixture.setPosition("X0 Y0 Z0");
            }
        }
 // Start / Stop
 public static void setAhrsAtpRunningSate(bool run)
 {
     if (run)
     {
         atpPointsRunning = true;
         uiForm.btStartAhrsAtp.BackColor = System.Drawing.Color.Green;
     }
     else
     {
         atpPointsRunning = false;
         //uiForm.btStartAhrsAtp.BackColor = System.Drawing.Color.DimGray;
         PosFixture.setPosition("X0 Y0 Z0");
     }
 }
Exemple #3
0
        // Start / Stop
        public static void setBiasRunningSate(bool run)
        {
            if (run)
            {
                // Start with gyro cal

                biasPointsRunning  = true;
                scalePointsRunning = false;

                //biasPointsRunning = false;
                //scalePointsRunning = true;

                readPointsRunning = false;
                prgPointsRunning  = false;

                // Initial values
                currentState = 0;
                calData[(int)CAL_POINT.BIASX] = 0;
                calData[(int)CAL_POINT.BIASY] = 0;
                calData[(int)CAL_POINT.BIASZ] = 0;
                calData[(int)CAL_POINT.XONX]  = 1;
                calData[(int)CAL_POINT.XONY]  = 0;
                calData[(int)CAL_POINT.XONZ]  = 0;
                calData[(int)CAL_POINT.YONX]  = 0;
                calData[(int)CAL_POINT.YONY]  = 1;
                calData[(int)CAL_POINT.YONZ]  = 0;
                calData[(int)CAL_POINT.ZONX]  = 0;
                calData[(int)CAL_POINT.ZONY]  = 0;
                calData[(int)CAL_POINT.ZONZ]  = 1;

                // Button color
                uiForm.btStartGyroCal.BackColor = System.Drawing.Color.Green;
            }
            else
            {
                // State
                biasPointsRunning  = false;
                scalePointsRunning = false;
                readPointsRunning  = false;
                prgPointsRunning   = false;

                // Button color
                uiForm.btStartGyroCal.BackColor = System.Drawing.Color.DimGray;

                // Move home
                PosFixture.setPosition("X0 Y0 Z0");
            }
        }
Exemple #4
0
        // Start / Stop
        public static void setBiasRunningSate(bool run)
        {
            if (run)
            {
                // Start with bias cal
                biasPointsRunning  = true;
                scalePointsRunning = false;
                readPointsRunning  = false;
                prgPointsRunning   = false;


                uiForm.btLru0MagParmName.Text  = "Bias Values";
                uiForm.btLru0MagParmValue.Text = "Set Position";
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.MAGX, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.MAGY, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.MAGZ, RATE.RATE_50HZ);

                // Initial values
                currentState = 0;
                loopCount    = 0;

                uiForm.btLru0MagXMin.Text = "9999.0";
                uiForm.btLru0MagYMin.Text = "9999.0";
                uiForm.btLru0MagZMin.Text = "9999.0";
                uiForm.btLru0MagXMax.Text = "-9999.0";
                uiForm.btLru0MagYMax.Text = "-9999.0";
                uiForm.btLru0MagZMax.Text = "-9999.0";


                // Button color
                uiForm.btStartMagCal.BackColor = System.Drawing.Color.Green;
            }
            else
            {
                // State
                biasPointsRunning  = false;
                scalePointsRunning = false;
                readPointsRunning  = false;
                prgPointsRunning   = false;

                // Button color
                uiForm.btStartMagCal.BackColor = System.Drawing.Color.DimGray;

                // Move home
                PosFixture.setPosition("X0 Y0 Z0");
            }
        }
        // State machine update
        public static void update()
        {
            string msg = "";

            // Handle state
            if (atpPointsRunning)
            {
                msg = atpStateMachine();
            }

            // Handle next step
            if (msg == "next")
            {
                currentState++;
            }
            else if (msg == "back")
            {
                currentState--;
            }
            else if (msg == "skip")
            {
                currentState += 2;
            }
            else if (msg == "wait")
            {
            }
            else if (msg == "end")
            {
                // Turn off outputs
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                atpPointsRunning = false;

                // Back to zero
                PosFixture.setPosition("X0 Y0 Z0");

                //// Data
                //uiForm.btAhrsAtpParmName.Text = "Complete";
                //uiForm.btLru0AhrsAtpParmValue.Text = "";

                // Button color
                //uiForm.btStartAhrsAtp.BackColor = System.Drawing.Color.DimGray;
            }
        }
        // Start / Stop
        public static void setCalRunningSate(bool run)
        {
            if (run)
            {
                // Start with accel cal
                currentState      = 0;
                calPointsRunning  = true;
                readPointsRunning = false;
                prgPointsRunning  = false;

                // Initial values
                calData[(int)CAL_POINT.AXZ] = 0;
                calData[(int)CAL_POINT.AXP] = 1;
                calData[(int)CAL_POINT.AXN] = -1;
                calData[(int)CAL_POINT.AYZ] = 0;
                calData[(int)CAL_POINT.AYP] = 1;
                calData[(int)CAL_POINT.AYN] = -1;
                calData[(int)CAL_POINT.AZZ] = 0;
                calData[(int)CAL_POINT.AZP] = 1;
                calData[(int)CAL_POINT.AZN] = -1;

                // Button color
                uiForm.btStartAccelCal.BackColor = System.Drawing.Color.Green;
            }
            else
            {
                // State
                calPointsRunning  = false;
                readPointsRunning = false;
                prgPointsRunning  = false;

                // Button color
                //uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;

                // Move home
                PosFixture.setPosition("X0 Y0 Z0");
            }
        }
        // Start / Stop
        public static void setCalRunningSate(bool run)
        {
            if (run)
            {
                // Start with accel cal
                currentState    = 0;
                calAccelRunning = true;
                calGyroRunning  = false;

                // Initial values
                axz = 0;
                axp = 1;
                axn = -1;
                ayz = 0;
                ayp = 1;
                ayn = -1;
                azz = 0;
                azp = 1;
                azn = -1;

                // Button color
                uiForm.btStartAccelCal.BackColor = System.Drawing.Color.Green;
            }
            else
            {
                // Shutdown state
                calAccelRunning = false;
                calGyroRunning  = false;

                // Button color
                uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;

                // Move home
                PosFixture.setPosition("X0 Y0 Z0");
            }
        }
        // State machine update
        public static void update()
        {
            string msg = "";


            // Show data based on state
            if ((currentState >= (int)CAL_STATE.SET_ACCELX0) && (currentState <= (int)CAL_STATE.GET_ACCELXN1))
            {
                //uiForm.btLru0AccelParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELXR].ToString("00.00000");
            }
            if ((currentState >= (int)CAL_STATE.SET_ACCELY0) && (currentState <= (int)CAL_STATE.GET_ACCELYN1))
            {
                uiForm.btLru0AccelParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELYR0].ToString("00.00000");
            }
            if ((currentState >= (int)CAL_STATE.SET_ACCELZ0) && (currentState <= (int)CAL_STATE.GET_ACCELZN1))
            {
                uiForm.btLru0AccelParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELZR0].ToString("00.00000");
            }


            // Handle state
            if (calPointsRunning)
            {
                // Accel cal points
                msg = accelStateMachine();
            }
            else if (prgPointsRunning)
            {
                // Program points
                msg = prgPointsStateMachine();
            }
            else if (readPointsRunning)
            {
                // Read points
                msg = readPointsStateMachine();
            }

            // Handle next step
            if (msg == "next")
            {
                currentState++;
            }
            else if (msg == "back")
            {
                currentState--;
            }
            else if (msg == "skip")
            {
                currentState += 2;
            }
            else if (msg == "wait")
            {
            }
            else if (msg == "end")
            {
                // Turn off outputs
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                calPointsRunning  = false;
                prgPointsRunning  = false;
                readPointsRunning = false;

                // Back to zero
                PosFixture.setPosition("X0 Y0 Z0");

                // Data
                uiForm.btLru0AccelParmName.Text  = "Complete";
                uiForm.btLru0AccelParmValue.Text = "";

                // Button color
                //uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;
            }
        }
        private static string atpStateMachine()
        {
            switch (currentState)
            {
            case (int)ATP_STATE.AHRS_RES:
            {
                PosFixture.setPosition("X0 Y0 Z0");
                //uiForm.btAhrsAtpParmName.Text = "AHRS Reset";
                //uiForm.btLru0AhrsAtpParmValue.Text = "AHRS Reset";
                // Put into slaving mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
                // Reset AHRS
                uiForm.lru0.setEngWriteF(ENG.ARES, 1.0);
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.RPYWEIGHT, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.BIASWEIGHT, RATE.RATE_50HZ);
                loopCount   = 0;
                stableCount = 0;
                Thread.Sleep(1000);
                return("next");
            }

            case (int)ATP_STATE.AHRS_WAIT:
            {
                double weight = uiForm.lru0.dataParam[(int)PARAM.RPYWEIGHT];
                //uiForm.btLru0AhrsAtpParmValue.Text = weight.ToString("00");
                if (((int)uiForm.lru0.dataParam[(int)PARAM.STATUS] & STATUS.AHRS_STABLE) != 0)
                {
                    if (stableCount < 100)
                    {
                        stableCount++;
                        return("wait");
                    }
                    else
                    {
                        return("next");
                    }
                }
                else
                {
                    return("wait");
                }
            }

            case (int)ATP_STATE.AHRS_PREP:
            {
                //uiForm.btAhrsAtpParmName.Text = "Scale Test Points";
                //uiForm.btLru0AhrsAtpParmValue.Text = "Set Position";
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.YAW, RATE.RATE_50HZ);
                loopCount = 0;

                // Wait for serial data to start and get parsed
                Thread.Sleep(2000);

                return("next");
            }

            case (int)ATP_STATE.SET_POSITION:
            {
                if (loopCount < atpMovePoints.Length)
                {
                    // Put into free gyro mode
                    uiForm.lru0.setEngWriteF(ENG.FRGM, 1.0);

                    // Get initial values
                    iRoll  = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    iPitch = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    iYaw   = uiForm.lru0.dataParam[(int)PARAM.YAW];


                    PosFixture.setPosition(atpMovePoints[loopCount]);
                    //uiForm.btAhrsAtpParmName.Text = atpMovePointsDesc[loopCount];
                    sampleCount = 0;
                    return("next");
                }
                else
                {
                    currentState = (int)ATP_STATE.END_ATP;
                    return("none");
                }
            }

            case (int)ATP_STATE.WAIT_POSITION:
            {
                //uiForm.btLru0AhrsAtpParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.00") +
                //	" P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00") +
                //	" Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.00");
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    // Wait for full motion to stop and get readings
                    if (sampleCount++ > 50)
                    {
                        return("next");
                    }
                    else
                    {
                        return("wait");
                    }
                }
            }

            case (int)ATP_STATE.GET_POSITION:
            {
                roll  = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                pitch = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                yaw   = uiForm.lru0.dataParam[(int)PARAM.YAW];

                double rollError = 10, pitchError = 10, yawError = 10, testValue = 0;
                switch (loopCount)
                {
                // Roll +
                case 0:
                    testValue  = roll;
                    rollError  = roll - iRoll - atpMoveValue[loopCount];
                    pitchError = pitch - iPitch;
                    yawError   = yaw - iYaw;
                    if (yawError > 180)
                    {
                        yawError -= 360;
                    }
                    if (yawError < -180)
                    {
                        yawError += 360;
                    }
                    break;

                // Roll -
                case 1:
                    testValue  = roll;
                    rollError  = roll - iRoll - atpMoveValue[loopCount];
                    pitchError = pitch - iPitch;
                    yawError   = yaw - iYaw;
                    if (yawError > 180)
                    {
                        yawError -= 360;
                    }
                    if (yawError < -180)
                    {
                        yawError += 360;
                    }
                    break;

                // Pitch +
                case 2:
                    testValue  = pitch;
                    rollError  = roll - iRoll;
                    pitchError = pitch - iPitch - atpMoveValue[loopCount];
                    yawError   = yaw - iYaw;
                    if (yawError > 180)
                    {
                        yawError -= 360;
                    }
                    if (yawError < -180)
                    {
                        yawError += 360;
                    }
                    break;

                // Pitch -
                case 3:
                    testValue  = pitch;
                    rollError  = roll - iRoll;
                    pitchError = pitch - iPitch - atpMoveValue[loopCount];
                    yawError   = yaw - iYaw;
                    if (yawError > 180)
                    {
                        yawError -= 360;
                    }
                    if (yawError < -180)
                    {
                        yawError += 360;
                    }
                    break;

                // Yaw +
                case 4:
                    testValue  = yaw;
                    rollError  = roll - iRoll;
                    pitchError = pitch - iPitch;
                    yawError   = yaw - iYaw - atpMoveValue[loopCount];
                    if (yawError > 180)
                    {
                        yawError -= 360;
                    }
                    if (yawError < -180)
                    {
                        yawError += 360;
                    }
                    break;

                // Yaw -
                case 5:
                    testValue  = yaw;
                    rollError  = roll - iRoll;
                    pitchError = pitch - iPitch;
                    yawError   = yaw - iYaw - atpMoveValue[loopCount];
                    if (yawError > 180)
                    {
                        yawError -= 360;
                    }
                    if (yawError < -180)
                    {
                        yawError += 360;
                    }
                    break;
                }

                //switch (loopCount)
                //{
                //	case 0: // + Roll
                //		atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                //		break;
                //	case 1: // - Roll
                //		atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                //		break;
                //	case 2: // + Pitch
                //		atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                //		break;
                //	case 3: // - Pitch
                //		atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                //		break;
                //	case 4: // + Yaw
                //		atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
                //		break;
                //	case 5: // - Yaw
                //		atpDataPoint[loopCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
                //		break;
                //}

                string passFail;
                if ((Math.Abs(rollError) > TEST_TOL) || (Math.Abs(pitchError) > TEST_TOL) || (Math.Abs(yawError) > TEST_TOL))
                {
                    passFail = "FAIL";
                }
                else
                {
                    passFail = "PASS";
                }
                addTableEntry(atpMovePointsDesc[loopCount], testValue.ToString("00.00"), rollError.ToString("0.00"), pitchError.ToString("0.00"), yawError.ToString("0.00"), passFail);

                loopCount++;
                return("next");
            }

            case (int)ATP_STATE.SET_ZERO:
            {
                // Put into slaving mode mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);

                PosFixture.setPosition("X0 Y0 Z0");
                sampleCount = 0;
                return("next");
            }


            case (int)ATP_STATE.WAIT_ZERO:
            {
                //uiForm.btLru0AhrsAtpParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.00") +
                //	" P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00") +
                //	" Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.00");
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    // Hold position to re-zero - let some align occur
                    if (sampleCount++ > 200)
                    {
                        // Move to next test
                        currentState = (int)ATP_STATE.SET_POSITION;
                        return("none");
                    }
                    else
                    {
                        return("wait");
                    }
                }
            }



            case (int)ATP_STATE.END_ATP:
            {
                PosFixture.setPosition("X0 Y0 Z0");
                //// Check for chain
                //if (uiForm.cbChainGyrolCalToPrg.Checked)
                //{
                //	// Start Program cycle
                //	setPrgRunningState(true);
                //	return ("wait");
                //}
                return("end");
            }

            default:
            {
                return("end");
            }
            }
        }
Exemple #10
0
        private static string biasStateMachine()
        {
            int       loop      = 0;
            const int COM_DELAY = 200;              // Delay for communication

            switch (currentState)
            {
            //
            // Bias XYZ
            //
            case (int)BIAS_STATE.SET_BIAS:
            {
                uiForm.btLru0GyroParmName.Text  = "Bias Values";
                uiForm.btLru0GyroParmValue.Text = "Set Position";
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROXR0, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROYR0, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROZR0, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROXR1, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROYR1, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROZR1, RATE.RATE_50HZ);
                PosFixture.setPosition("X0 Y0 Z0");

                // Clear bias values in unit to get raw readings without bias applied
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, 0);

                // Clear cross coupling only if requested
                if (!uiForm.cbSetUnityGain.Checked)
                {
                    // Clear cross matrix if full calibration
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 3, 1.0);                                        // xonx
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 4, 0);
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 5, 0);

                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 6, 0);
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 7, 1.0);                                        // yony
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 8, 0);

                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 9, 0);
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 10, 0);
                    uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 11, 1.0);                                       // zonz
                }
                uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);
                return("next");
            }

            case (int)BIAS_STATE.WAIT_BIAS:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    System.Threading.Thread.Sleep(1000);
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIASX:
            {
                uiForm.btLru0GyroParmName.Text = "X Bias Value";
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]        = uiForm.lru0.dataParam[(int)PARAM.GYROXR0];
                    dataSample1[sampleCount]        = uiForm.lru0.dataParam[(int)PARAM.GYROXR1];
                    uiForm.btLru0GyroParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.BIASX] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0GyroParmName.Text, calData[(int)CAL_POINT.BIASX], "MEASURE");
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIASY:
            {
                uiForm.btLru0GyroParmName.Text = "Y Bias Value";
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]        = uiForm.lru0.dataParam[(int)PARAM.GYROYR0];
                    dataSample1[sampleCount]        = uiForm.lru0.dataParam[(int)PARAM.GYROYR1];
                    uiForm.btLru0GyroParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.BIASY] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0GyroParmName.Text, calData[(int)CAL_POINT.BIASY], "MEASURE");
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIASZ:
            {
                uiForm.btLru0GyroParmName.Text = "Z Bias Value";

                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]        = uiForm.lru0.dataParam[(int)PARAM.GYROZR0];
                    dataSample1[sampleCount]        = uiForm.lru0.dataParam[(int)PARAM.GYROZR1];
                    uiForm.btLru0GyroParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.BIASZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0GyroParmName.Text, calData[(int)CAL_POINT.BIASZ], "MEASURE");
                    return("next");
                }
            }

            case (int)BIAS_STATE.PROG_BIAS:
            {
                uiForm.btLru0GyroParmName.Text = "Program Bias XYZ";
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, calData[(int)CAL_POINT.BIASX]);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, calData[(int)CAL_POINT.BIASY]);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, calData[(int)CAL_POINT.BIASZ]);
                uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);

                // Start Scale if not bias only
                if (!uiForm.cbBiasCalOnly.Checked)
                {
                    setScaleRunningSate(true);
                }
                biasPointsRunning = false;
                System.Threading.Thread.Sleep(500);
                return("none");
            }


            default:
            {
                return("end");
            }
            }
        }
Exemple #11
0
        private static string scaleStateMachine()
        {
            switch (currentState)
            {
            case (int)SCALE_STATE.AHRS_RES:
            {
                PosFixture.setPosition("X0 Y0 Z0");
                uiForm.btLru0GyroParmName.Text  = "AHRS Reset";
                uiForm.btLru0GyroParmValue.Text = "AHRS Reset";
                uiForm.btGyroCalPass.Text       = passCount.ToString("0");

                // Put into slaving mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
                // Reset AHRS
                uiForm.lru0.setEngWriteF(ENG.ARES, 1.0);
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.RPYWEIGHT, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.BIASWEIGHT, RATE.RATE_50HZ);
                loopCount = 0;
                Thread.Sleep(1000);
                return("next");
            }

            case (int)SCALE_STATE.AHRS_WAIT:
            {
                double weight = uiForm.lru0.dataParam[(int)PARAM.RPYWEIGHT];
                uiForm.btLru0GyroParmValue.Text = weight.ToString("00");
                if (((int)uiForm.lru0.dataParam[(int)PARAM.STATUS] & STATUS.AHRS_STABLE) != 0)
                {
                    return("next");
                }
                else
                {
                    return("wait");
                }
            }

            case (int)SCALE_STATE.PREP_SCALE:
            {
                uiForm.btLru0GyroParmName.Text  = "Scale Test Points";
                uiForm.btLru0GyroParmValue.Text = "Set Position";
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.YAW, RATE.RATE_50HZ);
                loopCount = 0;

                // Put into free gyro mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 1.0);

                return("next");
            }

            case (int)SCALE_STATE.SET_SCALE:
            {
                if (loopCount < gyroMovePoints.Length)
                {
                    PosFixture.setPosition(gyroMovePoints[loopCount]);
                    uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
                    sampleCount = 0;
                    return("next");
                }
                else
                {
                    currentState = (int)SCALE_STATE.END_SCALE;
                    return("none");
                }
            }

            case (int)SCALE_STATE.WAIT_SCALE:
            {
                uiForm.btLru0GyroParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.0") +
                                                  " P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.0") +
                                                  " Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.0");
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    // Wait for full motion to stop and get readings
                    if (sampleCount++ > 5)
                    {
                        return("next");
                    }
                    else
                    {
                        return("wait");
                    }
                }
            }

            case (int)SCALE_STATE.GET_SCALE:
            {
                switch (loopCount)
                {
                // X Effect (Roll)
                case 0:
                    xonx[passCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    xony[passCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    xonz[passCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
                    break;

                case 1:
                    xonx[passCount] = (1.0 / ((-xonx[passCount] + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / (ANGLE * 2)));
                    xony[passCount] = ((-xony[passCount] + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / (ANGLE * 2));
                    xonz[passCount] = ((-xonz[passCount] + uiForm.lru0.dataParam[(int)PARAM.YAW]) / (ANGLE * 2));
                    addTableEntry("X on X", xonx[passCount], "MEASURE");
                    addTableEntry("X on Y", xony[passCount], "MEASURE");
                    addTableEntry("X on Z", xonz[passCount], "MEASURE");
                    PosFixture.setPosition("X0 Y0 Z0");
                    break;

                // Y Effect (Pitch)
                case 2:
                    // Capture roll left
                    yonx[passCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    yony[passCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    yonz[passCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
                    break;

                case 3:
                    // Capture roll right and get overall scale
                    yonx[passCount] = ((-yonx[passCount] + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / (ANGLE * 2));
                    yony[passCount] = (1.0 / ((-yony[passCount] + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / (ANGLE * 2)));
                    yonz[passCount] = ((-yonz[passCount] + uiForm.lru0.dataParam[(int)PARAM.YAW]) / (ANGLE * 2));
                    addTableEntry("Y on X", yonx[passCount], "MEASURE");
                    addTableEntry("Y on Y", yony[passCount], "MEASURE");
                    addTableEntry("Y on Z", yonz[passCount], "MEASURE");
                    PosFixture.setPosition("X0 Y0 Z0");
                    break;

                // Z Effect (Yaw)
                case 4:
                    // Capture roll left
                    zonx[passCount] = uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    zony[passCount] = uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    zonz[passCount] = uiForm.lru0.dataParam[(int)PARAM.YAW];
                    break;

                case 5:
                    // Capture roll right and get overall scale
                    zonx[passCount] = ((-zonx[passCount] + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / (ANGLE * 2));
                    zony[passCount] = ((-zony[passCount] + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / (ANGLE * 2));
                    zonz[passCount] = (1.0 / ((-zonz[passCount] + uiForm.lru0.dataParam[(int)PARAM.YAW]) / (ANGLE * 2)));
                    addTableEntry("Z on X", zonx[passCount], "MEASURE");
                    addTableEntry("Z on Y", zony[passCount], "MEASURE");
                    addTableEntry("Z on Z", zonz[passCount], "MEASURE");
                    PosFixture.setPosition("X0 Y0 Z0");
                    break;
                }

                loopCount++;
                currentState = (int)SCALE_STATE.SET_SCALE;
                return("none");
            }

            case (int)SCALE_STATE.END_SCALE:
            {
                // Put back into slave mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
                PosFixture.setPosition("X0 Y0 Z0");


                // Check for pass count
                if (++passCount < SCALE_PASSES)
                {
                    // Loop back
                    Thread.Sleep(1000);
                    currentState = (int)SCALE_STATE.AHRS_RES;
                    return("none");
                }

                // Check for chain
                if (uiForm.cbChainGyrolCalToPrg.Checked)
                {
                    // Start Program cycle
                    setPrgRunningState(true);
                    return("wait");
                }
                return("end");
            }

            default:
            {
                return("end");
            }
            }
        }
Exemple #12
0
        private static string biasStateMachine()
        {
            int loop = 0;

            switch (currentState)
            {
            //
            // Bias XYZ
            //
            case (int)BIAS_STATE.SET_BIAS:
            {
                uiForm.btLru0GyroParmName.Text  = "Bias Values";
                uiForm.btLru0GyroParmValue.Text = "Set Position";
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROXR, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROYR, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROZR, RATE.RATE_50HZ);
                PosFixture.setPosition("X0 Y0 Z0");

                // Clear bias values in unit to get raw readings without bias applied
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, 0);
                uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);

                return("next");
            }

            case (int)BIAS_STATE.WAIT_BIAS:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIASX:
            {
                uiForm.btLru0GyroParmName.Text = "X Bias Value";
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.GYROXR];
                    uiForm.btLru0GyroParmValue.Text = dataSample[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.BIASX] = dataSample.Average();
                    addTableEntry(uiForm.btLru0GyroParmName.Text, dataSample.Average(), "MEASURE");
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIASY:
            {
                uiForm.btLru0GyroParmName.Text = "Y Bias Value";
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.GYROYR];
                    uiForm.btLru0GyroParmValue.Text = dataSample[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.BIASY] = dataSample.Average();
                    addTableEntry(uiForm.btLru0GyroParmName.Text, dataSample.Average(), "MEASURE");
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIASZ:
            {
                uiForm.btLru0GyroParmName.Text = "Z Bias Value";

                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.GYROZR];
                    uiForm.btLru0GyroParmValue.Text = dataSample[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.BIASZ] = dataSample.Average();
                    addTableEntry(uiForm.btLru0GyroParmName.Text, dataSample.Average(), "MEASURE");
                    return("next");
                }
            }

            case (int)BIAS_STATE.PROG_BIAS:
            {
                uiForm.btLru0GyroParmName.Text = "Program Bias XYZ";
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 0, calData[(int)CAL_POINT.BIASX]);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 1, calData[(int)CAL_POINT.BIASY]);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 2, calData[(int)CAL_POINT.BIASZ]);

                // Clear cross matrix
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 3, 1.0);                                    // xonx
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 4, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 5, 0);

                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 6, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 7, 1.0);                                    // yony
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 8, 0);

                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 9, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 10, 0);
                uiForm.lru0.setCalWriteF(FLASH_LOC_GYRO_CAL_START + 11, 1.0);                                   // zonz

                uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);

                // Start Scale
                setScaleRunningSate(true);
                return("none");
            }


            default:
            {
                return("end");
            }
            }
        }
Exemple #13
0
        // State machine update
        public static void update()
        {
            string msg = "";


            //// Show data based on state
            //if ((currentState >= (int)CAL_BIAS.SET_ACCELX0) && (currentState <= (int)BIAS_STATE.GET_ACCELXN1) )
            //{
            //	uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELXR].ToString("00.00000");
            //}
            //if ((currentState >= (int)BIAS_STATE.SET_ACCELY0) && (currentState <= (int)BIAS_STATE.GET_ACCELYN1))
            //{
            //	uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELYR].ToString("00.00000");
            //}
            //if ((currentState >= (int)BIAS_STATE.SET_ACCELZ0) && (currentState <= (int)BIAS_STATE.GET_ACCELZN1))
            //{
            //	uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELZR].ToString("00.00000");
            //}


            // Handle state
            if (biasPointsRunning)
            {
                msg = biasStateMachine();
            }
            else if (scalePointsRunning)
            {
                msg = scaleStateMachine();
            }
            else if (prgPointsRunning)
            {
                msg = prgPointsStateMachine();
            }
            else if (readPointsRunning)
            {
                msg = readPointsStateMachine();
            }

            // Handle next step
            if (msg == "next")
            {
                currentState++;
            }
            else if (msg == "back")
            {
                currentState--;
            }
            else if (msg == "skip")
            {
                currentState += 2;
            }
            else if (msg == "wait")
            {
            }
            else if (msg == "end")
            {
                // Turn off outputs
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                biasPointsRunning  = false;
                scalePointsRunning = false;
                prgPointsRunning   = false;
                readPointsRunning  = false;

                // Back to zero
                PosFixture.setPosition("X0 Y0 Z0");

                // Data
                uiForm.btLru0GyroParmName.Text  = "Complete";
                uiForm.btLru0GyroParmValue.Text = "";

                // Button color
                uiForm.btStartGyroCal.BackColor = System.Drawing.Color.DimGray;
            }
        }
Exemple #14
0
        private static string scaleStateMachine()
        {
            switch (currentState)
            {
            case (int)SCALE_STATE.SETUP:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROXSUM, RATE.RATE_25HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROYSUM, RATE.RATE_25HZ);
                uiForm.lru0.setParameterReq((int)PARAM.GYROZSUM, RATE.RATE_25HZ);
                return("next");
            }

            case (int)SCALE_STATE.MOVE_START:
            {
                if (loopCount < gyroMovePoints.Length)
                {
                    PosFixture.setPosition(gyroMovePoints[loopCount]);
                    uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
                    System.Threading.Thread.Sleep(1000);
                    return("next");
                }
                else
                {
                    // Exit scale calibration
                    currentState = (int)SCALE_STATE.END_SCALE;
                    return("none");
                }
            }

            case (int)SCALE_STATE.WAIT_START:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    // Clear gyro sums
                    uiForm.lru0.setEngWriteF(ENG.SCLR, 1.0);
                    System.Threading.Thread.Sleep(2000);
                    loopCount++;                                        // Next point
                    return("next");
                }
            }

            case (int)SCALE_STATE.MOVE_END:
            {
                if (loopCount < gyroMovePoints.Length)
                {
                    PosFixture.setPosition(gyroMovePoints[loopCount]);
                    uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
                    System.Threading.Thread.Sleep(500);
                    return("next");
                }
                else
                {
                    // Exit scale calibration
                    currentState = (int)SCALE_STATE.END_SCALE;
                    return("none");
                }
            }

            case (int)SCALE_STATE.WAIT_END:
            {
                uiForm.btLru0GyroParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.GYROXSUM].ToString("000.00") +
                                                  " P:" + uiForm.lru0.dataParam[(int)PARAM.GYROYSUM].ToString("000.00") +
                                                  " Y:" + uiForm.lru0.dataParam[(int)PARAM.GYROZSUM].ToString("000.00");
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    System.Threading.Thread.Sleep(500);
                    return("next");
                }
            }

            case (int)SCALE_STATE.GET_SCALE:
            {
                switch (loopCount)
                {
                // X Effect (Roll)
                case 1:
                    System.Threading.Thread.Sleep(200);
                    deltaX = uiForm.lru0.dataParam[(int)PARAM.GYROXSUM];
                    deltaY = uiForm.lru0.dataParam[(int)PARAM.GYROYSUM];
                    deltaZ = uiForm.lru0.dataParam[(int)PARAM.GYROZSUM];
                    xonx   = (1.0 / (deltaX / SCALE_SWEEP_ANGLE));
                    xony   = (deltaY / SCALE_SWEEP_ANGLE);
                    xonz   = (deltaZ / SCALE_SWEEP_ANGLE);
                    break;

                // Y Effect (Pitch)
                case 3:
                    System.Threading.Thread.Sleep(200);
                    deltaX = uiForm.lru0.dataParam[(int)PARAM.GYROXSUM];
                    deltaY = uiForm.lru0.dataParam[(int)PARAM.GYROYSUM];
                    deltaZ = uiForm.lru0.dataParam[(int)PARAM.GYROZSUM];
                    yonx   = (deltaX / SCALE_SWEEP_ANGLE);
                    yony   = (1.0 / (deltaY / SCALE_SWEEP_ANGLE));
                    yonz   = (deltaZ / SCALE_SWEEP_ANGLE);
                    break;

                // Z Effect (Yaw)
                case 5:
                    System.Threading.Thread.Sleep(200);
                    deltaX = uiForm.lru0.dataParam[(int)PARAM.GYROXSUM];
                    deltaY = uiForm.lru0.dataParam[(int)PARAM.GYROYSUM];
                    deltaZ = uiForm.lru0.dataParam[(int)PARAM.GYROZSUM];
                    zonx   = (deltaX / SCALE_SWEEP_ANGLE);
                    zony   = (deltaY / SCALE_SWEEP_ANGLE);
                    zonz   = (1.0 / (deltaZ / SCALE_SWEEP_ANGLE));
                    break;
                }

                loopCount++;
                currentState = (int)SCALE_STATE.MOVE_START;
                return("none");
            }

            case (int)SCALE_STATE.END_SCALE:
            {
                PosFixture.setPosition("X0 Y0 Z0");

                // Start Program cycle
                setPrgRunningState(true);
                return("wait");
            }

            default:
            {
                return("end");
            }
            }
        }
        private static string scaleStateMachine()
        {
            switch (currentState)
            {
            case (int)SCALE_STATE.AHRS_RES:
            {
                PosFixture.setPosition("X0 Y0 Z0");
                uiForm.btLru0GyroParmName.Text  = "AHRS Reset";
                uiForm.btLru0GyroParmValue.Text = "AHRS Reset";
                // Put into slaving mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
                // Reset AHRS
                uiForm.lru0.setEngWriteF(ENG.ARES, 1.0);
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.RPYWEIGHT, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.BIASWEIGHT, RATE.RATE_50HZ);
                loopCount   = 0;
                stableCount = 0;
                Thread.Sleep(1000);
                return("next");
            }

            case (int)SCALE_STATE.AHRS_WAIT:
            {
                double weight = uiForm.lru0.dataParam[(int)PARAM.RPYWEIGHT];
                uiForm.btLru0GyroParmValue.Text = weight.ToString("00");
                if (((int)uiForm.lru0.dataParam[(int)PARAM.STATUS] & STATUS.AHRS_STABLE) != 0)
                {
                    if (stableCount < 100)
                    {
                        stableCount++;
                        return("wait");
                    }
                    else
                    {
                        return("next");
                    }
                }
                else
                {
                    return("wait");
                }
            }

            case (int)SCALE_STATE.PREP_SCALE:
            {
                uiForm.btLru0GyroParmName.Text  = "Scale Test Points";
                uiForm.btLru0GyroParmValue.Text = "Set Position";
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.STATUS, RATE.RATE_1HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.YAW, RATE.RATE_50HZ);
                loopCount = 0;

                // Put into free gyro mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 1.0);

                return("next");
            }

            case (int)SCALE_STATE.SET_SCALE:
            {
                if (loopCount < gyroMovePoints.Length)
                {
                    PosFixture.setPosition(gyroMovePoints[loopCount]);
                    uiForm.btLru0GyroParmName.Text = gyroMovePoints[loopCount];
                    sampleCount = 0;
                    return("next");
                }
                else
                {
                    currentState = (int)SCALE_STATE.END_SCALE;
                    return("none");
                }
            }

            case (int)SCALE_STATE.WAIT_SCALE:
            {
                uiForm.btLru0GyroParmValue.Text = "R:" + uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("00.00") +
                                                  " P:" + uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00") +
                                                  " Y:" + uiForm.lru0.dataParam[(int)PARAM.YAW].ToString("00.00");
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    // Wait for full motion to stop and get readings
                    if (sampleCount++ > 5)
                    {
                        return("next");
                    }
                    else
                    {
                        return("wait");
                    }
                }
            }

            case (int)SCALE_STATE.GET_SCALE:
            {
                switch (loopCount)
                {
                // X Effect (Roll)
                case 0:
                    xonx0 = -uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    xony0 = -uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    xonz0 = -uiForm.lru0.dataParam[(int)PARAM.YAW];
                    break;

                case 1:
                    if (scalePassNumber == 1)
                    {
                        xonx1 = 1.0 / ((xonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
                        xony1 = ((xony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
                        xonz1 = ((xonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
                        addTableEntry("X on X 1", xonx1, "MEASURE");
                        addTableEntry("X on Y 1", xony1, "MEASURE");
                        addTableEntry("X on Z 1", xonz1, "MEASURE");
                    }
                    if (scalePassNumber == 2)
                    {
                        xonx2 = 1.0 / ((xonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
                        xony2 = ((xony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
                        xonz2 = ((xonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
                        addTableEntry("X on X 2", xonx2, "MEASURE");
                        addTableEntry("X on Y 2", xony2, "MEASURE");
                        addTableEntry("X on Z 2", xonz2, "MEASURE");
                    }
                    PosFixture.setPosition("X0 Y0 Z0");
                    break;

                // Y Effect (Pitch)
                case 2:
                    yonx0 = -uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    yony0 = -uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    yonz0 = -uiForm.lru0.dataParam[(int)PARAM.YAW];
                    break;

                case 3:
                    if (scalePassNumber == 1)
                    {
                        yonx1 = ((yonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
                        yony1 = 1.0 / ((yony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
                        yonz1 = ((yonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);                                                 //-
                        addTableEntry("Y on X 1", yonx1, "MEASURE");
                        addTableEntry("Y on Y 1", yony1, "MEASURE");
                        addTableEntry("Y on Z 1", yonz1, "MEASURE");
                    }
                    if (scalePassNumber == 2)
                    {
                        yonx2 = ((yonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
                        yony2 = 1.0 / ((yony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);
                        yonz2 = ((yonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);                                                 //-
                        addTableEntry("Y on X 2", yonx2, "MEASURE");
                        addTableEntry("Y on Y 2", yony2, "MEASURE");
                        addTableEntry("Y on Z 2", yonz2, "MEASURE");
                    }

                    PosFixture.setPosition("X0 Y0 Z0");
                    break;

                // Z Effect (Yaw)
                case 4:
                    // Capture roll left
                    zonx0 = -uiForm.lru0.dataParam[(int)PARAM.ROLL];
                    zony0 = -uiForm.lru0.dataParam[(int)PARAM.PITCH];
                    zonz0 = -uiForm.lru0.dataParam[(int)PARAM.YAW];
                    break;

                case 5:
                    if (scalePassNumber == 1)
                    {
                        zonx1 = ((zonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
                        zony1 = ((zony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);                                                 //-
                        zonz1 = 1.0 / ((zonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
                        addTableEntry("Z on X 1", zonx1, "MEASURE");
                        addTableEntry("Z on Y 1", zony1, "MEASURE");
                        addTableEntry("Z on Z 1", zonz1, "MEASURE");
                    }
                    if (scalePassNumber == 2)
                    {
                        zonx2 = ((zonx0 + uiForm.lru0.dataParam[(int)PARAM.ROLL]) / SCALE_SWEEP_ANGLE);
                        zony2 = ((zony0 + uiForm.lru0.dataParam[(int)PARAM.PITCH]) / SCALE_SWEEP_ANGLE);                                                 //-
                        zonz2 = 1.0 / ((zonz0 + uiForm.lru0.dataParam[(int)PARAM.YAW]) / SCALE_SWEEP_ANGLE);
                        addTableEntry("Z on X 2", zonx2, "MEASURE");
                        addTableEntry("Z on Y 2", zony2, "MEASURE");
                        addTableEntry("Z on Z 2", zonz2, "MEASURE");
                    }
                    PosFixture.setPosition("X0 Y0 Z0");
                    break;
                }

                loopCount++;
                currentState = (int)SCALE_STATE.SET_SCALE;
                return("none");
            }

            case (int)SCALE_STATE.END_SCALE:
            {
                // Put back into slave mode
                uiForm.lru0.setEngWriteF(ENG.FRGM, 0.0);
                PosFixture.setPosition("X0 Y0 Z0");

                // Check for chain
                if (uiForm.cbChainGyrolCalToPrg.Checked)
                {
                    // Start Program cycle
                    setPrgRunningState(true);
                    return("wait");
                }
                return("end");
            }

            default:
            {
                return("end");
            }
            }
        }
        private static string accelStateMachine()
        {
            int loop = 0;

            switch (currentState)
            {
            //
            // Accel X
            //
            case (int)CAL_STATE.SET_ACCELX0:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELXR0, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELXR1, RATE.RATE_50HZ);
                uiForm.btLru0AccelParmName.Text = "Accel X Zero";
                ///PosFixture.setPosition("X5 Y5 Z5");
                PosFixture.setPosition("X0 Y0 Z0");
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELX0:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELX0:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.ACCELXR0];
                    dataSample1[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.ACCELXR1];
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");

                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AXZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AXZ], "MEASURE");
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELXP1:
            {
                uiForm.btLru0AccelParmName.Text = "Accel X Pos";
                PosFixture.setPosition("X0 Y0 Z-" + ANGLE);
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELXP1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELXP1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR0] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    dataSample1[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR1] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AXP] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AXP], "MEASURE");
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELXN1:
            {
                uiForm.btLru0AccelParmName.Text = "Accel X Neg";
                PosFixture.setPosition("X0 Y0 Z+" + ANGLE);
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELXN1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELXN1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR0] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    dataSample1[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELXR1] - calData[(int)CAL_POINT.AXZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AXN] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AXN], "MEASURE");
                    return("next");
                }
            }



            //
            // Accel Y
            //
            case (int)CAL_STATE.SET_ACCELY0:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELYR0, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELYR1, RATE.RATE_50HZ);
                uiForm.btLru0AccelParmName.Text = "Accel Y Zero";

                PosFixture.setPosition("X0 Y0 Z0");
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELY0:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELY0:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR0];
                    dataSample1[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR1];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AYZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AYZ], "MEASURE");
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELYP1:
            {
                uiForm.btLru0AccelParmName.Text = "Accel Y Pos";
                PosFixture.setPosition("X0 Y" + ANGLE + " Z" + ROLL_ON_PITCH_POS);
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELYP1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELYP1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR0] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    dataSample1[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR1] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AYP] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AYP], "MEASURE");
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELYN1:
            {
                uiForm.btLru0AccelParmName.Text = "Accel Y Neg";
                PosFixture.setPosition("X0 Y-" + ANGLE + " Z" + ROLL_ON_PITCH_NEG);
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELYN1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELYN1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR0] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    dataSample1[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELYR1] - calData[(int)CAL_POINT.AYZ]) * (1.0 / Math.Sin(ANGLE_IN_RAD));
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AYN] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AYN], "MEASURE");
                    return("next");
                }
            }



            //
            // Accel Z
            //
            case (int)CAL_STATE.SET_ACCELZ0:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELZR0, RATE.RATE_50HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELZR1, RATE.RATE_50HZ);
                uiForm.btLru0AccelParmName.Text = "Accel Z Zero";

                PosFixture.setPosition("X0 Y90 Z0");
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELZ0:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELZ0:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.ACCELZR0];
                    dataSample1[sampleCount]         = uiForm.lru0.dataParam[(int)PARAM.ACCELZR1];
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AZZ] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AZZ], "MEASURE");
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELZP1:
            {
                uiForm.btLru0AccelParmName.Text = "Accel Z Pos";
                PosFixture.setPosition("X0 Y0 Z0");
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELZP1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELZP1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR0] - calData[(int)CAL_POINT.AZZ]);
                    dataSample1[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR1] - calData[(int)CAL_POINT.AZZ]);
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AZP] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AZP], "MEASURE");
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELZN1:
            {
                uiForm.btLru0AccelParmName.Text = "Accel Z Neg";
                PosFixture.setPosition("X0 Y180 Z0");
                sampleCount = HOLD_COUNT;
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELZN1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    if (sampleCount-- > 0)
                    {
                        return("wait");
                    }
                    else
                    {
                        sampleCount = 0;
                        return("next");
                    }
                }
            }

            case (int)CAL_STATE.GET_ACCELZN1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample0[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR0] - calData[(int)CAL_POINT.AZZ]);
                    dataSample1[sampleCount]         = (uiForm.lru0.dataParam[(int)PARAM.ACCELZR1] - calData[(int)CAL_POINT.AZZ]);
                    uiForm.btLru0AccelParmValue.Text = dataSample0[sampleCount].ToString("00.00000");
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    calData[(int)CAL_POINT.AZN] = (dataSample0.Average() + dataSample1.Average()) / 2.0;
                    addTableEntry(uiForm.btLru0AccelParmName.Text, calData[(int)CAL_POINT.AZN], "MEASURE");

                    // Back to zero
                    PosFixture.setPosition("X0 Y0 Z0");

                    // Start Prog cycle
                    setPrgRunningState(true);
                    return("wait");
                }
            }



            default:
            {
                return("end");
            }
            }
        }
Exemple #17
0
        private static string biasStateMachine()
        {
            switch (currentState)
            {
            case (int)BIAS_STATE.SET_BIAS:
            {
                if (loopCount < movePoints.Length)
                {
                    PosFixture.setPosition(movePoints[loopCount]);
                    uiForm.btLru0MagParmName.Text = movePoints[loopCount];

                    return("next");
                }
                else
                {
                    //currentState = (int)BIAS_STATE.PROG_BIAS;
                    return("none");
                }
            }

            case (int)BIAS_STATE.WAIT_BIAS:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)BIAS_STATE.GET_BIAS:
            {
                uiForm.btLru0MagParmName.Text = movePointDesc[loopCount];

                switch (loopCount)
                {
                // X Positive
                case 0:
                    xBiasPos = uiForm.lru0.dataParam[(int)PARAM.MAGX];
                    break;

                // X Negative
                case 1:
                    xBiasNeg = uiForm.lru0.dataParam[(int)PARAM.MAGX];
                    break;
                }
                loopCount++;
                currentState = (int)BIAS_STATE.SET_BIAS;
                return("none");
            }

            case (int)BIAS_STATE.PROG_BIAS:
            {
                return("end");
                //uiForm.btLru0MagParmName.Text = "Program Bias XYZ";
                //uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 0, xBiasPos - xBiasNeg;
                //uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 1, calData[(int)CAL_POINT.BIASY]);
                //uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 2, calData[(int)CAL_POINT.BIASZ]);

                //// Clear scales
                //uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 3, 1);
                //uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 4, 1);
                //uiForm.lru0.setCalWriteF(FLASH_LOC_MAG_CAL_START + 5, 1);

                //uiForm.lru0.setCalWriteF((int)PARAM.FLUSH, 0);

                //// Start Scale
                //setScaleRunningSate(true);
                //return ("none");
            }


            default:
            {
                return("end");
            }
            }
        }
Exemple #18
0
        // State machine update
        public static void update()
        {
            string msg = "";

            // Handle state
            if (biasPointsRunning)
            {
                // Display Current
                uiForm.btLru0MagXCur.Text = uiForm.lru0.dataParam[(int)PARAM.MAGX].ToString("0000");
                uiForm.btLru0MagYCur.Text = uiForm.lru0.dataParam[(int)PARAM.MAGY].ToString("0000");
                uiForm.btLru0MagZCur.Text = uiForm.lru0.dataParam[(int)PARAM.MAGZ].ToString("0000");

                // Default Colors
                uiForm.btLru0MagXMin.BackColor  = Color.Transparent;
                uiForm.btLru0MagYMin.BackColor  = Color.Transparent;
                uiForm.btLru0MagZMin.BackColor  = Color.Transparent;
                uiForm.btLru0MagXMax.BackColor  = Color.Transparent;
                uiForm.btLru0MagYMax.BackColor  = Color.Transparent;
                uiForm.btLru0MagZMax.BackColor  = Color.Transparent;
                uiForm.btLru0MagXNorm.BackColor = Color.Transparent;
                uiForm.btLru0MagYNorm.BackColor = Color.Transparent;
                uiForm.btLru0MagZNorm.BackColor = Color.Transparent;

                if ((uiForm.lru0.dataParam[(int)PARAM.MAGX] != 0) &&
                    (uiForm.lru0.dataParam[(int)PARAM.MAGY] != 0) &&
                    (uiForm.lru0.dataParam[(int)PARAM.MAGZ] != 0))
                {
                    // Mag Min
                    if (uiForm.lru0.dataParam[(int)PARAM.MAGX] < double.Parse(uiForm.btLru0MagXMin.Text))
                    {
                        uiForm.btLru0MagXMin.Text      = uiForm.lru0.dataParam[(int)PARAM.MAGX].ToString("F1");
                        uiForm.btLru0MagXMin.BackColor = Color.DarkGreen;
                    }
                    if (uiForm.lru0.dataParam[(int)PARAM.MAGY] < double.Parse(uiForm.btLru0MagYMin.Text))
                    {
                        uiForm.btLru0MagYMin.Text      = uiForm.lru0.dataParam[(int)PARAM.MAGY].ToString("F1");
                        uiForm.btLru0MagYMin.BackColor = Color.DarkGreen;
                    }
                    if (uiForm.lru0.dataParam[(int)PARAM.MAGZ] < double.Parse(uiForm.btLru0MagZMin.Text))
                    {
                        uiForm.btLru0MagZMin.Text      = uiForm.lru0.dataParam[(int)PARAM.MAGZ].ToString("F1");
                        uiForm.btLru0MagZMin.BackColor = Color.DarkGreen;
                    }

                    // Mag Max
                    if (uiForm.lru0.dataParam[(int)PARAM.MAGX] > double.Parse(uiForm.btLru0MagXMax.Text))
                    {
                        uiForm.btLru0MagXMax.Text      = uiForm.lru0.dataParam[(int)PARAM.MAGX].ToString("F1");
                        uiForm.btLru0MagXMax.BackColor = Color.DarkGreen;
                    }
                    if (uiForm.lru0.dataParam[(int)PARAM.MAGY] > double.Parse(uiForm.btLru0MagYMax.Text))
                    {
                        uiForm.btLru0MagYMax.Text      = uiForm.lru0.dataParam[(int)PARAM.MAGY].ToString("F1");
                        uiForm.btLru0MagYMax.BackColor = Color.DarkGreen;
                    }
                    if (uiForm.lru0.dataParam[(int)PARAM.MAGZ] > double.Parse(uiForm.btLru0MagZMax.Text))
                    {
                        uiForm.btLru0MagZMax.Text      = uiForm.lru0.dataParam[(int)PARAM.MAGZ].ToString("F1");
                        uiForm.btLru0MagZMax.BackColor = Color.DarkGreen;
                    }

                    // Delta
                    uiForm.btLru0MagXDelta.Text = Math.Abs(double.Parse(uiForm.btLru0MagXMax.Text) - double.Parse(uiForm.btLru0MagXMin.Text)).ToString("F1");
                    uiForm.btLru0MagYDelta.Text = Math.Abs(double.Parse(uiForm.btLru0MagYMax.Text) - double.Parse(uiForm.btLru0MagYMin.Text)).ToString("F1");
                    uiForm.btLru0MagZDelta.Text = Math.Abs(double.Parse(uiForm.btLru0MagZMax.Text) - double.Parse(uiForm.btLru0MagZMin.Text)).ToString("F1");

                    uiForm.btLru0MagXOffset.Text = (-double.Parse(uiForm.btLru0MagXMin.Text) - (double.Parse(uiForm.btLru0MagXDelta.Text) / 2)).ToString("F2");
                    uiForm.btLru0MagYOffset.Text = (-double.Parse(uiForm.btLru0MagYMin.Text) - (double.Parse(uiForm.btLru0MagYDelta.Text) / 2)).ToString("F2");
                    uiForm.btLru0MagZOffset.Text = (-double.Parse(uiForm.btLru0MagZMin.Text) - (double.Parse(uiForm.btLru0MagZDelta.Text) / 2)).ToString("F2");

                    uiForm.btLru0MagXCent.Text = (uiForm.lru0.dataParam[(int)PARAM.MAGX] + double.Parse(uiForm.btLru0MagXOffset.Text)).ToString("F3");
                    uiForm.btLru0MagYCent.Text = (uiForm.lru0.dataParam[(int)PARAM.MAGY] + double.Parse(uiForm.btLru0MagYOffset.Text)).ToString("F3");
                    uiForm.btLru0MagZCent.Text = (uiForm.lru0.dataParam[(int)PARAM.MAGZ] + double.Parse(uiForm.btLru0MagZOffset.Text)).ToString("F3");

                    uiForm.btLru0MagXNorm.Text = (double.Parse(uiForm.btLru0MagXCent.Text) / (double.Parse(uiForm.btLru0MagXDelta.Text) / 2)).ToString("F3");
                    uiForm.btLru0MagYNorm.Text = (double.Parse(uiForm.btLru0MagYCent.Text) / (double.Parse(uiForm.btLru0MagYDelta.Text) / 2)).ToString("F3");
                    uiForm.btLru0MagZNorm.Text = (double.Parse(uiForm.btLru0MagZCent.Text) / (double.Parse(uiForm.btLru0MagZDelta.Text) / 2)).ToString("F3");

                    if (Math.Abs(double.Parse(uiForm.btLru0MagXNorm.Text)) > 0.95)
                    {
                        uiForm.btLru0MagXNorm.BackColor = Color.DarkGreen;
                    }
                    if (Math.Abs(double.Parse(uiForm.btLru0MagYNorm.Text)) > 0.95)
                    {
                        uiForm.btLru0MagYNorm.BackColor = Color.DarkGreen;
                    }
                    if (Math.Abs(double.Parse(uiForm.btLru0MagZNorm.Text)) > 0.95)
                    {
                        uiForm.btLru0MagZNorm.BackColor = Color.DarkGreen;
                    }


                    // Computed Heading
                    double heading = Math.Atan2(double.Parse(uiForm.btLru0MagYNorm.Text), double.Parse(uiForm.btLru0MagXNorm.Text));
                    if (heading < 0)
                    {
                        heading += 2 * 3.14;
                    }
                    heading *= 180 / 3.14;

                    //if ((heading > 0) && (heading < 360))
                    //{
                    //	heading = 360.0 - heading;
                    //	currentHeading = accumHeading / 10;
                    //	accumHeading += heading - currentHeading;
                    //}

                    uiForm.btLru0Heading.Text = heading.ToString("F0");
                }

//				msg = biasStateMachine();
            }
            //else if (scalePointsRunning)
            //{
            //	msg = scaleStateMachine();
            //}
            //else if (prgPointsRunning)
            //{
            //	msg = prgPointsStateMachine();
            //}
            //else if (readPointsRunning)
            //{
            //	msg = readPointsStateMachine();
            //}

            // Handle next step
            if (msg == "next")
            {
                currentState++;
            }
            else if (msg == "back")
            {
                currentState--;
            }
            else if (msg == "skip")
            {
                currentState += 2;
            }
            else if (msg == "wait")
            {
            }
            else if (msg == "end")
            {
                // Turn off outputs
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                biasPointsRunning  = false;
                scalePointsRunning = false;
                prgPointsRunning   = false;
                readPointsRunning  = false;

                // Back to zero
                PosFixture.setPosition("X0 Y0 Z0");

                // Data
                uiForm.btLru0MagParmName.Text  = "Complete";
                uiForm.btLru0MagParmValue.Text = "";

                // Button color
                uiForm.btStartMagCal.BackColor = System.Drawing.Color.DimGray;
            }
        }
        private static string accelStateMachine()
        {
            int loop = 0;

            switch (currentState)
            {
            //
            // Accel X
            //
            case (int)CAL_STATE.SET_ACCELX0:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELXR, RATE.RATE_50HZ);
                uiForm.btLru0ParmName.Text = "Accel X Zero";
                PosFixture.setPosition("X10 Y0 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELX0:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELX0:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    axz = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELXP1:
            {
                uiForm.btLru0ParmName.Text = "Accel X Pos";
                PosFixture.setPosition("X0 Y0 Z-90");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELXP1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELXP1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    axp = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELXN1:
            {
                uiForm.btLru0ParmName.Text = "Accel X Neg";
                PosFixture.setPosition("X0 Y0 Z+90");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELXN1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELXN1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELXR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    axn = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }



            //
            // Accel Y
            //
            case (int)CAL_STATE.SET_ACCELY0:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELYR, RATE.RATE_50HZ);
                uiForm.btLru0ParmName.Text = "Accel Y Zero";

                PosFixture.setPosition("X0 Y0 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELY0:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELY0:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    ayz = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELYP1:
            {
                uiForm.btLru0ParmName.Text = "Accel Y Pos";
                PosFixture.setPosition("X0 Y90 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELYP1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELYP1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    ayp = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELYN1:
            {
                uiForm.btLru0ParmName.Text = "Accel Y Neg";
                PosFixture.setPosition("X0 Y-90 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELYN1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELYN1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELYR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    ayn = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }



            //
            // Accel Z
            //
            case (int)CAL_STATE.SET_ACCELZ0:
            {
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                uiForm.lru0.setParameterReq((int)PARAM.ACCELZR, RATE.RATE_50HZ);
                uiForm.btLru0ParmName.Text = "Accel Z Zero";

                PosFixture.setPosition("X0 Y90 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELZ0:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELZ0:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    azz = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELZP1:
            {
                uiForm.btLru0ParmName.Text = "Accel Z Pos";
                PosFixture.setPosition("X0 Y0 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELZP1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELZP1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    azp = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }

            case (int)CAL_STATE.SET_ACCELZN1:
            {
                uiForm.btLru0ParmName.Text = "Accel Z Neg";
                PosFixture.setPosition("X0 Y180 Z0");
                return("next");
            }

            case (int)CAL_STATE.WAIT_ACCELZN1:
            {
                if (PosFixture.getInMotion())
                {
                    return("wait");
                }
                else
                {
                    sampleCount = 0;
                    return("next");
                }
            }

            case (int)CAL_STATE.GET_ACCELZN1:
            {
                if (sampleCount < SAMPLE_COUNT)
                {
                    dataSample[sampleCount] = uiForm.lru0.dataParam[(int)PARAM.ACCELZR];
                    sampleCount++;
                    return("wait");
                }
                else
                {
                    azn = dataSample.Average();
                    addTableEntry(uiForm.btLru0ParmName.Text, dataSample.Average());
                    return("next");
                }
            }



            default:
            {
                return("end");
            }
            }
        }
        // State machine update
        public static void update()
        {
            string msg = "";


            // Show data based on state
            if ((currentState >= (int)CAL_STATE.SET_ACCELX0) && (currentState <= (int)CAL_STATE.GET_ACCELXN1))
            {
                uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELXR].ToString("00.00000");
            }
            if ((currentState >= (int)CAL_STATE.SET_ACCELY0) && (currentState <= (int)CAL_STATE.GET_ACCELYN1))
            {
                uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELYR].ToString("00.00000");
            }
            if ((currentState >= (int)CAL_STATE.SET_ACCELZ0) && (currentState <= (int)CAL_STATE.GET_ACCELZN1))
            {
                uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.ACCELZR].ToString("00.00000");
            }


            // Handle state
            if (calAccelRunning)
            {
                // General field updates
                uiForm.bADCTestPointNum.Text = (BcaCalPoints.currentCalPoint + 1).ToString() + " OF " + BcaCalPoints.bcaCalPoint.Length.ToString();

                // Accel cal points
                msg = accelStateMachine();
            }

            else
            {
                //// Idle data
                //try
                //{
                //	uiForm.lru0.setParameterReq((int)PARAM.ROLL, RATE.RATE_50HZ);
                //	uiForm.lru0.setParameterReq((int)PARAM.PITCH, RATE.RATE_50HZ);
                //}
                //catch
                //{
                //}

                //// LRU Roll Pitch values
                //uiForm.btLru0ParmName.Text = uiForm.lru0.dataParam[(int)PARAM.ROLL].ToString("000.00");
                //uiForm.btLru0ParmValue.Text = uiForm.lru0.dataParam[(int)PARAM.PITCH].ToString("00.00");
            }



            //if (PosFixture.getInHomeMotion())
            //{
            //	uiForm.btHomeFixture.BackColor = System.Drawing.Color.Red;
            //}
            //else
            //{
            //	uiForm.btHomeFixture.BackColor = System.Drawing.Color.Transparent;

            //}
            // Get current state from running SM
            //if (calGyroRunning)
            //{
            //	// General field updates
            //	//// Gyro raw rates
            //	//uiForm.btGyroXRate.Text = uiForm.lru0.dataParam[(int)PARAM.GYROXR].ToString("0000.00");
            //	//uiForm.btGyroYRate.Text = uiForm.lru0.dataParam[(int)PARAM.GYROYR].ToString("0000.00");
            //	//uiForm.btGyroZRate.Text = uiForm.lru0.dataParam[(int)PARAM.GYROZR].ToString("0000.00");

            //	msg = gyroCalstateMachine();
            //}


            // Handle next step
            if (msg == "next")
            {
                currentState++;
            }
            else if (msg == "back")
            {
                currentState--;
            }
            else if (msg == "skip")
            {
                currentState += 2;
            }
            else if (msg == "wait")
            {
            }
            else if (msg == "end")
            {
                // Turn off outputs
                uiForm.lru0.setParameterReq((int)PARAM.NONE, RATE.RATE_0HZ);
                calAccelRunning   = false;
                calGyroRunning    = false;
                prgPointsRunning  = false;
                readPointsRunning = false;

                // Back to zero
                PosFixture.setPosition("X0 Y0 Z0");

                // Data
                uiForm.btLru0ParmName.Text  = "Complete";
                uiForm.btLru0ParmValue.Text = "";

                // Button color
                uiForm.btStartAccelCal.BackColor = System.Drawing.Color.DimGray;
            }
        }