void PmodHB5_INIT(UART_MODULE uartID)
{
	PORTSetPinsDigitalOut(IOPORT_D, BIT_7); //HB5 Direction
	PORTSetPinsDigitalOut(IOPORT_D, BIT_1); //HB5 Enable
	PORTSetPinsDigitalIn(IOPORT_D, BIT_9);  //HB5 Sensor A
	PORTSetPinsDigitalIn(IOPORT_C, BIT_1);  //HB5 Sensor B

	hBridge.sensorAport = IOPORT_D;
	hBridge.sensorAportBit = BIT_9;
	hBridge.sensorBport = IOPORT_C;
	hBridge.sensorBportBit = BIT_1;
	hBridge.directionPort = IOPORT_D;
	hBridge.directionPortBit = BIT_7;
	hBridge.currentDirection = PMOD_HB5_DIR_CW;
	hBridge.newDirection = PMOD_HB5_DIR_CW;
	hBridge.ocChannel = 2;
	

    OpenOC2(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
    OpenTimer2(T2_ON | T2_PS_1_256, SYSTEM_CLOCK/PB_DIV/PRESCALE/(TOGGLES_PER_SEC/2));
    
	OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_256, T1_TICK);
    ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2);
	INTEnableSystemMultiVectoredInt();
	
	UARTPutS("\r\nPmodHB5 init complete\r\n",uartID);

}

static void InitPIC32(void)
{
    int  value;

    #if defined(RUN_AT_60MHZ)
        // Use OSCCON default
    #else
        OSCCONCLR = 0x38000000; //PLLODIV
        #if defined(RUN_AT_48MHZ)
            OSCCONSET = 0x08000000; //PLLODIV /2
        #elif defined(RUN_AT_24MHZ)
            OSCCONSET = 0x10000000; //PLLODIV /4
        #else
            #error Cannot set OSCCON
        #endif
    #endif

    value = SYSTEMConfigWaitStatesAndPB( GetSystemClock() );

    // Enable the cache for the best performance
    CheKseg0CacheOn();

    INTEnableSystemMultiVectoredInt();

    DDPCONbits.JTAGEN = 0;

    value = OSCCON;
    while (!(value & 0x00000020))
    {
        value = OSCCON;    // Wait for PLL lock to stabilize
    }

    INTEnableInterrupts();
}

void setup() {

    int value;

    value = SYSTEMConfigWaitStatesAndPB(GetSystemClock());

    // Enable the cache for the best performance
    CheKseg0CacheOn();

    INTEnableSystemMultiVectoredInt();

    value = OSCCON;
    while (!(value & 0x00000020)) {
        value = OSCCON; // Wait for PLL lock to stabilize
    }

    LATB=0;

    ANSELA = 0x00;
    ANSELB = 0x00;
    TRISA = 0xFFFF;
    TRISB = 0xFFFF;

    TRISBbits.TRISB1= OUTPUT_PIN;
    TRISBbits.TRISB2= OUTPUT_PIN;
    TRISBbits.TRISB3= OUTPUT_PIN;

    //?timijk debug
    //TRISBbits.TRISB4= OUTPUT_PIN;
    
    ConfigurePinReMap();
    
}

/****************************************************************************
 Function
     Roach_Init

 Parameters
    None.

 Returns
     SUCCESS if operation successful
     ERROR  otherwise

 Description
      Performs all the initialization necessary for the roach.
      this includes initializing the PWM module, the A/D converter, the
      data directions on some pins, and setting the initial motor directions.
 Notes
     None.

 Author
    Max Dunne, 2012.01.06
 ****************************************************************************/
char Roach_Init(void) {
    //Initialize the serial port
    SERIAL_Init();
    TIMERS_Init();
    //set the control pins for the motors
    PWM_Init(LEFT_PWM | RIGHT_PWM, 200);
    LEFT_DIR_TRIS = 0;
    LEFT_DIR_INV_TRIS = 0;
    RIGHT_DIR_TRIS = 0;
    RIGHT_DIR_INV_TRIS = 0;
    LEFT_DIR = 0;
    LEFT_DIR_INV = ~LEFT_DIR;
    RIGHT_DIR = 0;
    RIGHT_DIR_INV = ~RIGHT_DIR;

    //set up the hall effect and divorce all the A/D pins
    AD1PCFG = 0xFF;
    HALL_FRONT_LEFT_TRIS = 1;
    HALL_FRONT_RIGHT_TRIS = 1;
    HALL_REAR_RIGHT_TRIS = 1;
    HALL_REAR_LEFT_TRIS = 1;


    //Initialize the light sensor
    AD_Init(LIGHT_SENSOR);

    //enable interrupts
    INTEnableSystemMultiVectoredInt();
}

int main()
{
	mJTAGPortEnable(0);
	mPORTASetPinsDigitalOut(BIT_0|BIT_1|BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7);
	mPORTAClearBits(BIT_0|BIT_1|BIT_2|BIT_3|BIT_4|BIT_5|BIT_6|BIT_7);
	INTCONSET=0x1000;
	INTEnableSystemMultiVectoredInt();
	IEC0 =0;
	IFS0 = 0;
	INTCONCLR = 0x00000018;
	IEC0SET = 0x00088000;
	IPC3 = 0x1F000000;
	IPC4 = 0X17000000;
	lcdconfig();

while(1)
{
	if(IFS0 & 0x8000==1)
	{
		lcddata('3');
		PORTA=0X0F;
	}
	if(IFS0 & 0x80000==1)
	{
		lcddata('4');
		PORTA=0XFF;
	}
}
}

BOOL InitializeSystem ( void )
{

    int  value;

    value = SYSTEMConfigWaitStatesAndPB( GetSystemClock() );

    // Enable the cache for the best performance
    CheKseg0CacheOn();

    INTEnableSystemMultiVectoredInt();

    value = OSCCON;
    while (!(value & 0x00000020))
    {
        value = OSCCON;    // Wait for PLL lock to stabilize
    }

    // Init UART
    UART2Init();
    // Set Default demo state
    DemoState = DEMO_INITIALIZE;
	if(USBHostIsochronousBuffersCreate(&isocData,2,1024)){
    	UART2PrintString( "CreateIsochronousBuffers\r\n" );
	}else{
        UART2PrintString( "Fail:CreateIsochronousBuffers\r\n" );
	}

    return TRUE;
} // InitializeSystem

//************************************************************************
void init()
{

#ifdef _ENABLE_PIC_RTC_
	// 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..
	__PIC32_pbClk	=	SYSTEMConfig(F_CPU, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
#else
	__PIC32_pbClk	=	SYSTEMConfigPerformance(F_CPU);
#endif


	OpenCoreTimer(CORE_TICK_RATE);

	// set up the core timer interrupt with a prioirty of 2 and zero sub-priority
	mConfigIntCoreTimer((CT_INT_ON | CT_INT_PRIOR_2 | CT_INT_SUB_PRIOR_0));

	// enable multi-vector interrupts
	INTEnableSystemMultiVectoredInt();


#ifdef _ENABLE_PIC_RTC_
	RtccInit();									// init the RTCC
//	while(RtccGetClkStat() != RTCC_CLK_ON);		// wait for the SOSC to be actually running and RTCC to have its clock source
												// could wait here at most 32ms

	delay(50);
	// time is MSb: hour, min, sec, rsvd. date is MSb: year, mon, mday, wday.
	RtccOpen(0x10073000, 0x11010901, 0);
	RtccSetTimeDate(0x10073000, 0x10101701);
	// please note that the rsvd field has to be 0 in the time field!
#endif


	//*	as per [email protected], Jan 7, 2011
	//*	Disable the JTAG interface.
	DDPCONbits.JTAGEN	=	0;


#if defined (_BOARD_MEGA_)
	//*	Turn Secondary oscillator off
	//*	this is only needed on the mega board because the mega uses secondary ocsilator pins
	//*	as general I/O
	{
	unsigned int dma_status;
	unsigned int int_status;
	
		mSYSTEMUnlock(int_status, dma_status);

		OSCCONCLR	=	_OSCCON_SOSCEN_MASK;


		mSYSTEMLock(int_status, dma_status);
	}
	
#endif

}

/*********************************************************************
* Function: void SYSTEM_Initialize( SYSTEM_STATE state )
*
* Overview: Initializes the system.
*
* PreCondition: None
*
* Input:  SYSTEM_STATE - the state to initialize the system into
*
* Output: None
*
********************************************************************/
void SYSTEM_Initialize( SYSTEM_STATE state )
{
    int  value;
	
    switch(state)
    {
        case SYSTEM_STATE_USB_START:
            value = SYSTEMConfigWaitStatesAndPB( 64000000UL );

            // Enable the cache for the best performance
            CheKseg0CacheOn();

            INTEnableSystemMultiVectoredInt();

            value = OSCCON;
            while (!(value & 0x00000020))
            {
                value = OSCCON;    // Wait for PLL lock to stabilize
            }

            //Disable JTAG
            DDPCONbits.JTAGEN = 0;

            LED_Enable(LED_USB_DEVICE_STATE);
            BUTTON_Enable(BUTTON_USB_DEVICE_HID_MOUSE);
            break;
            
        default:
            break;
    }
}

void initUART1(int pbClk)
{
#define config1    UART_EN | UART_IDLE_CON | UART_RX_TX | UART_DIS_WAKE | UART_DIS_LOOPBACK | UART_DIS_ABAUD | UART_NO_PAR_8BIT | UART_1STOPBIT | UART_IRDA_DIS | UART_DIS_BCLK_CTS_RTS| UART_NORMAL_RX | UART_BRGH_SIXTEEN
#define config2      UART_TX_PIN_LOW | UART_RX_ENABLE | UART_TX_ENABLE | UART_INT_TX | UART_INT_RX_CHAR | UART_ADR_DETECT_DIS | UART_RX_OVERRUN_CLEAR
    OpenUART1(config1, config2, pbClk / 16 / DESIRED_BAUDRATE - 1); // calculate actual BAUD generate value.
    ConfigIntUART1(UART_INT_PR2 | UART_RX_INT_EN);
    INTEnableSystemMultiVectoredInt();
}

/**
 * @fn      void intConfig( void );
 * @brief   Configuration des interruptions
 */
void intConfig( void ){

    // Config Interruptions
    // Mode "multi-vectored". Vecteurs d'interruption multiples
    INTEnableSystemMultiVectoredInt();
    // Validation globale des interruptions
    INTEnableInterrupts();
//    uartPutString("Interrupts configured\r\n");
}

void Board_init()
{
    //unsigned int pbclk=PB_CLOCK;
    //SYSTEMConfig(SYSTEM_CLOCK,SYS_CFG_ALL);
    //SYSTEMConfigPB(PB_CLOCK);
    //OSCSetPBDIV(2);
    //Serial_init();
    INTEnableSystemMultiVectoredInt();
}

int main(void) {
    int i;
    unsigned short pattern;
    char j,k;

    SERIAL_Init();
    AD1PCFG = 0xFF;
    INTEnableSystemMultiVectoredInt();
    //enable LED bank 1
    TRISDbits.TRISD6 = 0;
    TRISDbits.TRISD11 = 0;
    TRISDbits.TRISD3 = 0;
    TRISDbits.TRISD5 = 0;

    printf("\nHello World!");
    Stepper_Init(100);
    printf("\nStepping forward 420 steps");
    Stepper_SetSteps(FORWARD,420);
    for (j=0;j<10;j++) {
        DELAY();
        printf(".");
        DELAY();
        printf("\t%u",Stepper_GetRemainingCount());
    }
    while(Stepper_GetRemainingCount() > 1) {
        printf("o");
        DELAY();
    }
    printf("\nForward steps done");
    Stepper_SetSteps(REVERSE,220);
    while (Stepper_GetRemainingCount() > 1) {
        printf("+");
    }
    Stepper_End();

    TRIS_COIL_A_DIRECTION = 0;
    TRIS_COIL_A_ENABLE = 0;
    TRIS_COIL_B_DIRECTION = 0;
    TRIS_COIL_B_ENABLE = 0;

    ShutDownDrive();
    COIL_A_ENABLE = 0;
    COIL_A_DIRECTION = 0;
    COIL_B_ENABLE = 0;
    COIL_B_DIRECTION = 0;
    printf("\n Turning on COIL_B_DIRECTION (PortZ-07");
    for (j=0;j<10;j++) {
        printf("o");
        DELAY();
    }
    while (1) {
        COIL_B_DIRECTION ^= 1;
        DELAY();
        printf(".");
    }
    return 0;
}

void DeviceInit()
{
	// Configure left motor direction pin and set default direction.
	trisMtrLeftDirSet	= ( 1 << bnMtrLeftDir );
	prtMtrLeftDirSet	= ( 1 << bnMtrLeftDir );	// forward
	
	// Configure right motor diretion pin and set default direction.
	trisMtrRightDirClr	= ( 1 << bnMtrRightDir );	//modify for JD
	prtMtrRightDirClr	= ( 1 << bnMtrRightDir );	// forward

	// Configure Output Compare 2 to drive the left motor.
	OC2CON	= ( 1 << 2 ) | ( 1 << 1 );	// pwm
	OC2R	= dtcMtrStopped;
	OC2RS	= dtcMtrStopped;

	// Configure Output Compare 3.
	OC3CON = ( 1 << 3 ) | ( 1 << 2 ) | ( 1 << 1 );	// pwm
	OC3R	= dtcMtrStopped;
	OC3RS	= dtcMtrStopped;

	// Configure Timer 2.
	TMR2	= 0;		// clear timer 2 count
	PR2		= 9999;

	// Configure Timer 3.
	TMR3	= 0;
	PR3		= 9999;

	// Start timers and output compare units.
	T2CON		= ( 1 << 15 ) | ( 1 << TCKPS20 )|(1 << TCKPS21);	// timer 2 prescale = 8
	OC2CONSET	= ( 1 << 15 );	// enable output compare module 2
	OC3CONSET	= ( 1 << 15 );	// enable output compare module 3
	T3CON		= ( 1 << 15 ) | ( 1 << TCKPS31 ) | ( 1 << TCKPS30); //timer3 prescale = 8

	// Configure Timer 5.
	TMR5	= 0;
	PR5		= 99; // period match every 100 us
	IPC5SET	= ( 1 << 4 ) | ( 1 << 3 ) | ( 1 << 2 ) | ( 1 << 1 ) | ( 1 << 0 ); // interrupt priority level 7, sub 3
	IFS0CLR = ( 1 << 20);
	IEC0SET	= ( 1 << 20);
	
	// Start timers.
	T5CON = ( 1 << 15 ) | ( 1 << 5 ) | ( 1 << 4 ); // fTimer5 = fPb / 8
	
	// Setup light sensor pins
	PORTSetPinsDigitalIn(IOPORT_B, BIT_0 | BIT_3);
	
	// Change notice configured for CN 2 through 5 for light sensors.
	mCNOpen(CN_ON, (CN2_ENABLE | CN3_ENABLE | CN4_ENABLE | CN5_ENABLE), CN_PULLUP_DISABLE_ALL);
	ConfigIntCN(CHANGE_INT_OFF | CHANGE_INT_PRI_2);
	    
	// Enable multi-vector interrupts.
	INTEnableSystemMultiVectoredInt();
}

void InitApp(void)
{
    /* Setup analog functionality and port direction */

    // enable multi-vector interrupts
    INTEnableSystemMultiVectoredInt();

    //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // STEP 2. configure Timer 1 using internal clock, 1:256 prescale

    OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, T1_TICK);

    // set up the timer interrupt with a priority of 2
    ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2);
    
    /* Zero to DELAY_COUNTER for Delay procedure. */
    StartTimer();

    /* Initialize peripherals */

    DisplayBacklightConfig();

//#if defined WANT_GOL_INIT
    GOLInit();
//#endif

//    TouchInit(NULL,NULL,NULL,NULL);

    DisplayBacklightOn();

    SPIFlashInit();
    
// initialize the components for Resistive Touch Screen
//#if defined WANT_TOUCH_INIT
//    TouchInit(NULL,NULL,NULL,NULL);
//#endif
//    SDCARDInit();
//    MP3Init();

    /* PORT Init                                                            */
    /* PORTD 0-3 bits inputs.                                               */
    /* PORTA 0-1 bits outputs.                                              */

    /* TODO Must be disable the JTAG port, that we are to appropriate using */
    /* RA0, and RA1 ports.  */

    DDPCONbits.JTAGEN = 0;  /* JTAG debug disabled */
    /* TODO D port output now !!!! */
    TRISDbits.TRISD14 = TRISDbits.TRISD15 = 0;
        /* Port D 14, 15, pins pull up resistor enabled */
//    CNPUEbits.CNPUE20 = CNPUEbits.CNPUE21 = 1;
    TRISAbits.TRISA0 = TRISAbits.TRISA1 = 0;

}

void systemInit(void)
{
   //Enable optimal performance
   SYSTEMConfigPerformance(80000000);

   //Configures peripheral bus divisor
   OSCSetPBDIV(OSC_PB_DIV_2);

   //Enable multi-vectored mode
   INTEnableSystemMultiVectoredInt();
}

//******************************************************************
void Stepper_InitIO (void)
{
	// Setup timers
	//Timer 1
	T1CON = 0x8030;	//Set up timer 1 Fosc/2, prescaled 1:256 
	
	DDPCONbits.JTAGEN = 0; //To release port A pins from use by JTAG port
	
	//Set Tris for Stepper
	//X
	 AD1PCFGbits.PCFG8 = 1;   //AtoD port cfg AN8/B8
	 TRISBbits.TRISB8    = OUTPUT;	//Step
	 AD1PCFGbits.PCFG9 = 1;   //AtoD port cfg AN9/B9
	 TRISBbits.TRISB9    = OUTPUT;	//Dir
	 AD1PCFGbits.PCFG13 = 1;   //AtoD port cfg AN13/B13
	 TRISBbits.TRISB13    = OUTPUT;	//Enable
	 TRISDbits.TRISD4    = INPUT;	//X_HomeSwitch
	//Y
	 TRISDbits.TRISD8    = OUTPUT;	//Step
	 TRISDbits.TRISD9    = OUTPUT;	//Dir
	 TRISDbits.TRISD10    = OUTPUT;	//Enable
	 TRISCbits.TRISC13     = INPUT;	//Y_HomeSwitch
	 
	//Z
	 TRISDbits.TRISD2     = OUTPUT;	//Step
	 TRISDbits.TRISD7    = OUTPUT;	//Dir
	 TRISDbits.TRISD3     = OUTPUT;	//Enable
	 TRISCbits.TRISC14     = INPUT;	//Z_HomeSwitch
	 
	X_Disable = TRUE; 		
	Y_Disable = TRUE; 		
	Z_Disable = TRUE; 		
	
	//Timer 4 is used to time the stepper motor steps
	//PB clk = 8MHz Post scaler = 2 to 1 tick every 25nS	
	T4CON = 0x0;			//Clear timer setting register
	T4CONSET = 0x0010;		//Set PS to 2
	TMR4 = 0;				//Reset clock
	PR4 = 500;				//Initialise PR4 value (when T4 matches PR4 it interrupts)
	mT4SetIntPriority(7);	//Set T4 interrupt to top priority
	INTEnableSystemMultiVectoredInt();//Set T4 to vectored i.e. Fast
	mT4ClearIntFlag();		//Clear interrupt flag
	T4CONSET = 0x8000;		//Turn on T4
	mT4IntEnable(!TRUE);	//Disable T4 interrputs
	
	// init the circular buffer pointers
	 SBR = 0;
	 SBW = 0;
	 // Init Rapid moves
	 G_CodeRapidXYMove=0;
	 G_CodeRapidZMove=0;
	 
}//InitStepper

int main(void)
{
    SYSTEMConfigWaitStatesAndPB(72000000);
    INTEnableSystemMultiVectoredInt();
    
    Ex16LCDInit(36000000);

	Ex16LCDWriteLine(2, "Main App");

    while(1)
        ;
}

int main(void)
{

  // === config the uart, DMA, SPI ===========
  PT_setup();

  // == SPI ==
  //enable SPI at 10 MHz clock to meet digital potentiometer specifications
  SpiChnOpen(spiChn, SPI_OPEN_ON | SPI_OPEN_MODE16 | SPI_OPEN_MSTEN | SPI_OPEN_CKE_REV , spiClkDiv);
  
  // === setup system wide interrupts  ====================
  INTEnableSystemMultiVectoredInt();
    
  // === set up i/o port pin ===============================
  //Port B bits, 3,7,8, and 9 are used to select digital output
  //Port B bit 4 is used as chip select for treble digital potentiometer
  //Port B bit 13 is used as a select signal for output effect selection multiplexer
  //Additional functionality would use the TFT to display which output was being
  //selected
  mPORTBSetPinsDigitalOut(BIT_4 | BIT_3|BIT_7 | BIT_8 | BIT_9 |BIT_13);    //Set port as output
 
  //Port A Bits 0,2,and 3 are used to configure digital potentiometers (chip selects). Port A bit 4 is used
  //for multiplexing whether to have input from Digital effector chip, distortion,
  //or pure tonestack sound
  mPORTASetPinsDigitalOut(BIT_0 | BIT_2 | BIT_3 | BIT_4);
  // === now the threads ===================================
  
  // init the threads
  PT_INIT(&pt_cmd);
  PT_INIT(&pt_time);
  
  //==Digipot spi stuff
   // SCK2 is pin 26 
    // SDO1 (MOSI) is in PPS output group 1, could be connected to RB5 which is pin 14
    PPSOutput(2, RPB5, SDO1);
    // control CS for DAC
    //mPORTBSetPinsDigitalOut(BIT_4); //CS
    mPORTBSetBits(BIT_4 | BIT_6);
    //===
    
    mPORTASetBits(BIT_0 | BIT_2 | BIT_3 | BIT_4); //CS pins active high
    mPORTAClearBits(BIT_4);
    mPORTBClearBits(BIT_13);
    mPORTBSetBits(BIT_13);
    
    
  // schedule the threads
  while(1) {
      //cmd used as command center for all effects
    PT_SCHEDULE(protothread_cmd(&pt_cmd));
  }
} // main

void initialize(void){
     SYSTEMConfig(80000000, SYS_CFG_ALL); // sets up periferal and clock configuration
     INTEnableSystemMultiVectoredInt();
     INTEnableInterrupts(); // enable interrupts
     delay();
     timers();
     delay();
     PWM();
     delay();
     UART();
     delay();
     beginLIDARdecoder(returned_data, &buffer_five);
}

/*-----------------------------------------------------------*/
static void prvSetupHardware(void)
{
    /* Setup the CPU clocks, and configure the interrupt controller. */
    SYSTEMConfigPerformance(configCPU_CLOCK_HZ);
    mOSCSetPBDIV(OSC_PB_DIV_2);
    INTEnableSystemMultiVectoredInt();

    /* LEDs off. */
    mPORTDClearBits(BIT_0 | BIT_1 | BIT_2);

    /* LEDs are outputs. */
    mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2);
}