Exemplo n.º 1
0
void CheckRev(void) {
	LoadCorePacket(&downstreamPacketTemp);
	downstreamPacketTemp.use.head.Method = BOWLER_GET;
	downstreamPacketTemp.use.head.RPC = GetRPCValue("_rev");
	downstreamPacketTemp.use.head.DataLegnth = 4;
	SendPacketToCoProc(&downstreamPacketTemp);
	if ((downstreamPacketTemp.use.data[0] == MAJOR_REV)
			&& (downstreamPacketTemp.use.data[1] == MINOR_REV)
			&& (downstreamPacketTemp.use.data[2] == FIRMWARE_VERSION)) {
		SetColor(0, 0, 1);
	} else {
		//SetColor(1, 0, 0);
		println_I("Rev. Check Failed! AVR:");
		p_int_I(downstreamPacketTemp.use.data[0]);
		print_I(".");
		p_int_I(downstreamPacketTemp.use.data[1]);
		print_I(".");
		p_int_I(downstreamPacketTemp.use.data[2]);
		print_I(" PIC:");
		p_int_I(MAJOR_REV);
		print_I(".");
		p_int_I(MINOR_REV);
		print_I(".");
		p_int_I(FIRMWARE_VERSION);

	}
}
Exemplo n.º 2
0
float runPdVelocityFromPointer(PD_VEL* vel, float currentState,float KP, float KD){

    		float currentTime = getMs();
		float timeMsDiff =  (currentTime -vel->lastTime);
		float timeDiff =  timeMsDiff/1000;
		float posDiff=currentState -vel->lastPosition;
		float currentVelocity = posDiff/timeDiff;
		//float velocityDiff = currentVelocity-vel->lastVelocity;
		float velocityDiff=0;
		float proportional =  currentVelocity-vel->unitsPerSeCond;
		float set = (proportional*KP)+(velocityDiff*KD)*timeMsDiff;
		vel->currentOutputVel-=(set);

		if (vel->currentOutputVel>200){
			vel->currentOutputVel=200;
                }else if(vel->currentOutputVel<-200){
			vel->currentOutputVel=-200;
                }

		println_I("\t Velocity: set=   ");p_fl_I(vel->unitsPerSeCond );print_I(" ticks/seCond" );
                println_I("\t current state=   ");p_fl_I(currentState );print_I(" ticks" );
                println_I("\t last state=      ");p_fl_I(vel->lastPosition );print_I(" ticks" );
		println_I("\t position diff=   ");p_fl_I(posDiff );print_I(" ticks" );
		println_I("\t MS diff=         ");p_fl_I(timeMsDiff );
		println_I("\t current=         ");p_fl_I(currentVelocity );print_I(" ticks/seCond" );
		println_I("\t Velocity offset= ");p_fl_I(set );
		println_I("\t Velocity set=    ");p_fl_I(vel->currentOutputVel );

		//cleanup
		vel->lastPosition=currentState;
		vel->lastVelocity=currentVelocity;
		vel->lastTime=currentTime;
                return vel->currentOutputVel;
}
Exemplo n.º 3
0
BYTE setXYZ(float x, float y, float z,float ms){
    updateCurrentPositions();
    float t0=0,t1=0,t2=0;
    if(hwMap.iK_callback( x,  y, z,  &t0, &t1, &t2)==0){
        println_I("New target angles t1=");p_fl_I(t0);print_I(" t2=");p_fl_I(t1);print_I(" t3=");p_fl_I(t2);
        setLinkAngle(0,t0,ms);
        setLinkAngle(1,t1,ms);
        setLinkAngle(2,t2,ms);
    }else{
        println_E("Interpolate failed, can't reach: x=");p_fl_E(x);print_E(" y=");p_fl_E(y);print_E(" z=");p_fl_E(z);
    }
}
void printAsync(){
	int i;
	println_I("Async Data ");p_fl_I(getMs());
	for(i=0;i<GetNumberOfIOChannels();i++){
		println_I("\t# ");p_int_I(i);
		print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal);
		print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.asyncDataPreviousVal);
		print_I("\tMode ");printAsyncType(getBcsIoDataTable()[i].PIN.asyncDataType);
		print_I("\tIteration ");p_fl_I(getBcsIoDataTable()[i].PIN.asyncDataTime.setPoint);
		print_I("\tLast ");p_fl_I(getBcsIoDataTable()[i].PIN.asyncDataTime.MsTime);
	}
}
void printModes(){
	int i;
	println_I("Modes");
	for(i=0;i<GetNumberOfIOChannels();i++){
		println_I("\t# ");p_int_I(i);
		print_I("\tCurrent ");printMode(getBcsIoDataTable()[i].PIN.currentChannelMode,INFO_PRINT);
		//print_I("\tPrevious ");printMode(getBcsIoDataTable()[i].PIN.previousChannelMode,INFO_PRINT);
	}
}
void printValues(){
	int i;
	println_I("Values");
	for(i=0;i<GetNumberOfIOChannels();i++){
		println_I("\t# ");p_int_I(i);
		print_I("\tCurrent ");p_int_I(getBcsIoDataTable()[i].PIN.currentValue);
		//print_I("\tPrevious ");p_int_I(getBcsIoDataTable()[i].PIN.previousValue);
	}
}
Exemplo n.º 7
0
int resetPositionMine(int group, int current){
    println_I("Resetting PID Local ");p_int_I(group);print_I(" to ");p_int_I(current);print_I(" from ");p_fl_I(getPositionMine(group));
    if(group<numPidMotors){
        setCurrentValue(group, current);
    }else{
        resetHeater(group, current);
    }
    return getPositionMine(group);
}
Exemplo n.º 8
0
void setOutputMine(int group, float v){

	if( dyPid[group].outputChannel==DYPID_NON_USED||
			((pidGroups[group].Enabled == FALSE) && (vel[group].enabled==FALSE)))
		return;
	Print_Level l = getPrintLevel();
	setPrintLevelNoPrint();
	int val = (int)(v);
	//BYTE center = getBcsIoDataTable()[dyPid[group].outputChannel].PIN.currentConfiguration;

	if(dyPid[group].outputMode == IS_SERVO){
		val += 128;
		if (val>254)
			val=254;
		if(val<0)
			val=0;
	}else if(dyPid[group].outputMode == IS_DO){
		if(val>0)
			val=1;
		else
			val=0;
	}else{
		val += 128;
		if (val>255)
			val=255;
		if(val<0)
			val=0;
	}
	int set = (int)val;
	if (dyPid[group].outVal==set){
		//if(!(RunEvery(&force[chan->channel])>0))
			return;
	}else{
		print_I(" Setting PID output, was ");p_int_I(dyPid[group].outVal);print_I(" is now: ");p_int_I(set);print_I(" on DyIO chan: ");p_int_I(dyPid[group].outputChannel);print_I(", ");
	}
	dyPid[group].outVal=set;

	println_I("PID setting output for group: ");p_int_I(group);
	SetChannelValueCoProc(dyPid[group].outputChannel,dyPid[group].outVal);
	setPrintLevel(l);
}
Exemplo n.º 9
0
void CheckSwitches(void){

	Print_Level l = getPrintLevel();
	setPrintLevelInfoPrint();
	switched=0;
	volt = GetRawVoltage();
	BOOL up = FALSE;

	BYTE reg = isRegulated_0();
	if (bankState[0] != reg ){
		bankState[0]=reg;
		up=TRUE;
	}


	reg = isRegulated_1();
	if (bankState[1] != reg){
		bankState[1] = reg;
		up=TRUE;
	}

	if ((lastVolt>RawVoltageMin) && (volt<RawVoltageMin)){
		up=TRUE;
		lastVolt = volt;
	}
	if ((lastVolt<RawVoltageMin) && (volt>RawVoltageMin)){
		up=TRUE;
		lastVolt = volt;
	}

	if(up){
		println_I("\nVoltage on raw:   \t");
		p_fl_I(volt);

		println_I("Voltage on bank0: \t");
		p_fl_I(GetRail0Voltage());

		println_I("Voltage on bank1:\t");
		p_fl_I(GetRail1Voltage());
		println_I("Pushing upstream Power Packet bank 0: ");p_int_I(bankState[0]);print_I(" bank 1: ");p_int_I(bankState[1]);
		println_I("Power Code 0: ");p_int_I(GetRawVoltageCode(0));
		println_I("Power Code 1 : ");p_int_I(GetRawVoltageCode(1));
		println_I("Raw: ");p_fl_I(GetRawVoltage());
		UpstreamPushPowerChange();
	}
	setPrintLevel(l);
}
Exemplo n.º 10
0
void printSortedData(){
    int x;
    print_I("Servo Data \r\n[ ");

    for(x=0;x<dataTableSize;x++){
        p_int_I(positionTemp[x]);print_I(" , ");
    }
    print_I(" ] ");

    print_I("\r\n[ ");

    for(x=0;x<dataTableSize;x++){
        p_int_I(sort[x]);print_I(" , ");
    }
    print_I(" ] ");

    print_I("\r\n[ ");

    for(x=0;x<dataTableSize;x++){
        p_int_I(positionTemp[sort[x]]);print_I(" , ");
    }
    print_I(" ] ");
}
Exemplo n.º 11
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);
}
Exemplo n.º 12
0
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 hardwareInit(){
	StartCritical();
	println_I("Getting MAC from flash");
	FlashGetMac(MyMAC.v);
	char macStr[13];
	int j=0,i=0;
	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);
#if defined(ROBOSUB_DEMO)
	//char * dev = "AHD Wave";
#else
	char * dev = "DyIO v.3";
#endif
	Pic32_Bowler_HAL_Init();
	usb_CDC_Serial_Init(dev,macStr,0x04D8,0x3742);

	mInitSwitch();

	for (i=0;i<6;i++){
		MyMAC.v[i]= MY_MAC_ADDRESS[i];
	}
	//Must initialize IO before hardware
	InitPins();
	println_I("Adding IO Namespace");
	addNamespaceToList((NAMESPACE_LIST * )get_bcsIoNamespace());
	println_I("Adding IO.Setmode Namespace");
	addNamespaceToList((NAMESPACE_LIST * )get_bcsIoSetmodeNamespace());
	println_I("Adding DyIO Namespace");
	addNamespaceToList((NAMESPACE_LIST * )get_neuronRoboticsDyIONamespace());
	println_I("Adding PID Namespace");
	addNamespaceToList((NAMESPACE_LIST * )getBcsPidNamespace());
	println_I("Adding DyIO PID Namespace");
	addNamespaceToList((NAMESPACE_LIST * )get_bcsPidDypidNamespace());
	println_I("Adding Safe Namespace");
	addNamespaceToList((NAMESPACE_LIST * )get_bcsSafeNamespace());


	Init_FLAG_BUSY_ASYNC();
	//InitCTS_RTS_HO();

	//AVR Reset pin
	InitAVR_RST();
	HoldAVRReset();

	//ConfigUARTOpenCollector();
	ConfigUARTRXTristate();


	InitLEDS();
	SetColor(0,0,1);
	//Starts Timer 3
	InitCounterPins();
	InitADC();


	BYTE rev [] = {MAJOR_REV,MINOR_REV,FIRMWARE_VERSION};
	FlashSetFwRev(rev);

	//Starts co-proc uart
	initCoProcCom();

	EndCritical();
	INTEnableSystemMultiVectoredInt();

//	initBluetooth();
//	if(!hasBluetooth()){
//		Pic32UARTSetBaud( 115200 );
//	}

}
Exemplo n.º 14
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((80000000L), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
    SYSTEMConfigPerformance(80000000);
    CHECONbits.PREFEN = 0;


    int j = 0, i = 0;

    for (i = 0; i < 6; i++) {
        MyMAC.v[i] = MY_MAC_ADDRESS[i];
    }
    StartCritical();

    println_I("MAC");
    enableFlashStorage(true);
    FlashGetMac(MyMAC.v);

    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);

    Pic32_Bowler_HAL_Init();
    usb_CDC_Serial_Init(dev, macStr, 0x04D8, 0x3742);
    InitLEDS();
    SetColor(0, 0, 1);

    mInitSwitch();

    //AVR Reset pin
    InitAVR_RST();
    HoldAVRReset();
    //AVR must be running before pin states can be synced in the pin initialization
    ReleaseAVRReset();
    //Starts co-proc uart
    initCoProcCom();
    
    InitPinFunction();
    //Must initialize IO before hardware
    LoadDefaultValues();
    //println_W("Pin States");
    SyncModes();
    //println_I("Modes synced, initializing channels");
    initAdvancedAsync();

    //println_I("Adding IO Namespace");
    addNamespaceToList( get_bcsIoNamespace());
    //println_I("Adding IO.Setmode Namespace");
    addNamespaceToList(get_bcsIoSetmodeNamespace());
    //println_I("Adding DyIO Namespace");
    addNamespaceToList(get_neuronRoboticsDyIONamespace());
    //println_I("Adding PID Namespace");
    addNamespaceToList( getBcsPidNamespace());
    //println_I("Adding DyIO PID Namespace");
    addNamespaceToList( get_bcsPidDypidNamespace());
    //println_I("Adding Safe Namespace");
    addNamespaceToList((NAMESPACE_LIST *) get_bcsSafeNamespace());


    Init_FLAG_BUSY_ASYNC();
    //InitCTS_RTS_HO();

    //ConfigUARTOpenCollector();
    ConfigUARTRXTristate();

    //Starts Timer 3
    InitCounterPins();
    InitADC();



    //SetFwRev(rev);



    GetName(Name);
    if(Name[0]==0xff){
            for(i=0;i<17;i++){
                    Name[i]=defaultName[i] ;
            }
            SetName(Name);
            GetName(Name);
    }

    if (!GetLockCode(LockCode)){
            for(i=0;i<4;i++){
                LockCode[i] = defaultlock[i];
            }
            SetLockCode(LockCode);
    }

    EndCritical();

	initBluetooth();
	if(!hasBluetooth()){
		Pic32UARTSetBaud( 115200 );
	}

    boolean defaultmac=true;
    for (i = 0; (i < 6) && defaultmac; i++) {
    	if(MyMAC.v[i] != MY_MAC_ADDRESS[i]){
    		defaultmac = false;
    	}
    }
    if(defaultmac){
    	srand((unsigned) GetRawVoltage());// random seed from the air

    	MyMAC.v[3] = MINOR_REV;
    	MyMAC.v[4] = FIRMWARE_VERSION;
    	MyMAC.v[5] = rand() % 255;
    	FlashSetMac(MyMAC.v);
		U1CON = 0x0000;
		DelayMs(100);
    	Reset();
    }

}
Exemplo n.º 15
0
BOOL setMode(BYTE pin,BYTE mode){
	println_I("Setting Mode: ");printMode(mode,INFO_PRINT);print_I(" on: ");p_int_I(pin);
	BYTE currentMode = GetChannelMode(pin);
	ClearCounter(pin);
	StopSPI(pin);
	clearPPM(pin);
	print_I(" \tHardware Cleared");
	switch (mode){
	case IS_SERVO:
		if(((pin < 12) && (isRegulated_0() == 0)) || ((pin >= 12) && (isRegulated_1()== 0))   ){
			print_I("|Mode is now servo");
			break;
		}else{
			if(getBrownOutDetect()){
				print_I(" Servo Mode could not be set, voltage invalid");
				return FALSE;
			}else{
				print_I(" Servo Mode set|");
				break;
			}
		}
		break;
	case IS_SPI_MOSI:
	case IS_SPI_MISO:
	case IS_SPI_SCK:
		if( pinHasFunction(pin, mode) != FALSE){
			print_I("|Mode is now SPI");
			InitSPI();
			break;
		}else{
			return FALSE;
		}
		break;
	case IS_COUNTER_INPUT_INT:
	case IS_COUNTER_INPUT_DIR:
	case IS_COUNTER_INPUT_HOME:
		if(pinHasFunction(pin, mode) != FALSE){
			print_I("|Mode is now Counter Input");
			StartCounterInput(pin);
			break;
		}else{
			print_I(", Counter Input not availible");
			return FALSE;
		}
		break;
	case IS_COUNTER_OUTPUT_INT:
	case IS_COUNTER_OUTPUT_DIR:
	case IS_COUNTER_OUTPUT_HOME:
		if(pinHasFunction(pin, mode) != FALSE){
			print_I("|Mode is now Counter Output");
			StartCounterOutput(pin);
			break;
		}else{
			print_I(", Counter Output not availible");
			return FALSE;
		}
		break;
	case IS_PPM_IN:
		println_I("Setting up PPM...");
		startPPM(pin);
		break;
	}
	print_I(" \tMode set");
	return TRUE;
}
BOOL setMode(BYTE pin,BYTE mode){

	ClearPinState(pin);
	println_I("Pin :");p_int_I(pin);print_I(" is mode: ");printMode(mode,INFO_PRINT);
	//BYTE pwm,dir;
	if (mode == NO_CHANGE){
		return TRUE;
	}
	switch (mode){
	case HIGH_IMPEDANCE:
		ClearPinState(pin);
		// Return here so as not to save this state to the eeprom
		return TRUE;
	case IS_UART_TX:
	case IS_UART_RX:
		if(pin == 17 || pin == 16){
			configPinMode(16,IS_UART_TX,OUTPUT,ON);
			configPinMode(17,IS_UART_RX,INPUT,ON);
			InitUART();
			return TRUE;
		}
		break;
	case IS_SPI_MOSI:
	case IS_SPI_MISO:
	case IS_SPI_SCK:
		if(pin == 0 || pin == 1||pin == 2   ){
			configPinMode(0,IS_SPI_SCK,INPUT,ON);
			configPinMode(1,IS_SPI_MISO,INPUT,ON);
			configPinMode(2,IS_SPI_MOSI,INPUT,ON);
			return TRUE;
		}
		break;
	case IS_ANALOG_IN:
		configPinMode(pin,mode,INPUT,OFF);
		if(InitADC(pin)){
			return TRUE;
		}
		break;
	case IS_PWM:
		if(InitPWM(pin)){
			return TRUE;
		}
		return FALSE;
	case IS_DC_MOTOR_VEL:
	case IS_DC_MOTOR_DIR:
		if(InitDCMotor(pin)){
			return TRUE;
		}
		return FALSE;
	case IS_SERVO:
		InitServo(pin);
		configPinMode(pin,mode,OUTPUT,OFF);
		return TRUE;
	case IS_DO:
		configPinMode(pin,mode,OUTPUT,OFF);
		return TRUE;
	case IS_DI:
	case IS_PPM_IN:
	case IS_COUNTER_OUTPUT_INT:
	case IS_COUNTER_OUTPUT_DIR:
	case IS_COUNTER_OUTPUT_HOME:
	case IS_COUNTER_INPUT_INT:
	case IS_COUNTER_INPUT_DIR:
	case IS_COUNTER_INPUT_HOME:
		configPinMode(pin,mode,INPUT,ON);
		return TRUE;
	default:
		configPinMode(pin,mode,INPUT,ON);
		return TRUE;
	}
	return FALSE;
}
Exemplo n.º 17
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

}