void PowerCycleAVR(){
	println_I("Power cycling the AVR");
	HoldAVRReset();
//	getCommand(progmode);
//	writeLowFuse();
//	writeHighFuse();
//	writeExtendedFuse();
	ReleaseAVRReset();
}
void programMode(void){
	HoldAVRReset();
	getCommand(progmode);
	DelayMs(20);
}
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();
    }

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

}