Esempio n. 1
0
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...");
}
Esempio n. 2
0
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();
    }
}
Esempio n. 3
0
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;
}
Esempio n. 4
0
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();
       }


    }
}
Esempio n. 5
0
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));
        }
    }
}
Esempio n. 6
0
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);
}
Esempio n. 7
0
float getLinkAngle(int index){
    int localIndex=linkToHWIndex(index);
    return GetPIDPosition(localIndex)*getLinkScale(index);
}
Esempio n. 8
0
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

}