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