/************************************************************************* $Archive: /pacs/OnBoard/GratCtrl.c $ $Revision: 1.17 $ $Date: 2009/04/23 13:51:12 $ $Author: amazy $ $Log: GratCtrl.c,v $ Revision 1.17 2009/04/23 13:51:12 amazy 6.029 * * 14 4/23/09 3:46p Pacs1 * 6.029 * * 13 30/09/08 15:19 Amazy * - increased grating power limit to 150mA and made it commandable * - added a synchro counter in HK * - new parameters value for DMC_SYNCHRONIZE_ON_DET to avoid using the * synchro to trigger the mechanisms move * - In photo, the RedSpuTransmission mode was not copied into red science * headers. It is now corrected * * 12 6/04/08 11:45p Amazy * - added a filter to the grating controller * - now resetting ChopperOut in the chopper enable * * 11 6/28/07 4:33p Amazy * v6.020 * * 10 5/31/07 11:27a Amazy * v6.018 * * 9 2/07/07 11:30a Amazy * - The open loop code for the grating has been updated and validated on * DMC FM with QM grating * - The position limit error is now signaled in the chopper controller * (error code 0x0b15). The position limit * is stored in the field 'scaling' of the chopper pid controller. Its * default value is 0x7FFFFFFF. * * 8 12/01/07 14:49 Amazy * Introduced a filter on the speed in the grating controller. The filter * order is stored in Kf (0 means no filter) * * 7 12/01/06 3:05p Amazy * updated DMC_GRAT_FW_DAC_OUT * * 6 13/11/06 11:36 Amazy * The grating open loop has been updated after tests with the FM grating * (it has not yet been tested) * * 47 3/14/06 4:31p Pacs Egse * Version 6.001 * Cleaned and commented *************************************************************************/ #ifdef SIMULATOR #include "virtuosoSim.h" #else // SIMULATOR #include "v_macro.h" #include "node1.h" #include "k_struct.h" #endif #include "params.h" #include "error.h" #include /**************************************************************** GLOBAL VARIABLES ****************************************************************/ /**************************************************************** FUNCTIONS DECLARATION ****************************************************************/ extern void ValidateFWSpecHk(int validity); extern void ValidateFWPhotoHk(int validity); extern void StopFWMove(FwControllerParams* p_controller); void StopGratMove(); void MoveGratSteps(GratingControllerParams* p_controller, int nbSteps); void DisableGratingController(); void ExitDegradedMode(); // commands function BOOL CommandMoveGratingAbsolute(CommandParameter param); BOOL CommandMoveGratingRelative(CommandParameter param); BOOL CommandSwitchOnGratingController(CommandParameter paramNotUsed); BOOL CommandSwitchOffGratingController(CommandParameter paramNotUsed); BOOL CommandEnableGratingController(CommandParameter paramNotUsed); BOOL CommandDisableGratingController(CommandParameter paramNotUsed); BOOL CommandHomeGrating(CommandParameter paramNotUsed); BOOL CommandEnterGratingDegradedMode(CommandParameter param); BOOL CommandExitGratingDegradedMode(CommandParameter paramNotUsed); BOOL CommandLockGrating(CommandParameter param); BOOL CommandUnlockGrating(CommandParameter paramNotUsed); BOOL CommandFwGrDacOut(CommandParameter param); /**************************************************************** FUNCTIONS IMPLEMENTATION ****************************************************************/ /* FUNCTION : void InitializeGratingControllerParams(GratingControllerParams* p_controller) **************************************************************************************** AUTHOR : Amazy USE : Initializes the grating controller PARAMS : p_controller : the controller to initialize */ void InitializeGratingControllerParams(GratingControllerParams* p_controller) { InitializePidControllerParams(&(p_controller->PidController)); p_controller->PidController.Kp = 2000; p_controller->PidController.Ki = 50000; p_controller->PidController.Kd = 18; p_controller->PidController.Kf = 0; //Kf is now the Derivate filter order p_controller->PidController.Rate = 12; p_controller->PidController.AccumulatorLimit = 17711889; p_controller->PidController.OutputLimit = 17711; p_controller->PidController.Scaling = 1398; p_controller->PidController.ErrorLimit = 23302; p_controller->PidController.PosOffset = 0; p_controller->Range = 0x100000; p_controller->HallSensorsOffset = 4230; /*this is actually an offset for the hall sensor amplifier */ p_controller->InductosynExcitationAmplitude = 1200; p_controller->PrevDerivGratError = 0.0f; p_controller->IntFilterN1 = 1000000; p_controller->IntFilterN2 = 0; p_controller->IntFilterN3 = 0; p_controller->IntFilterD1 = 0; p_controller->IntFilterD2 = 0; //degraded mode variables p_controller->DegModeRate = 32; p_controller->DegModeScaling = 8192; p_controller->DegModeCurrentStep = 0; p_controller->DegModeNbStepsForThisMove = 0; p_controller->DegModeCurrentPosInPhaseA = 0xFFFF; p_controller->NbInterruptsBeforeStoppingOutput = 5*8192; p_controller->CurrentNbInterruptsOfMaxOutput = 0; p_controller->MaxOutputLongDuration = 8855; //150mA gpMim->GratErrorT_0 = 0; // Values of previous errors gpMim->GratErrorT_1 = 0; // ErrorT_1 is Error at t-1 gpMim->GratPrevPos = 0; gpMim->GratCurPos = 0; gpMim->GratPosCarry = 0; gpMim->GratingPreviousError = 0; gpMim->GratAveragedDError = 0.0f; gpMim->GratAveragedError = 0.0f; } /* CMD_FUNCTION : BOOL CommandMoveGratingAbsolute(CommandParameter param) ********************************************************************** AUTHOR : Amazy USE : Start moving the grating to a new absolute position PARAMS : Uint : new absolute position */ BOOL CommandMoveGratingAbsolute(CommandParameter param) { if (TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_DEGRADED_MODE)) { MoveGratSteps(gpGratingController, param.Int); } else { if (!TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } if (!StartMovingToTarget(gpGratingPidController, param.Uint)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } } SequencerSendAck(DMC_MOVE_GRAT_ABS); return TRUE; }; /* CMD_FUNCTION : BOOL CommandMoveGratingRelative(CommandParameter param) ********************************************************************** AUTHOR : Amazy USE : Start moving the grating to a new relative position PARAMS : Uint : new absolute position */ BOOL CommandMoveGratingRelative(CommandParameter param) { if (TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_DEGRADED_MODE)) { MoveGratSteps(gpGratingController, gpGratingController->DegModeCurrentPosInPhaseA + param.Int); } else { if (!StartMovingToTarget(gpGratingPidController, gpGratingPidController->Target + param.Int)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } } SequencerSendAck(DMC_MOVE_GRAT_REL); return TRUE; }; /* CMD_FUNCTION : BOOL CommandSwitchOnGratingController(CommandParameter paramNotUsed) *********************************************************************************** AUTHOR : Amazy USE : Switches On the Grating Controller PARAMS : */ BOOL CommandSwitchOnGratingController(CommandParameter paramNotUsed) { // configure Hk Mux to get the values from the grating hall sensors // configure the drive to command the grating motor // switch on inductosyn SET_BITS(gpMim->ControlWord, K_BMASK_MIM_HK_MUX_SELECT_MASK | K_BMASK_MIM_DRIVE_SELECT_MASK | K_BMASK_MIM_INDUCTOSYN_POWER_CONTROL, K_BMASK_MIM_HK_MUX_SELECT_GRATING | K_BMASK_MIM_DRIVE_SELECT_GRATING | K_BMASK_MIM_INDUCTOSYN_POWER_ON); //ask the FPGA to get the value of grating hall sensors SET_BITS(gpFPGA->ControlRegister, K_BMASK_TIMING_MUXADR, K_BMASK_TIMING_MUXADR_GRATING); WriteFpgaRegisters(); //stop the FW if they were moving StopFWMove(gpFWSpecController); StopFWMove(gpFWPhotoController); //update status SET_BIT(gpGratingPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); CLEAR_BIT(gpFWSpecController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); CLEAR_BIT(gpFWPhotoController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); //validate housekeeping related to Grating VALIDATE_HK(DMC_GRAT_CUR_POS); VALIDATE_HK(DMC_FWGRAT_HALLA); VALIDATE_HK(DMC_FWGRAT_HALLB); VALIDATE_HK(DMC_GR_IND_READ); VALIDATE_HK(DMC_GR_PER_CAR); VALIDATE_HK(DMC_GR_TURN_CAR); VALIDATE_HK(DMC_GR_DEG_POS); SequencerSendAck(DMC_SWON_GRAT_CONT); return FALSE; }; /* CMD_FUNCTION : BOOL CommandSwitchOffGratingController(CommandParameter paramNotUsed) ************************************************************************************ AUTHOR : Amazy USE : Switches Off the Grating Controller PARAMS : */ BOOL CommandSwitchOffGratingController(CommandParameter paramNotUsed) { DisableGratingController(); ExitDegradedMode(); KS_TaskSleep(1); //wait that the IRQ really stops the controller // switch off inductosyn and back to grating for hk and drive, also switch off launch-locks SET_BITS(gpMim->ControlWord, K_BMASK_MIM_HK_MUX_SELECT_MASK | K_BMASK_MIM_DRIVE_SELECT_MASK | K_BMASK_MIM_INDUCTOSYN_POWER_CONTROL | K_BMASK_MIM_GRATING_LAUNCH_LOCK_MASK, K_BMASK_MIM_HK_MUX_SELECT_GRATING | K_BMASK_MIM_DRIVE_SELECT_GRATING | K_BMASK_MIM_INDUCTOSYN_POWER_OFF | K_BMASK_MIM_GRATING_LAUNCH_LOCK_OFF | K_BMASK_MIM_GRATING_LAUNCH_LOCK_OFF2); //stop all the mechanisms StopFWMove(gpFWSpecController); StopFWMove(gpFWPhotoController); StopGratMove(); //update status CLEAR_BIT(gpGratingPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); CLEAR_BIT(gpGratingPidController->TaskStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED); CLEAR_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED); CLEAR_BIT(gpFWSpecController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); CLEAR_BIT(gpFWPhotoController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); //invalidate housekeeping related to Grating INVALIDATE_HK(DMC_GRAT_CUR_POS); INVALIDATE_HK(DMC_FWGRAT_HALLA); INVALIDATE_HK(DMC_FWGRAT_HALLB); INVALIDATE_HK(DMC_GR_IND_READ); INVALIDATE_HK(DMC_GR_PER_CAR); INVALIDATE_HK(DMC_GR_TURN_CAR); INVALIDATE_HK(DMC_GR_DEG_POS); //invalidate housekeeping related to filter wheels ValidateFWSpecHk(FALSE); ValidateFWPhotoHk(FALSE); SequencerSendAck(DMC_SWOF_GRAT_CONT); return FALSE; }; /* CMD_FUNCTION : BOOL CommandEnableGratingController(CommandParameter paramNotUsed) ********************************************************************************* AUTHOR : Amazy USE : Enable the Grating Controller PARAMS : */ BOOL CommandEnableGratingController(CommandParameter paramNotUsed) { if (TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_DEGRADED_MODE)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } if (EnablePidController(gpGratingPidController)) { SequencerSendAck(DMC_ENABLE_GRAT_CONT); gpMim->GratErrorT_0 = 0; // Values of previous errors gpMim->GratErrorT_1 = 0; // ErrorT_1 is Error at t-1 gpMim->GratingPreviousError = 0; gpMim->GratAveragedDError = 0.0f; gpMim->GratAveragedError = 0.0f; gpMim->GratingOut = 0.0f; gpMim->GratingPrev1Out = 0.0f; gpMim->GratingPrev2Out = 0.0f; gpMim->GratingPrev3Out = 0.0f; gpMim->GratingPrev4Out = 0.0f; gpMim->GratingFilterOut = 0.0f; gpMim->GratingFilterPrev1Out = 0.0f; gpMim->GratingFilterPrev2Out = 0.0f; gpMim->GratingFilterPrev3Out = 0.0f; gpMim->GratingFilterPrev4Out = 0.0f; gpGratingController->FloatFilterN1 = ((float)gpGratingController->IntFilterN1)/1000000.0f; gpGratingController->FloatFilterN2 = ((float)gpGratingController->IntFilterN2)/1000000.0f; gpGratingController->FloatFilterN3 = ((float)gpGratingController->IntFilterN3)/1000000.0f; gpGratingController->FloatFilterD1 = ((float)gpGratingController->IntFilterD1)/1000000.0f; gpGratingController->FloatFilterD2 = ((float)gpGratingController->IntFilterD2)/1000000.0f; VALIDATE_HK(DMC_GRAT_OUTPUT); VALIDATE_HK(DMC_GRAT_SETPOIN); VALIDATE_HK(DMC_GRAT_TARGET); VALIDATE_HK(DMC_GRAT_PID_ERR); VALIDATE_HK(DMC_GRAT_PID_ACC); } else { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); } return FALSE; }; /* CMD_FUNCTION : BOOL CommandDisableGratingController(CommandParameter paramNotUsed) ********************************************************************************** AUTHOR : Amazy USE : Disable the Grating Controller PARAMS : */ BOOL CommandDisableGratingController(CommandParameter paramNotUsed) { DisableGratingController(); SequencerSendAck(DMC_DISABLE_GRAT_CONT); return FALSE; }; void DisableGratingController() { // disable pid and commutation SET_BITS(gpGratingPidController->ControlWord, K_BMASK_PID_CTRL_COMMAND_ENABLE_COMMAND | K_BMASK_PID_CTRL_COMMAND_COMMUTATAION_COMMAND, K_BMASK_PID_CTRL_COMMAND_DISABLE_PID | K_BMASK_PID_CTRL_COMMAND_DISABLE_COMMUTATION); #ifdef SIMULATOR CLEAR_BIT(gpGratingPidController->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED); #endif //reset the DAC gpMim->GratFwDAC1 = 0; gpMim->GratFwDAC2 = 0; INVALIDATE_HK(DMC_GRAT_OUTPUT); INVALIDATE_HK(DMC_GRAT_SETPOIN); INVALIDATE_HK(DMC_GRAT_TARGET); INVALIDATE_HK(DMC_GRAT_PID_ERR); INVALIDATE_HK(DMC_GRAT_PID_ACC); } /* CMD_FUNCTION : BOOL CommandHomeGrating(CommandParameter param) ************************************************************** AUTHOR : Amazy USE : Brings the grating agains one of the hardstop and initialize the absolut position encoding. PARAMS : 0 = go against the hardstop at zero absolute pos (and nominal limit switch) 1 = go against the hardstop at range absolute pos (and redundant limit switch) NOTE : If the parameter is 1 or if you are using the redundant grating, you must absolutely have initialized the RANGE parameters before (it has a default value but it should be measured accurately). */ BOOL CommandHomeGrating(CommandParameter param) { #ifdef SIMULATOR SET_BIT(gpGratingPidController->TaskStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED); SET_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED); #else //SIMULATOR if (TEST_ONE_BIT(gSelectControllersMode, (1 << K_BPOS_CTRL_MODE_GRATING))) //if we are in simulation mode { SET_BIT(gpGratingPidController->TaskStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED); SET_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_HOMING_COMPLETED); gpGratingPidController->CurrentSetPoint = 500000; } else //nominal mode { if (param.Uint == 1) { if (!StartMovingToTarget(gpGratingPidController, gpGratingPidController->CurrentPosition + 0x11c71d)) //move relative + 50° { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } } else if (param.Uint == 0) { if (!StartMovingToTarget(gpGratingPidController, gpGratingPidController->CurrentPosition - 0x11c71d)) //move relative - 50 ° { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } } else { SequencerSendNack(param.Uint, INVALID_PARAMETER); SetSequencerError(ERR_SEQUENCER_INVALID_PARAMETERS); return FALSE; } SET_BIT(gpGratingPidController->ControlWord, K_BMASK_GRAT_CTRL_COMMAND_HOME); } #endif SequencerSendAck(DMC_HOME_GRAT); return TRUE; } /* CMD_FUNCTION : BOOL CommandEnterGratingDegradedMode(CommandParameter param) *************************************************************************** AUTHOR : Amazy USE : Enter one of the grating degraded mode PARAMS : 0 = Open loop Other values might be defined later NOTE : It has to be done before the switch-on controller */ BOOL CommandEnterGratingDegradedMode(CommandParameter param) { if (param.Uint != 0) { SequencerSendNack(param.Uint, INVALID_PARAMETER); SetSequencerError(ERR_SEQUENCER_INVALID_PARAMETERS); return FALSE; } if (TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SET_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_DEGRADED_MODE); gpGratingController->DegModeIsMoving = 0; //get the position if (gpGratingController->DegModeCurrentPosInPhaseA == 0xFFFF) { gpGratingController->DegModeInitPhasePos = atan2(gHallSensorB + gpGratingController->HallSensorsOffset, gHallSensorA + gpGratingController->HallSensorsOffset)*((double)MAX_STEPS_IN_PHASE)/6.283185; gpGratingController->DegModeInitPhasePos += NB_STEPS_IN_PHASE; gpGratingController->DegModeInitPhasePos %= NB_STEPS_IN_PHASE; gpGratingController->DegModeCurrentPosInPhaseA = gpGratingController->DegModeInitPhasePos; } SequencerSendAck(DMC_ENTER_GRAT_CONT_DEG); return TRUE; } void ExitDegradedMode() { CLEAR_BIT(gpGratingPidController->PidStatus, K_BMASK_GRAT_CTRL_STATUS_DEGRADED_MODE); //reset the position gpGratingController->DegModeCurrentPosInPhaseA = 0xFFFF; // reset the DAC and other variables gpGratingController->DegModeIsMoving = 0; gpMim->GratFwDAC1 = 0; gpMim->GratFwDAC2 = 0; } /* CMD_FUNCTION : BOOL CommandExitGratingDegradedMode(CommandParameter paramNotUsed) ********************************************************************************* AUTHOR : Amazy USE : Go back to the grating nominal mode NOTE : */ BOOL CommandExitGratingDegradedMode(CommandParameter paramNotUsed) { ExitDegradedMode(); SequencerSendAck(DMC_EXIT_GRAT_CONT_DEG); return TRUE; } /* CMD_FUNCTION : BOOL CommandLockGrating(CommandParameter param) ************************************************************** AUTHOR : Amazy USE : Locks the grating. PARAMS : 2 : activate mechanical launch-lock for 5 second and ignore limit switch 6 : activate mechanical launch-lock and stop when limit switch is pressed */ BOOL CommandLockGrating(CommandParameter param) { if (!TEST_ONE_BIT(gpGratingPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } if (TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SET_BITS(gpGratingPidController->ControlWord, K_BMASK_GRAT_CTRL_COMMAND_LOCK, param.Uint); SequencerSendAck(DMC_LOCK_GRAT); return TRUE; } /* CMD_FUNCTION : BOOL CommandUnlockGrating(CommandParameter param) **************************************************************** AUTHOR : Amazy USE : Unlocks the grating. PARAMS : 0 : remove short-circuit and activate mechanical unlock for 1 second and ignore limit switch 4 : remove short-circuit and activate mechanical unlock and stop when limit switch is pressed */ BOOL CommandUnlockGrating(CommandParameter param) { if (!TEST_ONE_BIT(gpGratingPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } if (TEST_ONE_BIT(gpGratingPidController->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SET_BITS(gpGratingPidController->ControlWord, K_BMASK_GRAT_CTRL_COMMAND_LOCK, param.Uint); SequencerSendAck(DMC_UNLOCK_GRAT); return TRUE; } /* CMD_FUNCTION : BOOL CommandFwGrDacOut(CommandParameter param) ************************************************************* AUTHOR : Amazy USE : This commands writes directly in FW/Grating DAC PARAMS : Uint: the value to write in the DAC */ BOOL CommandFwGrDacOut(CommandParameter param) { gpMim->GratFwDAC1 = param.Uint >> 16; gpMim->GratFwDAC2 = param.Uint & 0xFFFF; SequencerSendAck(DMC_FW_GR_DAC_OUT); return TRUE; } /* FUNCTION : void StopGratMove() ****************************** AUTHOR : Amazy Stop the move of the grating */ void StopGratMove() { //reset the DAC gpMim->GratFwDAC1 = 0; gpMim->GratFwDAC2 = 0; } /* FUNCTION : void MoveGratSteps(GratingControllerParams* p_controller, int nbSteps) ********************************************************************************* AUTHOR : Amazy Move the grating in degraded mode. PARAMS: nbSteps: the number of steps in the sine array */ void MoveGratSteps(GratingControllerParams* p_controller, int nbSteps) { p_controller->DegModeNbStepsForThisMove = nbSteps - p_controller->DegModeCurrentPosInPhaseA; if (p_controller->DegModeNbStepsForThisMove > 0) { p_controller->DegModeCurrentInc = 1; } else { p_controller->DegModeCurrentInc = -1; p_controller->DegModeNbStepsForThisMove = -p_controller->DegModeNbStepsForThisMove; } p_controller->DegModeCurrentStep = 0; p_controller->DegModeIsrCounter = 0; p_controller->DegModeIsMoving = 1; }