void test_ref_func( double ref_in, double * ref_out )
{
static unsigned long one_count=0;
if( ulGetNow_mSec( )== one_count ) return;
one_count = ulGetNow_mSec( );
if ( ref_time < code_posi_duration_time ){
if( code_test_accel_time == 0) * ref_out = ref_in;
else test_ramp_proc( ref_in, ref_out);
} // Positive Command
else if ( ref_time < (code_posi_duration_time+code_zero_duration_time)){
if( code_test_decel_time == 0) * ref_out = 0.0;
else test_ramp_proc( 0.0, ref_out);
}
else if ( ref_time < (code_posi_duration_time+code_zero_duration_time+code_nega_duration_time)){
if( code_test_accel_time == 0) * ref_out = -ref_in;
else test_ramp_proc( -ref_in, ref_out);
}
else if (ref_time < (code_posi_duration_time+(code_zero_duration_time*2.0)+ code_nega_duration_time) ){
if( code_test_decel_time == 0 ) * ref_out = 0.0; // Zero Command
else test_ramp_proc( 0.0, ref_out);
}
else ref_time = 0.0; // 반복
}
void hyd_unit_proc(int state, double * ref_out)
{
static int hyd_low_count=0;
static double ref_in = 1.0;
double hyd_sensor_input;
static unsigned long ulCount;
if( ulGetNow_mSec( )== ulCount ) return;
ulCount = ulGetNow_mSec( );
hyd_sensor_input = analog_ref_a;
if( state == STATE_RUN ){
if( hyd_sensor_input < ( hyd_sensor_set_ref * hyd_high_speed_on_ratio ) ) hyd_low_count++;
else if ( hyd_sensor_input > hyd_sensor_set_ref ) hyd_low_count --;
if( hyd_low_count > 3){
hyd_low_count = 4;
ref_in = hyd_max_ref;
}
else if( hyd_low_count <= 0 ){
hyd_low_count = 0 ;
ref_in = digital_speed2;
}
}
else ref_in = 0.0;
RefFunc( ref_in, ref_out );
}
void hyd_unit_proc(int state, double * ref_out)
{
static double ref_in = 0.0;
// static int high_press_count=0;
// static int hyd_speed_state = 0;
static unsigned long ulCount;
unsigned int xbus_in,temp;
xbus_in = ZONE0_BUF[0x0050]; // debug
temp = xbus_in;
if( ulGetNow_mSec( )== ulCount ) return;
ulCount = ulGetNow_mSec( );
if( state == STATE_RUN ){
if( (temp & 0x0002 )== 0 ){ // speed 2 in
ref_in = hyd_max_ref;
hyd_ramp_proc( ref_in, ref_out );
}
else{
ref_in = hyd_idle_ref;
hyd_ramp_proc( ref_in, ref_out );
}
}
else{
ref_in = 0.0;
hyd_ramp_proc( ref_in, ref_out );
}
}
void hyd_ramp_proc( double SetRef,double * pOutRef)
{
static unsigned long ulCount;
double OutRef;
double accel_time,decel_time;
OutRef = * pOutRef;
if( ulGetNow_mSec( )== ulCount ) return;
ulCount = ulGetNow_mSec( );
if ( OutRef < hyd_idle_ref ){
if(accel_time1 <= 0.0) accel_time1 = 0.01; // debug
if(decel_time1 <= 0.0) decel_time1 = 0.01; // debug
accel_time = accel_time1;
decel_time = decel_time1;
}
else{
accel_time = accel_time2;
decel_time = decel_time2;
}
if (SetRef > OutRef){
OutRef += 0.001 / accel_time;
if( OutRef > SetRef ) OutRef = SetRef;
}
else if (SetRef < OutRef){
OutRef -= 0.001 / decel_time ;
if( OutRef < SetRef ) OutRef = SetRef;
}
* pOutRef = OutRef;
}
void hyd_unit_proc_bk(int state, double * ref_out)
{
static double ref_in = 0.0;
static int high_press_count=0;
static int hyd_speed_state = 0;
static unsigned long ulCount;
if( ulGetNow_mSec( )== ulCount ) return;
ulCount = ulGetNow_mSec( );
if( state == STATE_RUN ){
if( hyd_speed_state == 0 ){
if( LPF_Te > hyd_press_ref * Te_rat ){
if( high_press_count > 3 ){
hyd_speed_state = 1;
high_press_count = 4;
* ref_out = hyd_max_ref;
}
else{
high_press_count ++;
ref_in = hyd_idle_ref;
hyd_ramp_proc( ref_in, ref_out );
}
}
else{
if( high_press_count > 0 ) high_press_count --;
else high_press_count = 0;
ref_in = hyd_idle_ref;
hyd_ramp_proc( ref_in, ref_out );
}
}
// in case high speed state
else{
if( LPF_Te < (( hyd_press_ref - hyd_press_ctrl_band) * Te_rat)){
if( high_press_count > 3 ) high_press_count = 3;
else high_press_count --;
if( high_press_count < 1 ){
hyd_speed_state = 0;
ref_in = hyd_idle_ref;
hyd_ramp_proc( ref_in, ref_out );
}
else * ref_out = hyd_max_ref;
}
else * ref_out = hyd_max_ref;
}
}
else ref_in = 0.0;
hyd_ramp_proc( ref_in, ref_out );
}
void get_adc_vdc_high()
{
int LoopCtrl;
Uint32 RunTimeMsec,StartTimeMsec;
double adc_Vdc_in, adc_Vdc_out;
UNION32 u32data;
load_sci_tx_mail_box( "Start ADC at Vdc high"); delay_msecs(10);
gfRunTime=0.0;
LoopCtrl = 1;
gMachineState = STATE_READY;
while(LoopCtrl == 1)
{
if( gfRunTime >= 1.0 ) LoopCtrl = 0;
RunTimeMsec = ulGetTime_mSec( StartTimeMsec);
if(RunTimeMsec > 1){
StartTimeMsec = ulGetNow_mSec( );
adc_Vdc_in = (double)giAdcVdc;
LPF1(0.002,10.0,adc_Vdc_in, & adc_Vdc_out);
}
}
if( gfRunTime >= 1.0 ){
code_adc_vdc_high = adc_Vdc_out;
u32data.dword = code_adc_vdc_high; write_code_2_eeprom(CODE_adc_vdc_high,u32data);
load_sci_tx_mail_box("OK adc_vdc_high Saved");delay_msecs(10);
}
}
interrupt void scibRxFifoIsr(void)
{
static Uint32 modebus_start_time=0;
static int scib_rx_count=0;
static char msg_box[17]={0};
// 5msec 이상 이면 start로 취급한다.
if( ulGetTime_mSec(modebus_start_time) > 10 ){
modebus_start_time = ulGetNow_mSec( );
msg_box[0] = ScibRegs.SCIRXBUF.all; // Read data
scib_rx_count = 0;
scib_rx_count++;
}
else if( scib_rx_count < 15 ){
msg_box[scib_rx_count] = ScibRegs.SCIRXBUF.all; // Read data
scib_rx_count++;
}
else if( scib_rx_count == 15 ){
msg_box[15] = ScibRegs.SCIRXBUF.all; // Read data
scib_rx_count = 0;
scib_rx_msg_flag =1;
strncpy( scib_rx_msg_box,msg_box,16);
}
else{
msg_box[0] = ScibRegs.SCIRXBUF.all; // Read data
scib_rx_count++;
}
ScibRegs.SCIFFRX.bit.RXFFOVRCLR=1; // Clear Overflow flag
ScibRegs.SCIFFRX.bit.RXFFINTCLR=1; // Clear Interrupt flag
// PieCtrlRegs.PIEACK.all|=0x0080; // IN8
PieCtrlRegs.PIEACK.all|=0x0100; // IN9
}
void RefFunc( double SetRef,double * pOutRef)
{
static unsigned long ulCount;
double OutRef;
double accel_time,decel_time;
OutRef = * pOutRef;
if( ulGetNow_mSec( )== ulCount ) return;
ulCount = ulGetNow_mSec( );
if(code_accel_time1 <= 0.0) code_accel_time1 = 0.01; // debug
if(code_decel_time1 <= 0.0) code_decel_time1 = 0.01; // debug
if(code_accel_time2 <= 0.01) code_accel_time2 = code_accel_time1; // debug
if(code_decel_time2 <= 0.01) code_decel_time2 = code_decel_time1; // debug
if( fabs(OutRef) <= code_digital_speed2 ) { accel_time = code_accel_time1; decel_time = code_decel_time1; }
else { accel_time = code_accel_time2; decel_time = code_decel_time2; }
//-- 설정값이 양수인 경우
if( OutRef >= 0 )
{
if (SetRef > OutRef){
OutRef += 0.001 / accel_time;
if( OutRef > SetRef ) OutRef = SetRef;
}
else if (SetRef < OutRef){
OutRef -= 0.001 / decel_time ;
if( OutRef < SetRef ) OutRef = SetRef;
}
}
//-- 설정값이 음수인 경우
else{
if (SetRef > OutRef){
OutRef += 0.001 / decel_time ;
if( OutRef > SetRef ) OutRef = SetRef;
}
else if (SetRef < OutRef){
OutRef -= 0.001 / accel_time ;
if( OutRef < SetRef ) OutRef = SetRef;
}
}
* pOutRef = OutRef;
}
void get_adc_offset()
{
int LoopCtrl;
Uint32 RunTimeMsec,StartTimeMsec;
double u_offset_in, v_offset_in;
double R_offset_in, S_offset_in;
double u_offset_out, v_offset_out;
double R_offset_out, S_offset_out;
UNION32 u32data;
load_sci_tx_mail_box( "\n***********************"); delay_msecs(10);
load_sci_tx_mail_box( "\n Start ADC Offset Calc "); delay_msecs(10);
load_sci_tx_mail_box( "\n***********************"); delay_msecs(10);
gfRunTime=0.0;
LoopCtrl = 1;
while(LoopCtrl == 1)
{
if( gfRunTime >= 5.0 ) LoopCtrl = 0;
RunTimeMsec = ulGetTime_mSec( StartTimeMsec);
if(RunTimeMsec > 1){
StartTimeMsec = ulGetNow_mSec( );
u_offset_in = (double)giAdcUphase;
v_offset_in = (double)giAdcVphase;
R_offset_in = (double)giAdcRphase;
S_offset_in = (double)giAdcSphase;
LPF1(0.002,10.0,u_offset_in, & u_offset_out);
LPF1(0.002,10.0,v_offset_in, & v_offset_out);
LPF1(0.002,10.0,R_offset_in, & R_offset_out);
LPF1(0.002,10.0,S_offset_in, & S_offset_out);
}
}
if( gfRunTime >= 5.0 ){
adc_u_offset = (int)u_offset_out;
adc_v_offset = (int)v_offset_out;
adc_R_offset = (int)R_offset_out;
adc_S_offset = (int)S_offset_out;
u32data.word.word0 = adc_u_offset; write_code_2_eeprom(CODE_adc_u_offset,u32data);
u32data.word.word0 = adc_v_offset; write_code_2_eeprom(CODE_adc_v_offset,u32data);
u32data.word.word0 = adc_R_offset; write_code_2_eeprom(CODE_adc_R_offset,u32data);
u32data.word.word0 = adc_S_offset; write_code_2_eeprom(CODE_adc_S_offset,u32data);
load_sci_tx_mail_box("\n*********************");delay_msecs(10);
load_sci_tx_mail_box("\n OK Adc offset Saved ");delay_msecs(10);
load_sci_tx_mail_box("\n*********************");delay_msecs(10);
}
}
int periodic_check(unsigned long msec)
{
static unsigned long count_msec=0;
if( ulGetTime_mSec(count_msec) > msec ){
count_msec = ulGetNow_mSec( );
return 0;
}
return -1;
}
void delay_msecs( unsigned long ulmSec)
{
unsigned long Start_mSec,ulTemp;
ulTemp =0;
Start_mSec = ulGetNow_mSec( );
while( ulTemp < ulmSec)
{
ulTemp = ulGetTime_mSec( Start_mSec );
}
}
// read data format "9:4:123:x.xxxe-x"
// write data format "9:6:123:1.234e-3"
void scic_cmd_proc( int * sci_cmd, double * sci_ref)
{
unsigned long sci_watchdog_count;
static unsigned long start_count=0;
static int onOff;
double data,dbtemp;
int addr,check,temp;
char str[30]={0};
TRIP_INFO * TripData;
* sci_cmd = CMD_NULL;
* sci_ref = 0.0;
if( scic_rx_msg_flag == 0){
sci_watchdog_count = ulGetTime_mSec( start_count);
if ( sci_watchdog_count > 5000){
scic_fifo_init();
start_count = ulGetNow_mSec( );
}
return;
}
scic_rx_msg_flag = 0;
if ( scic_rx_msg_box[0] != '9') return;
addr = (scic_rx_msg_box[4]- '0')* 100 +(scic_rx_msg_box[5]- '0')*10 + (scic_rx_msg_box[6]- '0');
scic_rx_msg_box[16]=0;
data = atof( & scic_rx_msg_box[8]);
// regist write function decoding
if( scic_rx_msg_box[2] == '6'){
if( addr == 900 ){
check = (int)data;
if(check == 10){
* sci_cmd = CMD_START;
// * sci_ref = code_btn_start_ref;
load_scic_tx_mail_box("UART CMD_START");
}
else if( check == 20 ){
* sci_cmd = CMD_STOP; * sci_ref = 0.0;
load_scic_tx_mail_box("UART CMD_STOP");
}
else if( check == 30 ){
* sci_cmd = CMD_RESET; * sci_ref = 0.0;
load_scic_tx_mail_box("UART CMD_RESET");
}
else if( data == 40 ){
* sci_cmd = CMD_SAVE; * sci_ref = 0.0;
load_scic_tx_mail_box("UART CMD_SAVE");
}
else if( data == 50 ){
* sci_cmd = CMD_READ_ALL; * sci_ref = 0.0;
load_scic_tx_mail_box("UART CMD_READ_ALL");
}
else if( data == 80 ){
* sci_cmd = CMD_NULL; * sci_ref = 0.0;
get_adc_offset();
}
else if( data == 90 ){
* sci_cmd = CMD_NULL; * sci_ref = 0.0;
load_scic_tx_mail_box("EEPROM init Start");
check = init_eprom_data(); // 0이 아니면 address value
if( check != 0) load_scic_tx_mail_box("EEPROM init Fail");
else load_scic_tx_mail_box("EEPROM init OK");
}
else{
load_scic_tx_mail_box("Illegal CMD data");
}
}
else{
// registor_write_proc(addr,data);
check = SaveDataProc(addr, data);
Nop();
}
}
//==================
// read routine
//====================
else if(scic_rx_msg_box[2] == '4'){
if(addr == 901){ // monitor state
check = (int)data;
if(check == 0){
switch(gMachineState){
case STATE_POWER_ON: load_scic_tx_mail_box("[POWE_ON] "); break;
case STATE_READY: load_scic_tx_mail_box("[READY] "); break;
case STATE_RUN: load_scic_tx_mail_box("[RUN ] "); break;
case STATE_TRIP: load_scic_tx_mail_box("[TRIP] "); break;
case STATE_INIT_RUN: load_scic_tx_mail_box("[INIT] "); break;
case STATE_GO_STOP: load_scic_tx_mail_box("[GO_STOP] "); break;
case STATE_BREAK_OFF: load_scic_tx_mail_box("STATE_WAIT_BREAK_OFF"); break;
default: load_scic_tx_mail_box("Unknown State"); break;
}
}
return;
}
//--- HMI monitor
else if(addr == 902){ // 내부 변수 read
check = (int)data;
switch( check ){
case 0 :
monitor[0] = I_out;
monitorPrint("Io=%d[A]",str,monitor[0]);
load_scic_tx_mail_box(str);
break;
case 1 :
monitor[1] = Power_out;
monitorPrint("Po=%d kW",str,monitor[1]);
load_scic_tx_mail_box(str);
break;
case 2 :
monitor[2] = Vout;
monitorPrint("Vo=%d[V]",str,monitor[2]);
load_scic_tx_mail_box(str);
break;
case 3 :
monitor[3] = Vdc;
monitorPrint("Vp=%d[V]",str,monitor[3]);
load_scic_tx_mail_box(str);
break;
case 4 :
if( onOff ){ onOff = 0; strncpy(str," Power",10);}
else{ onOff = 1; strncpy(str," TechWin",10);}
load_scic_tx_mail_box(str);
break;
case 5 : // Reset;
gMachineState = STATE_POWER_ON;
Nop();
asm (" .ref _c_int00"); // ;Branch to start of boot.asm in RTS library
asm (" LB _c_int00"); // ;Branch to start of boot.asm in RTS library
break;
default:
break;
}
// check 에 따른 변수를 포인터로 두어서 처리 한다.
// snprintf( str,30,"\n Vdc =%10.3e \n",Vdc); load_scic_tx_mail_box(str);
return;
}
else if(addr == 903){ // 내부 변수 read // EEPROM TRIP DATA
check = (int)data;
if( data == 0 ){
snprintf( str,4,"%03d:",TripInfoNow.CODE);
load_scic_tx_mail_box(str); delay_msecs(180);
load_scic_tx_mail_box(TripInfoNow.MSG); delay_msecs(220);
load_scic_tx_mail_box(TripInfoNow.TIME); delay_msecs(180);
dbtemp = TripInfoNow.VOUT;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,20,"Vo=%3d[A]",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
dbtemp = TripInfoNow.VDC;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,20," VDC =%4d",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
dbtemp = TripInfoNow.CURRENT;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,10,"I1 =%4d ",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
dbtemp = TripInfoNow.DATA;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,10," DATA=%4d",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
}
else{
TripData = (TRIP_INFO*)malloc(sizeof(TRIP_INFO));
GetTripInfo(check + 1,TripData);
strncpy(gStr1,TripInfoNow.MSG,20);
snprintf( str,4,"%03d:",TripData->CODE);
load_scic_tx_mail_box(str); delay_msecs(180);
load_scic_tx_mail_box(TripData->MSG); delay_msecs(220);
load_scic_tx_mail_box(TripData->TIME); delay_msecs(180);
dbtemp = TripData->VOUT;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,10,"Vo=%3d[A]",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
dbtemp = TripData->VDC;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,10," VDC =%4d",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
dbtemp = TripData->CURRENT;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,10,"I1 =%4d ",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
dbtemp = TripData->DATA;
temp = (int)(floor(dbtemp +0.5));
snprintf( str,10," DATA=%4d",temp);
load_scic_tx_mail_box(str); delay_msecs(180);
free(TripData);
}
return;
}
else if(addr == 904){ // DATE & TIME SET
check = (int)data;
switch( check ){
case 0:
TimeInput[0] = scic_rx_msg_box[10];
TimeInput[1] = scic_rx_msg_box[11];
TimeInput[2] = scic_rx_msg_box[12];
TimeInput[3] = scic_rx_msg_box[13];
TimeInput[4] = scic_rx_msg_box[14];
TimeInput[5] = scic_rx_msg_box[15];
break;
case 1:
TimeInput[6] = scic_rx_msg_box[10];
TimeInput[7] = scic_rx_msg_box[11];
TimeInput[8] = scic_rx_msg_box[12];
TimeInput[9] = scic_rx_msg_box[13];
TimeInput[10] = scic_rx_msg_box[14];
TimeInput[11] = scic_rx_msg_box[15];
delay_msecs(50);
WriteTimeToDS1307(TimeInput);
load_scic_tx_mail_box("Date & Time Saved");
break;
case 2:
// load_scic_tx_mail_box("WAIT FOR CLEAR DATA!");
// delay_msecs(180);
ClearTripDataToEeprom();
break;
}
return;
}
else if(addr == 905){ // RUN & STOP
check = (int)data;
switch( check ){
case 0:
* sci_cmd = CMD_START;
// * sci_ref = code_btn_start_ref;
break;
case 1:
* sci_cmd = CMD_STOP;
* sci_ref = 0.0;
break;
case 2:
* sci_cmd = CMD_SPEED_UP;
break;
case 3:
* sci_cmd = CMD_SPEED_DOWN;
break;
default:
* sci_cmd = CMD_NULL;
break;
}
return;
}
else if(addr == 906){
GetTimeAndDateStr(str);
load_scic_tx_mail_box(str);
return;
}
else if (( addr > 979) && ( addr < 996)){
check = addr - 980;
snprintf( str,19,"adc =%4d",adc_result[check]);
load_scic_tx_mail_box(str);
delay_msecs(10);
return;
}
check = get_code_information( addr, CMD_READ_DATA , & code_inform);
if( check == 0 ){
check = (int)data;
switch(check)
{
case 0:
snprintf( str,19,"CODE=%4d",addr);
load_scic_tx_mail_box(str);
delay_msecs(10);
break;
case 1:
load_scic_tx_mail_box(code_inform.disp);delay_msecs(10);
break;
case 2:
if( code_inform.type == TYPE_DOUBLE ){
snprintf( str,20,"Data =%10.3e",code_inform.code_value.doubles);
}
else{
snprintf( str,20,"Data =%10d",code_inform.code_value.ints);
}
load_scic_tx_mail_box(str); delay_msecs(10);
break;
default:
snprintf( str,19,"CODE=%4d",addr);
load_scic_tx_mail_box(str);delay_msecs(10);
load_scic_tx_mail_box(code_inform.disp);delay_msecs(10);
if( code_inform.type == TYPE_DOUBLE )
snprintf( str,20,"Data =%10.3e",code_inform.code_value.doubles);
else
snprintf( str,20,"Data =%10d",code_inform.code_value.ints);
load_scic_tx_mail_box(str); delay_msecs(10);
break;
}
}
else{
load_scic_tx_mail_box("Err Invalid Addr");delay_msecs(10);
}
return;
}
}
