void ProcessAsyncData(BowlerPacket * Packet){ //println_I("**Got Async Packet**"); //printPacket(Packet,INFO_PRINT); Print_Level l = getPrintLevel(); setPrintLevelInfoPrint(); if (Packet->use.head.RPC==GCHV){ BYTE pin = Packet->use.data[0]; BYTE mode = GetChannelMode(pin); if(mode == IS_ANALOG_IN ){ UINT16_UNION ana; ana.byte.SB = Packet->use.data[1]; ana.byte.LB = Packet->use.data[2]; //ADC_val[pin-8]=ana.Val; if(ana.Val>=0 && ana.Val<1024) SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val; println_I("***Setting analog value: ");p_int_I(pin);print_I(", ");p_int_I(ana.Val); } else if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME) || mode == IS_SERVO){ //DIG_val[pin]=Packet->use.data[1]; SetValFromAsync(pin,Packet->use.data[1]);//asyncData[pin].currentVal=Packet->use.data[1]; println_I("***Setting digital value: ");p_int_I(pin);print_I(", ");p_int_I(Packet->use.data[1]);//printStream(DIG_val,NUM_PINS); }else { if(IsAsync(pin)){ println_I("Sending async packet, not digital or analog"); PutBowlerPacket(Packet); } } }else if (Packet->use.head.RPC==AASN){ int i; for(i=0;i<8;i++){ BYTE pin = i+8; BYTE mode = GetChannelMode(pin); if(mode == IS_ANALOG_IN ){ UINT16_UNION ana; ana.byte.SB = Packet->use.data[i*2]; ana.byte.LB = Packet->use.data[(i*2)+1]; //ADC_val[pin-8]=ana.Val if(ana.Val>=0 && ana.Val<1024); SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val; } } }else if (Packet->use.head.RPC==DASN){ int i; for(i=0;i<GetNumberOfIOChannels();i++){ BYTE mode = GetChannelMode(i); if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME)|| (mode == IS_SERVO)){ SetValFromAsync(i,Packet->use.data[i]);//asyncData[i].currentVal=Packet->use.data[i]; } } //println_I("***Setting All Digital value: "); }if (Packet->use.head.RPC==GetRPCValue("gacv")){ int i; int val; for(i=0;i<GetNumberOfIOChannels();i++){ val = get32bit(Packet, i*4); if(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal!=val){ println_I("Data on Pin ");p_int_I(i);print_I(" to val ");p_int_I(val); SetValFromAsync(i,val);// } } }else{ println_W("***Async packet not UNKNOWN***"); printPacket(Packet,WARN_PRINT); } // println_I("***Setting All value: ["); // int i; // for(i=0;i<NUM_PINS;i++){ // p_int_I(asyncData[i].currentVal);print_I(" "); // } // print_I("]"); setPrintLevel(l); }
void checkLinkHomingStatus(int group) { if (!(GetPIDCalibrateionState(group) == CALIBRARTION_home_down || GetPIDCalibrateionState(group) == CALIBRARTION_home_up || GetPIDCalibrateionState(group) == CALIBRARTION_home_velocity ) ) { return; //Calibration is not running } float current = GetPIDPosition(group); float currentTime = getMs(); if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) { //println_W("Check Homing "); if (GetPIDCalibrateionState(group) != CALIBRARTION_home_velocity) { float boundVal = getPidGroupDataTable(group)->homing.homingStallBound; if (bound(getPidGroupDataTable(group)->homing.previousValue, current, boundVal, boundVal ) ) { pidReset(group, getPidGroupDataTable(group)->homing.homedValue); //after reset the current value will have changed current = GetPIDPosition(group); getPidGroupDataTable(group)->config.tipsScale = 1; println_W("Homing Velocity for group "); p_int_W(group); print_W(", Resetting position to: "); p_fl_W(getPidGroupDataTable(group)->homing.homedValue); print_W(" current "); p_fl_W(current); float speed = -20.0; if (GetPIDCalibrateionState(group) == CALIBRARTION_home_up) speed *= 1.0; else if (GetPIDCalibrateionState(group) == CALIBRARTION_home_down) speed *= -1.0; else { println_E("Invalid homing type"); return; } Print_Level l = getPrintLevel(); //setPrintLevelInfoPrint(); setOutput(group, speed); setPrintLevel(l); getPidGroupDataTable(group)->timer.MsTime = getMs(); getPidGroupDataTable(group)->timer.setPoint = 2000; SetPIDCalibrateionState(group, CALIBRARTION_home_velocity); getPidGroupDataTable(group)->homing.lastTime = currentTime; } } else { current = GetPIDPosition(group); float posDiff = current - getPidGroupDataTable(group)->homing.previousValue; //ticks float timeDiff = (currentTime - getPidGroupDataTable(group)->homing.lastTime) / 1000.0; // float tps = (posDiff / timeDiff); getPidGroupDataTable(group)->config.tipsScale = 20 / tps; println_E("New scale factor: "); p_fl_E(getPidGroupDataTable(group)->config.tipsScale); print_E(" speed "); p_fl_E(tps); print_E(" on "); p_int_E(group); print_E(" Position difference "); p_fl_E(posDiff); print_E(" time difference "); p_fl_E(timeDiff); OnPidConfigure(group); SetPIDCalibrateionState(group, CALIBRARTION_DONE); } getPidGroupDataTable(group)->homing.previousValue = current; } }