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;
}
}
