void Conjunct::prefix_print(FILE *output_file, int debug) { static char dir_glyphs[] = { '-', '?', '+' }; if (debug) { Formula::print_head(output_file); if(pres_debug>=2) fprintf(output_file, "(@%p)", this); fprintf(output_file, "%s CONJUNCT, ", exact ? "EXACT" : "INEXACT"); if (simplified) fprintf(output_file, "simplified, "); if (verified) fprintf(output_file, "verified, "); if (possible_leading_0s != -1 && guaranteed_leading_0s != -1) assert (guaranteed_leading_0s <= possible_leading_0s); if (guaranteed_leading_0s != -1 && guaranteed_leading_0s == possible_leading_0s) fprintf(output_file,"# leading 0's = %d,", possible_leading_0s); else if (possible_leading_0s != -1 || guaranteed_leading_0s != -1) { if (guaranteed_leading_0s != -1) fprintf(output_file,"%d <= ",guaranteed_leading_0s); fprintf(output_file,"#O's"); if (possible_leading_0s != -1) fprintf(output_file," <= %d",possible_leading_0s); fprintf(output_file,", "); } if (dir_glyphs[leading_dir+1] != '?') fprintf(output_file," first = %c, ", dir_glyphs[leading_dir+1]); fprintf(output_file,"myLocals=["); String s=""; for (Variable_ID_Iterator VI(myLocals); VI; ) { assert( (*VI)->kind() == Wildcard_Var); s += (*VI)->name(); print_var_addrs(s, *VI); VI++; if(VI) s += ","; } s += "] mappedVars=["; for(Variable_ID_Iterator MVI(mappedVars); MVI; ) { s += (*MVI)->name(); print_var_addrs(s, *MVI); MVI++; if(MVI) s += ","; } fprintf(output_file, "%s]\n", (const char *) s); } else level++; setOutputFile(output_file); setPrintLevel(level+1); problem->printProblem(debug); setPrintLevel(0); Formula::prefix_print(output_file, debug); }
void setOutputMine(int group, float v){ if( dyPid[group].outputChannel==DYPID_NON_USED|| !pidGroups[group].config.Enabled) return; Print_Level l = getPrintLevel(); //setPrintLevelNoPrint(); int val = (int)(v); if(dyPid[group].outputMode == IS_SERVO){ val += 128; if (val>255) val=255; if(val<0) val=0; }else if(dyPid[group].outputMode == IS_DO){ if(val>0) val=1; else val=0; }else{ val += 128; if (val>255) val=255; if(val<0) val=0; } int set = (int)val; //print_W(" set ");p_int_W(group);print_W(" to ");p_int_W(set); dyPid[group].outVal=set; SetChannelValueCoProc(dyPid[group].outputChannel,dyPid[group].outVal); setPrintLevel(l); }
void CheckSwitches(void){ Print_Level l = getPrintLevel(); setPrintLevelInfoPrint(); switched=0; volt = GetRawVoltage(); boolean up = false; uint8_t reg = isRegulated_0(); if (bankState[0] != reg ){ bankState[0]=reg; up=true; } reg = isRegulated_1(); if (bankState[1] != reg){ bankState[1] = reg; up=true; } if ((lastVolt>RawVoltageMin) && (volt<RawVoltageMin)){ up=true; lastVolt = volt; } if ((lastVolt<RawVoltageMin) && (volt>RawVoltageMin)){ up=true; lastVolt = volt; } setPrintLevel(l); }
CAL_STATE pidHysterisis(int group) { if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) { Print_Level l = getPrintLevel(); //setPrintLevelInfoPrint(); float boundVal = 150.0; float extr = GetPIDPosition(group); if (bound(0, extr, boundVal, boundVal)) {// check to see if the encoder has moved //we have not moved // println_I("NOT moved ");p_fl_I(extr); if (getPidGroupDataTable(group)->calibration.state == forward) { incrementHistoresis(group); } else if (getPidGroupDataTable(group)->calibration.state == backward) { decrementHistoresis(group); } int historesisBound = 25; if (getPidGroupDataTable(group)->config.lowerHistoresis < (-historesisBound) && getPidGroupDataTable(group)->calibration.state == backward) { println_E("Backward Motor seems damaged, more then counts of historesis #"); p_int_I(group); getPidGroupDataTable(group)->calibration.state = forward; } if (getPidGroupDataTable(group)->config.upperHistoresis > (historesisBound) && getPidGroupDataTable(group)->calibration.state == forward) { println_E("Forward Motor seems damaged, more then counts of historesis #"); p_int_I(group); getPidGroupDataTable(group)->calibration.state = done; } } else { pidReset(group, 0); setOutput(group, 0); println_E("Moved "); p_fl_E(extr); if (getPidGroupDataTable(group)->calibration.state == forward) { println_I("Backward Calibrated for link# "); p_int_I(group); getPidGroupDataTable(group)->calibration.state = backward; } else { println_I("Calibration done for link# "); p_int_I(group); getPidGroupDataTable(group)->calibration.state = done; float offset = .9; getPidGroupDataTable(group)->config.lowerHistoresis *= offset; getPidGroupDataTable(group)->config.upperHistoresis *= offset; calcCenter(group); } } if (getPidGroupDataTable(group)->calibration.state == forward) { setOutput(group, 1.0f); } else if (getPidGroupDataTable(group)->calibration.state == backward) { setOutput(group, -1.0f); } setPrintLevel(l); } if (getPidGroupDataTable(group)->calibration.state == done) SetPIDCalibrateionState(group, CALIBRARTION_DONE); return getPidGroupDataTable(group)->calibration.state; }
BOOL onCartesianPost(BowlerPacket *Packet){ Print_Level l = getPrintLevel(); switch(Packet->use.head.RPC){ case _SLI: if(FifoGetPacketSpaceAvailible(&packetFifo)>0){ if(Packet->use.data[0]==1){ processLinearInterpPacket(Packet); }else{ println_I("Cached linear Packet ");p_int_I(FifoGetPacketSpaceAvailible(&packetFifo)); FifoAddPacket(&packetFifo,Packet); } Packet->use.head.Method = BOWLER_STATUS; INT32_UNION tmp; tmp.Val=FifoGetPacketSpaceAvailible(&packetFifo); Packet->use.data[0]=tmp.byte.FB; Packet->use.data[1]=tmp.byte.TB; Packet->use.data[2]=tmp.byte.SB; Packet->use.data[3]=tmp.byte.LB; tmp.Val=SIZE_OF_PACKET_BUFFER; Packet->use.data[4]=tmp.byte.FB; Packet->use.data[5]=tmp.byte.TB; Packet->use.data[6]=tmp.byte.SB; Packet->use.data[7]=tmp.byte.LB; Packet->use.head.DataLegnth=4+4+4; if(tmp.Val == 0){ full=TRUE; } setPrintLevel(l); }else{ println_I("###ERROR BUFFER FULL!!");p_int_I(FifoGetPacketSpaceAvailible(&packetFifo)); setPrintLevel(l); ERR(Packet,33,33); } return TRUE; case PRCL: cancelPrint(); READY(Packet,35,35); return TRUE; } return FALSE; }
void RunPPMCheck(void){ if(GetChannelMode(23) != IS_PPM_IN){ //setMode(23,IS_PPM_IN); return; } if(startedLinks==false){ startedLinks=true; readPPMLink(ppmLink); } if(writeLinks==true){ writeLinks=false; writePPMLink(ppmLink); } int i; boolean up = false; for(i=0;i<NUM_PPM_CHAN;i++){ float fTime =MyTickConvertToMilliseconds(((float)ppmDataTmp[i])+(((float)TickGetUpper())*((float) 4294967295ul ))); if(fTime>.9 && fTime<2.2){ float fVal = (fTime-.99)*255; if(fVal>254) fVal=254; if(fVal<0) fVal=0; uint8_t time =((BYTE) fVal); if((time>(ppmData[i]+3)) || (time<(ppmData[i]-3))){ ppmData[i]=time; //setHeartBeatState( FALSE, 0); up=true; } } } if(up){ //push upstream pushPPMPacket(); //print_I("\nUpdating ppm chan ");printStream(ppmData,NUM_PPM_CHAN); //print_I(" PPM cross link ");printStream(ppmLink,NUM_PPM_CHAN); for(i=0;i<NUM_PPM_CHAN;i++){ if(ppmLink[i] != INVALID_PPM_LINK){ if(ppmLastSent[i] != ppmData[i]){ ppmLastSent[i] = ppmData[i]; Print_Level l = getPrintLevel(); setPrintLevelInfoPrint(); //println_I("PPM setting output"); //SetChannelValueCoProc(ppmLink[i],ppmData[i]); getBcsIoDataTable(ppmLink[i])->PIN.currentValue=ppmData[i]; //SetValFromAsync(ppmLink[i],ppmData[i]); setPrintLevel(l); } } } } }
void cancelPrint(){ Print_Level l = getPrintLevel(); println_I("Cancel Print"); setPrintLevel(l); while(FifoGetPacketCount(&packetFifo)>0){ FifoGetPacket(&packetFifo,&linTmpPack); } setInterpolateXYZ(0, 0, getmaxZ(), 0); ZeroPID(hwMap.Extruder0.index); SetPIDTimed(hwMap.Heater0.index,0,0); }
boolean cartesianAsyncEventCallback(BowlerPacket * Packet, boolean(*pidAsyncCallbackPtr)(BowlerPacket *Packet)) { Print_Level l = getPrintLevel(); //setPrintLevelNoPrint(); if (!getRunPidIsr()) { RunPIDControl(); } setPrintLevel(l); //printPIDvals(6); // uses its own async callback cartesianAsync(); return false; }
uint8_t Bowler_Server_Local(BowlerPacket * Packet){ Print_Level l = getPrintLevel(); //setPrintLevelNoPrint(); if (GetBowlerPacket_arch(Packet)){ //setLed(1,1,1); if(Packet->use.head.RPC != _PNG){ println_I("Got:");printPacket(Packet,INFO_PRINT); } if ( (CheckAddress(MyMAC.v,Packet->use.head.MAC.v) == true) || ((CheckAddress((uint8_t *)Broadcast.v,(uint8_t *)Packet->use.head.MAC.v) == true) )) { float start=getMs(); Process_Self_Packet(Packet); if(getMs()-start>5){ println_E("Process too long: ");p_fl_E(getMs()-start); } for (i=0;i<6;i++){ Packet->use.head.MAC.v[i]=MyMAC.v[i]; } SetCRC(Packet); start=getMs(); PutBowlerPacket(Packet); if(getMs()-start>5){ println_E("Return too long: ");p_fl_E(getMs()-start); } if(Packet->use.head.RPC != _PNG){ println_I("Response:");printPacket(Packet,INFO_PRINT); } }else{ //println_I("Packet not addressed to me: ");printByteArray(Packet->use.head.MAC.v,6); print_I(" is not mine: ");printByteArray(MyMAC.v,6); } //setLed(0,0,1); setPrintLevel(l); return true; }//Have a packet setPrintLevel(l); return false; }
/** * Set a setpoint for a servo with an intrerpolated time */ void setServo(uint8_t PIN, uint8_t val,float time){ if(time<30) time=0; // velocity[PIN].setTime=time; // velocity[PIN].set=(float)val; // velocity[PIN].start=(float)position[PIN]; // velocity[PIN].startTime=getMs(); // if (val==position[PIN]){ // velocity[PIN].setTime=0; // } position[PIN]=val; Print_Level l = getPrintLevel(); //println_I("\tSrv ");p_int_I(PIN); print_I(" v=");p_int_I(val); setPrintLevel(l); }
void CheckSwitches(void){ Print_Level l = getPrintLevel(); setPrintLevelInfoPrint(); switched=0; volt = GetRawVoltage(); BOOL up = FALSE; BYTE reg = isRegulated_0(); if (bankState[0] != reg ){ bankState[0]=reg; up=TRUE; } reg = isRegulated_1(); if (bankState[1] != reg){ bankState[1] = reg; up=TRUE; } if ((lastVolt>RawVoltageMin) && (volt<RawVoltageMin)){ up=TRUE; lastVolt = volt; } if ((lastVolt<RawVoltageMin) && (volt>RawVoltageMin)){ up=TRUE; lastVolt = volt; } if(up){ println_I("\nVoltage on raw: \t"); p_fl_I(volt); println_I("Voltage on bank0: \t"); p_fl_I(GetRail0Voltage()); println_I("Voltage on bank1:\t"); p_fl_I(GetRail1Voltage()); println_I("Pushing upstream Power Packet bank 0: ");p_int_I(bankState[0]);print_I(" bank 1: ");p_int_I(bankState[1]); println_I("Power Code 0: ");p_int_I(GetRawVoltageCode(0)); println_I("Power Code 1 : ");p_int_I(GetRawVoltageCode(1)); println_I("Raw: ");p_fl_I(GetRawVoltage()); UpstreamPushPowerChange(); } setPrintLevel(l); }
BOOL onCartesianPacket(BowlerPacket *Packet){ Print_Level l = getPrintLevel(); println_I("Packet Checked by Cartesian Controller"); BOOL ret = FALSE; switch(Packet->use.head.Method){ case BOWLER_POST: ret = onCartesianPost(Packet); break; case BOWLER_GET: ret = onCartesianGet(Packet); break; case BOWLER_CRIT: ret = onCartesianCrit(Packet); break; } setPrintLevel(l); return ret; }
void setOutputMine(int group, float v){ if( dyPid[group].outputChannel==DYPID_NON_USED|| ((pidGroups[group].Enabled == FALSE) && (vel[group].enabled==FALSE))) return; Print_Level l = getPrintLevel(); setPrintLevelNoPrint(); int val = (int)(v); //BYTE center = getBcsIoDataTable()[dyPid[group].outputChannel].PIN.currentConfiguration; if(dyPid[group].outputMode == IS_SERVO){ val += 128; if (val>254) val=254; if(val<0) val=0; }else if(dyPid[group].outputMode == IS_DO){ if(val>0) val=1; else val=0; }else{ val += 128; if (val>255) val=255; if(val<0) val=0; } int set = (int)val; if (dyPid[group].outVal==set){ //if(!(RunEvery(&force[chan->channel])>0)) return; }else{ print_I(" Setting PID output, was ");p_int_I(dyPid[group].outVal);print_I(" is now: ");p_int_I(set);print_I(" on DyIO chan: ");p_int_I(dyPid[group].outputChannel);print_I(", "); } dyPid[group].outVal=set; println_I("PID setting output for group: ");p_int_I(group); SetChannelValueCoProc(dyPid[group].outputChannel,dyPid[group].outVal); setPrintLevel(l); }
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); }
JournalList() : __os(NULL) { setPrintLevel(3); }
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; } }