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...");
}
void interpolateZXY(){
// if(interpolationCounter<5){
// interpolationCounter++;
// return;
// }
interpolationCounter=0;
if(!configured){
HomeLinks();
return;
}
keepCartesianPosition=TRUE;
if(keepCartesianPosition){
float x=0,y=0,z=0;
float ms= getMs();
x = interpolate((INTERPOLATE_DATA *)&intCartesian[0],ms);
y = interpolate((INTERPOLATE_DATA *)&intCartesian[1],ms);
z = interpolate((INTERPOLATE_DATA *)&intCartesian[2],ms);
if(isCartesianInterpolationDone() == FALSE){
// println_W("Interp \r\n\tx=");p_fl_W(x);print_W(" \tc=");p_fl_W(xCurrent);
//
// print_W("\r\n\ty=");p_fl_W(y);print_W(" \tc=");p_fl_W(yCurrent);
//
// print_W("\r\n\tz=");p_fl_W(z);print_W(" \tc=");p_fl_W(zCurrent);
setXYZ( x, y, z, 0);
}else if( FifoGetPacketCount(&packetFifo)>0){
println_W("Loading new packet ");
if(FifoGetPacket(&packetFifo,&linTmpPack)){
processLinearInterpPacket(&linTmpPack);
}
}else{
keepCartesianPosition=FALSE;
}
}
}
uint8_t getInterpolatedPin(uint8_t pin){
if(GetChannelMode(pin)!=IS_SERVO){
return 0;
}
//char cSREG;
//cSREG = SREG;
/* store SREG value */
/*
disable interrupts during timed sequence */
//StartCritical();
float ip = interpolate(&velocity[pin],getMs());
//SREG = cSREG;
boolean error = false;
if(ip>(255- SERVO_BOUND)){
println_W("Upper=");
error = true;
}
if(ip<SERVO_BOUND){
println_W("Lower=");
error = true;
}
int dataTableSet = (getDataTableCurrentValue(pin)&0x000000ff);
int interpolatorSet = ((int32_t)velocity[pin].set);
float time = (float)((getDataTableCurrentValue(pin)>>16)&0x0000ffff);
if(dataTableSet!=interpolatorSet){
// println_W("Setpoint=");
// error = true;
SetServoPosDataTable( pin, dataTableSet,time);
}
if(error){
// p_fl_W(ip);print_W(" on chan=");p_int_W(pin);print_W(" target=");p_int_W(interpolatorSet);
// print_W(" Data Table=");p_int_W(dataTableSet);
// println_W("set= \t");p_fl_W(velocity[pin].set);
// println_W("start= \t");p_fl_W(velocity[pin].start);
// println_W("setTime= \t");p_fl_W(velocity[pin].setTime);
// println_W("startTime=\t");p_fl_W(velocity[pin].startTime);
ip=velocity[pin].set;
SetServoPosDataTable(pin, dataTableSet,time);
}
int tmp = (int)ip;
return tmp;
}
BYTE setInterpolateXYZ(float x, float y, float z,float ms){
int i=0;
if(ms<.01)
ms=0;
float start = getMs();
intCartesian[0].set=x;
intCartesian[1].set=y;
intCartesian[2].set=z;
updateCurrentPositions();
intCartesian[0].start=xCurrent;
intCartesian[1].start=yCurrent;
intCartesian[2].start=zCurrent;
println_W("\n\nSetting new position x=");p_fl_W(x);print_W(" y=");p_fl_W(y);print_W(" z=");p_fl_W(z);print_W(" Time MS=");p_fl_W(ms);
println_W("Current position cx=");p_fl_W(xCurrent);
print_W(" cy=");p_fl_W(yCurrent);
print_W(" cz=");p_fl_W(zCurrent);
println_W("Current angles Alpha=");p_fl_W(getLinkAngle(0));print_W(" Beta=");p_fl_W(getLinkAngle(1));print_W(" Gamma=");p_fl_W(getLinkAngle(2));
for(i=0;i<3;i++){
intCartesian[i].setTime=ms;
intCartesian[i].startTime=start;
}
if(ms==0){
setXYZ( x, y, z,0);
}else{
keepCartesianPosition=TRUE;
float ms= getMs();
x = interpolate((INTERPOLATE_DATA *)&intCartesian[0],ms);
y = interpolate((INTERPOLATE_DATA *)&intCartesian[1],ms);
z = interpolate((INTERPOLATE_DATA *)&intCartesian[2],ms);
setXYZ( x, y, z,0);
}
}
BOOL bcsIoAsyncEventCallback(BowlerPacket *Packet,BOOL (*pidAsyncCallbackPtr)(BowlerPacket *)){
int i;
BOOL update=FALSE;
println_W("Async ");print_W(ioNSName);
for(i=0;i<GetNumberOfIOChannels();i++){
//println_W("Checking ");p_int_W(i);
if(pushAsyncReady(i)){
update=TRUE;
}
}
if(update){
println_I(__FILE__);println_I("Async: ");
populateGACV(Packet);
Packet->use.head.Method=BOWLER_ASYN;
FixPacket(Packet);
printBowlerPacketDEBUG(Packet,INFO_PRINT);
if(pidAsyncCallbackPtr!=NULL){
pidAsyncCallbackPtr(Packet);
}
}
println_W("Done ");print_W(ioNSName);
return FALSE;
}
BOOL initFlashLocal(){
if(bytesOfRaw > 0x1000-FLASHSTORE){
println_E("Too much data to store");
SoftReset();
}
println_W("Size of Flash data = ");p_int_W(bytesOfRaw);
SetFlashData( localData.data ,bytesOfRaw/4);
FlashLoad();
int i=0,j=0, index;
BOOL rawFlashDetect=FALSE;
for(i=0;i<numPidTotal;i++){
index = i*sizeof(AbsPID_Config);
for(j=0;j<sizeof(AbsPID_Config)/4;j++){
getPidGroupDataTable()[i].raw[j]=localData.data[index+j];
}
if( (getPidGroupDataTable()[i].config.Enabled != 1 &&
getPidGroupDataTable()[i].config.Enabled != 0) ){
rawFlashDetect = TRUE;
println_E("Detected raw flash, setting defaults : ");p_int_E(i);
printPIDvals(i);
getPidGroupDataTable()[i].config.Enabled = FALSE;
getPidGroupDataTable()[i].config.Async=0;
getPidGroupDataTable()[i].config.IndexLatchValue=0;
getPidGroupDataTable()[i].config.stopOnIndex=0;
getPidGroupDataTable()[i].config.useIndexLatch=0;
getPidGroupDataTable()[i].config.K.P=.1;
getPidGroupDataTable()[i].config.K.I=0;
getPidGroupDataTable()[i].config.K.D=0;
getPidGroupDataTable()[i].config.V.P=.1;
getPidGroupDataTable()[i].config.V.D=0;
getPidGroupDataTable()[i].config.Polarity=1;
getPidGroupDataTable()[i].config.stop=0;
getPidGroupDataTable()[i].config.upperHistoresis=0;
getPidGroupDataTable()[i].config.lowerHistoresis=0;
}
}
if(rawFlashDetect )
writeFlashLocal();
return !rawFlashDetect;
}
void setOutput(int group, float val) {
if(bound(0,getPidGroupDataTable(group)->config.tipsScale, .001, .001)){
println_W("PID TPS Sclale close to zero");p_fl_W(getPidGroupDataTable(group)->config.tipsScale);
}
val *= getPidGroupDataTable(group)->config.tipsScale;
val += getPidStop(group);
if (val > getPidStop(group) && val < getUpperPidHistoresis(group) ){
val = getUpperPidHistoresis(group);
println_E("Upper histerisys");
}
if (val < getPidStop(group) && val > getLowerPidHistoresis(group)){
val = getLowerPidHistoresis(group);
println_E("Lower histerisys");
}
getPidGroupDataTable(group)->OutputSet = val;
//
setOutputLocal(group, val);
}
void writeFlashLocal(){
if(bytesOfRaw > 0x1000-FLASHSTORE){
println_E("Too much data to store");
SoftReset();
}
println_W("Writing values to Flash");
int i=0,j=0, index;
for(i=0;i<numPidTotal;i++){
index = i*sizeof(AbsPID_Config);
for(j=0;j<sizeof(AbsPID_Config)/4;j++){
localData.data[index+j]=getPidGroupDataTable()[i].raw[j];
}
}
FlashSync();
FlashLoad();
for(i=0;i<numPidTotal;i++){
printPIDvals(i);
}
}
void UserInit(void){
//setPrintStream(&USBPutArray);
setPrintLevelInfoPrint();
println_I("\n\nStarting PIC initialization");
//DelayMs(1000);
hardwareInit();
println_I("Hardware Init done");
ReleaseAVRReset();
CheckRev();
LoadEEstore();
LoadDefaultValues();
CartesianControllerInit();
InitPID();
UpdateAVRLED();
lockServos();
setPrintLevelInfoPrint();
BOOL brown = getEEBrownOutDetect();
setCoProcBrownOutMode(brown);
setBrownOutDetect(brown);
println_I("###Starting PIC In Debug Mode###\n");// All printfDEBUG functions do not need to be removed from code if debug is disabled
DelayMs(1000);
//setPrintLevelErrorPrint();
println_E("Error level printing");
println_W("Warning level printing");
println_I("Info level printing");
}
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();
}
}
}
boolean GetName(char * name) {
//WORD_VAL raw;
uint8_t i = 0;
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_GET;
downstreamPacketTemp.use.head.RPC = GetRPCValue(eepd);
downstreamPacketTemp.use.data[0] = NAMESTART;
downstreamPacketTemp.use.data[1] = LOCKSTART;
downstreamPacketTemp.use.head.DataLegnth = 6;
SendPacketToCoProc(&downstreamPacketTemp);
printPacket(&downstreamPacketTemp,WARN_PRINT);
while (downstreamPacketTemp.use.data[i] != '\0') {
name[i] = downstreamPacketTemp.use.data[i];
i++;
buttonCheck(11);
if (i == NAMESIZE)
break;
}
name[i] = '\0';
println_W(name);
return isAscii(name);
}
void ProcessAsyncData(BowlerPacket * Packet){
//println_I("**Got Async Packet**");
//printPacket(Packet,INFO_PRINT);
Print_Level l = getPrintLevel();
setPrintLevelInfoPrint();
if (Packet->use.head.RPC==GCHV){
BYTE pin = Packet->use.data[0];
BYTE mode = GetChannelMode(pin);
if(mode == IS_ANALOG_IN ){
UINT16_UNION ana;
ana.byte.SB = Packet->use.data[1];
ana.byte.LB = Packet->use.data[2];
//ADC_val[pin-8]=ana.Val;
if(ana.Val>=0 && ana.Val<1024)
SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val;
println_I("***Setting analog value: ");p_int_I(pin);print_I(", ");p_int_I(ana.Val);
}
else if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME) || mode == IS_SERVO){
//DIG_val[pin]=Packet->use.data[1];
SetValFromAsync(pin,Packet->use.data[1]);//asyncData[pin].currentVal=Packet->use.data[1];
println_I("***Setting digital value: ");p_int_I(pin);print_I(", ");p_int_I(Packet->use.data[1]);//printStream(DIG_val,NUM_PINS);
}else {
if(IsAsync(pin)){
println_I("Sending async packet, not digital or analog");
PutBowlerPacket(Packet);
}
}
}else if (Packet->use.head.RPC==AASN){
int i;
for(i=0;i<8;i++){
BYTE pin = i+8;
BYTE mode = GetChannelMode(pin);
if(mode == IS_ANALOG_IN ){
UINT16_UNION ana;
ana.byte.SB = Packet->use.data[i*2];
ana.byte.LB = Packet->use.data[(i*2)+1];
//ADC_val[pin-8]=ana.Val
if(ana.Val>=0 && ana.Val<1024);
SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val;
}
}
}else if (Packet->use.head.RPC==DASN){
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
BYTE mode = GetChannelMode(i);
if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME)|| (mode == IS_SERVO)){
SetValFromAsync(i,Packet->use.data[i]);//asyncData[i].currentVal=Packet->use.data[i];
}
}
//println_I("***Setting All Digital value: ");
}if (Packet->use.head.RPC==GetRPCValue("gacv")){
int i;
int val;
for(i=0;i<GetNumberOfIOChannels();i++){
val = get32bit(Packet, i*4);
if(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal!=val){
println_I("Data on Pin ");p_int_I(i);print_I(" to val ");p_int_I(val);
SetValFromAsync(i,val);//
}
}
}else{
println_W("***Async packet not UNKNOWN***");
printPacket(Packet,WARN_PRINT);
}
// println_I("***Setting All value: [");
// int i;
// for(i=0;i<NUM_PINS;i++){
// p_int_I(asyncData[i].currentVal);print_I(" ");
// }
// print_I("]");
setPrintLevel(l);
}
void ReleaseAVRReset(void){
AVR_RST_IO(1);
println_E("Starting AVR...");
DelayMs(1000);
println_W("AVR started");
}
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;
}
}
BOOL bcsSafeAsyncEventCallback(BowlerPacket *Packet,BOOL (*pidAsyncCallbackPtr)(BowlerPacket *Packet)){
println_W("Async ");print_W(safeNSName);
return FALSE;
}
void InitPID(void){
//println_I("Starting PID initialization ");
uint8_t i;
//uint16_t loop;
for (i=0;i<NUM_PID_GROUPS;i++){
//DyPID values
dyPid[i].inputChannel=DYPID_NON_USED;
dyPid[i].outputChannel=DYPID_NON_USED;
dyPid[i].inputMode=0;
dyPid[i].outputMode=0;
dyPid[i].flagValueSync=false;
pidGroups[i].config.tipsScale=1.0;
pidGroups[i].config.Enabled = false;
pidGroups[i].config.Async = 1;
pidGroups[i].config.IndexLatchValue = 0;
pidGroups[i].config.stopOnIndex = 0;
pidGroups[i].config.useIndexLatch = 0;
pidGroups[i].config.K.P = .1;
pidGroups[i].config.K.I = 0;
pidGroups[i].config.K.D = 0;
pidGroups[i].config.V.P = .1;
pidGroups[i].config.V.D = 0;
pidGroups[i].config.Polarity = 0;
pidGroups[i].config.stop = 0;
pidGroups[i].config.upperHistoresis = 0;
pidGroups[i].config.lowerHistoresis = 0;
pidGroups[i].config.offset = 0.0;
pidGroups[i].config.calibrationState = CALIBRARTION_DONE;
pidGroups[i].interpolate.set=0;
pidGroups[i].interpolate.setTime=0;
pidGroups[i].interpolate.start=0;
pidGroups[i].interpolate.startTime=0;
limits[i].type=NO_LIMIT;
LoadPIDvals(&pidGroups[i],&dyPid[i],i);
if( dyPid[i].inputChannel==DYPID_NON_USED ||
dyPid[i].outputChannel==DYPID_NON_USED ||
dyPid[i].outputChannel==dyPid[i].inputChannel||
pidGroups[i].config.Enabled !=true)
{
dyPid[i].inputChannel=DYPID_NON_USED;
dyPid[i].outputChannel=DYPID_NON_USED;
pidGroups[i].config.Enabled = false;
WritePIDvalues(&pidGroups[i],&dyPid[i],i);
}
force[i].MsTime=0;
force[i].setPoint=200;
}
InitilizePidController( pidGroups,
NUM_PID_GROUPS,
&getPositionMine,
&setOutputMine,
&resetPositionMine,
&onPidConfigureMine,
&checkPIDLimitEventsMine);
for (i=0;i<NUM_PID_GROUPS;i++){
//
if(pidGroups[i].config.Enabled){
println_W("PID ");p_int_W(i);
printPIDvals(i);
initPIDChans(i);
int value = getPositionMine(i);
pidGroups[i].CurrentState=value;
pidReset(i,value);
}
}
}
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
}
