//=============================================================================
//--------------------------------------------------------------------
void TestOutput()
{
DWORD t=0;
//if(Fsu22.Timer_Test<10) return;
Fsu22.Timer_Test=10;
STRB1=0;
D_STRB1=1;
Sleep_1_5mk();
__DI();
Fsu22.Test.bits.O1= ~(K1 | K2);
Fsu22.Test.bits.O2= ~(K3 | K4);
Fsu22.Test.bits.O3= ~(K5 | K6);
__EI();
D_STRB1=0;
STRB1=1;
STRB2=0;
D_STRB2=1;
Sleep_1_5mk();
__DI();
Fsu22.Test.bits.O4= ~(K1 | K2);
Fsu22.Test.bits.O5= ~(K3 | K4);
Fsu22.Test.bits.O6= ~(K5 | K6);
__EI();
D_STRB2=0;
STRB2=1;
D_STRB3=1;
STRB3=0;
Sleep_1_5mk();
__DI();
Fsu22.Test.bits.O7= ~(K1 | K2);
Fsu22.Test.bits.O8= ~(K3 | K4);
Fsu22.Test.bits.O9= ~(K5 | K6);
__EI();
D_STRB3=0;
STRB3=1;
D_STRB4=1;
STRB4=0;
Sleep_1_5mk();
__DI();
Fsu22.Test.bits.O10= ~(K1 | K2);
Fsu22.Test.bits.O11= ~(K3 | K4);
Fsu22.Test.bits.O12= ~(K5 | K6);
__EI();
D_STRB4=0;
STRB4=1;
}
void Operate_Max1329_RegMode(BYTE Num, BYTE RW, BYTE Adr, BYTE Count)
{
BYTE i;
if(Num == 0xFF) SelectAll_DAC();
else SelectSPI_DAC(Num);
__DI();
pak_spi_rm.data = 0;
pak_spi_rm.bits.RW = RW;
pak_spi_rm.bits.adr = Adr;
Send_SIO(pak_spi_rm.data);
if(RW == READ_MODE_SPI)
{
for(i=0;i<Count;i++)
Spi_Buf_In[i] = Send_SIO(0);
stop_sio();
}else
{
for(i=0;i<Count;i++)
Send_SIO(Spi_Buf_Out[i]);
stop_sio();
}
__EI();
UnSelectAll_DAC();
}
//===================================================================
void Driver_NDD20P1()
{
BYTE i;
i=ADDR;
Ndd.Info.bits.Addr =i;
if(i!=ADDR_NODE)
{
setState(Initialisation);
ADDR_NODE =ADDR;
}
__DI();
Ndd.Info.bits.Mode =MODE;
if(Ndd.PowerNew != Ndd.PowerOld)
{
for(i=0;i<4;i++)
{
if(digit(Ndd.PowerOld,i) != digit(Ndd.PowerNew,i))
{
Ndd.St1[i] =1;
Ndd.Timer2[i] =0;
Ndd.CurStep[i] =0;
}
}
Ndd.PowerOld = Ndd.PowerNew;
}
//=========================================
for(i=0;i<4;i++)
UpdateDataSector(i);
//=========================================
__EI();
}
/*---------------------------------------------------------------------------
DefaultIRQHandler()
This function is a placeholder for all vector definitions. Either use
your own placeholder or add necessary code here.
-----------------------------------------------------------------------------*/
__interrupt void DefaultIRQHandler( void )
{
__DI(); /* disable interrupts */
while( 1 )
{
__wait_nop(); /* halt system */
}
}
// р рывани Reload Timer 1
__interrupt void ReloadTimer1(void)
{
__DI();
TMCSR1_UF = 0; /* reset underflow interrupt request flag */
//receiveMsgHandler(0);
//receiveMsgHandler(1);
__EI();
}
/*------------------------------------------------------------------------
DefaultIRQHandler()
This function is a placeholder for all vector definitions. Either use
your own placeholder or add necessary code here.
-------------------------------------------------------------------------*/
__interrupt
void DefaultIRQHandler (void)
{
/* RB_SYNC; */ /* Synchronisation with R-Bus */
/* May be required, if there is */
/* no R-Bus access after the */
/* reset of the interrupt flag */
__DI(); /* disable interrupts */
while(1) {
Kick_Watchdog(); /* feed hardware watchdog */
}
/* halt system */
}
//удаление таймера
void del_timer(TYPE_DATA_TIMER * tmr)
{
BYTE cnt;
cnt = COUNT_USER_TIMERS;
while (cnt--)
{
if (SystemTimers.timers[cnt]==tmr) // таймер найден
{
__DI();
SystemTimers.timers[cnt] = 0;
SystemTimers.count--;
__EI();
return;
}
}
}
void Operate_Max1329_ADC_Convert(BYTE Num, BYTE Mux, BYTE Gain, BYTE Bip)
{
if(Num == 0xFF) SelectAll_DAC();
else SelectSPI_DAC(Num);
__DI();
pak_spi_adc.bits.Mux = Mux;
pak_spi_adc.bits.Gain = Gain;
pak_spi_adc.bits.Bip = Bip;
pak_spi_adc.bits.start = 1;
Send_SIO(pak_spi_adc.data);
stop_sio();
__EI();
UnSelectAll_DAC();
}
void Operate_Max1329_Reset(BYTE Num)
{
if(Num == 0xFF) SelectAll_DAC();
else SelectSPI_DAC(Num);
__DI();
pak_spi_rm.data = 0;
pak_spi_rm.bits.RW = WRITE_MODE_SPI;
pak_spi_rm.bits.adr = 0x1F;
Send_SIO(pak_spi_rm.data);
Send_SIO(0xFF);
stop_sio();
__EI();
UnSelectAll_DAC();
}
//========================================================================
//========================================================================
void Operate_Max1329_DAC_Write(BYTE Num, BYTE ch, WORD data)
{
if(Num == 0xFF) SelectAll_DAC();
else SelectSPI_DAC(Num);
__DI();
pak_spi_dac.bits.ch = ch;
pak_spi_dac.bits.d1 = 1;
pak_spi_dac.bits.start = 0;
pak_spi_dac.bits.dac = data;
pak_spi_dac.bits.rw = WRITE_MODE_SPI;
Send_SIO(pak_spi_dac.data[1]);
Send_SIO(pak_spi_dac.data[0]);
stop_sio();
__EI();
UnSelectAll_DAC();
}
void Operate_Max1329_DAC_Write_Mask(WORD Mask, BYTE ch, WORD data)
{
SelectMask_DAC(Mask);
__DI();
pak_spi_dac.bits.ch = ch;
pak_spi_dac.bits.d1 = 1;
pak_spi_dac.bits.start = 0;
pak_spi_dac.bits.dac = data;
pak_spi_dac.bits.rw = WRITE_MODE_SPI;
Send_SIO(pak_spi_dac.data[1]);
Send_SIO(pak_spi_dac.data[0]);
stop_sio();
__EI();
UnSelectAll_DAC();
}
//===================================================================================
void DriverRVV()
{
__DI();
Rvv.Error&=0x7F;
Rvv.Error|=RIN2<<7;
__EI();
if(Rvv.newDn!=Rvv.Dn)
{
Rvv.Dn=Rvv.newDn;
WriteBlock(5,Rvv.Dn);
WriteBlock(6,Rvv.Dn>>4);
if((ReadBlock(5)!=(Rvv.Dn&0xF)) && ((ReadBlock(6)!=((Rvv.Dn>>4)&0xF))))
{
Rvv.Error|=0x1;
WriteBlock(6,0x8|(Rvv.Dn>>4));
}
else Rvv.Error&=0xFE;
BYTE uart0_get (void)
{
BYTE d;
int i;
do;
while (!RxFifo.ct);
i = RxFifo.ri;
d = RxFifo.buff[i++];
RxFifo.ri = i % BUFFER_SIZE;
__DI();
RxFifo.ct--;
__EI();
return d;
}
void uart0_put (BYTE d)
{
int i;
do;
while (TxFifo.ct >= BUFFER_SIZE);
i = TxFifo.wi;
TxFifo.buff[i++] = d;
TxFifo.wi = i % BUFFER_SIZE;
__DI();
TxFifo.ct++;
if (!TxRun) {
TxRun = 1;
UA0TIF = 1;
}
__EI();
}
/* ===================================================================*/
void UserInit(void)
{
UserDataInit(); /* Initialize user data. */
#if DEBUG
printf("| -UserDataInit finished.\n");
printf("|-+OnChipInit begins...\n");
#endif
__DI();
OnChipInit(); /* Initialize on-chip devices. */
__EI();
#if DEBUG
printf("| -OnChipInit finished.\n");
printf("|-+PeripheralInit begins...\n");
#endif
PeripheralInit(); /* Initialize peripheral devices. */
#if DEBUG
printf("| -PeripheralInit finished.\n");
#endif
}
//=============================================================================
void Driver_FSU22()
{
BYTE i;
__DI();
i=ADDR;
Fsu22.Info.bits.Addr =i;
if(i!=ADDR_NODE)
{
setState(Initialisation);
ADDR_NODE =ADDR;
}
Fsu22.Info.bits.Mode=MODE;
//**************************************************
Fsu22.Info.bits.F1=PG_F1;
//**************************************************
if(Fsu22.Prin.word!=Fsu22.State.word) //Устанавливаем новые команды
{
Fsu22.State.word=Fsu22.Prin.word;
S1=Fsu22.State.bits.O1;
S2=Fsu22.State.bits.O2;
S3=Fsu22.State.bits.O3;
S4=Fsu22.State.bits.O4;
S5=Fsu22.State.bits.O5;
S6=Fsu22.State.bits.O6;
S7=Fsu22.State.bits.O7;
S8=Fsu22.State.bits.O8;
S9=Fsu22.State.bits.O9;
S10=Fsu22.State.bits.O10;
S11=Fsu22.State.bits.O11;
S12=Fsu22.State.bits.O12;
Fsu22.Timer_Test=0;
}
__EI();
//**************************************************
TestOutput();
//**************************************************
}
/*
** ===================================================================
** Method : PE_low_level_init (bean MC9S12DP256_112)
**
** Description :
** This method is internal. It is used by Processor Expert
** only.
** ===================================================================
*/
void PE_low_level_init(void)
{
/* Common initialization of the CPU registers */
/* TSCR1: TEN=0,TSWAI=0,TSFRZ=1 */
output( TSCR1, input( TSCR1 ) & ~192 | 32 );
/* TCTL2: OM0=0,OL0=0 */
output( TCTL2, input( TCTL2 ) & ~3 );
/* TCTL1: OM7=0,OL7=0 */
output( TCTL1, input( TCTL1 ) & ~192 );
/* TIE: C0I=0 */
output( TIE, input( TIE ) & ~1 );
/* TTOV: TOV0=0 */
output( TTOV, input( TTOV ) & ~1 );
/* TSCR2: TOI=0,TCRE=1 */
output( TSCR2, input( TSCR2 ) & ~128 | 8 );
/* TIOS: IOS7=1,IOS0=1 */
output( TIOS, input( TIOS ) | 129 );
/* PWMCTL: PSWAI=0,PFRZ=0 */
output( PWMCTL, input( PWMCTL ) & ~12 );
/* PWMSDN: PWMIF=0,PWMIE=0,PWMRSTRT=0,PWMLVL=0,??=0,PWM7IN=0,PWM7INL=0,PWM7ENA=0 */
output( PWMSDN, 0 );
/* ICSYS: SH37=0,SH26=0,SH15=0,SH04=0,TFMOD=0,PACMX=0,BUFEN=0,LATQ=0 */
output( ICSYS, 0 );
/* MCCTL: MODMC=1 */
output( MCCTL, input( MCCTL ) | 64 );
/* ### MC9S12DP256_112 "Cpu" init code ... */
/* ### TimerInt "TickTimer" init code ... */
TickTimer_Init();
/* ### ByteIO "Byte1" init code ... */
PORTB = 255; /* Prepare value for output */
DDRB = 255; /* Set direction to output */
/* ### Asynchro serial "COM0" init code ... */
DDRS &= ~1;
PTS |= 2;
DDRS |= 2;
COM0_Init();
/* Common peripheral initialization - ENABLE */
/* TSCR1: TEN=1 */
output( TSCR1, input( TSCR1 ) | 128 );
INTCR_IRQEN = 0; /* Disable the IRQ interrupt. IRQ interrupt is enabled after CPU reset by default. */
__DI(); /* Disable interrupts */
}
//---------------------------------------------------------------------------------------------------------
// Процедура подавление дребезга входов DIn
// Вызывается для каждого модуля НДД
//addr - адрес модуля
//inputs - считанное состояние НДД
//time - Время подавления дребезга
void debounce_Din(unsigned char addr, unsigned char inputs, unsigned short time)
{
unsigned char i; // счетчик
unsigned char ch_cnt; // колличество входов у данного
unsigned char pr,tmp,bit; // временные переменные
__DI();
pr=Ndd.pr_data[addr];
tmp = inputs ^ pr; // в tmp - изменения на входах
if (time==0) time=1;
for (i=0;i<8;i++)
{
bit = 1<<i; // определение текущего входа
if (tmp & bit)
{ // состояние входа изменилось - сброс счетчика
Ndd.counters[addr][i]=0;
Ndd.counters_dr[addr][i]++;
if(Ndd.counters_dr[addr][i]>=time) //Входной сигнал не устанавливается в устойчивое состояние
{
Ndd.counters_dr[addr][i]=time;
Ndd.drebezg[addr]&=~bit; //
Ndd.drebezg[addr]|=bit; //Запись сигнала у которого дребезг
}
else Ndd.drebezg[addr]&=~bit;
}
else
{ // состояние входа не изменилось - инкремент
Ndd.counters_dr[addr][i]=0;
Ndd.counters[addr][i]++; // счетчика
if (Ndd.counters[addr][i]>=time-1)
{ // состояние входа установилось
Ndd.state[addr]&=~bit; // запись состояния входа
Ndd.state[addr]|=(inputs & bit);
Ndd.counters[addr][i]--; // декремент счетчика
}
}
}
Ndd.pr_data[addr] = inputs; // сохранение текущих значений как предыдущих
__EI();
}
void Operate_Max1329_RegMode_Mask(WORD Mask, BYTE Adr, BYTE Count)// только запись
{
BYTE i;
SelectMask_DAC(Mask);
__DI();
pak_spi_rm.data = 0;
pak_spi_rm.bits.RW = WRITE_MODE_SPI;;
pak_spi_rm.bits.adr = Adr;
Send_SIO(pak_spi_rm.data);
for(i=0;i<Count;i++)
Send_SIO(Spi_Buf_Out[i]);
stop_sio();
__EI();
UnSelectAll_DAC();
}
/*
** ===================================================================
** Method : PE_low_level_init (bean MC9S12C32_80)
**
** Description :
** This method is internal. It is used by Processor Expert
** only.
** ===================================================================
*/
void PE_low_level_init(void)
{
/* Common initialization of the CPU registers */
/* TSCR1: TEN=0,TSWAI=0,TSFRZ=1 */
output( TSCR1, input( TSCR1 ) & ~192 | 32 );
/* TCTL2: OM0=0,OL0=0 */
output( TCTL2, input( TCTL2 ) & ~3 );
/* TCTL1: OM7=0,OL7=0 */
output( TCTL1, input( TCTL1 ) & ~192 );
/* TIE: C0I=0 */
output( TIE, input( TIE ) & ~1 );
/* TTOV: TOV0=0 */
output( TTOV, input( TTOV ) & ~1 );
/* TSCR2: TOI=0,TCRE=1 */
output( TSCR2, input( TSCR2 ) & ~128 | 8 );
/* TIOS: IOS7=1,IOS0=1 */
output( TIOS, input( TIOS ) | 129 );
/* PPSP: PPSP0=0 */
output( PPSP, input( PPSP ) & ~1 );
/* PERP: PERP0=1 */
output( PERP, input( PERP ) | 1 );
/* DDRP: DDRP0=0 */
output( DDRP, input( DDRP ) & ~1 );
/* PWMCTL: PSWAI=0,PFRZ=0 */
output( PWMCTL, input( PWMCTL ) & ~12 );
/* PWMSDN: PWMIF=0,PWMIE=0,PWMRSTRT=0,PWMLVL=0,??=0,PWM7IN=0,PWM7INL=0,PWM7ENA=0 */
output( PWMSDN, 0 );
/* ### MC9S12C32_80 "Cpu" init code ... */
/* ### ByteIO "Byte1" init code ... */
PORTB = 0; /* Prepare value for output */
DDRB = 255; /* Set direction to output */
/* ### TimerInt "TickTimer" init code ... */
TickTimer_Init();
/* ### External interrupt "ButtonInterrupt" init code ... */
PIEP_PIEP0 = 0; /* Disable interrupt */
/* Common peripheral initialization - ENABLE */
/* TSCR1: TEN=1 */
output( TSCR1, input( TSCR1 ) | 128 );
INTCR_IRQEN = 0; /* Disable the IRQ interrupt. IRQ interrupt is enabled after CPU reset by default. */
__DI(); /* Disable interrupts */
}
WORD Operate_Max1329_DAC_Read(BYTE Num, BYTE ch)
{
WORD d;
if(Num == 0xFF) SelectAll_DAC();
else SelectSPI_DAC(Num);
__DI();
pak_spi_dac.bits.ch = ch;
pak_spi_dac.bits.d1 = 1;
pak_spi_dac.bits.start = 0;
pak_spi_dac.bits.dac = 0;
pak_spi_dac.bits.rw = READ_MODE_SPI;
d=Send_SIO(pak_spi_dac.data[1]);
d = (d&0xF)<<8;
d |= Send_SIO(pak_spi_dac.data[0]);
stop_sio();
__EI();
UnSelectAll_DAC();
return d;
}
//регистраци¤ пользовательского таймера в системе
//¬ход: указатель на таймер usrtmr
//
void add_timer(TYPE_DATA_TIMER * tmr)
{
BYTE cnt;
cnt = COUNT_USER_TIMERS;
while (cnt--)
{ //данный таймер уже зарегистрирован
if (SystemTimers.timers[cnt]==tmr) return; //выход
}
cnt = COUNT_USER_TIMERS;
while (cnt--)
{
if (SystemTimers.timers[cnt]==0) //место свободно
{
__DI();
SystemTimers.timers[cnt] = tmr;
SystemTimers.count++;
__EI();
return;
}
}
}
//================================================
void DriverFSU15_C()
{
BYTE i;
__DI();
i=ADDR;
Fsu15.Info.bits.Addr =i;
if(i!=ADDR_NODE)
{
setState(Initialisation);
ADDR_NODE =ADDR;
}
Fsu15.Info.bits.Mode=MODE;
Fsu15.Info.bits.F1=PG_F1;
#ifdef PLATA_FSU15
Fsu15.Info.bits.F2=PG_F2;
#endif
WriteOutput();
ReadTest();
__EI();
}
//********************************************************************************
// local functions
//********************************************************************************
void init(void)
{
/* --- MIE Disable --- */
__DI();
/* Set oscillation mode */
clk_setLsclk( CLK_XTM_CRYSTAL ); /* crystal/ceramic oscillation */
/* Wait stables crystal oscillation */
while( (clk_getClkStatus() & FSTAT_LOSCS) != 0 ) {}
clk_disHsclk(); // stop HSCLK oscillation
clk_setHsclk( (unsigned char)(CLK_SYSC_OSCLK | CLK_OSCM_RC | CLK_OUTC_OSCLK | CLK_LOSCON_DIS), CLK_LOSCON_DIS ); /* set HSCLK */ // BUG620Q504
clk_enaHsclk(); // start HSCLK oscillation
clk_setSysclk( CLK_SYSCLK_HSCLK ); // Choose HSCLK
clk_block_ctrl_init(); /* Block Control */
// irq handler initialization
irq_init(); /* Interrupt */
// Watch dog timer
irq_sethandler(IRQ_NO_WDTINT,watch_dog_isr);
wdt_init( WDTMOD_WDT1 | WDTMOD_WDT0);
wdt_disHaltCount();
// Initializing GPIO
lazurite_gpio_init();
// Initializing timer
init_timer();
/* MIE Enable */
rst_interrupts();
__EI();
return;
}
/*******************************************************************************
Routine Name: irq_di
Form: void irq_di( void )
Parameters: void
Return value: void
Description: DI.
******************************************************************************/
void irq_di( void )
{
__DI();
}
UNS8 buildPDO_CASH(UNS16 index)
{
UNS16 ind;
UNS8 subInd,i;
UNS8 * pMappingCount = NULL; // count of mapped objects...
void * pMappedAppObject = NULL; // pointer to the var which is mapped to a pdo
UNS32 * pMappingParameter = NULL; // pointer fo the var which holds the mapping parameter of an mapping entry
UNS8 * pSize;
UNS8 size;
UNS8 offset;
UNS8 status;
UNS32 objDict;
pSize = &size;
status = state1;
subInd=(UNS8)0x00;
offset = 0x00;
ind = index - 0x1800;
if( nodeState != Operational ) return 0xFF;
while (1)
{
if(status == state1)
{
status = state2;
}
if(status == state2)
{
objDict = getODentry( (UNS16)0x1A00 + ind, (UNS8)0,(void * *)&pMappingCount, pSize, 0 );
if( objDict == (UNS32)OD_SUCCESSFUL )
{
status = state3;
if(*pMappingCount==0)
return 0xFF;
}
else
return 0xFF;
}
if(status == state3)
{
objDict = getODentry( (UNS16)0x1A00 + ind, subInd + 1,(void * *)&pMappingParameter, pSize, 0 );
if( objDict == OD_SUCCESSFUL )
{
status = state4;
}
else
return 0xFF;
}
if(status == state4)
{
objDict = getODentry((UNS16)((*pMappingParameter) >> 16),
(UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
(void * *)&pMappedAppObject, pSize, 0 );
if( objDict == OD_SUCCESSFUL )
{
size = (*pMappingParameter & (UNS32)0x000000FF) >> 3 ; // in bytes
__DI();
for(i=0;i<size;i++)
process_PDO_CASH.msg[offset+i]=((UNS8 *)pMappedAppObject)[i];
__EI();
offset += size;
process_PDO_CASH.count = offset;
subInd++;
status = state5;
}
else
return 0xFF;
/*
** ===================================================================
** Method : PE_low_level_init (component MKE06Z128LK4)
**
** Description :
** Initializes components and provides common register
** initialization. The method is called automatically as a part
** of the application initialization code.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void PE_low_level_init(void)
{
/* RTOS initialization */
#ifdef PEX_RTOS_INIT
PEX_RTOS_INIT(); /* Initialization of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/* Debug pins port clock gate enable */
#if CPU_SWD_DIO_PIN | CPU_SWD_CLK_PIN
SIM_SCGC |= (uint32_t)SIM_SCGC_SWD_MASK; /* SWD pins clock gate enable */
#endif /* CPU_SWD_DIO_PIN | CPU_SWD_CLK_PIN */
/* NMI pin initialization */
#if CPU_NMI_PIN
SIM_SOPT0 |= (uint32_t)SIM_SOPT0_NMIE_MASK; /* Enable NMI pin */
#else
SIM_SOPT0 &= (uint32_t)~(uint32_t)SIM_SOPT0_NMIE_MASK; /* Disable NMI pin */
#endif /* CPU_NMI_PIN */
/* Reset pin initialization */
#if CPU_RESET_PIN
SIM_SOPT0 |= (uint32_t)SIM_SOPT0_RSTPE_MASK; /* Enable Reset pin */
#else
SIM_SOPT0 &= (uint32_t)~(uint32_t)SIM_SOPT0_RSTPE_MASK; /* Disable Reset pin */
#endif /* CPU_RESET_PIN */
/* SWD pins initialization */
#if CPU_SWD_DIO_PIN | CPU_SWD_CLK_PIN
SIM_SOPT0 |= (uint32_t)SIM_SOPT0_SWDE_MASK; /* Enable SWD pins */
#else
SIM_SOPT0 &= (uint32_t)~(uint32_t)SIM_SOPT0_SWDE_MASK; /* Disable SWD pins */
#endif /* CPU_SWD_DIO_PIN | CPU_SWD_CLK_PIN */
/* Common initialization */
#if CPU_COMMON_INIT
Common_Init();
#endif /* CPU_COMMON_INIT */
/* Peripheral initialization components initialization */
#if CPU_PERIPHERALS_INIT
Peripherals_Init();
#endif /* CPU_PERIPHERALS_INIT */
/* OnReset event */
#ifdef CPU_ON_RESET_EVENT_NAME
CPU_ON_RESET_EVENT_NAME((uint16_t)SIM_SRSID); /* Invoke an user event */
#endif /* CPU_ON_RESET_EVENT_NAME */
/* Rest of components initialization */
#if CPU_COMPONENTS_INIT
Components_Init();
#endif /* CPU_COMPONENTS_INIT */
/* Interrupts priority level initialization */
#if CPU_INT_PRIORITY == 0
__DI(); /* Disable interrupts */
#elif CPU_INT_PRIORITY == 1
__EI(); /* Enable interrupts */
#else
#error Unsupported interrupt priority level
#endif
}
void CPU_DisableInt(void)
{
__DI();
}
/***********************************************************************
This is the main controller loop.
sequences of operations:
- reads from CAN bus or serial port 1.
- reads encoders (or ADC).
- computes the control value (a PID in this version).
- checks limits and other errors.
- sets PWM
- does extra functions (e.g. communicate with neighboring cards).
***********************************************************************/
void main(void)
{
Int32 PWMoutput [JN];
Int32 PWMoutput_old [JN];
byte i=0;
byte wi=0;
byte k=0;
UInt16 *value=0;
Int32 t1val=0;
Int32 PID_R= 2;
Int32 kpp=1;
Int16 current_turn=0;
Int16 print_number=0;
Int16 real_pos=0;
byte first_step=0;
#if (VERSION == 0x0351)
#define winSizeMax 32
#define initialWindowSize 4
#else
#define winSizeMax 32
#define initialWindowSize 30
#endif
byte divJntPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divJntVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte headJntPos[JN]=INIT_ARRAY(0); //current joint pos
byte tailJntPos[JN]=INIT_ARRAY(0);
byte headJntVel[JN]=INIT_ARRAY(0); //current joint vel
byte tailJntVel[JN]=INIT_ARRAY(0);
byte headMotPos[JN]=INIT_ARRAY(0); //current motor pos
byte tailMotPos[JN]=INIT_ARRAY(0);
byte headMotVel[JN]=INIT_ARRAY(0); //current motor vel
byte tailMotVel[JN]=INIT_ARRAY(0);
Int32 jntPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 jntVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int16 _safeband[JN]; //it is a value for reducing the JOINT limit of 2*_safeband [tick encoder]
#ifdef TEMPERATURE_SENSOR
byte TempSensCount1 = 0;
UInt32 TempSensCount2 = 0;
byte temp_sens_status=0;
overtemp[0]=0;
overtemp[1]=0;
errortemp[0]=0;
errortemp[1]=0;
#endif
/* gets the address of flash memory from the linker */
_flash_addr = get_flash_addr();
/* enable interrupts */
setReg(SYS_CNTL, 0);
// IPL channels from 0 to 6 enabled
// external interrupts IRQA and IRQB disabled
setRegBits(IPR, 0xFE00);
// enable FAULT
__ENIGROUP (61, 3);
#if (VERSION == 0x0254)
#else
__ENIGROUP (60, 3);
#endif
// enable SCI
__ENIGROUP (52, 4);
__ENIGROUP (53, 4);
__ENIGROUP (50, 4);
__ENIGROUP (51, 4);
// enable data flash
__ENIGROUP (13, 4);
// enable CAN
__ENIGROUP (14, 6);
__ENIGROUP (15, 6);
__ENIGROUP (16, 6);
__ENIGROUP (17, 6);
// enable ADCA/ADCB
__ENIGROUP (55, 6);
__ENIGROUP (54, 6);
//enable PWM reload
__ENIGROUP (59, 7); // PMWA
#if (VERSION == 0x0254)
#else
__ENIGROUP (58, 7); // PWMB
#endif
// enable timers
// TIMER_A
__ENIGROUP (45, 7); //Timer for the encoder commutation if used
__ENIGROUP (44, 7); //
__ENIGROUP (43, 7); //
__ENIGROUP (42, 4); //TI1 1ms delay main loop
// TIMER_B
__ENIGROUP (41, 7); //
__ENIGROUP (40, 7); //
__ENIGROUP (39, 7); //
__ENIGROUP (38, 7);
// TIMER_C
__ENIGROUP (37, 1);
__ENIGROUP (36, 1);
__ENIGROUP (35, 1);
__ENIGROUP (34, 1);
// TIMER_D
__ENIGROUP (33, 7); //1ms delay duty cycle
__ENIGROUP (32, 1);
__ENIGROUP (31, 1);
__ENIGROUP (30, 1);
__EI();
flash_interface_init (JN);
readFromFlash (_flash_addr);
if (_version==_flash_version)
{
}
else
{
writeToFlash(_flash_addr);
}
__DI();
#warning "debug"// ;
__EI();
init_leds ();
#if (VERSION == 0x0254)
Init_Brushless_Comm (1,HALL);
#else
Init_Brushless_Comm (JN,HALL);
#endif
can_interface_init (JN);
init_strain ();
init_position_abs_ssi ();
#if VERSION ==0x0257
init_relative_position_abs_ssi();
#endif
init_faults (true,true,true);
init_position_encoder ();
TI1_init ();
//variable init
mainLoopOVF=0;
_count=0;
for(i=0;i<JN;i++)
{
_received_pid[i].rec_pid=0;
}
BUS_OFF=false;
#warning "debug"// ;
//__EI();
// print_version ();
/* initialization */
for (i=0; i<JN; i++) _calibrated[i] = false;
/* reset trajectory generation */
for (i=0; i<JN; i++) abort_trajectory (i, 0);
///////////////////////////////////////
// reset of the ABS_SSI
// this is needed because the AS5045 gives the first value wrong !!!
for (i=0; i<JN; i++) _position[i]=(Int32) Filter_Bit(get_position_abs_ssi(i));
for (i=0; i<JN; i++) _max_real_position[i]=Filter_Bit(4095);
//////////////////////////////////////
/* initialize speed and acceleration to zero (useful later on) */
for (i=0; i<JN; i++) _position_old[i] = 0;
for (i=0; i<JN; i++) _speed[i] = 0;
for (i=0; i<JN; i++) _accel[i] = 0;
for (i=0; i<JN; i++) _safeband[i] =5; //5 ticks => 1 grado di AEA.
for (i=0; i<JN; i++) PWMoutput [i] = PWMoutput_old[i] = 0;
/* reset the recursive windows for storage of position and velocity data */
/* (for velocity and position estimates) */
for(i=0;i<JN;i++)
{
for(wi=0;wi<winSizeMax;wi++)
{
jntPosWindow[wi][i]=_position[i];
jntVelWindow[wi][i]=0;
motPosWindow[wi][i]=0;
motVelWindow[wi][i]=0;
}
}
//set_relative_position_abs_ssi(1,get_absolute_real_position_abs_ssi(1));
/* main control loop */
for(_counter = 0;; _counter ++)
{
if (_counter >= CAN_SYNCHRO_STEPS) _counter = 0;
led3_on
while (_wait);
_count=0;
led3_off
// BUS_OFF check
if (getCanBusOffstatus() )
{
#ifdef DEBUG_CAN_MSG
can_printf("DISABLE BUS OFF");
#endif
for (i=0; i<JN; i++) put_motor_in_fault(i);
led1_off
}
else
led1_on
// READING CAN MESSAGES
can_interface();
for (i=0; i<JN; i++)
if (_pad_enabled[i]==false && _control_mode[i]!=MODE_HW_FAULT) _control_mode[i]=MODE_IDLE;
//Position calculation
// This is used to have a shift of the zero-cross out of the
// joint workspace
//
// max_real_position is the limit of the joint starting from
// 4095 and going to decrease this number without zero-cross
// untill the joint limit is reached
#if VERSION == 0x0257
_position_old[0]=_position[0];
if(get_error_abs_ssi(0)==ERR_OK)
_position[0]=Filter_Bit (get_position_abs_ssi(0));
_position_old[1]=_position[1];
if(get_error_abs_ssi(1)==ERR_OK)
_position[1]=Filter_Bit (get_position_abs_ssi(1));
#else
for (i=0; i<JN; i++)
{
_position_old[i]=_position[i];
if(get_error_abs_ssi(i)==ERR_OK)
_position[i]=Filter_Bit (get_position_abs_ssi(i));
}
#endif
// get_commutations() is used to read the incremental encoder of the motors.
// the variable _motor_position is then used to estimate the rotor speed and
// compensate the back-EMF of the motor.
for (i=0; i<JN; i++) _motor_position[i]=get_position_encoder(i);//get_commutations(i);
///////////////////////////////////////////DEBUG////////////
#if (VERSION !=0x0254)
for (i=0; i<JN; i++)
{
if (get_error_abs_ssi(i)==ERR_ABS_SSI)
{
put_motor_in_fault(i);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",i);
#endif
}
}
#endif
#if (VERSION ==0x0254)
if (get_error_abs_ssi(0)==ERR_ABS_SSI)
{
put_motor_in_fault(0);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",0);
#endif
}
#endif
//DO NOTHING
// decoupling the position
decouple_positions();
/* velocity and acceleration estimators */
{
for (i=0; i<JN; i++)
{
//joint velocity estimator
tailJntPos[i]=headJntPos[i]+(winSizeMax-divJntPos[i]); if(tailJntPos[i]>=winSizeMax) tailJntPos[i]=tailJntPos[i]%winSizeMax;
_speed_old[i] = _speed[i];
jntPosWindow[headJntPos[i]][i]=_position[i];
_speed[i] = (Int32) (((jntPosWindow[headJntPos[i]][i] - jntPosWindow[tailJntPos[i]][i] ))<<_jntVel_est_shift[i]);
// _speed[i] <<= _jntVel_est_shift[i];
_speed[i] = (Int32)(_speed[i]) / divJntPos[i];
headJntPos[i]=headJntPos[i]+1; if(headJntPos[i]>=winSizeMax) headJntPos[i]=0;
/*
//joint acceleration estimator
tailJntVel[i]=headJntVel[i]+(winSizeMax-divJntVel[i]); if(tailJntVel[i]>=winSizeMax) tailJntVel[i]=tailJntVel[i]%winSizeMax;
_accel_old[i] = _accel[i];
jntVelWindow[headJntVel[i]][i]=_speed[i];
_accel[i] = ((jntVelWindow[headJntVel[i]][i] - jntVelWindow[tailJntVel[i]][i] ));
_accel[i] << _jntAcc_est_shift[i];
_accel[i] = (Int32)(_accel[i]) / divJntVel[i];
headJntVel[i]=headJntVel[i]+1; if(headJntVel[i]>=winSizeMax) headJntVel[i]=0;
*/
//motor velocity estimator
tailMotPos[i]=headMotPos[i]+(winSizeMax-divMotPos[i]); if(tailMotPos[i]>=winSizeMax) tailMotPos[i]=tailMotPos[i]%winSizeMax;
_motor_speed_old[i] = _motor_speed[i];
motPosWindow[headMotPos[i]][i]=_motor_position[i];
_motor_speed[i] = ((motPosWindow[headMotPos[i]][i] - motPosWindow[tailMotPos[i]][i] ));
_motor_speed[i] <<= _motVel_est_shift[i];
_motor_speed[i] = (_motor_speed[i]) / divMotPos[i];
headMotPos[i]=headMotPos[i]+1; if(headMotPos[i]>=winSizeMax) headMotPos[i]=0;
}
}
/* in position? */
#if (VERSION != 0x0254)
for (i=0; i<JN; i++) _in_position[i] = check_in_position(i);
#else
_in_position[0] = check_in_position(0);
#endif
/* in reference configuration for calibration? */
//for (i=0; i<JN; i++) check_in_position_calib(i);
//******************************************* POSITION LIMIT CHECK ***************************/
for (i=0; i<JN; i++) check_range(i, _safeband[i], PWMoutput);
//******************************************* COMPUTES CONTROLS *****************************/
//FT sensor watchdog update
for (i=0; i<STRAIN_MAX; i++)
if (_strain_wtd[i]>0) _strain_wtd[i]--;
for (i=0; i<JN; i++)
{
//computing the PWM value (PID)
PWMoutput[i] = compute_pwm(i);
// PWM filtering in torque control if there is no bemf compensation
#if (VERSION != 0x0351)
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
if (_useFilter[i] == 3) PWMoutput[i] = lpf_ord1_3hz (PWMoutput[i], i);
}
// saving the PWM value before the decoupling
_bfc_PWMoutput[i] = PWMoutput_old[i] = PWMoutput[i];
// applying the saturation to the PWM
if (_bfc_PWMoutput[i] < -MAX_DUTY) _bfc_PWMoutput[i]=-MAX_DUTY;
else if (_bfc_PWMoutput[i] > MAX_DUTY) _bfc_PWMoutput[i]= MAX_DUTY;
#endif //(VERSION != 0x0351)
}
//decouple PWM
decouple_dutycycle(PWMoutput);
//******************************************* SATURATES CONTROLS ***************************/
/* back emf compensation + controls saturation (if necessary) */
for (i=0; i<JN; i++)
{
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
#if (VERSION != 0x0351)
// Back emf compensation
//PWMoutput[i]+=compensate_bemf(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_bemf(i, _speed[i]); //use the joint speed
//add the coulomb friction compensation term
if (_kstp_torque[i] != 0 ||
_kstn_torque[i] != 0)
//PWMoutput[i]+=compensate_friction(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_friction(i, _speed[i]); //use the joint speed
// Protection for joints out of the admissible range during force control
check_range_torque(i, _safeband[i], PWMoutput);
// PWM saturation
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit_torque[i] );
#else //(VERSION != 0x0351)
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
#endif //(VERSION != 0x0351)
}
else
{
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
}
if (_pid[i] < -MAX_DUTY) _pid[i]=-MAX_DUTY;
else if (_pid[i] > MAX_DUTY) _pid[i]= MAX_DUTY;
}
/* generate PWM */
for (i=0; i<JN; i++)
{
if (!mode_is_idle(i)) {PWM_generate(i,_pid[i]);}
}
/* Check Current done in T1 */
/* do extra functions, communicate, etc. */
//send broadcast data
can_send_broadcast();
//send additional debug information
//can_send_broadcast_debug(1,1);
/***********************************************************************
// Check Current is made here
/***********************************************************************/
#if (VERSION != 0x0254)
for (i=0; i<JN; i++)
#else
for (i=0; i<1; i++)
#endif
{
if ((get_current(i)>=25000) || (get_current(i)<=-25000))
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d curr %f",i,get_current(i));
#endif
}
check_current(i, (_pid[i] > 0));
compute_i2t(i);
if (_filt_current[i] > MAX_I2T_CURRENT)
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d filtcurr %f",i,_filt_current[i]);
#endif
}
}
// Check for the MAIN LOOP duration
// t1val= (UInt16) TI1_getCounter();
if ( _count>0)
{
mainLoopOVF=1;
_count=0;
}
/* tells that the control cycle is completed */
_wait = true;
} /* end for(;;) */
void DriverDAC10(void)
{
BYTE i, bData;
WORD wData;//, tmp;
//int iData;
float fData;
//-------------------------------------
#ifdef READ_TEMP
if(Dac10.TimerTem>=TIME_TEM)
{
Dac10.TimerTem=0;
}
#endif
//-------------------------------------
ControlKoeff();
//-------------------------------------
#ifndef CASH_PDO_DATA
__DI();
#endif
Dac10.Info.bits.ErrSet = Dac10.ErrorSet;
i=ADDR;
Dac10.Info.bits.Addr =i;
if(i!=ADDR_NODE)
{
setState(Initialisation);
ADDR_NODE =ADDR;
}
Dac10.Info.bits.Mode =MODE;
Dac10.Info.bits.F1 =0;
Dac10.Info.bits.F2 =0;
#ifndef CASH_PDO_DATA
__EI();
#endif
Dac10.AddData = DAC_STATUS_NEIGHBOUR|(DAC_ADD_ADDR<<8);
//----------------------------------------------
if(Dac10.TarrStatus==true)
SelectMasterDAC();
//--------------------------------------------------
// Проверка записанного значения в ЦАП с помощью АЦП
#ifndef CASH_PDO_DATA
__DI();
#endif
/*if(Dac10.TimerErrDac>=80)
{
Dac10.TimerErrDac=80;
for(i=0;i<COUNT_DAC_CH;i++)
{
if((Dac10.fDAC_Set[i]>(Dac10.fADC[i]+dERR_DAC_mA)) ||
(Dac10.fDAC_Set[i]<(Dac10.fADC[i]-dERR_DAC_mA)))
{
Dac10.wError|=1<<i;
}else
{
Dac10.wError&=(WORD)(~(1<<i));
}
}
}*/
//----------------------------------------------
if(Dac10.TimerDout >= PeriodDout)
{
Dac10.TimerDout = 0;
Dac10.HiLoDec = true;
Dac10.HiLo = ~Dac10.HiLo;
}
//=======================================================================================
// Запись дискретных выходов
// выбор режима работы цап по току или напряжению
// коммутация на выход
// коммутируем только если мы мастер
//=======================================================================================
//if(Dac10.wError != 0)
if(Dac10.wOldError != Dac10.wError)
{
Dac10.TimerMasterError = 0;
Dac10.ErrorSet = true;
}
wData = Dac10.EnOutDac&(~Dac10.wError);
if((Dac10.Master == true)||(Dac10.TarrStatus==false))
{
if((wData/*Dac10.EnOutDac*/ != 0)&&(Dac10.HiLoDec == true))
{
if(Dac10.HiLo == 0)
Spi_Buf_Out[0]=7;
else
Spi_Buf_Out[0]=3;
Operate_Max1329_RegMode_Mask(wData/*Dac10.EnOutDac*/, 0x13,1);
}
}
Dac10.HiLoDec = false;
//Operate_Max1329_DAC_Write_Mask((~Dac10.EnOutDac), 1, (MAX_COD_DAC/2));
//----------------------------------------------
for(i=0;i<COUNT_MAX1329;i++)
{
//=============================
// Запись в ЦАП по изменениям
//=============================
if(Dac10.fDAC_New[i] != Dac10.fDAC_Set[i])
{
if(Dac10.TarrStatus == false)
{
Dac10.fDAC_Set[i] = Dac10.fDAC_New[i];
WriteNormalDataDac(i, Dac10.fDAC_Set[i]);
}else
{
//if((Dac10.EnOutDac&(((WORD)1)<<i)) != 0)
{
fData = Dac10.fDAC_New[i];
if(fData>MaxValueDac[i]) fData = MaxValueDac[i];
if(fData<MinValueDac[i]) fData = MinValueDac[i];
if(fData != Dac10.fDAC_Set[i])
{
Dac10.fDAC_Set[i] = fData;
WriteNormalDataDac(i, fData);
}
}
}
}else
{
//if(((Dac10.EnOutDac^Dac10.setEnOutDac)&(((WORD)1)<<i)) != 0)
//{
// WriteNormalDataDac(i, Dac10.fDAC_Set[i]);
//}
}
//=============================================================================================
// Опрашиваем дискретные входы
//=============================================================================================
if(Dac10.TimerDin >= PeriodOprosDin)
{
Operate_Max1329_RegMode(i, READ_MODE_SPI, 0x11,1);
bData = Spi_Buf_In[0]&0x0F;
Dac10.DIN[i] = bData;
if((bData&8) == 0) SETBIT(Dac10.DiagRele,i);
else CLEARBIT(Dac10.DiagRele,i);
//if((bData&5) == 5) CLEARBIT(Dac10.wError,i);
//else SETBIT(Dac10.wError,i);
if((bData&5) != 5) SETBIT(Dac10.wError,i);
if(i == (COUNT_MAX1329-1))
Dac10.TimerDin = 0;
}
//===================================================
// Опрашиваем АЦП
//===================================================
/*Operate_Max1329_RegMode(i, READ_MODE_SPI, 2,2);
tmp = Spi_Buf_In[0];
wData = (tmp<<4)&0x0FF0;
wData |= (Spi_Buf_In[1]>>4)&0xF;
iData = wData&0xFFF;
Dac10.fADC[i] = TarrRAM_ADC[i].k*(float)iData + TarrRAM_ADC[i].ofs;
Operate_Max1329_ADC_Convert(i, 0, 0, 0);*/
//===================================================
// Опрашиваем температурные датчики
/*if(Dac10.TimerTemp >= PeriodOprosTemp)
{
Operate_Max1329_RegMode(i,READ_MODE_SPI, 2,2);
tmp = Spi_Buf_In[0];
wData = (tmp<<4)&0x0FF0;
wData |= (Spi_Buf_In[1]>>4)&0xF;
iData = ConvertADCtoINT(wData);
Dac10.Temperature[i] = iData*0.125;
//Operate_Max1329_ADC_Convert(i,0x8, 0, 0);
if(i == (COUNT_MAX1329-1))
Dac10.TimerTemp = 0;
}*/
}
// -------------------------------------------------------------------------
GetPak_Uart(0);
if(Dac10.SendFlagUart==1)
{
Dac10.SendFlagUart=0;
for(i=0;i<COUNT_DIN_CH;i++)
{
SendPak1.DIN[i] =Dac10.DIN[i];
}
for(i=0;i<COUNT_DOUT_CH;i++)
{
SendPak1.DOUT[i] =Dac10.DOUT_Set[i];
}
for(i=0;i<COUNT_DAC_CH;i++)
{
SendPak1.fStateDAC[i] = Dac10.fDAC_Set[i];
//SendPak1.fStateDAC[i] = Dac10.wDAC_Set[i];
}
for(i=0;i<COUNT_ADC_CH;i++)
{
SendPak1.fStateADC[i] = Dac10.fADC[i];
}
for(i=0;i<COUNT_MAX1329;i++)
SendPak1.Temperature[i] = Dac10.Temperature[i];
for(i=0;i<10;i++)
SendPak1.Temp[i] = Dac10.Temp[i];
Send_Pkt(0,(BYTE *)(&SendPak1),sizeof(TSendPak1),1,1);
}
if(Dac10.SendFlagUart==2)
{
Dac10.SendFlagUart=0;
for(i=0;i<COUNT_DAC_CH;i++)
{
SendPak2.tTarDAC[i].k = TarrRAM_DAC[i].k;
SendPak2.tTarDAC[i].ofs = TarrRAM_DAC[i].ofs;
}
for(i=0;i<COUNT_ADC_CH;i++)
{
SendPak2.tTarADC[i].k = 1;//TarrRAM_ADC[i].k;
SendPak2.tTarADC[i].ofs = 0;//TarrRAM_ADC[i].ofs;
}
Send_Pkt(0,(BYTE *)(&SendPak2),sizeof(TSendPak2),1,2);
}
}
