/************************************************************************* $Archive: /pacs/OnBoard/pid_ctrl.c $ $Revision: 1.18 $ $Date: 2010/09/29 12:41:17 $ $Author: amazy $ $Log: pid_ctrl.c,v $ Revision 1.18 2010/09/29 12:41:17 amazy v6.032 * * 12 9/21/10 5:55p Pacs1 * * 11 9/16/10 4:30p Pacs1 * * 10 16/09/10 14:36 Amazy * - Introduced EDAC test code * - Introduced code to reset the DEC FPGA in SET_PARAM_BOTH * * 9 4/23/09 3:46p Pacs1 * 6.029 * * 7 9/10/08 12:05 Amazy * * 6 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 * * 5 3/20/07 3:14p Amazy * * 37 3/14/06 4:32p 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 "u_bits.h" #include "hk.h" #include "params.h" #include "l_memory.h" /* FUNCTION : void InitializePidControllerParams(PidControllerParams* p_controller) ******************************************************************************** AUTHOR : Amazy USE : This function initializes the parameters that are common to every PID controller PARAMS : p_controller : a pointer to a PID controller */ void InitializePidControllerParams(PidControllerParams* p_controller) { //common initialization p_controller->CurrentPosition = 0; p_controller->Output = 0; p_controller->Error = 0; p_controller->Accumulator = 0; p_controller->CurrentSetPoint = 0; p_controller->PidStatus = 0; p_controller->ControlWord = 0; p_controller->Target = 0; p_controller->TaskStatus = 0; p_controller->PosOffset = 0; } /* FUNCTION : BOOL EnablePidController(PidControllerParams* p_controller) ********************************************************************** AUTHOR : Amazy USE : Enable a PID controller PARAMS : p_controller : a pointer to a PID controller RETURN VALUE : TRUE : if the PID controller could be enabled FALSE : if the PID controller could not be enabled */ BOOL EnablePidController(PidControllerParams* p_controller) { // if the controller is not switched on, reject the command if (!TEST_ONE_BIT(p_controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON)) { return FALSE; } // reset the integral accumulator p_controller->Accumulator = 0; if (p_controller == gpGratingPidController) { // set the current setpoint to current position p_controller->CurrentSetPoint = p_controller->CurrentPosition; // set the current target to current position p_controller->Target = p_controller->CurrentPosition; } else if (p_controller == gpChopperPidController) { //if we are in open loop, the setpoint must be zero because the setpoint will be copied in the DAC if (TEST_ONE_BIT(p_controller->PidStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP)) { // set the current chopper setpoint to zero p_controller->CurrentSetPoint = 0; // set the current chopper target to zero p_controller->Target = 0; } else { // set the current setpoint to current position p_controller->CurrentSetPoint = p_controller->CurrentPosition; // set the current target to current position p_controller->Target = p_controller->CurrentPosition; } } else { // set the current chopper setpoint to zero p_controller->CurrentSetPoint = 0; // set the current chopper target to zero p_controller->Target = 0; } // reset the move bits CLEAR_BIT(p_controller->ControlWord, K_BMASK_PID_CTRL_COMMAND_MOVE_COMMAND); // enable controller and commutation p_controller->ControlWord = K_BMASK_PID_CTRL_COMMAND_ENABLE_PID | K_BMASK_PID_CTRL_COMMAND_ENABLE_COMMUTATION; #ifdef SIMULATOR SET_BIT(p_controller->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED); #endif return TRUE; } /* FUNCTION : BOOL StartMovingToTarget(PidControllerParams* p_controller, int target) ********************************************************************************** AUTHOR : Amazy USE : Start a movement for a PID controller PARAMS : p_controller : a pointer to a PID controller int : the target RETURN VALUE : TRUE : if the movement could be started FALSE : if not */ BOOL StartMovingToTarget(PidControllerParams* p_controller, int target) { int controlWord; // if the controller is not enabled, return FALSE if (!TEST_ONE_BIT(p_controller->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED)) { return FALSE; } // test if previous move is finished if ((p_controller->PidStatus & K_BMASK_PID_CTRL_STATUS_MOVING_STATUS) != 0) { return FALSE; } // set the target p_controller->Target = target; if (target > p_controller->CurrentSetPoint) { controlWord = K_BMASK_PID_CTRL_COMMAND_MOVE_UP; } else { controlWord = K_BMASK_PID_CTRL_COMMAND_MOVE_DOWN; } if (gpMim->MechanismsUseSynchro) { controlWord += K_BMASK_PID_CTRL_COMMAND_USE_SYNCHRO; } p_controller->ControlWord = controlWord; return TRUE; } /* COMMAND : BOOL CommandSelectMechControlMode(CommandParameter param) ******************************************************************* AUTHOR : Amazy USE : Select in which mode each of the controller is operated PARAMS : bit field: bit0: 0 = grating use nominal controller 1 = grating is simulated (pos = setpoint) bit1: 0 = chopper use nominal controller 1 = chopper is simulated (pos = setpoint) bit2: 0 = fw_spec use nominal position 1 = fw_spec use simulated position bit3: 0 = fw_photo use nominal position 1 = fw_photo use simulated position bit4: 0 = CS1 use nominal resistor reading 1 = CS1 is simulated (resistor = setpoint) bit5: 0 = CS2 use nominal resistor reading 1 = CS2 is simulated (resistor = setpoint) bit8-10: temporary configuration of DM EDAC bit11: 1 = perform test with temporary configuration of DM EDAC bit12-14: temporary configuration of PM EDAC bit15: 1 = perform test with temporary configuration of PM EDAC bit16-18: configuration of DM EDAC bit 19: 1 = apply new settings, 0 = do nothing bit20-22: configuration of PM EDAC bit 23: 1 = apply new settings, 0 = do nothing */ BOOL CommandSelectMechControlMode(CommandParameter param) { int dmConfBank0CurrentConfig = 0; int pmConfBank1CurrentConfigLSB = 0; int pmConfBank1CurrentConfigMSB = 0; int dmEDAC_Config = 0; int pmEDAC_Config = 0; int dmConfBank0 = 0; int pmConfBank1_MSB = 0; int pmConfBank1_LSB = 0; int volatile testVariable = 0; int volatile testVariable2 = 0; gSelectControllersMode = param.Int; dmConfBank0CurrentConfig = *((int*)K_DMADD_DMPSC_CONFBANK0); pmConfBank1CurrentConfigMSB = MEM_ReadPM32bitMSWord(K_PMADD_PMPSC_CONFBANK1); pmConfBank1CurrentConfigLSB = MEM_ReadPM16bitLSWord(K_PMADD_PMPSC_CONFBANK1); //shall we perform a test of DM EDAC ? if (TEST_ONE_BIT(gSelectControllersMode, 0x800)) { testVariable = MEM_ReadDMWord(0x50000); //extract DM EDAC configuration bits dmEDAC_Config = (gSelectControllersMode & 0x700) >> 8; //Write the test config dmConfBank0 = *((int*)K_DMADD_DMPSC_CONFBANK0); SET_BITS(dmConfBank0, 0x00000007, dmEDAC_Config); *((int*)K_DMADD_DMPSC_CONFBANK0) = dmConfBank0; //now that the test config is set, write somtehing in the test variable MEM_WriteDMWord(0x50000, 1); //reconfigure the DM EDAC as before; *((int*)K_DMADD_DMPSC_CONFBANK0) = dmConfBank0CurrentConfig; testVariable = MEM_ReadDMWord(0x50000); } //shall we perform a test of PM EDAC ? if (TEST_ONE_BIT(gSelectControllersMode, 0x8000)) { //extract PM EDAC Configuration bits pmEDAC_Config = (gSelectControllersMode & 0x7000) >> 12; testVariable = MEM_ReadPM32bitMSWord(0xAF00); testVariable2 = MEM_ReadPM16bitLSWord(0xAF00); //Write the test config pmConfBank1_MSB = MEM_ReadPM32bitMSWord(K_PMADD_PMPSC_CONFBANK1); pmConfBank1_LSB = MEM_ReadPM16bitLSWord(K_PMADD_PMPSC_CONFBANK1); SET_BITS(pmConfBank1_MSB, 0x00000007, pmEDAC_Config); MEM_WritePMWord(K_PMADD_PMPSC_CONFBANK1, pmConfBank1_MSB, pmConfBank1_LSB); //now that the test config is set, write somtehing in PRAM (at the very end of the segment, where there is no source code and data) testVariable += 1; MEM_WritePMWord(0xAF00, testVariable, testVariable2); //reconfigure the PM EDAC as before; MEM_WritePMWord(K_PMADD_PMPSC_CONFBANK1, pmConfBank1CurrentConfigMSB, pmConfBank1CurrentConfigLSB); //read the PRAM again testVariable = MEM_ReadPM32bitMSWord(0xAF00); testVariable -= 1; MEM_WritePMWord(0xAF00, testVariable, testVariable2); } //shall we perform a new configuration of DM EDAC ? if (TEST_ONE_BIT(gSelectControllersMode, 0x80000)) { //extract DM EDAC configuration bits dmEDAC_Config = (gSelectControllersMode & 0x70000) >> 16; //Write the DM EDAC config dmConfBank0 = *((int*)K_DMADD_DMPSC_CONFBANK0); SET_BITS(dmConfBank0, 0x00000007, dmEDAC_Config); *((int*)K_DMADD_DMPSC_CONFBANK0) = dmConfBank0; } //shall we perform a new configuration of PM EDAC ? if (TEST_ONE_BIT(gSelectControllersMode, 0x800000)) { //extract PM EDAC Configuration bits pmEDAC_Config = (gSelectControllersMode & 0x700000) >> 20; //Write the test config pmConfBank1_MSB = MEM_ReadPM32bitMSWord(K_PMADD_PMPSC_CONFBANK1); pmConfBank1_LSB = MEM_ReadPM16bitLSWord(K_PMADD_PMPSC_CONFBANK1); SET_BITS(pmConfBank1_MSB, 0x00000007, pmEDAC_Config); MEM_WritePMWord(K_PMADD_PMPSC_CONFBANK1, pmConfBank1_MSB, pmConfBank1_LSB); } SequencerSendAck(DMC_SELECT_MECH_CTRL_MODE); return TRUE; }