void initPIDChans(BYTE group){
if(dyPid[group].inputChannel==DYPID_NON_USED || dyPid[group].outputChannel==DYPID_NON_USED)
return;
switch(dyPid[group].inputMode){
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
dyPid[group].inputChannel = getCounterIntChannnel( channelToCounterGroup(dyPid[group].inputChannel));
StartCounterInput(dyPid[group].inputChannel);
break;
}
println_I("Setting Modes for PID");
SetCoProcMode(dyPid[group].inputChannel,dyPid[group].inputMode);
SetCoProcMode(dyPid[group].outputChannel,dyPid[group].outputMode);
SyncModes();
if(dyPid[group].inputMode== IS_ANALOG_IN){
pidGroups[group].SetPoint=GetValFromAsync(dyPid[group].inputChannel);
}else{
pidGroups[group].SetPoint=0;
}
}
int resetPositionMine(int group, int current){
if(dyPid[group].inputChannel==DYPID_NON_USED)
return current;
if(dyPid[group].inputMode == IS_COUNTER_INPUT_INT){
SetCounterInput(dyPid[group].inputChannel,current);
}else if(dyPid[group].inputMode == IS_ANALOG_IN){
current=GetValFromAsync(dyPid[group].inputChannel);
}else if(dyPid[group].inputMode == IS_DI){
current=GetDigitalValFromAsync(dyPid[group].inputChannel);
}
return current;
}
void PushAllDiVal(){
#if defined(WPIRBE)
return;
#endif
println_I("Pushing digital");
//FlagBusy_IO=1;
//_delay_us(800);
sendHeader(4+24,"dasn");
int i=0;
for(i=0;i<24;i++){
send(GetValFromAsync(i));
}
//FlagBusy_IO=0;
//_delay_us(800);
}
void PushAllAdcVal(){
#if defined(WPIRBE)
return;
#endif
//println_I("Pushing analog");
UINT16_UNION an;
//FlagBusy_IO=1;
//_delay_us(800);
sendHeader(4+16,"aasn");
int i=0;
for(i=8;i<16;i++){
an.Val=GetValFromAsync(i);
send(an.byte.SB);
send(an.byte.LB);
}
//FlagBusy_IO=0;
//_delay_us(800);
}
boolean checkDigital(){
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
uint8_t mode = GetChannelMode(i);
boolean run = (((mode == IS_DI) ) || ((mode == IS_COUNTER_INPUT_HOME)||(mode == IS_COUNTER_OUTPUT_HOME) || (mode == IS_SERVO)));
if (run){
if(mode == IS_SERVO){
//setDataTableCurrentValue(i,GetServoPos(i));
}else{
if(GetValFromAsync(i) != GetDIO(i)){
setDataTableCurrentValue(i,GetDIO(i));
//printAsync();
}
}
}
if(mode == IS_UART_RX){
setDataTableCurrentValue(i,Get_UART_Byte_CountPassThrough());
}
}
return true;
}
float getPositionMine(int group){
if(dyPid[group].inputChannel==DYPID_NON_USED||
((pidGroups[group].Enabled == FALSE) && (vel[group].enabled==FALSE)))
return 0;
LONG pos = 0;
//print_I("\nGetting PID value from group: ");p_int_I(chan->channel);print_I(" of mode: ");printMode(chan->inputMode);print_I(" From channel: ");p_int_I(chan->inputChannel);print_I("\n");
switch(dyPid[group].inputMode){
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
pos=GetCounterByChannel( dyPid[group].inputChannel );
break;
case IS_ANALOG_IN:
pos=GetValFromAsync(dyPid[group].inputChannel);
break;
case IS_DI:
pos = GetDigitalValFromAsync(dyPid[group].inputChannel);
break;
}
return ((float)pos);
}
void initPIDChans(uint8_t group){
if(dyPid[group].inputChannel==DYPID_NON_USED || dyPid[group].outputChannel==DYPID_NON_USED){
return;
}
switch(dyPid[group].inputMode){
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
dyPid[group].inputChannel = getCounterIntChannnel( channelToCounterGroup(dyPid[group].inputChannel));
StartCounterInput(dyPid[group].inputChannel);
break;
}
// println_W("PID In chan: ");
// p_int_W(dyPid[group].inputChannel);
// println_W(" mode: ");
// printMode(dyPid[group].inputMode, WARN_PRINT);
// println_W("PID Out chan: ");
// p_int_W(dyPid[group].outputChannel);
// println_W(" mode: ");
//printMode(dyPid[group].outputMode, WARN_PRINT);
SetCoProcMode(dyPid[group].inputChannel,dyPid[group].inputMode);
SetCoProcMode(dyPid[group].outputChannel,dyPid[group].outputMode);
//SyncModes();
if(dyPid[group].inputMode== IS_ANALOG_IN){
pidGroups[group].SetPoint=GetValFromAsync(dyPid[group].inputChannel);
}else{
pidGroups[group].SetPoint=0;
}
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
getPidGroupDataTable( group)->config.Async=true;
//getPidGroupDataTable( group)->config.Enabled=true;
}
float getPositionMine(int group){
if(dyPid[group].inputChannel==DYPID_NON_USED || !pidGroups[group].config.Enabled)
return 0;
int32_t pos = 0;
switch(dyPid[group].inputMode){
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
pos=GetCounterByChannel( dyPid[group].inputChannel );
break;
case IS_ANALOG_IN:
pos=GetValFromAsync(dyPid[group].inputChannel);
break;
case IS_DI:
pos = GetDigitalValFromAsync(dyPid[group].inputChannel);
break;
default:
return 0;
}
//println_W("Get PID ");p_int_W(group);print_W(" is ");p_int_W(pos);
return ((float)pos);
}