void InitilizePidController(AbsPID * groups, int numberOfGroups,
float (*getPositionPtr)(int),
void (*setOutputPtr)(int, float),
int (*resetPositionPtr)(int, int),
void (*onPidConfigurePtr)(int),
PidLimitEvent* (*checkPIDLimitEventsPtr)(uint8_t group)) {
if (groups == 0 ||
getPositionPtr == 0 ||
setOutputPtr == 0 ||
resetPositionPtr == 0 ||
checkPIDLimitEventsPtr == 0 ||
onPidConfigurePtr == 0) {
println("Null pointer exception in PID Configure", ERROR_PRINT);
while (1);
}
pidGroupsInternal =groups;
number_of_pid_groups = numberOfGroups;
getPosition = getPositionPtr;
setOutputLocal = setOutputPtr;
resetPosition = resetPositionPtr;
onPidConfigureLocal = onPidConfigurePtr;
checkPIDLimitEvents = checkPIDLimitEventsPtr;
SetControllerMath(&RunAbstractPIDCalc);
int i;
for (i = 0; i < numberOfGroups; i++) {
int enabled = getPidGroupDataTable(i)->config.Enabled;
pidReset(i, 0);
//getPidGroupDataTable(i)->config.Enabled = enabled;
SetPIDEnabled(i, enabled);
//println_I("PID ");p_int_I(i);print_I(" enabled");
}
}
void startHomingLink(int group, PidCalibrationType type, float homedValue) {
float speed = 20.0;
if (type == CALIBRARTION_home_up)
speed *= 1.0;
else if (type == CALIBRARTION_home_down)
speed *= -1.0;
else {
println_E("Invalid homing type");
return;
}
getPidGroupDataTable(group)->config.tipsScale = 1;
SetPIDCalibrateionState(group, type);
setOutput(group, speed);
getPidGroupDataTable(group)->timer.MsTime = getMs();
getPidGroupDataTable(group)->timer.setPoint = 1000;
getPidGroupDataTable(group)->homing.homingStallBound = 20;
getPidGroupDataTable(group)->homing.previousValue = GetPIDPosition(group);
getPidGroupDataTable(group)->homing.lastTime = getMs();
getPidGroupDataTable(group)->homing.homedValue = homedValue;
SetPIDEnabled(group,true);
}
void runPidHysterisisCalibration(int group) {
if (!getPidGroupDataTable(group)->config.Enabled) {
println_E("Axis disabled for calibration #");
p_int_E(group);
getPidGroupDataTable(group)->config.Enabled = true;
}
getPidGroupDataTable(group)->config.lowerHistoresis = 0;
getPidGroupDataTable(group)->config.upperHistoresis = 0;
getPidGroupDataTable(group)->config.stop = 0;
// println_I("\tReset PID");
pidReset(group, 0); // Zero encoder reading
// println_I("\tDisable PID Output");
SetPIDEnabled(group, true);
SetPIDCalibrateionState(group, CALIBRARTION_hysteresis);
getPidGroupDataTable(group)->calibration.state = forward;
// println_I("\tSetting slow move");
setOutput(group, -1.0f);
getPidGroupDataTable(group)->timer.setPoint = 2000;
getPidGroupDataTable(group)->timer.MsTime = getMs();
}
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
}
