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
}
