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; }
void printValues(){ int i; println_I("Values"); for(i=0;i<GetNumberOfIOChannels();i++){ println_I("\t# ");p_int_I(i); print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.currentValue); //print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.previousValue); } }
int resetPositionMine(int group, int current){ println_I("Resetting PID Local ");p_int_I(group);print_I(" to ");p_int_I(current);print_I(" from ");p_fl_I(getPositionMine(group)); if(group<numPidMotors){ setCurrentValue(group, current); }else{ resetHeater(group, current); } return getPositionMine(group); }
void printAsync(){ int i; println_I("Async Data ");p_fl_I(getMs()); for(i=0;i<GetNumberOfIOChannels();i++){ println_I("\t# ");p_int_I(i); print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal); print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.asyncDataPreviousVal); print_I("\tMode ");printAsyncType(getBcsIoDataTable()[i].PIN.asyncDataType); print_I("\tIteration ");p_fl_I(getBcsIoDataTable()[i].PIN.asyncDataTime.setPoint); print_I("\tLast ");p_fl_I(getBcsIoDataTable()[i].PIN.asyncDataTime.MsTime); } }
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 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 printConfigurations(){ int i; println_I("Configurations"); for(i=0;i<GetNumberOfIOChannels();i++){ println_I("\t# ");p_int_I(i); //print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.currentConfiguration); //print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.previousConfiguration); } }
void printModes(){ int i; println_I("Modes"); for(i=0;i<GetNumberOfIOChannels();i++){ println_I("\t# ");p_int_I(i); print_I("\tCurrent ");printMode(getBcsIoDataTable()[i].PIN.currentChannelMode,INFO_PRINT); //print_I("\tPrevious ");printMode(getBcsIoDataTable()[i].PIN.previousChannelMode,INFO_PRINT); } }
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 CheckRev(void) { LoadCorePacket(&downstreamPacketTemp); downstreamPacketTemp.use.head.Method = BOWLER_GET; downstreamPacketTemp.use.head.RPC = GetRPCValue("_rev"); downstreamPacketTemp.use.head.DataLegnth = 4; SendPacketToCoProc(&downstreamPacketTemp); if ((downstreamPacketTemp.use.data[0] == MAJOR_REV) && (downstreamPacketTemp.use.data[1] == MINOR_REV) && (downstreamPacketTemp.use.data[2] == FIRMWARE_VERSION)) { SetColor(0, 0, 1); } else { //SetColor(1, 0, 0); println_I("Rev. Check Failed! AVR:"); p_int_I(downstreamPacketTemp.use.data[0]); print_I("."); p_int_I(downstreamPacketTemp.use.data[1]); print_I("."); p_int_I(downstreamPacketTemp.use.data[2]); print_I(" PIC:"); p_int_I(MAJOR_REV); print_I("."); p_int_I(MINOR_REV); print_I("."); p_int_I(FIRMWARE_VERSION); } }
void printSortedData(){ int x; print_I("Servo Data \r\n[ "); for(x=0;x<dataTableSize;x++){ p_int_I(positionTemp[x]);print_I(" , "); } print_I(" ] "); print_I("\r\n[ "); for(x=0;x<dataTableSize;x++){ p_int_I(sort[x]);print_I(" , "); } print_I(" ] "); print_I("\r\n[ "); for(x=0;x<dataTableSize;x++){ p_int_I(positionTemp[sort[x]]);print_I(" , "); } print_I(" ] "); }
float setLinkAngle(int index, float value, float ms){ int localIndex=linkToHWIndex(index); float v = value/getLinkScale(index); if( index ==0|| index ==1|| index ==2 ){ // if(v>1650){ // println_E("Upper Capped link ");p_int_E(index);print_E(", attempted: ");p_fl_E(value); // v=1650; // } // if(v<-6500){ // v=-6500; // println_E("Lower Capped link ");p_int_E(index);print_E(", attempted: ");p_fl_E(value); // } } println_I("Setting position from cartesian controller ");p_int_I(index);print_I(" to ");p_fl_I(v); return SetPIDTimed(localIndex,v,ms); }
void LoadPIDvals(AbsPID * pid, DYIO_PID * dy){ LoadEEstore(); BYTE i = pid->channel; if(pidEEPRomVal[i].outputChannel==pidEEPRomVal[i].inputChannel) return; if(pidEEPRomVal[i].outputChannel>=GetNumberOfIOChannels() ||pidEEPRomVal[i].inputChannel>=GetNumberOfIOChannels() ) return; if(pidEEPRomVal[i].outputMode==pidEEPRomVal[i].inputMode) return; println_I("Using values for chan: ");p_int_I(i); pid->Enabled=pidEEPRomVal[i].Enabled; pid->Polarity=pidEEPRomVal[i].Polarity; //pidChans->Async=pidEEPRomVal[i].Async; dy->inputMode=pidEEPRomVal[i].inputMode; dy->outputMode=pidEEPRomVal[i].outputMode; dy->outputChannel=pidEEPRomVal[i].outputChannel; dy->inputChannel=pidEEPRomVal[i].inputChannel; pid->K.P=pidEEPRomVal[i].K.P; pid->K.I=pidEEPRomVal[i].K.I; pid->K.D=pidEEPRomVal[i].K.D; }
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); }
BOOL setMode(BYTE pin,BYTE mode){ ClearPinState(pin); println_I("Pin :");p_int_I(pin);print_I(" is mode: ");printMode(mode,INFO_PRINT); //BYTE pwm,dir; if (mode == NO_CHANGE){ return TRUE; } switch (mode){ case HIGH_IMPEDANCE: ClearPinState(pin); // Return here so as not to save this state to the eeprom return TRUE; case IS_UART_TX: case IS_UART_RX: if(pin == 17 || pin == 16){ configPinMode(16,IS_UART_TX,OUTPUT,ON); configPinMode(17,IS_UART_RX,INPUT,ON); InitUART(); return TRUE; } break; case IS_SPI_MOSI: case IS_SPI_MISO: case IS_SPI_SCK: if(pin == 0 || pin == 1||pin == 2 ){ configPinMode(0,IS_SPI_SCK,INPUT,ON); configPinMode(1,IS_SPI_MISO,INPUT,ON); configPinMode(2,IS_SPI_MOSI,INPUT,ON); return TRUE; } break; case IS_ANALOG_IN: configPinMode(pin,mode,INPUT,OFF); if(InitADC(pin)){ return TRUE; } break; case IS_PWM: if(InitPWM(pin)){ return TRUE; } return FALSE; case IS_DC_MOTOR_VEL: case IS_DC_MOTOR_DIR: if(InitDCMotor(pin)){ return TRUE; } return FALSE; case IS_SERVO: InitServo(pin); configPinMode(pin,mode,OUTPUT,OFF); return TRUE; case IS_DO: configPinMode(pin,mode,OUTPUT,OFF); return TRUE; case IS_DI: case IS_PPM_IN: case IS_COUNTER_OUTPUT_INT: case IS_COUNTER_OUTPUT_DIR: case IS_COUNTER_OUTPUT_HOME: case IS_COUNTER_INPUT_INT: case IS_COUNTER_INPUT_DIR: case IS_COUNTER_INPUT_HOME: configPinMode(pin,mode,INPUT,ON); return TRUE; default: configPinMode(pin,mode,INPUT,ON); return TRUE; } return FALSE; }
BOOL setMode(BYTE pin,BYTE mode){ println_I("Setting Mode: ");printMode(mode,INFO_PRINT);print_I(" on: ");p_int_I(pin); BYTE currentMode = GetChannelMode(pin); ClearCounter(pin); StopSPI(pin); clearPPM(pin); print_I(" \tHardware Cleared"); switch (mode){ case IS_SERVO: if(((pin < 12) && (isRegulated_0() == 0)) || ((pin >= 12) && (isRegulated_1()== 0)) ){ print_I("|Mode is now servo"); break; }else{ if(getBrownOutDetect()){ print_I(" Servo Mode could not be set, voltage invalid"); return FALSE; }else{ print_I(" Servo Mode set|"); break; } } break; case IS_SPI_MOSI: case IS_SPI_MISO: case IS_SPI_SCK: if( pinHasFunction(pin, mode) != FALSE){ print_I("|Mode is now SPI"); InitSPI(); break; }else{ return FALSE; } break; case IS_COUNTER_INPUT_INT: case IS_COUNTER_INPUT_DIR: case IS_COUNTER_INPUT_HOME: if(pinHasFunction(pin, mode) != FALSE){ print_I("|Mode is now Counter Input"); StartCounterInput(pin); break; }else{ print_I(", Counter Input not availible"); return FALSE; } break; case IS_COUNTER_OUTPUT_INT: case IS_COUNTER_OUTPUT_DIR: case IS_COUNTER_OUTPUT_HOME: if(pinHasFunction(pin, mode) != FALSE){ print_I("|Mode is now Counter Output"); StartCounterOutput(pin); break; }else{ print_I(", Counter Output not availible"); return FALSE; } break; case IS_PPM_IN: println_I("Setting up PPM..."); startPPM(pin); break; } print_I(" \tMode set"); return TRUE; }