int main() { init_platform(); XIOModule_Initialize(&iomodule, XPAR_IOMODULE_0_DEVICE_ID); XIOModule_Start(&iomodule); XIOModule_CfgInitialize(&iomodule, NULL, 1); xil_printf ("\n\r\n\rRobot Console v0.01\n\r\n\r"); Xil_Out32 (MX_LIMIT_LO, -14400); // gripper rotate Xil_Out32 (MX_LIMIT_HI, 14400); Xil_Out32 (MX_POSITION, 0); Xil_Out32 (MY_LIMIT_LO, -2800); // wrist Xil_Out32 (MY_LIMIT_HI, 3960); Xil_Out32 (MY_POSITION, 3960); Xil_Out32 (MZ_LIMIT_LO, -7200); // lower arm rotate Xil_Out32 (MZ_LIMIT_HI, 7200); Xil_Out32 (MZ_POSITION, 0); Xil_Out32 (MA_LIMIT_LO, -9257); // elbow Xil_Out32 (MA_LIMIT_HI, 12750); Xil_Out32 (MA_POSITION, 12750); Xil_Out32 (MB_LIMIT_LO, -5520); // shoulder Xil_Out32 (MB_LIMIT_HI, 6900); Xil_Out32 (MB_POSITION, -5520); Xil_Out32 (MC_LIMIT_LO, -2823); // base Xil_Out32 (MC_LIMIT_HI, 2823); Xil_Out32 (MC_POSITION, 0); Xil_Out32 (MOTION_ALARM, 0x3f); InitCommandProcessing (); for (;;) { if (GetCommand ()) { ProcessCommand (); } } }
/** * * Initialize a specific interrupt controller instance/driver. The * initialization entails: * * - Initialize fields of the XIOModule structure * - Initial vector table with stub function calls * - All interrupt sources are disabled * - Interrupt output is disabled * - All timers are initialized * * @param InstancePtr is a pointer to the XIOModule instance to be * worked on. * @param DeviceId is the unique id of the device controlled by this * XIOModule instance. Passing in a device id associates the * generic XIOModule instance to a specific device, as chosen * by the caller or application developer. * * @return * - XST_SUCCESS if initialization was successful * - XST_DEVICE_IS_STARTED if the device has already been started * - XST_DEVICE_NOT_FOUND if device configuration information was * not found for a device with the supplied device ID. * * @note None. * ******************************************************************************/ int XIOModule_Initialize(XIOModule * InstancePtr, u16 DeviceId) { u8 Id; XIOModule_Config *CfgPtr; u32 NextBitMask = 1; int i; Xil_AssertNonvoid(InstancePtr != NULL); /* * If the device is started, disallow the initialize and return a status * indicating it is started. This allows the user to stop the device * and reinitialize, but prevents a user from inadvertently initializing */ if (InstancePtr->IsStarted == XIL_COMPONENT_IS_READY) { return XST_DEVICE_IS_STARTED; } /* * Lookup the device configuration in the CROM table. Use this * configuration info down below when initializing this component. */ CfgPtr = XIOModule_LookupConfig(DeviceId); if (CfgPtr == NULL) { return XST_DEVICE_NOT_FOUND; } /* * Set some default values */ InstancePtr->IsReady = 0; InstancePtr->IsStarted = 0; /* not started */ InstancePtr->CfgPtr = CfgPtr; InstancePtr->CfgPtr->Options = XIN_SVC_SGL_ISR_OPTION; /* * Initialize GPO value from INIT parameter */ for (i = 0; i < XGPO_DEVICE_COUNT; i++) InstancePtr->GpoValue[i] = CfgPtr->GpoInit[i]; /* * Save the base address pointer such that the registers of the * IO Module can be accessed */ InstancePtr->BaseAddress = CfgPtr->BaseAddress; /* * Initialize all the data needed to perform interrupt processing for * each interrupt ID up to the maximum used */ for (Id = 0; Id < XPAR_IOMODULE_INTC_MAX_INTR_SIZE; Id++) { /* * Initalize the handler to point to a stub to handle an * interrupt which has not been connected to a handler. Only * initialize it if the handler is 0 or XNullHandler, which * means it was not initialized statically by the tools/user. * Set the callback reference to this instance so that * unhandled interrupts can be tracked. */ if ((InstancePtr->CfgPtr->HandlerTable[Id].Handler == 0) || (InstancePtr->CfgPtr->HandlerTable[Id].Handler == XNullHandler)) { InstancePtr->CfgPtr->HandlerTable[Id].Handler = StubHandler; } InstancePtr->CfgPtr->HandlerTable[Id].CallBackRef = InstancePtr; /* * Initialize the bit position mask table such that bit * positions are lookups only for each interrupt id, with 0 * being a special case * (XIOModule_BitPosMask[] = { 1, 2, 4, 8, ... }) */ XIOModule_BitPosMask[Id] = NextBitMask; NextBitMask *= 2; } /* * Disable all interrupt sources * Acknowledge all sources */ XIomodule_Out32(InstancePtr->BaseAddress + XIN_IER_OFFSET, 0); XIomodule_Out32(InstancePtr->BaseAddress + XIN_IMR_OFFSET, 0); XIomodule_Out32(InstancePtr->BaseAddress + XIN_IAR_OFFSET, 0xFFFFFFFF); InstancePtr->CurrentIER = 0; InstancePtr->CurrentIMR = 0; /* * If the fast Interrupt mode is enabled then set all the * interrupts as normal mode and initialize the interrupt hardware * vector table to default ((BaseVector & 0xFFFFFF80) | 0x10). */ if (InstancePtr->CfgPtr->FastIntr == TRUE) { XIomodule_Out32(InstancePtr->BaseAddress + XIN_IMR_OFFSET, 0); for (Id = 0; Id < XPAR_IOMODULE_INTC_MAX_INTR_SIZE; Id++) { XIomodule_Out32(InstancePtr->BaseAddress + XIN_IVAR_OFFSET + Id * 4, (InstancePtr->CfgPtr->BaseVector & 0xFFFFFF80) | 0x10); } } /* * Initialize all Programmable Interrupt Timers */ XIOModule_Timer_Initialize(InstancePtr, DeviceId); /* * Initialize all UART related status */ XIOModule_CfgInitialize(InstancePtr, CfgPtr, 0); /* * Save the IO Bus base address pointer such that the memory mapped * IO can be accessed */ InstancePtr->IoBaseAddress = CfgPtr->IoBaseAddress; /* * Indicate the instance is now ready to use, successfully initialized */ InstancePtr->IsReady = XIL_COMPONENT_IS_READY; return XST_SUCCESS; }