uint8_t ConfigPID(BowlerPacket * Packet){ uint8_t chan = Packet->use.data[0]; // int i; // println_W("Starting config"); // for(i=0;i<getNumberOfPidChannels();i++){ // printPIDvals(i); // } getPidGroupDataTable(chan)->config.Polarity = ((Packet->use.data[2]==0)?0:1); getPidGroupDataTable(chan)->config.Async = ((Packet->use.data[3]==0)?0:1); float KP=0; float KI=0; float KD=0; float temp=0; KP=(float)get32bit(Packet,4); KI=(float)get32bit(Packet,8); KD=(float)get32bit(Packet,12); if(Packet->use.head.DataLegnth>(4+16)){ temp=(float)get32bit(Packet,16); getPidGroupDataTable(chan)->config.useIndexLatch= Packet->use.data[20]; getPidGroupDataTable(chan)->config.stopOnIndex = Packet->use.data[21]; getPidGroupDataTable(chan)->config.stop=(float)get32bit(Packet,22)/1000.0; getPidGroupDataTable(chan)->config.upperHistoresis=(float)get32bit(Packet,26)/1000.0; getPidGroupDataTable(chan)->config.lowerHistoresis=(float)get32bit(Packet,30)/1000.0; getPidGroupDataTable(chan)->config.calibrationState = CALIBRARTION_DONE; }else{ temp=0; getPidGroupDataTable(chan)->config.useIndexLatch= true; getPidGroupDataTable(chan)->config.stopOnIndex = true; getPidGroupDataTable(chan)->config.stop=0; getPidGroupDataTable(chan)->config.upperHistoresis=0; getPidGroupDataTable(chan)->config.lowerHistoresis=0; } getPidGroupDataTable(chan)->config.IndexLatchValue=(float)temp; getPidGroupDataTable(chan)->config.K.P=KP/100; getPidGroupDataTable(chan)->config.K.I=KI/100; getPidGroupDataTable(chan)->config.K.D=KD/100; //b_println("Resetting PID channel from Config:",INFO_PRINT);printBowlerPacketDEBUG(Packet,INFO_PRINT); //b_println("From Config Current setpoint:",INFO_PRINT);p_fl(getPidGroupDataTable(chan)->SetPoint,INFO_PRINT); OnPidConfigure(chan); getPidGroupDataTable(chan)->config.Enabled = ((Packet->use.data[1]==0)?0:1); return true; }
void initializeCartesianController(){ InitPacketFifo(&packetFifo,buffer,SIZE_OF_PACKET_BUFFER); int i=0; for(i=0;i<3;i++){ if(getPidGroupDataTable()[linkToHWIndex(i)].config.Enabled!=TRUE){ getPidGroupDataTable()[linkToHWIndex(i)].config.Enabled=TRUE; getPidGroupDataTable()[linkToHWIndex(i)].config.Polarity=TRUE; getPidGroupDataTable()[linkToHWIndex(i)].config.K.P=.12; getPidGroupDataTable()[linkToHWIndex(i)].config.K.I=.4; OnPidConfigure(linkToHWIndex(i)); } } }
uint8_t ConfigPDVelovity(BowlerPacket * Packet){ uint8_t chan = Packet->use.data[0]; float KP=0; float KD=0; KP=(float)get32bit(Packet,1); KD=(float)get32bit(Packet,5); getPidGroupDataTable(chan)->config.V.P=KP/100.0; getPidGroupDataTable(chan)->config.V.D=KD/100.0; OnPidConfigure(chan); return true; }
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 }