static void LaserTest() { InitMotorBoard(); continueTest = true; while (continueTest) { TurnOnLaser(); MB_Sleep(1000); TurnOffLaser(); MB_Sleep(1000); } }
void init_feedback_system(void) { // Calibrating the sensor PWM_SetParams(&instPWMTimer, PWM_FREQ_005KHZ, 0); MB_Sleep(2000); TSL235R_SetMinThreshold((void*) XPAR_TSL235R_0_S00_AXI_BASEADDR); PWM_SetParams(&instPWMTimer, PWM_FREQ_005KHZ, 99); MB_Sleep(2000); TSL235R_SetMaxThreshold((void*) XPAR_TSL235R_0_S00_AXI_BASEADDR); // Putting PWM in a know state before returning PWM_SetParams(&instPWMTimer, PWM_FREQ_005KHZ, 0); }
void DemoRun() { u32 count = 0; u16 kypdStateNew, kypdStateOld, kypdIndex = 0; xil_printf("starting...\n\r"); kypdStateOld = KYPD_getStatus(&myDevice); while(1) { kypdStateNew = KYPD_getStatus(&myDevice); for(count = 0; count < 16; count++) { kypdIndex = 1 << count; if((kypdStateNew & kypdIndex) == 0) { //check if rising edge if(((kypdStateNew & kypdIndex) == 0) && ((kypdStateOld & kypdIndex) != 0)){ xil_printf("%x pressed\n\r", myDevice.keyLookUp[count]); } //debounce #ifdef XPAR_MICROBLAZE_ID MB_Sleep(5); #else usleep(5000); #endif } } kypdStateOld = kypdStateNew; } }
void LED_Init(void) { LEDPIN_Init(); /*while(1) { //MB_Sleep(1000); LED_WrCmd(0xaa); //MB_Sleep(1000); XGpio_DiscreteWrite(&led2, reset_CHANNEL, 0x1); XGpio_DiscreteWrite(&led1, reset_CHANNEL, 0x0); }*/ XGpio_DiscreteWrite(&reset, reset_CHANNEL, 0x0); //usleep(800); MB_Sleep(80); XGpio_DiscreteWrite(&reset, reset_CHANNEL, 0x1); Set_Display_On_Off(0x00); // Display Off (0x00/0x01) Set_Display_Clock(0x80); // Set Clock as 100 Frames/Sec Set_Multiplex_Ratio(0x3F); // 1/64 Duty (0x0F~0x3F) Set_Display_Offset(0x00); // Shift Mapping RAM Counter (0x00~0x3F) SetStartLine(0x00); // Set Mapping RAM Display Start Line (0x00~0x3F) Set_Charge_Pump(0x04); // Enable Embedded DC/DC Converter (0x00/0x04) SetAddressingMode(0x02); // Set Page Addressing Mode (0x00/0x01/0x02) Set_Segment_Remap(0x01); // Set SEG/Column Mapping 0x00左右反置 0x01正常 Set_Common_Remap(0x08); // Set COM/Row Scan Direction 0x00上下反置 0x08正常 Set_Common_Config(0x10); // Set Sequential Configuration (0x00/0x10) SetContrastControl(0xCF); // Set SEG Output Current Set_Precharge_Period(0xF1); // Set Pre-Charge as 15 Clocks & Discharge as 1 Clock Set_VCOMH(0x40); // Set VCOM Deselect Level Set_Entire_Display(0x00); // Disable Entire Display On (0x00/0x01) Set_Inverse_Display(0x00); // Disable Inverse Display On (0x00/0x01) Set_Display_On_Off(0x01); // Display On (0x00/0x01) LED_Fill(0x00); //初始清屏 LED_Set_Pos(0, 0); }
/***************************************************************************//** * @brief mdelay *******************************************************************************/ void mdelay(uint32_t msecs) { #ifdef _XPARAMETERS_PS_H_ usleep(msecs * 1000); #else MB_Sleep(msecs); #endif }
static void runImageProcessing(void) { //video_set_input_frame(2); video_set_input_enabled(0); targeting_begin_transfer(&sAxiTargetingDma, sDebugOutputEnabled); while(ip_busy()) MB_Sleep(200); draw_debug_dots(); //video_set_input_frame(0); video_set_input_enabled(1); }
void ManualMainLoop(void) { // Set up the turret control IP. InitMotorBoard(); TurnOnLaser(); EnablePanServo(); EnableTiltServo(); SetTiltAngle(0); SetPanAngle(0); continueTest = true; xil_printf("Entering manual mode.\r\n"); while(continueTest) { // We allow users to press more than one button at a time. if (sButtonState & BUTTON_LEFT) { PanLeft(1); } if (sButtonState & BUTTON_RIGHT) { PanRight(1); } if (sButtonState & BUTTON_UP) { TiltUp(1); } if (sButtonState & BUTTON_DOWN) { TiltDown(1); } MB_Sleep(7); } SetTiltAngle(0); SetPanAngle(0); MB_Sleep(100); DisableTiltServo(); DisablePanServo(); TurnOffLaser(); }
void DemoRun() { CLS_DisplayClear(&myDevice); strcpy(szInfo1, " PmodCLS Demo"); strcpy(szInfo2, " Hello World!"); CLS_WriteStringAtPos(&myDevice, 0, 0, szInfo1); CLS_WriteStringAtPos(&myDevice, 1, 0, szInfo2); #ifdef XPAR_MICROBLAZE_ID MB_Sleep(1000); #else usleep(500000); #endif while(1){ CLS_DisplayClear(&myDevice); strcpy(szInfo1, "->PmodCLS Demo<- "); CLS_WriteStringAtPos(&myDevice, 0, 0, szInfo1); CLS_WriteStringAtPos(&myDevice, 1, 0, szInfo2); #ifdef XPAR_MICROBLAZE_ID MB_Sleep(1000); #else usleep(500000); #endif CLS_DisplayClear(&myDevice); strcpy(szInfo1, " PmodCLS Demo "); CLS_WriteStringAtPos(&myDevice, 0, 0, szInfo1); CLS_WriteStringAtPos(&myDevice, 1, 0, szInfo2); #ifdef XPAR_MICROBLAZE_ID MB_Sleep(1000); #else usleep(500000); #endif } }
static void DisplayHeman(void) { u32 *buff = (u32 *) FRAMES_BASE_ADDR; SdFile *file = &sSdFileBoard[FILE_ID_HEMAN]; if (file->loaded) { video_set_input_enabled(0); memcpy(buff, file->baseAddr, file->length); targeting_begin_transfer(&sAxiTargetingDma, 1); MB_Sleep(3000); video_set_input_enabled(1); } else { print("File not loaded; can't diplay!"); } }
static void MotorPatternTest() { InitMotorBoard(); TurnOnLaser(); EnablePanServo(); EnableTiltServo(); continueTest = true; while (continueTest) { for (int i = -60; i < 0; i++) { SetPanAngle(i); SetTiltAngle(i + 60); MB_Sleep(15); } MB_Sleep(20); for (int i = 0; i < 60; i++) { SetPanAngle(i); SetTiltAngle(60 - i); MB_Sleep(15); } MB_Sleep(20); for (int i = 60; i > 0; i--) { SetPanAngle(i); SetTiltAngle(i - 60); MB_Sleep(15); } MB_Sleep(20); for (int i = 0; i > -60; i--) { SetPanAngle(i); SetTiltAngle(-60 - i); MB_Sleep(15); } MB_Sleep(20); } TurnOffLaser(); DisablePanServo(); DisableTiltServo(); }
void init_welcom(void) { // display the greeting PMDIO_LCD_setcursor(1,0); PMDIO_LCD_wrstring(" Project 01 "); PMDIO_LCD_setcursor(2,0); PMDIO_LCD_wrstring(" by Chad Sutfin "); NX4IO_setLEDs(0x0000FFFF); MB_Sleep(2000); NX4IO_setLEDs(0x00000000); // write the static text to the display PMDIO_LCD_clrd(); PMDIO_LCD_setcursor(1,0); PMDIO_LCD_wrstring("G|FR: DCY: %"); PMDIO_LCD_setcursor(2,0); PMDIO_LCD_wrstring("M|FR: DCY: %"); // turn off the LEDs and clear the seven segment display NX4IO_setLEDs(0x00000000); NX410_SSEG_setAllDigits(SSEGLO, CC_BLANK, CC_BLANK, CC_BLANK, CC_BLANK, DP_NONE); NX410_SSEG_setAllDigits(SSEGHI, CC_BLANK, CC_BLANK, CC_BLANK, CC_BLANK, DP_NONE); }
/** * * API to set high speed in card and host. Changes clock in host accordingly. * * * @param InstancePtr is a pointer to the XSdPs instance. * * @return * - XST_SUCCESS if successful. * - XST_FAILURE if fail. * * @note None. * ******************************************************************************/ int XSdPs_Change_BusSpeed(XSdPs *InstancePtr) { u32 Status = 0; u32 StatusReg = 0x0; u32 Arg = 0; #if( ffconfigSDIO_DRIVER_USES_INTERRUPT == 0 ) u32 ulPollCount; #endif #ifndef MMC_CARD /* u32 ClockReg; */ #ifdef __ICCARM__ #pragma data_alignment = 32 u8 ReadBuff[64]; #pragma data_alignment = 4 #else u8 ReadBuff[64] __attribute__ ((aligned(32))); #endif u16 BlkCnt; u16 BlkSize; #endif Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); #ifndef MMC_CARD BlkCnt = XSDPS_SWITCH_CMD_BLKCNT; BlkSize = XSDPS_SWITCH_CMD_BLKSIZE; BlkSize &= XSDPS_BLK_SIZE_MASK; XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_BLK_SIZE_OFFSET, BlkSize); XSdPs_SetupADMA2DescTbl(InstancePtr, BlkCnt, ReadBuff); Xil_DCacheFlushRange( ( unsigned )ReadBuff, 64); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_XFER_MODE_OFFSET, XSDPS_TM_DAT_DIR_SEL_MASK | XSDPS_TM_DMA_EN_MASK); Arg = XSDPS_SWITCH_CMD_HS_SET; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 1); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } #if( ffconfigSDIO_DRIVER_USES_INTERRUPT != 0 ) StatusReg = XSdPs_WaitInterrupt( InstancePtr, XSDPS_INTR_ERR_MASK | XSDPS_INTR_TC_MASK ); if( ( StatusReg & XSDPS_INTR_TC_MASK ) != 0 ) { Status = XST_SUCCESS; } else { Status = XST_FAILURE; goto RETURN_PATH; } #else /* * Check for transfer complete * Polling for response * Limit the time spent here with a simpler counter 'ulPollCount' */ ulPollCount = 0; do { StatusReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET); if (StatusReg & XSDPS_INTR_ERR_MASK) { /* * Write to clear error bits */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); Status = XST_FAILURE; goto RETURN_PATH; } ulPollCount++; if( ulPollCount == POLLCOUNT_MAX ) { Status = XST_FAILURE; goto RETURN_PATH; } } while ((StatusReg & XSDPS_INTR_TC_MASK) == 0); /* * Write to clear bit */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_INTR_TC_MASK); #endif /* ffconfigSDIO_DRIVER_USES_INTERRUPT */ /* * Change the clock frequency to 50 MHz */ Status = XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_50_MHZ); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_SPEED_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET,StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); #else Arg = XSDPS_MMC_HIGH_SPEED_ARG; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } #ifdef __arm__ usleep(XSDPS_MMC_DELAY_FOR_SWITCH); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_52_MHZ); StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_SPEED_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET,StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); #endif Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * * Identify type of card using CMD0 + CMD1 sequence * * * @param InstancePtr is a pointer to the XSdPs instance. * ******************************************************************************/ static s32 XSdPs_IdentifyCard(XSdPs *InstancePtr) { s32 Status; u32 OperCondReg; u8 ReadReg; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); /* 74 CLK delay after card is powered up, before the first command. */ #if defined (__arm__) || defined (__aarch64__) usleep(XSDPS_INIT_DELAY); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif /* CMD0 no response expected */ Status = XSdPs_CmdTransfer(InstancePtr, CMD0, 0U, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* Host High Capacity support & High voltage window */ Status = XSdPs_CmdTransfer(InstancePtr, CMD1, XSDPS_ACMD41_HCS | XSDPS_CMD1_HIGH_VOL, 0U); if (Status != XST_SUCCESS) { InstancePtr->CardType = XSDPS_CARD_SD; } else { InstancePtr->CardType = XSDPS_CARD_MMC; } XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_NORM_INTR_ALL_MASK); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); /* "Software reset for all" is initiated */ XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_SW_RST_OFFSET, XSDPS_SWRST_CMD_LINE_MASK); /* Proceed with initialization only after reset is complete */ ReadReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_SW_RST_OFFSET); while ((ReadReg & XSDPS_SWRST_CMD_LINE_MASK) != 0U) { ReadReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_SW_RST_OFFSET); } Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * * Switches the SD card voltage from 3v3 to 1v8 * * * @param InstancePtr is a pointer to the XSdPs instance. * ******************************************************************************/ static s32 XSdPs_Switch_Voltage(XSdPs *InstancePtr) { s32 Status; u16 CtrlReg; u32 ReadReg; /* Send switch voltage command */ Status = XSdPs_CmdTransfer(InstancePtr, CMD11, 0U, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; } /* Wait for CMD and DATA line to go low */ ReadReg = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_PRES_STATE_OFFSET); while ((ReadReg & (XSDPS_PSR_CMD_SG_LVL_MASK | XSDPS_PSR_DAT30_SG_LVL_MASK)) != 0U) { ReadReg = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_PRES_STATE_OFFSET); } /* Stop the clock */ CtrlReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_CLK_CTRL_OFFSET); CtrlReg &= ~(XSDPS_CC_SD_CLK_EN_MASK | XSDPS_CC_INT_CLK_EN_MASK); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_CLK_CTRL_OFFSET, CtrlReg); /* Wait minimum 5mSec */ #if defined (__arm__) || defined (__aarch64__) (void)usleep(5000U); #endif #ifdef __MICROBLAZE__ MB_Sleep(5U); #endif /* Enabling 1.8V in controller */ CtrlReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL2_OFFSET); CtrlReg |= XSDPS_HC2_1V8_EN_MASK; XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL2_OFFSET, CtrlReg); /* Start clock */ Status = XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_400_KHZ); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* Wait for CMD and DATA line to go high */ ReadReg = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_PRES_STATE_OFFSET); while ((ReadReg & (XSDPS_PSR_CMD_SG_LVL_MASK | XSDPS_PSR_DAT30_SG_LVL_MASK)) != (XSDPS_PSR_CMD_SG_LVL_MASK | XSDPS_PSR_DAT30_SG_LVL_MASK)) { ReadReg = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_PRES_STATE_OFFSET); } RETURN_PATH: return Status; }
/* CalibrateOneAxisGravitational ** ** Synopsis: ** CalibrateOneAxisGravitational(&ACLobj, PAR_AXIS_ZP); ** Parameters: ** PmodACL *InstancePtr - the PmodACL object to communicate with ** uint8_t bAxisInfo - Parameter specifying axes orientation. Can be one of the following: ** 0 PAR_AXIS_XP - X axis is oriented in the gravitational direction ** 1 PAR_AXIS_XN - X axis is oriented in the opposite gravitational direction ** 2 PAR_AXIS_YP - Y axis is oriented in the gravitational direction ** 3 PAR_AXIS_YN - Y axis is oriented in the opposite gravitational direction ** 4 PAR_AXIS_ZP - Z axis is oriented in the gravitational direction ** 5 PAR_AXIS_ZN - Z axis is oriented in the opposite gravitational direction ** ** Return Value: ** ** Errors: ** ** ** Description: ** The accepted argument values are between 0 and +5. ** This function performs the calibration of the accelerometer by setting the offset registers in the following manner: ** computes the correction factor that must be loaded in the offset registers so that the acceleration readings are: ** 1 for the gravitational axis, if positive orientation ** -1 for the gravitational axis, if negative orientation ** 0 for the other axes ** The accepted argument values are between 0 and 5. ** If the argument value is outside this range, the function does nothing. ** */ void CalibrateOneAxisGravitational(PmodACL* InstancePtr, uint8_t bAxisInfo) { // perform calibration float dX, dSumX = 0, dY, dSumY = 0, dZ, dSumZ = 0; // set the offset registers to 0 //Put the device into standby mode to configure it. SetMeasure(InstancePtr,false); SetOffsetG(InstancePtr, PAR_AXIS_X, 0); SetOffsetG(InstancePtr, PAR_AXIS_Y, 0); SetOffsetG(InstancePtr, PAR_AXIS_Z, 0); SetMeasure(InstancePtr,true); // read average acceleration on the three axes int idxAvg; int nCntMeasurements = 128; // consume some readings for(idxAvg = 0; idxAvg < nCntMeasurements; idxAvg++) { ReadAccelG(InstancePtr, &dX, &dY, &dZ); } // compute average values for(idxAvg = 0; idxAvg < nCntMeasurements; idxAvg++) { ReadAccelG(InstancePtr, &dX, &dY, &dZ); dSumX = dSumX + dX; dSumY = dSumY + dY; dSumZ = dSumZ + dZ; } dX = dSumX/nCntMeasurements; dY = dSumY/nCntMeasurements; dZ = dSumZ/nCntMeasurements; // computes the correction that must be put in the offset registers so that the acceleration readings are: // 1 (for the gravitational axis, if positive // -1 (for the gravitational axis, if negative // 0 (for the other axes) switch (bAxisInfo) { case PAR_AXIS_XP: dX = 1.0 - dX; dY = 0.0 - dY; dZ = 0.0 - dZ; break; case PAR_AXIS_XN: dX = -1.0 - dX; dY = 0.0 - dY; dZ = 0.0 - dZ; break; case PAR_AXIS_YP: dY = 1.0 - dY; dX = 0.0 - dX; dZ = 0.0 - dZ; break; case PAR_AXIS_YN: dY = -1.0 - dY; dX = 0.0 - dX; dZ = 0.0 - dZ; break; case PAR_AXIS_ZP: dZ = 1.0 - dZ; dY = 0.0 - dY; dX = 0.0 - dX; break; case PAR_AXIS_ZN: dZ = -1.0 - dZ; dY = 0.0 - dY; dX = 0.0 - dX; break; } //Put the device into standby mode to configure it. SetMeasure(InstancePtr, false); // set the offset data to registers SetOffsetG(InstancePtr, PAR_AXIS_X, dX); SetOffsetG(InstancePtr, PAR_AXIS_Y, dY); SetOffsetG(InstancePtr, PAR_AXIS_Z, dZ); SetMeasure(InstancePtr, true); // some delay is needed MB_Sleep(1000); }
/** * * Initializes a specific XSdPs instance such that the driver is ready to use. * * * @param InstancePtr is a pointer to the XSdPs instance. * @param ConfigPtr is a reference to a structure containing information * about a specific SD device. This function initializes an * InstancePtr object for a specific device specified by the * contents of Config. * @param EffectiveAddr is the device base address in the virtual memory * address space. The caller is responsible for keeping the address * mapping from EffectiveAddr to the device physical base address * unchanged once this function is invoked. Unexpected errors may * occur if the address mapping changes after this function is * called. If address translation is not used, use * ConfigPtr->Config.BaseAddress for this device. * * @return * - XST_SUCCESS if successful. * - XST_DEVICE_IS_STARTED if the device is already started. * It must be stopped to re-initialize. * * @note This function initializes the host controller. * Initial clock of 400KHz is set. * Voltage of 3.3V is selected as that is supported by host. * Interrupts status is enabled and signal disabled by default. * Default data direction is card to host and * 32 bit ADMA2 is selected. Defualt Block size is 512 bytes. * ******************************************************************************/ s32 XSdPs_CfgInitialize(XSdPs *InstancePtr, XSdPs_Config *ConfigPtr, u32 EffectiveAddr) { s32 Status; u8 PowerLevel; u8 ReadReg; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(ConfigPtr != NULL); /* Set some default values. */ InstancePtr->Config.BaseAddress = EffectiveAddr; InstancePtr->Config.InputClockHz = ConfigPtr->InputClockHz; InstancePtr->IsReady = XIL_COMPONENT_IS_READY; InstancePtr->Config.CardDetect = ConfigPtr->CardDetect; InstancePtr->Config.WriteProtect = ConfigPtr->WriteProtect; /* Disable bus power */ XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_POWER_CTRL_OFFSET, 0U); /* Delay to poweroff card */ #if defined (__arm__) || defined (__aarch64__) (void)sleep(1U); #endif #ifdef __MICROBLAZE__ MB_Sleep(1000U); #endif /* "Software reset for all" is initiated */ XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_SW_RST_OFFSET, XSDPS_SWRST_ALL_MASK); /* Proceed with initialization only after reset is complete */ ReadReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_SW_RST_OFFSET); while ((ReadReg & XSDPS_SWRST_ALL_MASK) != 0U) { ReadReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_SW_RST_OFFSET); } /* Host Controller version is read. */ InstancePtr->HC_Version = (u8)(XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL_VER_OFFSET) & XSDPS_HC_SPEC_VER_MASK); /* * Read capabilities register and update it in Instance pointer. * It is sufficient to read this once on power on. */ InstancePtr->Host_Caps = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_CAPS_OFFSET); /* Select voltage and enable bus power. */ XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_POWER_CTRL_OFFSET, XSDPS_PC_BUS_VSEL_3V3_MASK | XSDPS_PC_BUS_PWR_MASK); /* Change the clock frequency to 400 KHz */ Status = XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_400_KHZ); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH ; } if ((InstancePtr->Host_Caps & XSDPS_CAP_VOLT_3V3_MASK) != 0U) { PowerLevel = XSDPS_PC_BUS_VSEL_3V3_MASK; } else if ((InstancePtr->Host_Caps & XSDPS_CAP_VOLT_3V0_MASK) != 0U) { PowerLevel = XSDPS_PC_BUS_VSEL_3V0_MASK; } else if ((InstancePtr->Host_Caps & XSDPS_CAP_VOLT_1V8_MASK) != 0U) { PowerLevel = XSDPS_PC_BUS_VSEL_1V8_MASK; } else { PowerLevel = 0U; } /* Select voltage based on capability and enable bus power. */ XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_POWER_CTRL_OFFSET, PowerLevel | XSDPS_PC_BUS_PWR_MASK); /* Enable ADMA2 in 64bit mode. */ XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET, XSDPS_HC_DMA_ADMA2_32_MASK); /* Enable all interrupt status except card interrupt initially */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_EN_OFFSET, XSDPS_NORM_INTR_ALL_MASK & (~XSDPS_INTR_CARD_MASK)); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_EN_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); /* Disable all interrupt signals by default. */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_SIG_EN_OFFSET, 0x0U); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_SIG_EN_OFFSET, 0x0U); /* * Transfer mode register - default value * DMA enabled, block count enabled, data direction card to host(read) */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_XFER_MODE_OFFSET, XSDPS_TM_DMA_EN_MASK | XSDPS_TM_BLK_CNT_EN_MASK | XSDPS_TM_DAT_DIR_SEL_MASK); /* Set block size to 512 by default */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_BLK_SIZE_OFFSET, XSDPS_BLK_SIZE_512_MASK); Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * * API to set high speed in card and host. Changes clock in host accordingly. * * * @param InstancePtr is a pointer to the XSdPs instance. * * @return * - XST_SUCCESS if successful. * - XST_FAILURE if fail. * * @note None. * ******************************************************************************/ s32 XSdPs_Change_BusSpeed(XSdPs *InstancePtr) { s32 Status; u32 StatusReg; u32 Arg; u32 ClockReg; u16 BlkCnt; u16 BlkSize; u8 ReadBuff[64]; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); if (InstancePtr->CardType == XSDPS_CARD_SD) { BlkCnt = XSDPS_SWITCH_CMD_BLKCNT; BlkSize = XSDPS_SWITCH_CMD_BLKSIZE; BlkSize &= XSDPS_BLK_SIZE_MASK; XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_BLK_SIZE_OFFSET, BlkSize); XSdPs_SetupADMA2DescTbl(InstancePtr, BlkCnt, ReadBuff); Xil_DCacheFlushRange((INTPTR)ReadBuff, 64); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_XFER_MODE_OFFSET, XSDPS_TM_DAT_DIR_SEL_MASK | XSDPS_TM_DMA_EN_MASK); Arg = XSDPS_SWITCH_CMD_HS_SET; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 1U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Check for transfer complete * Polling for response for now */ do { StatusReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET); if ((StatusReg & XSDPS_INTR_ERR_MASK) != 0U) { /* Write to clear error bits */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); Status = XST_FAILURE; goto RETURN_PATH; } } while ((StatusReg & XSDPS_INTR_TC_MASK) == 0U); /* Write to clear bit */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_INTR_TC_MASK); /* Change the clock frequency to 50 MHz */ InstancePtr->BusSpeed = XSDPS_CLK_50_MHZ; Status = XSdPs_Change_ClkFreq(InstancePtr, InstancePtr->BusSpeed); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } } else if (InstancePtr->CardType == XSDPS_CARD_MMC) { Arg = XSDPS_MMC_HIGH_SPEED_ARG; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Check for transfer complete */ do { StatusReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET); if ((StatusReg & XSDPS_INTR_ERR_MASK) != 0U) { /* * Write to clear error bits */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); Status = XST_FAILURE; goto RETURN_PATH; } } while ((StatusReg & XSDPS_INTR_TC_MASK) == 0U); /* * Write to clear bit */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_INTR_TC_MASK); /* Change the clock frequency to 52 MHz */ InstancePtr->BusSpeed = XSDPS_CLK_52_MHZ; Status = XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_52_MHZ); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } } else { Arg = XSDPS_MMC_HS200_ARG; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Check for transfer complete */ do { StatusReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET); if ((StatusReg & XSDPS_INTR_ERR_MASK) != 0U) { /* * Write to clear error bits */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); Status = XST_FAILURE; goto RETURN_PATH; } } while ((StatusReg & XSDPS_INTR_TC_MASK) == 0U); /* * Write to clear bit */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_INTR_TC_MASK); /* Change the clock frequency to 200 MHz */ InstancePtr->BusSpeed = XSDPS_MMC_HS200_MAX_CLK; Status = XSdPs_Change_ClkFreq(InstancePtr, InstancePtr->BusSpeed); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } Status = XSdPs_Execute_Tuning(InstancePtr); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } #if defined (__arm__) || defined (__aarch64__) /* Program the Tap delays */ XSdPs_SetTapDelay(InstancePtr); #endif } #if defined (__arm__) || defined (__aarch64__) usleep(XSDPS_MMC_DELAY_FOR_SWITCH); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif StatusReg = (s32)XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_SPEED_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET, (u8)StatusReg); Status = (s32)XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * Mmc initialization is done in this function * * * @param InstancePtr is a pointer to the instance to be worked on. * * @return * - XST_SUCCESS if initialization was successful * - XST_FAILURE if failure - could be because * a) MMC is already initialized * b) There is no card inserted * c) One of the steps (commands) in the initialization * cycle failed * @note This function initializes the SD card by following its * initialization and identification state diagram. * CMD0 is sent to reset card. * CMD1 sent to identify voltage and high capacity support * CMD2 and CMD3 are sent to obtain Card ID and * Relative card address respectively. * CMD9 is sent to read the card specific data. * ******************************************************************************/ int XSdPs_MmcCardInitialize(XSdPs *InstancePtr) { u32 PresentStateReg; u32 Status; u32 RespOCR = 0x0; u32 CSD[4]; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); /* * Check the present state register to make sure * card is inserted and detected by host controller */ PresentStateReg = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_PRES_STATE_OFFSET); if ((PresentStateReg & XSDPS_PSR_CARD_INSRT_MASK) == 0) { Status = XST_FAILURE; goto RETURN_PATH; } /* * 74 CLK delay after card is powered up, before the first command. */ #ifdef __arm__ usleep(XSDPS_INIT_DELAY); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif /* * CMD0 no response expected */ Status = XSdPs_CmdTransfer(InstancePtr, CMD0, 0, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } RespOCR = 0; /* * Send CMD1 while card is still busy with power up */ while ((RespOCR & XSDPS_RESPOCR_READY) == 0) { /* * Host High Capacity support & High volage window */ Status = XSdPs_CmdTransfer(InstancePtr, CMD1, XSDPS_ACMD41_HCS | XSDPS_CMD1_HIGH_VOL, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Response with card capacity */ RespOCR = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); } /* * Update HCS support flag based on card capacity response */ if (RespOCR & XSDPS_ACMD41_HCS) InstancePtr->HCS = 1; /* * CMD2 for Card ID */ Status = XSdPs_CmdTransfer(InstancePtr, CMD2, 0, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } InstancePtr->CardID[0] = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); InstancePtr->CardID[1] = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_RESP1_OFFSET); InstancePtr->CardID[2] = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_RESP2_OFFSET); InstancePtr->CardID[3] = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_RESP3_OFFSET); Status = XSdPs_CmdTransfer(InstancePtr, CMD3, 0, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Relative card address is stored as the upper 16 bits * This is to avoid shifting when sending commands */ InstancePtr->RelCardAddr = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET) & 0xFFFF0000; Status = XSdPs_CmdTransfer(InstancePtr, CMD9, (InstancePtr->RelCardAddr), 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Card specific data is read. * Currently not used for any operation. */ CSD[0] = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); CSD[1] = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP1_OFFSET); CSD[2] = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP2_OFFSET); CSD[3] = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP3_OFFSET); Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * * API to set bus width to 4-bit in card and host * * * @param InstancePtr is a pointer to the XSdPs instance. * * @return * - XST_SUCCESS if successful. * - XST_FAILURE if fail. * * @note None. * ******************************************************************************/ int XSdPs_Change_BusWidth(XSdPs *InstancePtr) { u32 Status = 0; u32 StatusReg = 0x0; u32 Arg = 0; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); if (InstancePtr->CardType == XSDPS_CARD_SD) { Status = XSdPs_CmdTransfer(InstancePtr, CMD55, InstancePtr->RelCardAddr, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } InstancePtr->BusWidth = XSDPS_4_BIT_WIDTH; Arg = InstancePtr->BusWidth; Status = XSdPs_CmdTransfer(InstancePtr, ACMD6, Arg, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } } else { if ((InstancePtr->HC_Version == XSDPS_HC_SPEC_V3) && (InstancePtr->CardType == XSDPS_CHIP_EMMC)) { /* in case of eMMC data width 8-bit */ InstancePtr->BusWidth = XSDPS_8_BIT_WIDTH; } else { InstancePtr->BusWidth = XSDPS_4_BIT_WIDTH; } if (InstancePtr->BusWidth == XSDPS_8_BIT_WIDTH) { Arg = XSDPS_MMC_8_BIT_BUS_ARG; } else { Arg = XSDPS_MMC_4_BIT_BUS_ARG; } Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } } #ifdef __arm__ usleep(XSDPS_MMC_DELAY_FOR_SWITCH); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); /* Width setting in controller */ if (InstancePtr->BusWidth == XSDPS_8_BIT_WIDTH) { StatusReg |= XSDPS_HC_EXT_BUS_WIDTH; } else { StatusReg |= XSDPS_HC_WIDTH_MASK; } XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET, StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * * API to set bus width to 4-bit in card and host * * * @param InstancePtr is a pointer to the XSdPs instance. * * @return * - XST_SUCCESS if successful. * - XST_FAILURE if fail. * * @note None. * ******************************************************************************/ s32 XSdPs_Change_BusWidth(XSdPs *InstancePtr) { s32 Status; u32 StatusReg; u32 Arg; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); if (InstancePtr->CardType == XSDPS_CARD_SD) { Status = XSdPs_CmdTransfer(InstancePtr, CMD55, InstancePtr->RelCardAddr, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } InstancePtr->BusWidth = XSDPS_4_BIT_WIDTH; Arg = ((u32)InstancePtr->BusWidth); Status = XSdPs_CmdTransfer(InstancePtr, ACMD6, Arg, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } } else { if ((InstancePtr->HC_Version == XSDPS_HC_SPEC_V3) && (InstancePtr->CardType == XSDPS_CHIP_EMMC)) { /* in case of eMMC data width 8-bit */ InstancePtr->BusWidth = XSDPS_8_BIT_WIDTH; } else { InstancePtr->BusWidth = XSDPS_4_BIT_WIDTH; } if (InstancePtr->BusWidth == XSDPS_8_BIT_WIDTH) { Arg = XSDPS_MMC_8_BIT_BUS_ARG; } else { Arg = XSDPS_MMC_4_BIT_BUS_ARG; } Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 0U); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* Check for transfer complete */ do { StatusReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET); if ((StatusReg & XSDPS_INTR_ERR_MASK) != 0U) { /* Write to clear error bits */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); Status = XST_FAILURE; goto RETURN_PATH; } } while((StatusReg & XSDPS_INTR_TC_MASK) == 0U); /* Write to clear bit */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_INTR_TC_MASK); } #if defined (__arm__) || defined (__aarch64__) usleep(XSDPS_MMC_DELAY_FOR_SWITCH); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); /* Width setting in controller */ if (InstancePtr->BusWidth == XSDPS_8_BIT_WIDTH) { StatusReg |= XSDPS_HC_EXT_BUS_WIDTH; } else { StatusReg |= XSDPS_HC_WIDTH_MASK; } XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET, (u8)StatusReg); Status = (s32)XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); Status = XST_SUCCESS; RETURN_PATH: return Status; }
void DemoRun() { int nextFrame = 0; char userInput = 0; u32 locked; XGpio *GpioPtr = &videoCapt.gpio; /* Flush UART FIFO */ while (!XUartLite_IsReceiveEmpty(UART_BASEADDR)) { XUartLite_ReadReg(UART_BASEADDR, XUL_RX_FIFO_OFFSET); } while (userInput != 'q') { fRefresh = 0; DemoPrintMenu(); /* Wait for data on UART */ while (XUartLite_IsReceiveEmpty(UART_BASEADDR) && !fRefresh) {} /* Store the first character in the UART receive FIFO and echo it */ if (!XUartLite_IsReceiveEmpty(UART_BASEADDR)) { userInput = XUartLite_ReadReg(UART_BASEADDR, XUL_RX_FIFO_OFFSET); xil_printf("%c", userInput); } else //Refresh triggered by video detect interrupt { userInput = 'r'; } switch (userInput) { case '1': DemoChangeRes(); break; case '2': nextFrame = dispCtrl.curFrame + 1; if (nextFrame >= DISPLAY_NUM_FRAMES) { nextFrame = 0; } DisplayChangeFrame(&dispCtrl, nextFrame); break; case '3': DemoPrintTest(pFrames[dispCtrl.curFrame], dispCtrl.vMode.width, dispCtrl.vMode.height, DEMO_STRIDE, DEMO_PATTERN_0); break; case '4': DemoPrintTest(pFrames[dispCtrl.curFrame], dispCtrl.vMode.width, dispCtrl.vMode.height, DEMO_STRIDE, DEMO_PATTERN_1); break; case '5': if (videoCapt.state == VIDEO_STREAMING) VideoStop(&videoCapt); else VideoStart(&videoCapt); break; case '6': nextFrame = videoCapt.curFrame + 1; if (nextFrame >= DISPLAY_NUM_FRAMES) { nextFrame = 0; } VideoChangeFrame(&videoCapt, nextFrame); break; case '7': nextFrame = videoCapt.curFrame + 1; if (nextFrame >= DISPLAY_NUM_FRAMES) { nextFrame = 0; } VideoStop(&videoCapt); DemoInvertFrame(pFrames[videoCapt.curFrame], pFrames[nextFrame], videoCapt.timing.HActiveVideo, videoCapt.timing.VActiveVideo, DEMO_STRIDE); VideoStart(&videoCapt); DisplayChangeFrame(&dispCtrl, nextFrame); break; case '8': nextFrame = videoCapt.curFrame + 1; if (nextFrame >= DISPLAY_NUM_FRAMES) { nextFrame = 0; } VideoStop(&videoCapt); DemoScaleFrame(pFrames[videoCapt.curFrame], pFrames[nextFrame], videoCapt.timing.HActiveVideo, videoCapt.timing.VActiveVideo, dispCtrl.vMode.width, dispCtrl.vMode.height, DEMO_STRIDE); VideoStart(&videoCapt); DisplayChangeFrame(&dispCtrl, nextFrame); break; case 'q': break; case 'r': locked = XGpio_DiscreteRead(GpioPtr, 2); xil_printf("%d", locked); break; default : xil_printf("\n\rInvalid Selection"); MB_Sleep(50); } } return; }
static void AutoMainLoop(void) { int x_diff; int y_diff; int x_adj; int y_adj; InitMotorBoard(); TurnOnLaser(); EnablePanServo(); EnableTiltServo(); SetTiltAngle(DEFAULT_X_POS); SetPanAngle(DEFAULT_Y_POS); int acquired = 0; int sleep_interval; continueTest = true; xil_printf("Entering auto mode.\r\n"); while(continueTest) { runImageProcessing(); SetObjIdValue(1); TargetingState state = get_targeting_state(); int target_x = state.target_loc.x; int target_y = state.target_loc.y; u32 target_size = state.target_size; xil_printf( "Targeting state: Laser = (%d,%d); Obj = (%d, %d) [sz=%d]\n\r", state.laser.x, state.laser.y, target_x, target_y, target_size); if (state.laser.x == 0 && state.laser.y == 0) { SetPanAngle(DEFAULT_X_POS); SetTiltAngle(DEFAULT_Y_POS); playNeg(); goto loop_sleep; } // Get the diffs in both dimensions. //x_diff = X_MIDDLE; //y_diff = Y_MIDDLE; x_diff = (target_x - state.laser.x); y_diff = (state.laser.y - target_y); // Determine how much to adjust the angles. x_adj = (x_diff > 0) ? x_diff : -x_diff; x_adj /= 16; int MAX_ANGLE = 10; if (x_adj > MAX_ANGLE) { x_adj = MAX_ANGLE; } else if (x_adj == 0 && x_diff != 0) { x_adj = 1; } y_adj = (y_diff > 0) ? y_diff : -y_diff; y_adj /= 16; if (y_adj > MAX_ANGLE) { y_adj = MAX_ANGLE; } else if (y_adj == 0 && y_diff != 0) { y_adj = 1; } // Adjust laser position to correct for these diffs. if (x_diff > LASER_TOLERANCE) { xil_printf("Need to move laser right by %d deg\r\n", x_adj); PanRight(x_adj); } else if (x_diff < -LASER_TOLERANCE) { xil_printf("Need to move laser left by %d deg\r\n", x_adj); PanLeft(x_adj); } else { xil_printf("X location on target!\r\n"); } if (y_diff > LASER_TOLERANCE) { xil_printf("Need to move laser up by %d deg\r\n", y_adj); TiltUp(y_adj); } else if (y_diff < -LASER_TOLERANCE) { xil_printf("Need to move laser down by %d deg\r\n", y_adj); TiltDown(y_adj); } else { xil_printf("Y location on target!\r\n"); } if (x_diff <= LASER_TOLERANCE && x_diff >= -LASER_TOLERANCE && y_diff <= LASER_TOLERANCE && y_diff >= -LASER_TOLERANCE) { if (acquired) { playPortalGunSound(); } else { playPos(); acquired = 1; } } else { acquired = 0; } loop_sleep: sleep_interval = sDebugOutputEnabled ? 3000 : 500; MB_Sleep(sleep_interval); } SetTiltAngle(0); SetPanAngle(0); MB_Sleep(100); DisableTiltServo(); DisablePanServo(); TurnOffLaser(); }
void DemoChangeRes() { int fResSet = 0; int status; char userInput = 0; /* Flush UART FIFO */ while (!XUartLite_IsReceiveEmpty(UART_BASEADDR)) { XUartLite_ReadReg(UART_BASEADDR, XUL_RX_FIFO_OFFSET); } while (!fResSet) { DemoCRMenu(); /* Wait for data on UART */ while (XUartLite_IsReceiveEmpty(UART_BASEADDR) && !fRefresh) {} /* Store the first character in the UART recieve FIFO and echo it */ userInput = XUartLite_ReadReg(UART_BASEADDR, XUL_RX_FIFO_OFFSET); xil_printf("%c", userInput); status = XST_SUCCESS; switch (userInput) { case '1': status = DisplayStop(&dispCtrl); DisplaySetMode(&dispCtrl, &VMODE_640x480); DisplayStart(&dispCtrl); fResSet = 1; break; case '2': status = DisplayStop(&dispCtrl); DisplaySetMode(&dispCtrl, &VMODE_800x600); DisplayStart(&dispCtrl); fResSet = 1; break; case '3': status = DisplayStop(&dispCtrl); DisplaySetMode(&dispCtrl, &VMODE_1280x720); DisplayStart(&dispCtrl); fResSet = 1; break; case '4': status = DisplayStop(&dispCtrl); DisplaySetMode(&dispCtrl, &VMODE_1280x1024); DisplayStart(&dispCtrl); fResSet = 1; break; case '5': status = DisplayStop(&dispCtrl); DisplaySetMode(&dispCtrl, &VMODE_1920x1080); DisplayStart(&dispCtrl); fResSet = 1; break; case 'q': fResSet = 1; break; default : xil_printf("\n\rInvalid Selection"); MB_Sleep(50); } if (status == XST_DMA_ERROR) { xil_printf("\n\rWARNING: AXI VDMA Error detected and cleared\n\r"); } } }
int main(void) { colorPoint tempRead; int x; //Initialize the color sensor init_csens(); //initialize the hapt AND Calibrate the hapt for (x=1; x<4; x++){ init_hapt(x); autoCal_hapt(x); } u8 val, ampR, ampG, ampB; int VIB, SLEEP; while (1) { //with a switch we turn it on val = Xil_In8 (base+leve); if ((val==1) || (val==2) || (val==4) || (val==8)) { xil_printf ("Il dispositivo è acceso\n"); tempRead = readRGB_csens(); xil_printf("Red value is %d. \n",tempRead.Red); xil_printf("Green value is %d. \n",tempRead.Green); xil_printf("Blue value is %d. \n",tempRead.Blue); xil_printf("Clear value is %d.\n",tempRead.Clear); ampR = 12 * tempRead.Red /25; ampG = 12 * tempRead.Green /25; ampB = 12 * tempRead.Blue /25; if (tempRead.Clear > 100) { VIB= 500; SLEEP=500; } else { VIB= 2000; SLEEP= 2000; } { write_hapt(MODE_Reg, 0x05, 1, 1); write_hapt(MODE_Reg, 0x05, 1, 2); write_hapt(MODE_Reg, 0x05, 1, 3); write_hapt(RTP_INPUT_Reg, ampR, 1, 1); write_hapt(RTP_INPUT_Reg, ampG, 1, 2); write_hapt(RTP_INPUT_Reg, ampB, 1, 3); MB_Sleep (VIB); write_hapt(MODE_Reg,0x40,1, 1); write_hapt(MODE_Reg,0x40,1, 2); write_hapt(MODE_Reg,0x40,1, 3); MB_Sleep (SLEEP); write_hapt(GO_Reg, 0x00,1, 1); write_hapt(GO_Reg, 0x00,1, 2); write_hapt(GO_Reg, 0x00,1, 3); } } } return 0; }
/** * * API to set bus width to 4-bit in card and host * * * @param InstancePtr is a pointer to the XSdPs instance. * * @return * - XST_SUCCESS if successful. * - XST_FAILURE if fail. * * @note None. * ******************************************************************************/ int XSdPs_Change_BusWidth(XSdPs *InstancePtr) { u32 Status = 0; u32 StatusReg = 0x0; u32 Arg = 0; Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); #ifndef MMC_CARD Status = XSdPs_CmdTransfer(InstancePtr, CMD55, InstancePtr->RelCardAddr, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } Arg = XSDPS_4_BIT_WIDTH; Status = XSdPs_CmdTransfer(InstancePtr, ACMD6, Arg, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_WIDTH_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET,StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); #else Arg = XSDPS_MMC_4_BIT_BUS_ARG; Status = XSdPs_CmdTransfer(InstancePtr, ACMD6, Arg, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } #ifdef __arm__ usleep(XSDPS_MMC_DELAY_FOR_SWITCH); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_WIDTH_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET,StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); #endif Status = XST_SUCCESS; RETURN_PATH: return Status; }
/****************************************************************************** * This function initializes the OLED display with the proper procedure. * * @param psSpi is the pointer to the SPI driver structure. * @param psGpio is the pointer to the GPIO driver structure. * * @return XST_SUCCESS - Everything went well * XST_FAILURE - Failure * * @note This is how the GPIO is mapped to the OLED controller: * gpio(0) = vbat * gpio(1) = vdd * gpio(2) = res * gpio(3) = dc *****************************************************************************/ XStatus fnOledDisplayInit(XSpi *psSpi, XGpio *psGpio) { XStatus Status; u8 rgbWrBuf[10]; // Clear the data/cmd bit XGpio_DiscreteClear(psGpio, 1, OLED_DC_MASK); // Start by turning VDD on and wait for the power to come up XGpio_DiscreteClear(psGpio, 1, OLED_VDD_MASK); MB_Sleep(1); // Send to SPI the display off command rgbWrBuf[0] = OLED_DISPLAY_OFF_CMD; u8TransferInProg = TRUE; Status = XSpi_Transfer(psSpi, rgbWrBuf, NULL, 1); while(u8TransferInProg); // Bring reset low and then high XGpio_DiscreteSet(psGpio, 1, OLED_RES_MASK); MB_Sleep(1); XGpio_DiscreteClear(psGpio, 1, OLED_RES_MASK); MB_Sleep(1); XGpio_DiscreteSet(psGpio, 1, OLED_RES_MASK); // Send the set charge pump and set pre-charge period commands rgbWrBuf[0] = 0x8D; rgbWrBuf[1] = 0x14; rgbWrBuf[2] = OLED_SET_PRECHARGE_PER_CMD; rgbWrBuf[3] = 0xF1; u8TransferInProg = TRUE; Status = XSpi_Transfer(psSpi, rgbWrBuf, NULL, 4); while(u8TransferInProg); // Turn on VCC and wait 100 ms XGpio_DiscreteClear(psGpio, 1, OLED_VBAT_MASK); MB_Sleep(100); // Set display contrast rgbWrBuf[0] = OLED_CONTRAST_CTRL_CMD; rgbWrBuf[1] = 0x0F; // Invert the display rgbWrBuf[2] = OLED_SET_SEGMENT_REMAP_CMD; rgbWrBuf[3] = OLED_SET_COM_DIR_CMD; // Select sequential COM configuration rgbWrBuf[4] = OLED_SET_COM_PINS_CMD; rgbWrBuf[5] = 0x00; rgbWrBuf[6] = 0xC0; rgbWrBuf[7] = 0x20; rgbWrBuf[8] = 0x00; // Turn display on rgbWrBuf[9]/*[6]*/ = OLED_DISPLAY_ON_CMD; u8TransferInProg = TRUE; Status = XSpi_Transfer(psSpi, rgbWrBuf, NULL, 10); while(u8TransferInProg); //Display Digilent logo fnOledPixelToDisplay(psSpi, psGpio, &sLogoPixmap); return Status; }
/*************************************************************************** * This is the main thread that will do all initializations. * It will call configure functions for all subsystems and system level * peripherals ***************************************************************************/ int main(void) { XPeriph *PeriphPtr; XVprocSs *VpssPtr; int status; u32 Timeout; static int Lock = FALSE; /* Bind instance pointer with definition */ PeriphPtr = &PeriphInst; VpssPtr = &VprocInst; /* Initialize ICache */ Xil_ICacheInvalidate(); Xil_ICacheEnable(); /* Initialize DCache */ Xil_DCacheInvalidate(); Xil_DCacheEnable(); xil_printf("\r\n--------------------------------------------------------\r\n"); xil_printf(" Video Processing Subsystem Example Design %s\r\n", XVPROCSS_SW_VER); xil_printf(" (c) 2015 by Xilinx Inc.\r\n"); xil_printf("--------------------------------------------------------\r\n"); xil_printf("\r\nInitialize System Design...\r\n"); status = XSys_Init(PeriphPtr, VpssPtr); if(status != XST_SUCCESS) { xil_printf("CRITICAL ERR:: System Init Failed. Cannot recover from this error. Check HW\n\r"); } #if (VERBOSE_MODE == 1) xil_printf("\r\nINFO> Setting up VPSS AXIS In/Out\r\n"); #endif //Set TPG default parameters XPeriph_SetTpgParams(PeriphPtr, 1920, 1080, XVIDC_CSF_RGB, XTPG_BKGND_COLOR_BARS, FALSE); //Set AXIS In to TPG settings XSys_SetStreamParam(VpssPtr, XSYS_VPSS_STREAM_IN, PeriphInst.TpgConfig.Width, PeriphInst.TpgConfig.Height, PeriphInst.TpgConfig.ColorFmt, PeriphInst.TpgConfig.IsInterlaced); if(VpssPtr->Config.Topology == XVPROCSS_TOPOLOGY_SCALER_ONLY) { /* Only Scaling Ratio can be changed. Stream out color format * must be same as stream in */ //Set AXIS Out XSys_SetStreamParam(VpssPtr, XSYS_VPSS_STREAM_OUT, 3840, 2160, PeriphInst.TpgConfig.ColorFmt, FALSE); } else //FULL_FLEDGED { //Set AXIS Out XSys_SetStreamParam(VpssPtr, XSYS_VPSS_STREAM_OUT, 3840, 2160, XVIDC_CSF_YCRCB_422, FALSE); } //Configure video processing subsystem status = XVprocSs_SetSubsystemConfig(VpssPtr); //Query vpss configuration XVprocSs_ReportSubsystemConfig(VpssPtr); if(status == XST_SUCCESS) { //Configure VTC with output timing XPeriph_ConfigVtc(PeriphPtr, &VpssPtr->VidOut, VprocInst.Config.PixPerClock); //Config TPG for AXIS In XPeriph_ConfigTpg(PeriphPtr); #if (VERBOSE_MODE == 1) XPeriph_TpgDbgReportStatus(PeriphPtr); #endif /* vtc is running at 9Mhz essentially providing < 2fps frame rate * Need to wait for 3-4 frames (~2sec) for vidout to acquire lock */ xil_printf("\r\nWaiting for output to lock: "); MB_Sleep(2000); /* check for output lock */ Timeout = VIDEO_MONITOR_LOCK_TIMEOUT; while(!Lock && Timeout) { if(XPeriph_IsVideoLocked(PeriphPtr)) { xil_printf("Locked\r\n"); Lock = TRUE; } --Timeout; } if(!Timeout) { xil_printf("\r\nTEST FAILED\r\n"); } else { xil_printf("\r\nTEST PASSED\r\n"); } } else { xil_printf("\r\nERR:: VProcss Configuration Failed. \r\n"); xil_printf("\r\nTEST FAILED\r\n"); } while(1) { //NOP } /* Clean up DCache. For writeback caches, the disable_dcache routine internally does the flush and invalidate. For write through caches, an explicit invalidation must be performed on the entire cache. */ #if XPAR_MICROBLAZE_DCACHE_USE_WRITEBACK == 0 Xil_DCacheInvalidate (); #endif Xil_DCacheDisable (); /* Clean up ICache */ Xil_ICacheInvalidate (); Xil_ICacheDisable (); return 0; }
/** * * API to set high speed in card and host. Changes clock in host accordingly. * * * @param InstancePtr is a pointer to the XSdPs instance. * * @return * - XST_SUCCESS if successful. * - XST_FAILURE if fail. * * @note None. * ******************************************************************************/ int XSdPs_Change_BusSpeed(XSdPs *InstancePtr) { u32 Status = 0; u32 StatusReg = 0x0; u32 Arg = 0; #ifndef MMC_CARD u32 ClockReg; u8 ReadBuff[64]; u16 BlkCnt; u16 BlkSize; #endif Xil_AssertNonvoid(InstancePtr != NULL); Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY); #ifndef MMC_CARD BlkCnt = XSDPS_SWITCH_CMD_BLKCNT; BlkSize = XSDPS_SWITCH_CMD_BLKSIZE; BlkSize &= XSDPS_BLK_SIZE_MASK; XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_BLK_SIZE_OFFSET, BlkSize); XSdPs_SetupADMA2DescTbl(InstancePtr, BlkCnt, ReadBuff); Xil_DCacheInvalidateRange(ReadBuff, 64); XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_XFER_MODE_OFFSET, XSDPS_TM_DAT_DIR_SEL_MASK | XSDPS_TM_DMA_EN_MASK); Arg = XSDPS_SWITCH_CMD_HS_SET; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 1); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } /* * Check for transfer complete * Polling for response for now */ do { StatusReg = XSdPs_ReadReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET); if (StatusReg & XSDPS_INTR_ERR_MASK) { /* * Write to clear error bits */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_ERR_INTR_STS_OFFSET, XSDPS_ERROR_INTR_ALL_MASK); Status = XST_FAILURE; goto RETURN_PATH; } } while ((StatusReg & XSDPS_INTR_TC_MASK) == 0); /* * Write to clear bit */ XSdPs_WriteReg16(InstancePtr->Config.BaseAddress, XSDPS_NORM_INTR_STS_OFFSET, XSDPS_INTR_TC_MASK); /* * Change the clock frequency to 50 MHz */ Status = XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_50_MHZ); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_SPEED_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET,StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); #else Arg = XSDPS_MMC_HIGH_SPEED_ARG; Status = XSdPs_CmdTransfer(InstancePtr, CMD6, Arg, 0); if (Status != XST_SUCCESS) { Status = XST_FAILURE; goto RETURN_PATH; } #ifdef __arm__ usleep(XSDPS_MMC_DELAY_FOR_SWITCH); #endif #ifdef __MICROBLAZE__ /* 2 msec delay */ MB_Sleep(2); #endif XSdPs_Change_ClkFreq(InstancePtr, XSDPS_CLK_52_MHZ); StatusReg = XSdPs_ReadReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET); StatusReg |= XSDPS_HC_SPEED_MASK; XSdPs_WriteReg8(InstancePtr->Config.BaseAddress, XSDPS_HOST_CTRL1_OFFSET,StatusReg); Status = XSdPs_ReadReg(InstancePtr->Config.BaseAddress, XSDPS_RESP0_OFFSET); #endif Status = XST_SUCCESS; RETURN_PATH: return Status; }
/** * * Main function * * This function is the main entry of the interrupt test. It does the following: * Initialize the audio * Initialize the debug uart * Enable the interrupts * * @param None * * @return * - XST_SUCCESS if example finishes successfully * - XST_FAILURE if example fails. * * @note None. * ******************************************************************************/ int main(void) { init_platform(); InitMotorBoard(); xil_printf("\r\n--- Entering main() --- \r\n"); int status = XST_SUCCESS; status |= initialize_audio(&sIic, &sAxiAudioDma); status |= fnInitInterruptController(&sIntc); status |= initialize_uart(&sUartLite); status |= initialize_video(&sVideoCapt, &sIntc); status |= initialize_targeting(&sAxiTargetingDma); status |= SetupSdGpio(&sGpio); if (status != XST_SUCCESS) { xil_printf("Failed to initialize system.\r\n"); return XST_FAILURE; } // Initialize static variables. for (int i = 0; i < FILE_ID_MAX; i++) { sSdFileBoard[i].loaded = false; } sSdFileMemTip = (u32 *) AUDIO_BASE_ADDR; sButtonState = 0; sLoopSelect = DEFAULT_LOOP; fnEnableInterrupts(&sIntc, &ivt[0], sizeof(ivt)/sizeof(ivt[0])); register_uart_response("test", test_fcn); register_uart_response("record", uart_rec_audio); register_uart_response("play", uart_play_audio); register_uart_response("kill", end_fcn); register_uart_response("exit", end_fcn); register_uart_response("dump", dump_mem); // Commands to run self-tests register_uart_response("lowlevel", LowLevelTest); register_uart_response("highlevel", HighLevelTest); register_uart_response("lasertest", EnterLaserTest); register_uart_response("motortest", EnterMotorTest); register_uart_response("stop", stopTest); register_uart_response("load_sounds", loadSounds); register_uart_response("load_images", loadImages); register_uart_response("still_alive", playPortalSong); register_uart_response("gun", playGunSound); register_uart_response("portal_gun", playPortalGunSound); register_uart_response("target", playTargetAcquired); register_uart_response("playpos", playPos); register_uart_response("playneg", playNeg); register_uart_response("manual", EnterManualMainLoop); register_uart_response("auto", EnterAutomaticMainLoop); register_uart_response("passthrough", passthroughHdmi); register_uart_response("runip", EnterIpTest); register_uart_response("videoinfo", print_video_info); register_uart_response("df1", df1); register_uart_response("df2", df2); register_uart_response("df0", df0); register_uart_response("vf1", vf1); register_uart_response("vf2", vf2); register_uart_response("vf0", vf0); register_uart_response("ipinfo", print_ip_info); register_uart_response("ipouttoggle", toggle_ip_output); register_uart_response("dummytarget", setDummyTarget); register_uart_response("lemon", DisplayLemon); register_uart_response("heman", DisplayHeman); register_uart_response("pass", SetPassthroughMode); register_uart_response("gray", SetGrayscaleMode); register_uart_response("sobel", SetSobelMode); register_uart_response("thresh", SetThresholdMode); register_uart_response("label", SetLabelMode); register_uart_response("colour", SetColourMode); register_uart_response("laser", SetLaserMode); register_uart_response("flood1", SetFlood1Mode); register_uart_response("flood2", SetFlood2Mode); register_uart_response("laseron", LaserOn); register_uart_response("laseroff", LaserOff); register_uart_response("redthresh", SetRedThreshold); register_uart_response("sobelthresh", SetSobelThreshold); register_uart_response("f1thresh", SetFlood1Threshold); register_uart_response("f2thresh", SetFlood2Threshold); register_uart_response("setminsize", SetSizeThreshold); register_uart_response("setobjid", SetObjId); register_uart_response("test_args", TestArgs); xil_printf("\r\n--- Done registering UART commands --- \r\n"); initialSetup(); xil_printf(PROMPT_STRING); while (do_run) { switch (sLoopSelect) { case MANUAL_MODE: ManualMainLoop(); break; case AUTOMATIC_MODE: AutoMainLoop(); break; case LASER_TEST: LaserTest(); break; case MOTOR_TEST: MotorPatternTest(); break; case IP_TEST: runImageProcessing(); sLoopSelect = DEFAULT_LOOP; break; default: MB_Sleep(100); break; } } xil_printf("\r\n--- Exiting main() --- \r\n"); return XST_SUCCESS; }