pynqでstepperモータのテスト

/*
 * Pl_timer_intr_test.c
 *
 *  Created on: 2012-6-8
 *      Author: yzhang
 */

#include <stdio.h>
#include "platform.h"
#include "xil_types.h"
#include "xtmrctr.h"
#include "xparameters.h"
#include "xil_io.h"
#include "xil_exception.h"
#include "xscugic.h"
#include "xgpio.h"

#define BTNS_DEVICE_ID		XPAR_BUTTONS_DEVICE_ID
#define BTN_INT 			XGPIO_IR_CH1_MASK
#define LEDS_DEVICE_ID		XPAR_LEDS_DEVICE_ID
#define SHIELDS_DEVICE_ID		XPAR_SHIELDS_DEVICE_ID

XGpio LEDInst;
XGpio BTNInst;
XGpio ShieldInst;
static int btn_value;

#define STEP_X_PIN 2
#define STEP_Y_PIN 3
#define STEP_Z_PIN 4
#define DIR_X_PIN 5
#define DIR_Y_PIN 6
#define DIR_Z_PIN 7
#define X_MIN_PIN 8
#define Y_MIN_PIN 9
#define Z_MIN_PIN 10

#define _SET(TARGET,BIT) (TARGET |= 1 << BIT)
#define _CLR(TARGET,BIT) (TARGET &= ~(1 << BIT))
#define _TGL(TARGET,BIT) (TARGET ^= 1 << BIT)
#define _CHK(TARGET,BIT) ((TARGET >> BIT) & 1)

static int shields_data;
static int intr_cntr;
static int pulse_status = 0;

void BTN_Intr_Handler(void *InstancePtr) {
	// Disable GPIO interrupts
	print(" Button Interrupt \n \r ");
	print("\r\n");
	print("\r\n");

	XGpio_InterruptDisable(&BTNInst, BTN_INT);
	// Ignore additional button presses
	if ((XGpio_InterruptGetStatus(&BTNInst) & BTN_INT) != BTN_INT) {
		return;
	}

	btn_value = XGpio_DiscreteRead(&BTNInst, 1);
	switch (btn_value){
	case 0b001:
		_TGL(shields_data , STEP_X_PIN);
		XGpio_DiscreteWrite(&ShieldInst, 1, shields_data);
		break;
	case 0b010:
		_TGL(shields_data , STEP_Y_PIN);
		XGpio_DiscreteWrite(&ShieldInst, 1, shields_data);
		break;
	case 0b100:
		_TGL(shields_data , STEP_Z_PIN);
		XGpio_DiscreteWrite(&ShieldInst, 1, shields_data);
		break;
	}
	XGpio_DiscreteWrite(&LEDInst, 1, btn_value);
	(void) XGpio_InterruptClear(&BTNInst, BTN_INT);

	// Enable GPIO interrupts
	XGpio_InterruptEnable(&BTNInst, BTN_INT);
}

XScuGic InterruptController; /* Instance of the Interrupt Controller */
static XScuGic_Config *GicConfig;/* The configuration parameters of the
controller */

//void print(char *str);
extern char inbyte(void);
void Timer_InterruptHandler(void *data, u8 TmrCtrNumber)
{
//	print(" Interrupt acknowledged \n \r ");
//	print("\r\n");
//	print("\r\n");
	intr_cntr++;
	if(pulse_status==0){
		if(intr_cntr>500) {
			step();
			pulse_status = 1;
			intr_cntr = 0;
		}
	} else {
		step();
		pulse_status = 0;
	}

//	XTmrCtr_Stop(data,TmrCtrNumber);
//	XTmrCtr_Reset(data,TmrCtrNumber);
//	XTmrCtr_Start(data,TmrCtrNumber);

}

void step(){
	_TGL(shields_data , STEP_X_PIN);
	XGpio_DiscreteWrite(&ShieldInst, 1, shields_data);
}

int SetUpInterruptSystem(XScuGic *XScuGicInstancePtr)
{
	/*
	 * Connect the interrupt controller interrupt handler to the hardware
	 * interrupt handling logic in the ARM processor.
	 */
	XGpio_InterruptEnable(&BTNInst, BTN_INT);
	XGpio_InterruptGlobalEnable(&BTNInst);

	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
			(Xil_ExceptionHandler) XScuGic_InterruptHandler,
			XScuGicInstancePtr);
	/*
	 * Enable interrupts in the ARM
	 */
	Xil_ExceptionEnable();
	return XST_SUCCESS;
}
int ScuGicInterrupt_Init(u16 DeviceId,XTmrCtr *TimerInstancePtr)
{
	int Status;
	/*
	 * Initialize the interrupt controller driver so that it is ready to
	 * use.
	 * */
	GicConfig = XScuGic_LookupConfig(DeviceId);
	if (NULL == GicConfig) {
		return XST_FAILURE;
	}
	Status = XScuGic_CfgInitialize(&InterruptController, GicConfig,
			GicConfig->CpuBaseAddress);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	/*
	 * Setup the Interrupt System
	 * */
	Status = SetUpInterruptSystem(&InterruptController);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	/*
	 * Connect a device driver handler that will be called when an
	 * interrupt for the device occurs, the device driver handler performs
	 * the specific interrupt processing for the device
	 */
	Status = XScuGic_Connect(&InterruptController,
			XPAR_FABRIC_AXI_TIMER_0_INTERRUPT_INTR,
			(Xil_ExceptionHandler)XTmrCtr_InterruptHandler,
			(void *)TimerInstancePtr);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	Status = XScuGic_Connect(&InterruptController,
			XPAR_FABRIC_BUTTONS_IP2INTC_IRPT_INTR,
				(Xil_ExceptionHandler) BTN_Intr_Handler,
				(void *) &BTNInst);
		if (Status != XST_SUCCESS)
			return XST_FAILURE;

		// Enable GPIO interrupts interrupt
		XGpio_InterruptEnable(&BTNInst, 1);
		XGpio_InterruptGlobalEnable(&BTNInst);

	/*
	 * Enable the interrupt for the device and then cause (simulate) an
	 * interrupt so the handlers will be called
	 */
	XScuGic_Enable(&InterruptController, XPAR_FABRIC_AXI_TIMER_0_INTERRUPT_INTR);
	XScuGic_Enable(&InterruptController, XPAR_FABRIC_BUTTONS_IP2INTC_IRPT_INTR);
	return XST_SUCCESS;
}

void initSteppers(){
 //no use of enable pin
	//	_CLR(shields_data , X_EN_PIN);
//	XGpio_SetDataDirection(&ShieldInst, 1, 0x00);

}


int main()
{
	int xStatus;
	shields_data = 0;
	// Initialise LEDs
	xStatus = XGpio_Initialize(&LEDInst, LEDS_DEVICE_ID);
	if (xStatus != XST_SUCCESS)
		return XST_FAILURE;
	// Initialise Push Buttons
	xStatus = XGpio_Initialize(&BTNInst, BTNS_DEVICE_ID);
	if (xStatus != XST_SUCCESS)
		return XST_FAILURE;
	xStatus = XGpio_Initialize(&ShieldInst, SHIELDS_DEVICE_ID);
	if (xStatus != XST_SUCCESS)
		return XST_FAILURE;
	// Set LEDs direction to outputs
	XGpio_SetDataDirection(&LEDInst, 1, 0x00);
	// Set all buttons direction to inputs
	XGpio_SetDataDirection(&BTNInst, 1, 0xFF);
	// Set Shields direction to outputs
	XGpio_SetDataDirection(&ShieldInst, 1, 0x00);

	//initStepper();

	XTmrCtr TimerInstancePtr;

	print("##### Application Starts #####\n\r");
	print("\r\n");
	xStatus = XTmrCtr_Initialize(&TimerInstancePtr,XPAR_AXI_TIMER_0_DEVICE_ID);
	if(XST_SUCCESS != xStatus)
		print("TIMER INIT FAILED \n\r");

	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//Set Timer Handler
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

	XTmrCtr_SetHandler(&TimerInstancePtr,
			Timer_InterruptHandler,
			&TimerInstancePtr);

	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//Setting timer Reset Value
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

	XTmrCtr_SetResetValue(&TimerInstancePtr,
			0, //Change with generic value
			0xffffff37);
			//			0xf8000000);

	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//Setting timer Option (Interrupt Mode And Auto Reload )
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

	XTmrCtr_SetOptions(&TimerInstancePtr,
			XPAR_AXI_TIMER_0_DEVICE_ID,
			(XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION ));

	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//SCUGIC interrupt controller Intialization
	//Registration of the Timer ISR
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

	xStatus=ScuGicInterrupt_Init(XPAR_PS7_SCUGIC_0_DEVICE_ID,&TimerInstancePtr);
	if(XST_SUCCESS != xStatus)
		print(" :( SCUGIC INIT FAILED \n\r");

	//Start Timer

	XTmrCtr_Start(&TimerInstancePtr,0);
	print("timer start \n\r");

	//Wait For interrupt;

	print("Wait for the Timer interrupt to tigger \r\n");
	print("########################################\r\n");
	print(" \r\n");

	while(1)
	{
	}
	cleanup_platform();
	return 0;
}