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"); } } }
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"); } } }
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 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"); } } }
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"); } } }
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"); } } }
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 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"); } } }
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"); } } }