/************************************************************************* $Archive: /pacs/OnBoard/ChopGrat.s $ $Revision: 1.19 $ $Date: 2009/04/23 13:51:12 $ $Author: amazy $ $Log: ChopGrat.s,v $ Revision 1.19 2009/04/23 13:51:12 amazy 6.029 ; ; 26 4/23/09 3:46p Pacs1 ; 6.029 ; ; 89 3/14/06 4:31p Pacs Egse ; Version 6.001 ; Cleaned and commented *************************************************************************/ #include #include "c:\virtuoso\adi21020\inc\macro.h" #define INCLUDE_DEFINE_ONLY #include "..\OnBoard\pid_ctrl.h" #include "..\OnBoard\mim.h" /* ISR : _user_isr AUTHORS : AMazy with contributions from J-M Gillis M Guiot B Marquet USE : In this function, the following tasks are performed : - acquiring the HK analog measurements - acquiring the digital measurements ( hardware status bits and timing FPGA data ) - controlling the chopper - generating the trajectory for the chopper - controlling the Grating - generating the trajectory for the Grating - controlling the filter wheel 1 ( spectrometer ) - controlling the filter wheel 2 ( photometer ) - copying the hardware control/mode bits and the calibration sources commands to the hardware SEE ALSO : "Seq.c" */ /*************************************************************************************/ /* START OF CODE SEGMENT */ /*************************************************************************************/ .segment /pm seg_pmco; /********************************************************/ /* EXTERNAL VARIABLES FROM VIRTUOSO */ /********************************************************/ .extern _K_ArgsP; /* pointer to argument stack for ISR - Virtuoso convention */ .extern _K_Args; /* argument stack for ISR - Virtuoso convention - not used */ .extern gSemaCS1; /* structure used to signal SEMA_TIMER_CS_1 */ .extern gSemaCS2; /* structure used to signal SEMA_TIMER_CS_2 */ .extern gSemaTempSensores; /* structure used to signal SEMA_TEMP_SENSORS */ .extern gCurrentCalSrc; .extern gCalSrc500msPeriodCounter; /************************************************************************************/ /* TEMPORARY VARIABLES ( LOCAL STORAGE ) */ /************************************************************************************/ .extern gHallSensorA; .extern gHallSensorB; /************************************************************************************/ /* EXTERNAL VARIABLES FOR FAILURE DETECTION AND DEBUG */ /************************************************************************************/ .extern gSelectControllersMode; .extern gpFPGA; .extern gpBolcFrequencyDivider; .extern gMeasuredTimingResidueLo; .extern gMeasuredTimingResidueHi; /*************************************************************************************/ /* START OF CODE */ /*************************************************************************************/ .global _user_isr; _user_isr: START_ISR; /* save MODE1, ASTAT, r0, r1, r2, r4, r8, r12, i4, i12 */ bit set MODE2 CAFRZ; /* freeze cache for the sake of the interrupted task */ nop; /* nop mandatory after a set system register */ /***************************************/ /* prologue : set up environment */ /***************************************/ /* change to alternate registers sets */ bit set MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; nop; /* nop mandatory after a set system register */ /********************************************************/ /* part 1 : Housekeeping */ /* assumption about environment : */ /* NOTE I REGISTERS 8-15 HAVE ONLY 24 BITS */ /* i15 is reserved as ISR call counter */ /* i0 is a pointer to grating PID block */ /* i1 is a pointer to FW1 ( spectro ) PID block */ /* i2 is a pointer to FW2 ( photo ) PID block */ /* i3 is a pointer to chopper PID block */ /* i4 is not used */ /* i5 is a pointer to cal src 1 or cal src 2 PID block */ /* i6 must be a pointer to HK array */ /* ( high level software interface ) */ /********************************************************/ /*************************************************************************************/ /* BUILD OUTPUT WORD FOR HARDWARE CONTROL */ /* copy 22lsb of MIM FPGA control word from high level software to the FPGA */ /*************************************************************************************/ r0 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r4 = 0; r4 = r4 or FDEP r0 by 0:22; dm(MIM_CTRL_REG) = r4; /* write to hardware */ modify ( i15, 1 ); /* increment ISR activation counter (DMC_IRS_COUNT) */ dm(ISR_COUNT,i6) = i15; /********************************************************************/ /* read ANALOG HK: CHOPPER POS + HALL SENSORS + ANALOG HK after this section: r0 = chopper position r3 = grating position (inductosyn readout) */ /********************************************************************/ /* read from TIMING_MUX area HK digital inputs */ r2=dm(TIMING_MUX); /* read MIM FPGA status (and index) */ r15=FEXT r2 BY 19:K_BLEN_MIM_STATUS_HK_INDEX; /* r15 = analog hk index */ r1=dm(MIM_CHOP_POS_AND_HK); /* read chopper pos (16msb) & HK measurements (16lsb)*/ read_chopper_pos: r0 = fext r1 by 0:16 (se); /* compute 32 bit signed value of chopper position */ read_analog_hk: m6=r15; /* m6 = analog hk index */ r15=FEXT r1 BY 16:16 (se); /* isolate HK value ( from grating front end / EM electronics */ dm(m6,i6)=r15; /* store HK in array */ read_hall_sensors: r1=dm(MIM_HALL_SENSORS); /* read Hall sensors */ r4 = fext r1 by 16:16 (se); /* store Hall A */ dm(_gHallSensorA) = r4; r4 = FDEP r1 BY 0:16 (se); /* store Hall B */ dm(_gHallSensorB) = r4; read_inductosyn: r3=dm(MIM_INDUCTOSYN); /* read Inductosyn counter */ /*******************************************************************/ /* WRITE FW-GRATING AND CHOPPER DAC */ /*******************************************************************/ /* write DAC values from each PID block to hardware */ /* note grating and fw share the same DAC and amplifier */ /*grating and fw DAC*/ r4 = 0; r5 = dm(GRAT_FW_DAC1, i6); /* grating/fw DACS phase 1 ( 16 bits ) */ dm(MIM_GRAT_FW_DAC1) = r5; r5 = dm(GRAT_FW_DAC2, i6); /* grating/fw DACS phase 2 ( 16 bits ) */ dm(MIM_GRAT_FW_DAC2) = r5; dm(MIM_GRAT_FW_DACWRT) = r5; /* mandatory write command to really modify the DAC value */ /*chopper DAC*/ r4 = dm(OY, i3); /* chopper DAC = 16 LSB, 16 MSB = don t care */ r5 = dm(SW, i3); /* is chopper in open loop ? */ BTST r5 BY K_BPOS_CHOP_CTRL_STATUS_OPEN_LOOP; if not sz r4 = dm(SP, i3); /* if yes, copy setpoint into output */ BTST r5 BY K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* if the controller is not enabled, the DAC is commanded zero */ if not sz jump chop_dac_command; r4 = 0; chop_dac_command: dm(MIM_CHOPPER_DAC) = r4; dm(MIM_CHOPPER_DACWRT) = r4; /* mandatory write command to really modify the DAC value */ /*write inductosyn amplitude command*/ r15=FEXT r2 BY 19:12; /* r2 = TIMING_MUX, r15 is a 12bit counter where lsb changes at every ISR */ r5 = dm(GR_IND_AMP, i0); /* r5 = inductosyn amplitude */ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ comp(r15, r5); if gt jump wrt_grat_ind_amp; /* when counter > amplitude, set output to 1 (this drives the voltage to 0V) */ r11 = bclr r11 by K_BPOS_MIM_GRATING_IND_AMP; /* when counter < amplitude, set output to 0 (this drives the voltage to 5V) */ jump wrt_grat_ind_amp2; wrt_grat_ind_amp: r11 = bset r11 by K_BPOS_MIM_GRATING_IND_AMP; wrt_grat_ind_amp2: dm(CONTROL_WORD, i6) = r11; /* write the control word, it will be written in the FPGA at the next ISR */ /***********************************************************************/ /* CALIBRATION SOURCE READ AND WRITE */ /***********************************************************************/ /********************************************************************/ /* get measurement and index before changing output ! */ /* there are 8 measurements in BB */ /* channel 0 = 0V fixed */ /* 1 = -Vref = -5V fixed */ /* 2 = +Vref = +5V fixed */ /* 3 = VDAC */ /* 4 = sensor voltage, gain = 2 */ /* 5 = sensor voltage, gain = 200 */ /* 6 = sensor current ( voltage on 100 ohm ) gain = 2 */ /* 7 = sensor current ( voltage on 100 ohm ) gain = 200 */ /* note new hardware version sum of 256 measurements */ /********************************************************************/ cs_begin: /*if no source is enabled, measure the inductosyn amplitude */ i5=dm(_gpCS2Controller); r5 = dm(CS_TK, i5); btst r5 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if not sz jump cs_begin2; i5=dm(_gpCS1Controller); r5 = dm(CS_TK, i5); btst r5 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if not sz jump cs_begin2; /******************************************************************* Measure the inductosyn amplitude. This is done via the calibration source ADC. *******************************************************************/ induct: /*if no source is enabled, measure the inductosyn amplitude */ r5 = dm(TIMING_MUX); /* r5 = read calibration source ADC and TIMING MUX*/ /*every 64ms, we swap between a measure of the sine and a measure of the cosine*/ induct_test_64ms: r4 = FEXT r5 BY 26 : 2; btst r4 by 0; if sz jump induct_change_latch; /* read the measure */ i5 = dm(_gpCS2Controller); r5 = FEXT r5 BY 0:16 (se); btst r4 by 1; /* test if we are now measuring the sine or the cosine */ if sz jump induct_measure_sin; induct_measure_cos: /* store the measure of the COS */ dm(INDUCT_COS, i5) = r5; jump(cs_begin2); induct_measure_sin: /* store the measure of the SIN */ dm(INDUCT_SIN, i5) = r5; jump(cs_begin2); induct_change_latch: /* reprogram the MUX for next measures */ btst r4 by 1; if sz jump induct_prepare_latch_sin; induct_prepare_latch_cos: r6 = 9; jump induct_latch_ok; induct_prepare_latch_sin: r6 = 5; induct_latch_ok: dm(_gCalSrcDacLatch) = r6; dm(MIM_CS_LATCH) = r6; /******************************************************************* Now, this is really the beginning of the calibration source code *******************************************************************/ cs_begin2: r5 = dm(TIMING_MUX); /* r5 = read calibration source ADC and TIMING MUX*/ cs_test_500ms: r4 = FEXT r5 BY K_BPOS_CS_MUXADR : K_BLEN_CS_MUXADR; r6 = 0; comp(r4, r6); /* all these muxadr shall be = 0 every 500ms */ if NE jump cs_test_64ms; /* if not, look for 64ms interval */ cs_every_500ms: r4 = dm(_gCalSrc500msPeriodCounter); /* we must stay 2*500ms on each src */ r4 = r4 + 1; dm(_gCalSrc500msPeriodCounter) = r4; r6 = 2; r7 = r4 and r6; r7 = ASHIFT r7 by 1; dm(_gCurrentCalSrc) = r7; cs_test_64ms: r4 = FEXT r5 BY K_BPOS_CS_MUXADR : 9; r6 = 0; comp(r4, r6); /* all these muxadr shall be = 0 every 64ms */ if NE jump cs_continue; cs_change_latch: /* the latch must be modified every 64ms */ /* here, r6 shall be zero*/ /*Now, we program the MUX address to have the measure we want*/ /*copy MUXADR12 in ADR0 of the latch*/ btst r5 by K_BPOS_CS_MUXADR12; if sz jump cs_prepare_latch3; r6 = bset r6 by 3; r6 = bset r6 by 7; cs_prepare_latch3: /*copy MUXADR13 in ADR1 of the latch*/ btst r5 by K_BPOS_CS_MUXADR13; if sz jump cs_prepare_latch4; r6 = bset r6 by 2; r6 = bset r6 by 6; cs_prepare_latch4: /*copy MUXADR14 in ADR2 of the latch*/ btst r5 by K_BPOS_CS_MUXADR14; if sz jump cs_prepare_latch5; r6 = bset r6 by 1; r6 = bset r6 by 5; cs_prepare_latch5: r4 = dm(_gCurrentCalSrc); r7 = 0; comp(r4, r7); if EQ jump cs_prepare_latch_for_src1; cs_prepare_latch_for_src2: r6 = bset r6 by 0; jump cs_latch_ok; cs_prepare_latch_for_src1: r6 = bset r6 by 4; cs_latch_ok: dm(_gCalSrcDacLatch) = r6; /*the MUXADR is changing every 64ms. In the first 32ms, we let the signal stabilize */ /*during the next 32ms, we accumulate 256 measure */ cs_continue: btst r5 by K_BPOS_CS_MUXADR11; if sz jump cs_write; /*if MUXADR11 == 0, wait for the signal to stabilize (nothing to do) */ /*if MUXADR11 == 1, accumulate */ r9 = FEXT r5 BY K_BPOS_CS_CHANNEL_INDEX:K_BLEN_CS_CHANNEL_INDEX; r4 = dm(_gCurrentCalSrc); r7 = 0; comp(r4, r7); if EQ jump cs_accumulate_on_src1; cs_accumulate_on_src2: i5=dm(_gpCS2Controller); jump cs_accumulate; cs_accumulate_on_src1: i5=dm(_gpCS1Controller); cs_accumulate: r7 = CS_M1; r9 = r9 + r7; m5 = r9; r5 = FEXT r5 BY 0:16 (se); /* accumulate */ r4 = dm ( m5, i5 ); r5 = r5 + r4; dm ( m5, i5 ) = r5; /* i5 is not modified */ /********************************************************************* CALIBRATION SOURCE DAC WRITE *********************************************************************/ cs_write: r6 = dm(_gCalSrcDacLatch); i5=dm(_gpCS1Controller); r5 = dm(CS_OUT, i5); /* read output from CS1 PID */ r4 = dm(CS_DAC_OFFSET, i5); r5 = r5 + r4; /* cancel DAC offset */ dm(MIM_CS1_DAC) = r5; /* write it to DAC */ i5=dm(_gpCS2Controller); r5 = dm(CS_OUT, i5); /* read output from CS2 PID */ r4 = dm(CS_DAC_OFFSET, i5); r5 = r5 + r4; /* cancel DAC offset */ dm(MIM_CS2_DAC) = r5; /* write it to DAC */ dm(MIM_CS_LATCH) = r6; /* mandatory write command to really modify the DAC value */ /************************************************************************************/ /* part 2 : PID measurements update and computation */ /* assumption about part 1 : */ /* r0 ( 16 lsb ) = chopper position */ /* r2 = MIM hardware status register */ /* r3 = grating position ( 24 bits ) */ /* only valid if grating power ON */ /* further registers usage convention */ /* r4 = error = setpoint - position */ /* r5 = delta error */ /* r6 = integral accumulator */ /* r7 = current setpoint */ /* r14 = servo status word ( SW ) */ /* r15 = servo control word ( CW ) */ /************************************************************************************/ /********************************************************/ /* part 3 : execute chopper PID */ /* execute function "activate" and "deactivate" */ /* execute commutation ( copy OY to OZ ) if active */ /* note : do not destroy following registers : */ /* r0 = chopper position */ /* r1 = free */ /* r2 = MIM hardware status register */ /* r3 = grating position ( 24 bits ) */ /* r4 ( error ) */ /* r5 ( speed ) */ /* r6 ( Integral Accumulator ) */ /* r7 ( current setpoint ) */ /* r8 ( temporary and frequency ) */ /* r9 ( 1/divisor ) */ /* r10 ( temporary and output limit ) */ /* r11 ( PID Coefficients ) */ /* r12 ( Accumulator Limit ) */ /* r13 ( temporary ) */ /* r14 ( servo status word ) */ /* r15 ( servo command word ) */ /********************************************************/ r7 = dm(_gSelectControllersMode); /* this word will tell you which position to use and which pid algorithm to use */ btst r7 by K_BPOS_CTRL_MODE_CHOPPER; /* test the chopper mode, 0 = nominal, 1 = simulate position */ if not sz jump chopper_simulate_position; r6 = dm(PO, i3); /* r6 = chopper pos offset */ r7 = r0; /* r7 = field plate voltage */ r0 = r0 + r6; /* r0 = chopper pos = chopper pos read + offset */ r15 = dm(CW, i3); /* r15 = servo control word */ r14 = dm(SW, i3); /* r14 = servo status word */ jump chopper_checkpos; chopper_simulate_position: r0 = dm(SP, i3); r15 = dm(CW, i3); /* r15 = servo control word */ r14 = dm(SW, i3); /* r14 = servo status word */ chopper_checkpos: dm(MP, i3) = r0; /* chopper current position in PID block */ /* signal the position error if position > position limit */ r4 = dm(CH_POS_LIMIT, i3); /* r4 = position limit */ r5 = ABS r0; /* r5 = ABS(position) */ comp(r5, r4); if le jump chopper_pid; r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_POSITION_ERROR; /* set error bit in PidStatus*/ jump chopper_deactivate_pid; chopper_pid: r4 = dm(CH_I_LUT_OFFSET, i3); r12 = r7 + r4; m5 = r12; f0 = dm(m5, i3); /* convert position using the lookup table */ f5 = dm(CHOP_CUR_POS,i6); dm(CHOP_PREV_POS,i6) = f5; /* Pos(t) becomes Pos(t-1) */ dm(CHOP_CUR_POS,i6) = f0; /* store Pos(t) */ r7 = dm(SP, i3); /* load current setpoint */ r7 = r7 + r4; r7 = r7 - r6; /* remove offset */ m5 = r7; f7 = dm(m5, i3); /* convert setpoint using the lookup table */ f4 = f7 - f0; /* f4 = Error(t) */ f1 = dm(CHOP_CUR_ERR,i6); /* Error(t) becomes Error(t-1) */ dm(CHOP_PREV_ERR,i6) = f1; dm(CHOP_CUR_ERR,i6) = f4; /* Store Error(t) */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* command to activate PID */ if not sz jump (chopper_activate_pid); btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* command to deactivate PID */ if not sz jump (chopper_deactivate_pid); r0 = dm(OY, i3); /* load output in case PID inactive */ r14 = bclr r14 by 3; /* reset PID executing bit */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* PID active bit */ if sz jump (chopper_idle); btst r14 by K_BPOS_CHOP_CTRL_STATUS_OPEN_LOOP; /* check if the chopper is in open-loop mode */ if not sz jump chopper_idle; r12 = fix f4; /* r12 = Error(t) */ r6 = dm(IA,i3); /* Load Integral Accumulator */ r6 = r6 + r12; /* New Integral Accumulator */ r12 = dm(IL,i3); /* Load Accumulator Limit */ r6 = CLIP r6 by r12; /* Limit Integral Accumulator */ dm(IA,i3) = r6; /* Store new integral accumulator */ f11 = dm(CH_F_KP,i3); /* Load proportional gain */ f0 = f11 * f4; /* KP/DIV*Error */ f5 = dm(CH_F_KI,i3); /* Load integral gain */ f6 = float r6; /* float IA */ f6= f5 * f6; /* S(Error) * KI/DIV */ f11 = dm(FREQ,i6); /* Load frequency in Hz */ f13 = recips f11; /* Invert frequency */ f6 = f6 * f13; /* S(error) * KI/DIV * 1/FREQ */ f0 = f0 + f6; /* KP/DIV*Error + S(Error)*KI/DIV*1/FREQ */ f6 = f1 * f11; /* f6= Error(t-1)*FREQ */ f12 = f4 * f11; /* f12 = Error(t)*FREQ */ f5 = f12 - f6; /* f5 = DError*Freq */ f6 = dm(CH_F_KD,i3); /* Load differentail gain */ f6 = f6 * f5; /* KD/DIV*DError*Freq */ f0 = f0 + f6; /* KP/DIV*Error + S(Error)*KI/DIV*1/FREQ + KD*DError*FREQ */ r6 = dm(OY, i3); /* I(t-1) */ f6 = float r6; f5 = dm(CH_F_KIC, i3); f6 = f6 * f5; /* KIC/DIV*I(t-1) */ f0 = f0 - f6; f5 = dm(CHOP_CUR_POS, i6); /* f5 = Pos(t) */ f6 = dm(CHOP_PREV_POS, i6); /* f6 = Pos(t-1) */ f5 = f5 - f6; /* f5 = Speed(t) */ f1 = dm(CHOP_AVG_SPEED, i6); /* f1 = AveragedSpeed(t-1) */ f12 = 1.; f6 = f12 * f1; /* 2*AveragedSpeed(t-1) */ f6 = f6 + f5; /* 2*AveragedSpeed(t-1)+Speed(t) */ f12 = 2.; f12 = recips f12; /* Invert f12 */ f5 = f6 * f12; /* f5 = AveragedSpeed(t) = (2*AveragedSpeed(t-1)+Speed(t))/3 */ dm(CHOP_AVG_SPEED, i6) = f5; /* Store AveragedSpeed(t) */ f6 = f5 * f11; /* f6 = AveragedSpeed(t)*Freq */ f11 = dm(CH_F_KF,i3); /* f11 = Kf/DIV */ f6 = f6 * f11; /* f6 = AveragedSpeed(t)*Freq * Kf/DIV */ f0 = f0 - f6; /* f0 = Output(t) */ /* filter */ f1 = dm(CHOP_PREV1_OUT,i6); /* Out1(t-1) becomes Out1(t-2) */ dm(CHOP_PREV2_OUT,i6) = f1; f1 = dm(CHOP_OUT,i6); /* Out1(t) becomes Out1(t-1) */ dm(CHOP_PREV1_OUT,i6) = f1; dm(CHOP_OUT,i6) = f0; /* Out1(t) is the output of the first part of the controller */ f1 = dm(CHOP_FILTER_PREV1_OUT,i6); /*FilterOut(t-1) becomes FilterOut(t-2) */ dm(CHOP_FILTER_PREV2_OUT,i6) = f1; f1 = dm(CHOP_FILTER_OUT,i6); /*FilterOut(t) becomes FilterOut(t-1) */ dm(CHOP_FILTER_PREV1_OUT,i6) = f1; f12 = dm(CH_F_FILT_N1, i3); f0 = f0 * f12; f12 = dm(CH_F_FILT_N2, i3); f1 = dm(CHOP_PREV1_OUT,i6); f6 = f12 * f1; f0 = f0 - f6; f12 = dm(CH_F_FILT_N3, i3); f1 = dm(CHOP_PREV2_OUT,i6); f6 = f12 * f1; f0 = f0 + f6; f12 = dm(CH_F_FILT_D1, i3); f1 = dm(CHOP_FILTER_PREV1_OUT, i6); f6 = f12 * f1; f0 = f0 + f6; f12 = dm(CH_F_FILT_D2, i3); f1 = dm(CHOP_FILTER_PREV2_OUT, i6); f6 = f12 * f1; f0 = f0 - f6; dm(CHOP_FILTER_OUT,i6) = f0; /* end filter */ f11 = dm(CH_F_CLG, i3); /* f11 = CLG (Control loop gain) */ f0 = f0 * f11; f11 = dm(FREQ,i6); /* f11 = Load frequency in Hz */ f6 = dm(CH_F_INDUCTANCE, i3); /* f6 = L (Inductance) */ f10 = f6 * f11; /* f10 = L*FREQ */ r6 = dm(OY, i3); /* I(t-1) */ f6 = float r6; f6 = f10 * f6; /* f6 = I(t-1) * L * FREQ */ f0 = f0 + f6; f11 = dm(CH_F_RESISTANCE, i3); /* f11 = Rt (resistor) */ f11 = f11 + f10; /* f11 = Rt + L*FREQ */ f11 = recips f11; f0 = f0 * f11; r0 = fix f0; r10 = dm(OL, i3); /* Load Output limit */ r0 = CLIP r0 BY r10; /* Saturate output to 16bits signed */ dm(OY, i3) = r0; /* store result */ r4 = fix f4; dm(ER, i3) = r4; /* store current error */ /* signal the following error if error > error limit */ r6 = dm(EL, i3); /* r6 = error limit */ r11 = ABS r4; /* r11 = ABS(Error(t)) */ comp(r11, r6); if gt r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_FOLLOWING_ERROR; /* set error bit in PidStatus*/ jump (chopper_idle); /********************************************************/ /* CHOPPER PID ACTIVATION HANDLING */ /********************************************************/ chopper_activate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* reset activate PID command bit */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* set PID active status bit */ r0 = 0; /* set output to zero */ jump (chopper_idle); chopper_deactivate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* reset deactivate PID command bit */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* reset PID active status bit */ r10=0; dm(OY, i3) = r10; /* output = 0 */ dm(IA, i3) = r10; /* accumulator = 0*/ f10=0.; /* clear all history variables*/ dm(CHOP_CUR_POS, i6) = f10; dm(CHOP_PREV_POS, i6) = f10; dm(CHOP_CUR_ERR, i6) = f10; dm(CHOP_PREV_ERR, i6) = f10; dm(CHOP_AVG_SPEED, i6) = f10; dm(CHOP_AVG_OUT, i6) = f10; dm(CHOP_AVG_DERROR, i6) = f10; chopper_idle: /* here r0 = output word in all cases */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; /* commutation active control bit */ if sz jump (chopper_idle2); r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; chopper_idle2: btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; /* commutation active control bit */ if sz jump (chopper_idle3); r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; chopper_idle3: btst r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; /* commutation active control bit */ if sz jump (chopper_no_out); /* if not, leave DAC word alone */ chopper_no_out: /* copy back control and status words to PID block */ dm(CW, i3) = r15; dm(SW, i3) = r14; /********************************************************/ /* part 4 : execute grating PID ( if active ) */ /* assumption about part 1-2-3 : */ /* r0 = free ( was used by chopper ) */ /* r1 = free ( was used by chopper ) */ /* r2 = MIM hardware status register */ /* r3 = grating position ( 24 bits ) */ /* only valid if grating power ON */ /* registers used and not saved ( internal usage ): */ /* r4 ( error ) */ /* r5 ( temporary ) */ /* r6 ( temporary ) */ /* r7 ( current setpoint ) */ /* r8 ( DACS calculation ) */ /* r9 ( DACS calculation ) */ /* r10 ( Output limit ) */ /* r11 ( temporary ) */ /* r12 ( 0 ) */ /* r13 ( range then temporary ) */ /* r14 ( servo status word ) */ /* r15 ( servo command word ) */ /* assumption about environment : */ /* high level is responsible of managing */ /* consistently the power ON, mode, PID active */ /* and trajectory control bits */ /* THEREFORE THIS CODE ASSUMES THAT */ /* IF THE PID ACTIVE BIT IS SET THEN */ /* THE HALL SENSORS AND POSITION ARE VALID !!! */ /********************************************************/ r11 = dm(TK, i0); btst r11 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if sz jump grating_end; /**********************************************/ /* GRATING INIT REGISTERS */ /**********************************************/ r12 = 0; /* r12 = 0 */ r13 = dm(GR_RA, i0); /* r13 = the range */ r14 = dm(SW, i0); /* r14 = PidStatus */ r15 = dm(CW, i0); /* r15 = ControlWord */ /* read limit switches and launch lock switches and copy them in PidStatus */ r11 = dm(MIM_STATUS_REG); r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_LOCKED; btst r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_LOCKED; if not sz r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_LOCKED; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_UNLOCKED; btst r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_UNLOCKED; if not sz r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_UNLOCKED; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LIMIT_SWITCH; btst r11 by K_BPOS_MIM_STATUS_GRATING_LIMIT_SWITCH; if not sz r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LIMIT_SWITCH; /**********************************************/ /* GRATING READ POSITION */ /**********************************************/ grating_read_position: r5 = dm(_gSelectControllersMode); /* this word will tell you if you have to replace the position by the setpoint */ btst r5 by K_BPOS_CTRL_MODE_GRATING; /* test the grating mode, 0 = nominal, 1 = simulate position */ if not sz jump grating_simulate_position; r0 = fext r3 by 0:24 (se); /* compute 32 bit signed value of grating position (extract 24bits)*/ dm(GRAT_IND_READ, i6) = r0; r11 = dm(GRAT_CUR_POS, i6); /* Note: in this block of code, GratPos is the readout of the inductosyn*/ dm(GRAT_PREV_POS, i6) = r11; /* GratPos(t) becomes GratPos(t-1) */ dm(GRAT_CUR_POS, i6) = r0; /* store GratPos(t) */ r11 = r0 - r11; /* r11 = delta_pos = GratPos(t) - GratPos(t-1) */ r11 = fext r11 by 0:16 (se); /* keep only the lowest 16 bits. If the delta_pospos is bigger, it means that something went wrong (inductosyn jump or period counter overflow in the FPGA) */ r0 = dm(GRAT_TEMP_POS, i6); r0 = r0 + r11; /* GratPos(t) = GratPos(t-1) + delta_pos */ dm(GRAT_TEMP_POS, i6) = r0; r0 = r13-r0; /* position = range - pos read */ jump grating_homing; grating_simulate_position: r0 = dm(SP, i0); /* position = setpoint */ /**********************************************/ /* GRATING HOMING */ /**********************************************/ grating_homing: /*Test if Homing has been ordered*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_HOME; if sz jump grating_homing_test_progress; /*Homing has been ordered, clear the bit in the CW and set it in SW*/ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_HOME; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; grating_homing_test_progress: /*Test if Homing is in progress*/ btst r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; if sz jump grating_homing_end; /*Homing is in progress, test if the move is finished (power > MaxOutputLongDuration during a given period)*/ r7 = dm(GR_CUR_STOP, i0); /* r7 = CurrentNbInterruptsOfMaxOutput */ r11 = dm(GR_NB_STOP, i0); r11 = ASHIFT r11 by -3; /* r11 = NbInterruptsBeforeStoppingOutput / 8 */ comp(r7, r11); if lt jump grating_homing_end; /*Move is finished, we record the period count in r11*/ r11 = 0xFFFF0000; r11 = r0 and r11; btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* test moving up flag */ if not sz jump (grating_homing_was_moving_up); btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* test moving down flag */ if sz jump (grating_homing_end); grating_homing_was_moving_down: dm(PO, i0) = r11; /* offset = current position*/ /*!!! Now, there is an offset!=0 so, next time the pid ctrl loop will execute, it will see a very big error and react accordingly. So, we'd better reset the PID now */ r11 = r0 - r11; dm(SP, i0) = r11; /* set setpoint to current pos (it should be zero)*/ dm(GRAT_ERRORT_0,i6) = r12; /* to avoid a big derivative effect, set last error to zero*/ r13 = 500000; dm(TG, i0) = r13; /* move the grating to a central position */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; r15 = bset r15 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; jump grating_homing_completed; grating_homing_was_moving_up: r11 = r11 - r13; /* offset = current position - range*/ dm(PO, i0) = r11; /*!!! Now, there is an offset!=0 so, next time the pid ctrl loop will execute, it will see a very big error and react accordingly. So, we'd better reset the PID now */ r13 = r0 - r11; dm(SP, i0) = r13; /* set setpoint to current pos (it should be the range)*/ dm(GRAT_ERRORT_0,i6) = r12; /* to avoid a big derivative effect, set last error to zero*/ r13 = 500000; dm(TG, i0) = r13; /* move the grating to a central position */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; r15 = bset r15 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; grating_homing_completed: r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; /* set/Clear the status bits*/ r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_COMPLETED; dm(SW, i0) = r14; grating_homing_end: /**********************************************/ /* GRATING LAUNCH_LOCK */ /**********************************************/ grating_ll_test_mech_lock_ordered: /*Test if Mechanical launch lock has been ordered*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL; if not sz jump grating_ll_lock_ordered; btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL2; if sz jump grating_ll_test_mech_unlock_ordered; grating_ll_lock_ordered: /*Send current into the lock motor (it will be done at next ISR execution)*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL; if sz jump grating_ll_lock_ordered2; r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bset r11 by K_BPOS_MIM_GRATING_LAUNCH_LOCK; dm(CONTROL_WORD, i6) = r11; grating_ll_lock_ordered2: btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL2; if sz jump grating_ll_lock_ordered3; r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bset r11 by K_BPOS_MIM_GRATING_LAUNCH_LOCK2; dm(CONTROL_WORD, i6) = r11; grating_ll_lock_ordered3: /*Start a counter to make sure not to send current in the motor for more than 40960 period*/ r11 = 40960; dm(GR_CO, i0) = r11; /*Lock has been ordered, clear the bit in the CW and set it in SW*/ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL; r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL2; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LOCK_MECHANICAL; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; jump grating_ll_test_lock_in_progress; grating_ll_test_mech_unlock_ordered: /*Test if Mechanical launch unlock has been ordered*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL; if not sz jump grating_ll_unlock_ordered; btst r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL2; if sz jump grating_ll_test_lock_in_progress; grating_ll_unlock_ordered: /*Send current into the unlock motor (it will be done at next ISR execution)*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL; if sz jump grating_ll_unlock_ordered2; r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bset r11 by K_BPOS_MIM_GRATING_LAUNCH_UNLOCK; dm(CONTROL_WORD, i6) = r11; grating_ll_unlock_ordered2: btst r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL2; if sz jump grating_ll_unlock_ordered3; r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bset r11 by K_BPOS_MIM_GRATING_LAUNCH_UNLOCK2; dm(CONTROL_WORD, i6) = r11; grating_ll_unlock_ordered3: /*Start a counter to make sure not to send current in the motor for more than 8192 period*/ r11 = 40960; dm(GR_CO, i0) = r11; /*Unlock has been ordered, clear the bit in the CW and set it in SW*/ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL; r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL2; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_UNLOCK_MECHANICAL; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; jump grating_ll_test_unlock_in_progress; grating_ll_test_lock_in_progress: /*Test if Mechanical launch locking is in progress*/ btst r14 by K_BPOS_GRAT_CTRL_STATUS_LOCK_MECHANICAL; if sz jump grating_ll_test_unlock_in_progress; grating_ll_lock_test_timeout: /* decrement timeout counter */ r11 = dm(GR_CO, i0); r11 = r11 - 1; dm(GR_CO, i0) = r11; comp(r11, r12); if gt jump grating_ll_end; /* if counter > 0, nothing to do */ /* timeout is reached, stop sending current in the motor */ grating_ll_lock_completed: r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LOCK_MECHANICAL; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; /*stop sending current into the lock motor (it will be done at next ISR execution)*/ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bclr r11 by K_BPOS_MIM_GRATING_LAUNCH_LOCK; r11 = bclr r11 by K_BPOS_MIM_GRATING_LAUNCH_LOCK2; dm(CONTROL_WORD, i6) = r11; jump grating_ll_end; grating_ll_test_unlock_in_progress: /*Test if Mechanical launch unlocking is in progress*/ btst r14 by K_BPOS_GRAT_CTRL_STATUS_UNLOCK_MECHANICAL; if sz jump grating_ll_end; grating_ll_unlock_test_timeout: /* decrement timeout counter */ r11 = dm(GR_CO, i0); r11 = r11 - 1; dm(GR_CO, i0) = r11; comp(r11, r12); if gt jump grating_ll_end; /* if counter > 0, nothing to do */ /* timeout is reached, stop sending current in the motor */ grating_ll_unlock_completed: r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_UNLOCK_MECHANICAL; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; /*stop sending current into the unlock motor (it will be done at next ISR execution)*/ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bclr r11 by K_BPOS_MIM_GRATING_LAUNCH_UNLOCK; r11 = bclr r11 by K_BPOS_MIM_GRATING_LAUNCH_UNLOCK2; dm(CONTROL_WORD, i6) = r11; grating_ll_end: /**********************************************/ /* GRATING PID */ /**********************************************/ grating_pid: r4 = dm(PO, i0); /* read offset */ r0 = r0-r4; /* and subtract it from current pos */ dm(MP, i0) = r0; /* grating current position in PID block */ btst r14 by K_BPOS_GRAT_CTRL_STATUS_DEGRADED_MODE; /* test if we are in degraded mode */ if not sz jump grating_degraded_move; /* if yes, jump to degraded mode code */ r7 = dm(SP, i0); /* load current setpoint */ r4 = r7 - r0; /* compute current error with correct sign (SP-MP for nom and MP-SP for red) */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* command to activate PID */ if not sz jump (grating_activate_pid); btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* command to deactivate PID */ if not sz jump (grating_deactivate_pid); r0 = dm(OY, i0); /* load output in case PID inactive */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* PID active bit */ if sz jump (grating_idle); /* compute PID algorithm */ /* r0 contains OY */ /* r1 is free */ /* r2, r3 are not free */ /* r4 contains current error */ /* r7 contains current setpoint */ /* r12 contains 0 */ /* result should be a properly scaled and clipped signed integer in r0 */ /* GRATING PID */ r1 = dm(GRAT_ERRORT_0, i6); /* GratError(t) */ dm(GRAT_ERRORT_1,i6) = r1; /* becomes GratError(t-1) */ dm(GRAT_ERRORT_0,i6) = r4; /* Store current error */ /* signal the following error if error > error limit */ r6 = dm(EL, i0); /* r6 = error limit */ r11 = ABS r4; /* r11 = ABS(Error(t)) */ comp(r11, r6); if le jump grating_pid_computation; r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_FOLLOWING_ERROR; /* set error bit in PidStatus*/ jump grating_deactivate_pid; grating_pid_computation: r6 = dm(IA,i0); /* Load Integral Accumulator */ r6 = r6 + r4; /* New Integral Accumulator */ r11 = dm(IL,i0); /* Load Accumulator Limit */ r6 = CLIP r6 by r11; /* Limit Integral Accumulator */ dm(IA,i0) = r6; /* Store new integral accumulator */ f8 = dm(DIVISOR,i6); /* DIVISOR to scale back KP KI KD */ f8 = recips f8; /* Invert DIVISOR */ r11 = dm(KP,i0); /* Load proportional gain (KP)*/ f11 = float r11; /* Float KP */ f4 = float r4; /* Float GratError(t) */ f0 = f11 * f4; /* KP * GratError */ r5 = dm(KI,i0); /* Load integral gain (KI)*/ f5 = float r5; /* Float KI */ f6 = float r6; /* Float IA */ f6= f5 * f6; /* S(GratError) * KI */ f11 = dm(FREQ,i6); /* Load frequency in Hz */ f13 = recips f11; /* Invert frequency */ f6 = f6 * f13; /* S(GratError) * KI * 1/FREQ */ f0 = f0 + f6; /* KP*GratError + S(GratError)*KI*1/FREQ */ f6 = float r1; /* Float GratError(t-1) */ f6 = f6 * f11; /* GratError(t-1)*FREQ*/ f11 = f4 * f11; /* GratError(t)*FREQ*/ f5 = f11 - f6; /* f5 = D(GratError)*FREQ */ /* filter D(GratError) */ f6 = dm(GR_PREV_DER, i0); /* f6 = D(t-1) */ r10 = dm(KF, i0); /* r10 = Filter order (stored in KF) */ r11 = 1; r11 = r11 + r10; f11 = float r11; f11 = recips f11; f10 = float r10; f6 = f6 * f10; /* f6 = K*D(t-1) */ f6 = f6 + f5; /* f6 = D(t) + K*D(t-1) */ f5 = f6 * f11; /* D(GratError) = (D(t) + K*D(t-1)) / (K+1) */ dm(GR_PREV_DER, i0) = f5; r6 = dm(KD,i0); /* Load differentail gain (KD)*/ f6 = float r6; /* Float KD */ f6 = f6 * f5; /* KD*D(GratError)*FREQ */ f0 = f0 + f6; /* KP*GratError + S(GratError)*KI*1/FREQ + KD*D(GratError)*FREQ */ f0 = f0 * f8; /* KP/DIVISOR*GratError + S(GratError)*KI/DIVISOR*1/FREQ + KD/DIVISOR*D(GratError)*FREQ */ /* filter */ f1 = dm(GRAT_PREV1_OUT,i6); /* Out1(t-1) becomes Out1(t-2) */ dm(GRAT_PREV2_OUT,i6) = f1; f1 = dm(GRAT_OUT,i6); /* Out1(t) becomes Out1(t-1) */ dm(GRAT_PREV1_OUT,i6) = f1; dm(GRAT_OUT,i6) = f0; /* Out1(t) is the output of the first part of the controller */ f1 = dm(GRAT_FILTER_PREV1_OUT,i6); /*FilterOut(t-1) becomes FilterOut(t-2) */ dm(GRAT_FILTER_PREV2_OUT,i6) = f1; f1 = dm(GRAT_FILTER_OUT,i6); /*FilterOut(t) becomes FilterOut(t-1) */ dm(GRAT_FILTER_PREV1_OUT,i6) = f1; f5 = dm(GR_F_FILT_N1, i0); f0 = f0 * f5; f5 = dm(GR_F_FILT_N2, i0); f1 = dm(GRAT_PREV1_OUT,i6); f6 = f5 * f1; f0 = f0 - f6; f5 = dm(GR_F_FILT_N3, i0); f1 = dm(GRAT_PREV2_OUT,i6); f6 = f5 * f1; f0 = f0 + f6; f5 = dm(GR_F_FILT_D1, i0); f1 = dm(GRAT_FILTER_PREV1_OUT, i6); f6 = f5 * f1; f0 = f0 + f6; f5 = dm(GR_F_FILT_D2, i0); f1 = dm(GRAT_FILTER_PREV2_OUT, i6); f6 = f5 * f1; f0 = f0 - f6; dm(GRAT_FILTER_OUT,i6) = f0; /* end filter */ r0 = fix f0; /* Convert f to r */ r10 = dm(OL, i0); /* Load Output limit */ r8 = r0; /* r8 = output before clipping */ r0 = CLIP r0 BY r10; /* Saturate output to output limit*/ dm(OY, i0) = r0; /* Store result */ r11 = dm(GR_MAX_OUT_LG, i0); /* clip output by 'long duration output limit' into a temporary register */ r11 = CLIP r8 BY r11; comp(r8, r11); /* check if output has been clipped */ if NE jump grat_out_is_max; dm(GR_CUR_STOP, i0) = r12; /* if it has not been clipped, reset the max_output_counter */ grat_out_is_max: r8 = dm(GR_CUR_STOP, i0); /* if it has been clipped, increment the max_output_counter */ r8 = r8 + 1; dm(GR_CUR_STOP, i0) = r8; r11 = dm(GR_NB_STOP, i0); comp(r8, r11); /* if the max_output_counter is big enough, cut the output and disable the controller */ if LT jump grating_scaling; r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_POWER_LIMIT_ERROR; /* set error bit in PidStatus*/ jump grating_deactivate_pid; grating_scaling: r11 = dm(KC, i0); /* Load Scaling */ r0 = r0 * r11 (SSI); /* output = output * scaling */ r0 = ASHIFT r0 by -10; /* normalize scaling */ jump (grating_idle); grating_degraded_move: r13 = dm(GR_DEG_MOVE, i0); comp(r12,r13); if eq jump grating_deg_move_end; /*init some register*/ r13 = 1; /* increment the ISR counter and compare it with 'Rate'*/ r10 = dm(GR_DEG_IC, i0); r10 = r10 + r13; r9 = dm(GR_DEG_RT, i0); comp(r10, r9); /* every 'Rate' interrupt, move the grating one step*/ if ge jump grating_deg_move_one_step; dm(GR_DEG_IC, i0) = r10; jump grating_deg_move_end; grating_deg_move_one_step: dm(GR_DEG_IC, i0) = r12; /* reset the ISR counter */ r10 = dm(GR_DEG_IS, i0); /* r10 = CurrentStep */ r9 = dm(GR_DEG_NS, i0); /* r9 = NbStepsForThisMove */ comp(r10, r9); /* if we have already moved the right number of steps, finish the move */ if eq jump grating_deg_move_finished; grating_deg_continue_move: r10 = r10 + r13; /* increment the step counter */ dm(GR_DEG_IS, i0) = r10; /* and save the value */ /*compute the current position in the phase */ r8 = dm(GR_DEG_CURA, i0); /* r8 = CurrentPosInPhaseA */ r9 = dm(GR_DEG_IN, i0); /* r9 = CurrentInc */ r8 = r8 + r9; /*ensure not to go over the sinus array bounds */ r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; dm(GR_DEG_CURA, i0) = r8; /* output in phase B */ i7=_gSinArray; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position) */ r10 = dm(GR_DEG_KC, i0); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(GR_DEG_OUTB, i0) = r10; r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC1, i6)= r10; /*dm(GRAT_FW_DAC2, i6)= r10;*/ /* write DAC value to MIM block */ /* output in phase A */ r10 = (NB_STEPS_IN_PHASE/4); /*r8 = r8 + r10;*/ r8 = r8 - r10; r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position - phase/4) */ r10 = dm(GR_DEG_KC, i0); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(GR_DEG_OUTA, i0) = r10; r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC2, i6)= r10; /*dm(GRAT_FW_DAC1, i6)= r10;*/ /* write DAC value to MIM block */ jump grating_deg_move_end; grating_deg_move_finished: dm(GR_DEG_MOVE, i0) = r12; /* the move is finished */ grating_deg_move_end: jump grating_no_out; grating_activate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* reset activate PID command bit */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* set PID active status bit */ r0 = 0; /* set output to zero */ jump (grating_idle); grating_deactivate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* reset deactivate PID command bit */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* reset PID active status bit */ dm(IA,i0) = r12; /* reset accumulator */ dm(GRAT_ERRORT_0,i6) = r12; /* reset error */ dm(GRAT_ERRORT_0+1,i6) = r12; /* reset previous error */ dm(OY, i0) = r12; /* reset output before offset was added */ /* DAC ouput to 0 */ r0 = 0x0; /* 0x0 = 0V */ dm(GRAT_FW_DAC1, i6) = r0; /* reset DACA */ dm(GRAT_FW_DAC2, i6) = r0; /* reset DACB */ grating_idle: r4 = dm(GRAT_ERRORT_0,i6); dm(ER, i0) = r4; /* Store current error */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; /* commutation active control bit */ if sz jump (grating_idle2); r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; grating_idle2: btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; /* commutation active control bit */ if sz jump (grating_idle3); r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; grating_idle3: btst r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; /* commutation active control bit */ if sz jump (grating_no_out); /* if not, leave DAC word alone */ /* compute output * Hall value = DAC */ r8 = dm(_gHallSensorA); /* r8 = first Hall sensor */ r9 = dm(GR_HSO, i0); r8 = r8 + r9; r8 = r8 * r0 (SSI); /* HallA multiplied by output */ r8 = ASHIFT r8 BY -15; dm(GRAT_FW_DAC1, i6)= r8; /* write DAC value to MIM block */ r1 = dm(_gHallSensorB); /* r1 = second Hall sensor */ r9 = dm(GR_HSO, i0); r1 = r1 + r9; r1 = r1 * r0 (SSI); /* HallB multiplied by output */ r1 = ASHIFT r1 by -15; dm(GRAT_FW_DAC2, i6)= r1; /* write DAC value to MIM block */ grating_no_out: /**********************************************/ /* GRATING SAVE REGISTERS */ /**********************************************/ /* copy back control and status words to PID block */ dm(CW, i0) = r15; dm(SW, i0) = r14; grating_end: /********************************************************/ /* part 5 : execute spectrometer FW SPEC Control */ /* assumption about part 1-2-3 : */ /* r0 ( 16 lsb ) = chopper position */ /* r1 = Hall sensors from motor selected by mode */ /* r2 = MIM hardware status register */ /* registers used and not saved ( internal usage ): */ /* r3 ( temporary ) */ /* r4 ( temporary ) */ /* r5 ( temporary ) */ /* r6 ( temporary ) */ /* r7 ( temporary ) */ /* r8 ( temporary ) */ /* r9 ( temporary ) */ /* r10 ( temporary ) */ /* r11 (Task Status) */ /* r12 (0) */ /* r13 (1) */ /* r14 ( temporary ) */ /* r15 ( temporary ) */ /* assumption about environment : */ /* high level is responsible of managing */ /* consistently the power ON, mode, PID active */ /* and trajectory control bits */ /********************************************************/ /*test if the fw spec is currently powered on. If not, nothing to do*/ r11 = dm(FW_TK, i1); btst r11 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if sz jump fw_spec_end; r15 = K_BMASK_FW_CTRL_STATUS_SENSORS_INVERTED_MASK; r11 = r11 and r15; /*clear sensor status bits*/ r15 = -1; /* r15 will be used to store the position, -1 means 'undefined' */ r7 = dm(_gSelectControllersMode); /* this word will tell you which position to use and which pid algorithm to use */ btst r7 by K_BPOS_CTRL_MODE_FW_SPEC; /* test the fw_spec mode, 0 = nominal, 1 = simulate position */ if not sz jump fw_spec_simulate_position; r12 = dm(FW_SPEC_POSA, i6); /* read the Hall sensor at pos A */ r13 = dm(FW_SPEC_POSB, i6); /* read the Hall sensor at pos B */ jump fw_spec_read_pos_a; fw_spec_simulate_position: r12 = dm (FW_SIM_SA, i1); /* copy the simulated value of SPEC Hall A */ r13 = dm (FW_SIM_SB, i1); /* copy the simulated value of SPEC Hall B */ fw_spec_read_pos_a: r14 = dm(FW_ST_STA, i1); /* read the threshold for the position sensor */ comp(r12, r14); /* if the sensor value is bigger than the threshold*/ if lt jump fw_spec_read_pos_a2; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; /* set a bit in the status */ r15 = 0; /* and store the current position in r15*/ fw_spec_read_pos_a2: r14 = dm(FW_STA, i1); /* read the commanding threshold for the position sensor*/ comp(r12, r14); if lt jump fw_spec_read_pos_b; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_A; fw_spec_read_pos_b: r14 = dm(FW_ST_STB, i1); /* read the threshold for the position sensor */ comp(r13, r14); /* read the Hall sensor at pos A */ if lt jump fw_spec_read_pos_b2; /* if the sensor value is bigger than the threshold*/ r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; /* set a bit in the status */ r15 = 1; /* and store the current position in r15*/ fw_spec_read_pos_b2: r14 = dm(FW_STB, i1); /* read the commanding threshold for the position sensor*/ comp(r13, r14); if lt jump fw_spec_read_pos_end; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_B; fw_spec_read_pos_end: /*report the position in the CurrentPos HK value*/ dm(FW_CP, i1) = r15; /*test if the fw spec is currently moving. If not, nothing to do*/ btst r11 by K_BPOS_FW_CTRL_STATUS_MOVING; if sz jump fw_spec_end; /*init some register*/ r12 = 0; r13 = 1; /********************************************/ /* FW SPEC MOVING */ /********************************************/ /* increment the ISR counter and compare it with 'Rate'*/ r10 = dm(FW_IC, i1); r10 = r10 + r13; r9 = dm(FW_RT, i1); comp(r10, r9); /* every 'Rate' interrupt, move the fw one step*/ if ge jump fw_spec_move_one_step; dm(FW_IC, i1) = r10; jump fw_spec_end; fw_spec_move_one_step: dm(FW_IC, i1) = r12; /* reset the ISR counter */ r10 = dm(FW_IS, i1); /* r10 = CurrentStep */ r9 = dm(FW_NS, i1); /* r9 = NbStepsForThisMove */ comp(r10, r9); /* if we have already moved the right number of steps, finish the move */ if eq jump fw_spec_move_finished; /*Do we have to stop when we reach a location ?*/ fw_spec_test_search_a: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; if sz jump fw_spec_test_search_b; btst r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_A; if sz jump fw_spec_test_search_b; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; jump fw_spec_move_finished; fw_spec_test_search_b: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; if sz jump fw_spec_continue_move; btst r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_B; if sz jump fw_spec_continue_move; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; jump fw_spec_move_finished; fw_spec_continue_move: r10 = r10 + r13; /* increment the step counter */ dm(FW_IS, i1) = r10; /* and save the value */ /*compute the current position in the phase */ r8 = dm(FW_CURA, i1); /* r8 = CurrentPosInPhaseA */ r9 = dm(FW_IN, i1); /* r9 = CurrentInc */ r8 = r8 + r9; /*ensure not to go over the sinus array bounds */ r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; dm(FW_CURA, i1) = r8; /* output in phase A */ i7=_gSinArray; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position) */ r10 = dm(FW_KC, i1); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTA, i1) = r10; r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC1, i6)= r10; /* write DAC value to MIM block */ /* output in phase B */ r10 = (NB_STEPS_IN_PHASE/4); r8 = r8 - r10; r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position - phase/4) */ r10 = dm(FW_KC, i1); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTB, i1) = r10; r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC2, i6)= r10; /* write DAC value to MIM block */ jump fw_spec_end; fw_spec_move_finished: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_MOVING; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; r9 = 0x0; /* reset DAC (0V) */ dm(GRAT_FW_DAC1, i6)= r9; /* write DAC value to Grating MIM block */ dm(GRAT_FW_DAC2, i6)= r9; /* write DAC value to Grating MIM block */ jump fw_spec_end; fw_spec_end: dm(FW_TK, i1) = r11; /********************************************************/ /* part 6 : execute photometer FW Photo control */ /* assumption about part 1-2-3 : */ /* r0 ( 16 lsb ) = chopper position */ /* r1 = Hall sensors from motor selected by mode */ /* r2 = MIM hardware status register */ /* registers used and not saved ( internal usage ): */ /* r3 ( temporary ) */ /* r4 ( error ) */ /* r5 ( delta error ) */ /* r6 ( integral accumulator ) */ /* r7 ( current setpoint ) */ /* r8 ( DACS calculation ) */ /* r9 ( temporary ) */ /* r10 */ /* r11 */ /* r12 */ /* r13 */ /* r14 ( servo status word ) */ /* r15 ( servo command word ) */ /* assumption about environment : */ /* high level is responsible of managing */ /* consistently the power ON, mode, PID active */ /* and trajectory control bits */ /* */ /********************************************************/ /*test if the fw photo is currently powered on. If not, nothing to do*/ r11 = dm(FW_TK, i2); btst r11 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if sz jump fw_photo_end; r15 = K_BMASK_FW_CTRL_STATUS_SENSORS_INVERTED_MASK; r11 = r11 and r15; /*clear sensor status bits*/ r15 = -1; /* r15 will be used to store the position, -1 means 'undefined' */ r7 = dm(_gSelectControllersMode); /* this word will tell you which position to use and which pid algorithm to use */ btst r7 by K_BPOS_CTRL_MODE_FW_PHOT; /* test the fw_photo mode, 0 = nominal, 1 = simulate position */ if not sz jump fw_photo_simulate_position; r12 = dm(FW_PHOTO_POSA, i6); /* read the Hall sensor at pos A */ r13 = dm(FW_PHOTO_POSB, i6); /* read the Hall sensor at pos B */ jump fw_photo_read_pos_a; fw_photo_simulate_position: r12 = dm (FW_SIM_SA, i2); /* copy the simulated value of SPEC Hall A */ r13 = dm (FW_SIM_SB, i2); /* copy the simulated value of SPEC Hall B */ fw_photo_read_pos_a: r14 = dm(FW_ST_STA, i2); /* read the threshold for the position sensor */ comp(r12, r14); /* if the sensor value is bigger than the threshold*/ if lt jump fw_photo_read_pos_a2; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; /* set a bit in the status */ r15 = 0; /* and store the current position in r15*/ fw_photo_read_pos_a2: r14 = dm(FW_STA, i2); /* read the commanding threshold for the position sensor*/ comp(r12, r14); if lt jump fw_photo_read_pos_b; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_A; fw_photo_read_pos_b: r14 = dm(FW_ST_STB, i2); /* read the threshold for the position sensor */ comp(r13, r14); if lt jump fw_photo_read_pos_b2; /* if the sensor value is bigger than the threshold*/ r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; /* set a bit in the status */ r15 = 1; /* and store the current position in r15*/ fw_photo_read_pos_b2: r14 = dm(FW_STB, i2); /* read the commanding threshold for the position sensor*/ comp(r13, r14); if lt jump fw_photo_read_pos_end; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_B; fw_photo_read_pos_end: /*report the position in the CurrentPos HK value*/ dm(FW_CP, i2) = r15; /*test if the fw photo is currently moving. If not, nothing to do*/ btst r11 by K_BPOS_FW_CTRL_STATUS_MOVING; if sz jump fw_photo_end; /*init some register*/ r12 = 0; /* r12 = 0 */ r13 = 1; /* r13 = 1 */ /********************************************/ /* FW PHOTO MOVING */ /********************************************/ /* increment the ISR counter and compare it with 'Rate'*/ r10 = dm(FW_IC, i2); r10 = r10 + r13; r9 = dm(FW_RT, i2); comp(r10, r9); /* every 'Rate' interrupt, move the fw one step*/ if ge jump fw_photo_move_one_step; dm(FW_IC, i2) = r10; jump fw_photo_end; fw_photo_move_one_step: dm(FW_IC, i2) = r12; /* reset the ISR counter */ r10 = dm(FW_IS, i2); /* r10 = CurrentStep */ r9 = dm(FW_NS, i2); /* r9 = NbStepsForThisMove */ comp(r10, r9); /* if we have already moved the right number of steps, finish the move */ if eq jump fw_photo_move_finished; /*Do we have to stop when we reach a location ?*/ fw_photo_test_search_a: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; if sz jump fw_photo_test_search_b; btst r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_A; if sz jump fw_photo_test_search_b; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; jump fw_photo_move_finished; fw_photo_test_search_b: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; if sz jump fw_photo_continue_move; btst r11 by K_BPOS_FW_CTRL_STATUS_CMD_THRESHOLD_B; if sz jump fw_photo_continue_move; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; jump fw_photo_move_finished; fw_photo_continue_move: r10 = r10 + r13; /* increment the step counter */ dm(FW_IS, i2) = r10; /* and save the value */ /*compute the current position in the phase */ r8 = dm(FW_CURA, i2); /* r8 = CurrentPosInPhaseA */ r9 = dm(FW_IN, i2); /* r9 = CurrentInc */ r8 = r8 + r9; /*ensure not to go over the sinus array bounds */ r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; dm(FW_CURA, i2) = r8; /* output in phase A */ i7=_gSinArray; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position) */ r10 = dm(FW_KC, i2); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTA, i2) = r10; r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC1, i6)= r10; /* write DAC value to MIM block */ /* output in phase B */ r10 = (NB_STEPS_IN_PHASE/4); r8 = r8 - r10; r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position - phase/4) */ r10 = dm(FW_KC, i2); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTB, i2) = r10; r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC2, i6)= r10; /* write DAC value to MIM block */ jump fw_photo_end; fw_photo_move_finished: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_MOVING; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; r9 = 0x0; /* reset DAC to 0V */ dm(GRAT_FW_DAC1, i6)= r9; /* write DAC value to MIM block */ dm(GRAT_FW_DAC2, i6)= r9; /* write DAC value to MIM block */ jump fw_photo_end; fw_photo_end: dm(FW_TK, i2) = r11; /********************************************************/ /* part 7 : read hardware status and update */ /* status registers to high level software */ /* OBT missing and other timing FPGA status bits */ /* test SYNC flag and signal to high level */ /* + trajectory generators */ /* note registers usage : */ /* r2 = MIM hardware status register */ /* r4 = copy of TIMING FPGA status bits */ /* r5 = copy of Isr_Stat ( sync status bits ) */ /* r6 = period counter calculation */ /********************************************************/ r4 = dm(TIMING_STATUS); /* TIMING FPGA status */ r5 = fext r2 by 0:16; /* take 16 lsb of MIM ( hardware ) status */ r5 = r5 or fdep r4 by 16:16; /* 16 msb of result are 16 lsb of TIMING FPGA status */ dm(STATUS_WORD, i6) = r5; /* write hardware status register for high level */ r6 = dm(TIMING_PERIOD); /* period measurement */ dm (PERIOD_COUNT, i6) = r6; r6 = dm(ISR_STAT, i6); /* load sync status register */ r6 = bclr r6 by K_BPOS_MIM_SYNC_STATUS; /* clear sync status */ r6 = bclr r6 by K_BPOS_MIM_SHIFTED_SYNC_FLAG; /* clear shifted sync status */ btst r4 by K_BPOS_TIMING_SYNC_SRC_SEL; /* test if the unshifted sync is present */ if sz jump(test_shifted_sync); got_direct_sync: r6 = bset r6 by K_BPOS_MIM_SYNC_STATUS; r5 = dm(SYNC_COUNT, i6); r5 = r5 + 1; /* increment sync counter */ dm(SYNC_COUNT, i6) = r5; r5 = dm(TIMING_RES_LO); /* save PLL status */ dm(_gMeasuredTimingResidueLo) = r5; r5 = dm(TIMING_RES_HI); r0 = 0xFFFF; r5 = r5 and r0; dm(_gMeasuredTimingResidueHi) = r5; test_shifted_sync: btst r4 by K_BPOS_TIMING_SYNC_PRESENT; /* test if the shifted sync is present */ if sz jump(end_sync); got_shifted_sync: r6 = bset r6 by K_BPOS_MIM_SHIFTED_SYNC_FLAG; r5 = dm(READOUT_COUNT, i6); r5 = r5 + 1; /* increment readout count in chopper position */ dm(READOUT_COUNT, i6) = r5; end_sync: dm(ISR_STAT, i6) = r6; /* write sync status register for high level */ /* following code should not alter r6 */ /********************************************************/ /* chopper trajectory generator */ /* first look whether we have to sync */ /* ( sync detected is status bit 0 in r6 ) */ /********************************************************/ f0 = dm(CH_F_SINE_STARTSET, i3); /* start setpoint */ f1 = dm(CH_F_SINE_INCR, i3); /* load sine transition increment */ f3 = dm(CH_F_SINE_AMP, i3); /* load sine transition amplitude */ f8 = dm(CH_F_SINE_CURPOS, i3); /* load sine transition current position in sine array */ r2 = dm(TG, i3); /* load target */ r14 = dm(SW, i3); /* load chopper status word */ r15 = dm(CW, i3); /* load chopper command word */ /* check status for PID disabled */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; if not sz jump (chopper_enabled); chopper_abort: /* clear "move" bits in r14 and r15 */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; jump (chopper_finished); /* exit, storing r14 and r15 */ chopper_enabled: /* check status for move in progress */ btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* moving up flag */ if not sz jump (chopper_move_up); btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* moving down flag */ if not sz jump (chopper_move_down); /* no current move, check command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* move up flag */ if not sz jump ( chopper_move); btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* move down flag */ if sz jump ( chopper_no_move); chopper_move: r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* reset status bit */ btst r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; if sz jump (chopper_no_sync); /* if chopper must move but does not need sync */ btst r6 by K_BPOS_MIM_SHIFTED_SYNC_FLAG; if sz jump (chopper_no_move); /* if needs sync but no sync then wait for sync */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* say sync found */ chopper_no_sync: /* start moving, reset readout count in chopper position */ r5 = r5-r5; dm(READOUT_COUNT, i6) = r5; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; /* reset sync bit in command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* which direction ? */ if sz jump (chopper_move_down); chopper_move_up: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* set move up flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* and clear in command word */ i7=_gFloatSinArray; f8 = f8 + f1; /* increment current position in sine array */ dm(CH_F_SINE_CURPOS, i3) = f8; /* and save it */ r8 = fix f8; m7 = r8; f9 = dm(m7, i7); /* f9 = sin (current position) - 1.0f */ f3 = f3 * f9; f0 = f0 + f3; /* setpoint = start setpoint + amplitude * sinus */ r0 = fix f0; r3 = NB_STEPS_IN_PHASE/2 + NB_STEPS_IN_PHASE/4; comp (r8, r3); if lt jump (chopper_still_moving); /* if we have not reached the end of the sinus, continue */ dm(SP, i3) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* reset move up flag in status word */ jump (chopper_finished); chopper_move_down: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* set move down flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* and clear in command word */ i7=_gFloatSinArray; f8 = f8 + f1; /* increment current position in sine array */ dm(CH_F_SINE_CURPOS, i3) = f8; /* and save it */ r8 = fix f8; m7 = r8; f9 = dm(m7, i7); /* f9 = sin (current position) - 1.0f */ f3 = f3 * f9; f0 = f0 + f3; /* setpoint = start setpoint + amplitude * sinus */ r0 = fix f0; r3 = NB_STEPS_IN_PHASE/2 + NB_STEPS_IN_PHASE/4; comp (r8, r3); if lt jump (chopper_still_moving); /* if we have not reached the end of the sinus, continue */ dm(SP, i3) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* reset move down flag in status word */ jump(chopper_finished); chopper_still_moving: dm(SP, i3) = r0; /* set setpoint to incremented setpoint */ chopper_finished: dm(CW, i3) = r15; /* save chopper command word */ dm(SW, i3) = r14; /* save chopper status word */ chopper_no_move: /********************************************************/ /* grating trajectory generator */ /* first look whether we have to sync */ /********************************************************/ r0 = dm(SP, i0); /* setpoint */ r1 = dm(RT, i0); /* rate */ r2 = dm(TG, i0); /* target */ r14 = dm(SW, i0); /* load grating status word */ r15 = dm(CW, i0); /* load grating command word */ /* check status for PID disabled */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; if not sz jump (grating_enabled); grating_abort: /* clear "move" bits in r14 and r15 */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; jump (grating_finished); /* exit, storing r14 and r15 */ grating_enabled: /* check status for move in progress */ btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* moving up flag */ if not sz jump (grating_move_up); btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* moving down flag */ if not sz jump (grating_move_down); /* no current move, check command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* move up flag */ if not sz jump ( grating_move); btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* move down flag */ if sz jump ( grating_no_move); grating_move: r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* reset status bit */ btst r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; if sz jump (grating_no_sync); /* if grating does not need sync */ btst r6 by K_BPOS_MIM_SHIFTED_SYNC_FLAG; if sz jump (grating_no_move); /* if needs sync but no sync */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* say sync found */ grating_no_sync: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; /* reset sync bit in command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* which direction ? */ if sz jump (grating_move_down); grating_move_up: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* set move up flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* and clear in command word */ r0 = r0 + r1; /* increment setpoint */ comp (r0, r2); /* over target */ if lt jump (grating_still_moving); /* setpoint less than target */ dm(SP, i0) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* reset move up flag in status word */ jump (grating_finished); grating_move_down: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* set move down flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* and clear in command word */ r0 = r0 - r1; /* decrement setpoint */ comp (r2, r0); /* over target */ if lt jump (grating_still_moving); /* setpoint less than target */ dm(SP, i0) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* reset move down flag in status word */ jump (grating_finished); grating_still_moving: dm(SP, i0) = r0; /* set setpoint to incremented setpoint */ grating_finished: dm(CW, i0) = r15; /* save grating command word */ dm(SW, i0) = r14; /* save grating status word */ grating_no_move: /*********************************************************************/ /* TEMPERATURE SENSORS BLOCK */ /*********************************************************************/ r5 = dm(TIMING_MUX); r4 = FEXT r5 BY K_BPOS_CS_MUXADR : K_BLEN_CS_MUXADR; /*The TempSensors task must be executed only at 2 Hz, at the beginning of a measure cycle (driven by FPGA) */ /*the mux address is equal to 1 every 500ms, we test it to generate the 2 Hz*/ /*If the counter is 1, we must trigger the tempSensors task*/ r5 = 1; comp(r4, r5); if EQ jump temp_sensors_signal; /*********************************************************************/ /* CALIBRATION SOURCE BLOCK */ /*********************************************************************/ /*The CS1 task must be executed only at 2 Hz, at the beginning of a measure cycle (driven by FPGA) */ /*the mux address is equal to zero every 500ms, we test it to generate the 2 Hz*/ /*If the counter is 2, this is the beginning of a measure cycle for CS1*/ r5 = 2; comp(r4, r5); if EQ jump cs1_signal; /*If the counter is 3, this is the beginning of a measure cycle for CS2*/ r5 = 3; comp(r4, r5); if EQ jump cs2_signal; jump exit_chop_grat; /* no semaphore to signal, exit the ISR */ cs1_signal: bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the CS Controller task*/ i4 = dm(_K_ArgsP); r2 = _gSemaCS1; PRHI_STACK_PSH; jump exit_chop_grat_final; cs2_signal: bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the CS Controller task*/ i4 = dm(_K_ArgsP); r2 = _gSemaCS2; PRHI_STACK_PSH; jump exit_chop_grat_final; temp_sensors_signal: bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the TempSensors task*/ i4 = dm(_K_ArgsP); r2 = _gSemaTempSensors; PRHI_STACK_PSH; jump exit_chop_grat_final; /**********************************************/ /* epilogue : signal end, restore context */ /**********************************************/ exit_chop_grat: bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*resotre primary registers*/ nop; /* nop mandatory after a set system register */ exit_chop_grat_final: bit clr MODE2 CAFRZ; /* free cache before return */ nop; /* nop mandatory after a set system register */ ENDISR1; .endseg;