void startHomingLinks(){
println_W("Homing links for kinematics");
homingAllLinks =TRUE;
startHomingLink(linkToHWIndex(0), CALIBRARTION_home_down);
startHomingLink(linkToHWIndex(1), CALIBRARTION_home_down);
startHomingLink(linkToHWIndex(2), CALIBRARTION_home_down);
println_W("Started Homing...");
}
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();
}
}
BOOL isCartesianInterpolationDone(){
updateCurrentPositions();
float targets[3] = {xCurrent,yCurrent,zCurrent};
int setpointBound = 200;
float mmPositionResolution = .3;
int i;
for(i=0;i<4;i++){
if(i<3){
if(!bound(intCartesian[i].set,targets[i],mmPositionResolution,mmPositionResolution)){
//println_W("Interpolation not done on to: ");p_fl_W(intCartesian[i].set);print_W(" is = ");p_fl_W(targets[i]);
return FALSE;
}
}
if( (isPIDArrivedAtSetpoint(linkToHWIndex(i), setpointBound) == FALSE) && (i==3) ){
println_W("LINK not done moving index = ");p_int_W(linkToHWIndex(i));
print_W(" currently is = "); p_fl_W(getPidGroupDataTable()[linkToHWIndex(i)].CurrentState);
print_W(" heading towards = "); p_fl_W(getPidGroupDataTable()[linkToHWIndex(i)].SetPoint);
return FALSE;
}
}
return TRUE;
}
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 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));
}
}
}
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);
}
float getLinkAngle(int index){
int localIndex=linkToHWIndex(index);
return GetPIDPosition(localIndex)*getLinkScale(index);
}
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
}
