int main(){ //setPrintLevelInfoPrint(); setPrintLevelWarningPrint(); //setPrintLevelNoPrint(); hardwareInit(); RunEveryData loop = {0.0,2000.0}; while(1){ if (_RF5==1){ setPrintLevelErrorPrint(); p_int_E(0);print_E(" Reset Button Pressed from loop"); SetColor(1,1,1); U1CON = 0x0000; DelayMs(100); Reset(); } if(RunEvery(&loop)>0){ // clearPrint(); // printCartesianData(); } if( printCalibrations == false && GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE ){ printCalibrations = true; int index=0; for(index=0;index<3;index++){ int group = linkToHWIndex(index); println_E("For Axis ");p_int_E(group); print_E(" upper: ");p_int_E(getPidGroupDataTable(group)->config.upperHistoresis); print_E(" lower: ");p_int_E(getPidGroupDataTable(group)->config.lowerHistoresis); print_E(" stop: ");p_int_E(getPidGroupDataTable(group)->config.stop); } startHomingLinks(); //Print_Level l= getPrintLevel(); //setPrintLevelInfoPrint(); printCartesianData(); int i; for(i=0;i<numPidMotors;i++){ printPIDvals(i); } //setPrintLevel(l); } bowlerSystem(); } }
void RunPIDControl() { int i; for (i = 0; i < getNumberOfPidChannels(); i++) { //println_E(" PID Loop ");p_int_E(i); getPidGroupDataTable(i)->CurrentState = getPosition(i) - getPidGroupDataTable(i)->config.offset; if (getPidGroupDataTable(i)->config.Enabled == true) { getPidGroupDataTable(i)->SetPoint = interpolate(getPidInterpolationDataTable(i), getMs()); MathCalculationPosition(getPidGroupDataTable(i), getMs()); if (GetPIDCalibrateionState(i) <= CALIBRARTION_DONE) { setOutput(i, getPidGroupDataTable(i)->Output); } else if (GetPIDCalibrateionState(i) == CALIBRARTION_hysteresis) { pidHysterisis(i); } else if ((GetPIDCalibrateionState(i) == CALIBRARTION_home_down) || (GetPIDCalibrateionState(i) == CALIBRARTION_home_up) || (GetPIDCalibrateionState(i) == CALIBRARTION_home_velocity)) { checkLinkHomingStatus(i); } } } }
void HomeLinks(){ if(homingAllLinks){ if ( GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE ){ homingAllLinks = FALSE; configured = TRUE; println_W("All linkes reported in"); pidReset(hwMap.Extruder0.index,0); int i; float Alpha,Beta,Gama; servostock_calcInverse(0, 0, getmaxZ(), &Alpha, &Beta, &Gama); for(i=0;i<3;i++){ pidReset(linkToHWIndex(i), (Alpha+getRodLength()/3)/getLinkScale(i)); } initializeCartesianController(); cancelPrint(); } } }
void RunPDVel(uint8_t chan){ //println_I("Running PID vel"); if(getPidVelocityDataTable(chan)->enabled==true) { getPidGroupDataTable(chan)->Output=runPdVelocityFromPointer(getPidVelocityDataTable(chan), getPidGroupDataTable(chan)->CurrentState, getPidGroupDataTable(chan)->config.V.P, getPidGroupDataTable(chan)->config.V.D ); if(GetPIDCalibrateionState(chan)<=CALIBRARTION_DONE) setOutput(chan,getPidGroupDataTable(chan)->Output); } }
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; } }
void hardwareInit(){ // Configure the device for maximum performance but do not change the PBDIV // Given the options, this function will change the flash wait states, RAM // wait state and enable prefetch cache but will not change the PBDIV. // The PBDIV value is already set via the pragma FPBDIV option above.. SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE); SYSTEMConfigPerformance(80000000); (_TRISF5)=INPUT; // for the reset sw ATX_DISENABLE(); CloseTimer2(); Pic32_Bowler_HAL_Init(); Bowler_Init(); clearPrint(); println_I("\n\n\nStarting PIC initialization"); FlashGetMac(MyMAC.v); DelayMs(2000);//This si to prevent runaway during programming // enable driven to 3.3v on uart 1 mPORTDOpenDrainClose(BIT_3); mPORTFOpenDrainClose(BIT_5); char macStr[13]; for (i=0;i<6;i++){ macStr[j++]=GetHighNib(MyMAC.v[i]); macStr[j++]=GetLowNib(MyMAC.v[i]); } macStr[12]=0; println_I("MAC address is ="); print_I(macStr); char * dev = "BowlerDevice"; println_I(dev); //This Method calls INTEnableSystemMultiVectoredInt(); usb_CDC_Serial_Init(dev,macStr,0x04D8,0x0001); addNamespaceToList((NAMESPACE_LIST *)getBcsCartesianNamespace()); addNamespaceToList((NAMESPACE_LIST *)getBcsPidNamespace()); ATX_ENABLE(); // Turn on ATX Supply, Must be called before talking to the Encoders!! println_I("Starting Encoders"); initializeEncoders();// Power supply must be turned on first println_I("Starting Heater"); initializeHeater(); println_I("Starting Servos"); initServos(); #if !defined(NO_PID) println_I("Starting PID"); initPIDLocal(); #endif initializeCartesianController(); DelayMs(100); if( GetPIDCalibrateionState(linkToHWIndex(0))!=CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(1))!=CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(2))!=CALIBRARTION_DONE ){ for(i=0;i<numPidMotors;i++){ SetPIDEnabled(i,true) ; } println_I("Running calibration for kinematics axis"); runPidHysterisisCalibration(linkToHWIndex(0)); runPidHysterisisCalibration(linkToHWIndex(1)); runPidHysterisisCalibration(linkToHWIndex(2)); DelayMs(100);//wait for ISR to fire and update all values for(i=0;i<3;i++){ setPIDConstants(linkToHWIndex(i),.2,.1,0); } OnPidConfigure(0); }else{ println_W("Axis are already calibrated"); } pid.MsTime=getMs(); //startHomingLinks(); disableSerialComs(true) ; (_TRISB0)=1; SetColor(1,1,1); HEATER_2_TRIS = OUTPUT; //HEATER_1_TRIS = OUTPUT; // Causes one of the axies to crawl downward in bursts when enabled and on... //HEATER_0_TRIS = OUTPUT; // causes device to twitc. These are touched by the USB stack somehow..... and as the reset button }