void main() { InitVoice(); // lock the external motor, prohibit motor moving when power up. InitElecmotor(); InitUART(BAUD9600); // #ifdef WX // InitUART(BAUD1200); // #endif InitSensor(); InitTransceiver(); Externalmotor = Close; Flash_Page_Erase(0x3000); Flash_Write_Data(0x3000, 0x00); Flash_Write_Data(0x3001, 0x25); Flash_Write_Data(0x3002, 0x12); Flash_Write_Data(0x3003, 0xf7); Flash_Write_Data(0x3004, 0x85); transmiter_EN = 1; while(1) { } }
void InitCIS(void) { XBYTE[0xFCC3] = 0x40; //CIS_ctrl. 0x40:CPU force master clock enable, any data won't write to FIFO. SENSOR_POWER_PIN = SENSOR_PIN_ON; InitSensor(); }
void InitSystem(void) { InitLed(); InitUart(); BluetoothInit(); InitAdc(); InitSensor(); InitFan(); InitRtc(); InitSecondTimer(); }
void Prepare(void) { int i; GetLegOrigin(LEGORIGIN); Update(); switch(STATE) { case IDLE: for(i = 0; i < 6; i++) { LEGGOAL[i*3] = LEGORIGIN[i*3]; LEGGOAL[i*3+1] = LEGORIGIN[i*3+1]; LEGGOAL[i*3 + 2] = LEGORIGIN[i*3 + 2] + GOAL_HIGHT; } break; case BALANCE: InitSensor(); for(i = 0; i < 6; i++) { LEGGOAL[i*3] = LEGORIGIN[i*3]; LEGGOAL[i*3+1] = LEGORIGIN[i*3+1]; LEGGOAL[i*3 + 2] = LEGORIGIN[i*3 + 2]; } SetOrigin(NORMAL, POSITION); STABLETIMER = timer_nowtime(); break; default: if(SWING == 0) SetOrigin(NORMAL, POSITION); for(i = 0; i < 6; i++) { LEGFRAME[i] = -1; GetLegGoal(GOALPOS, i, LEGGOAL); GetLegOpposite(GOALPOS, i, STATE,LEGOPPOSITE); if(SWING == 1) { LEGGOAL[i*3 + 2] = LEGGOAL[i*3 + 2] + 10.0; LEGOPPOSITE[i*3 + 2] = LEGOPPOSITE[i*3 + 2] + GOAL_HIGHT; } if(STATE == RCIRCLE || STATE == LCIRCLE) { Transform(ROTATE, GOAL_PITCH, GOAL_ROLL, LEGGOAL+3*i); Transform(-ROTATE, GOAL_PITCH, GOAL_ROLL, LEGOPPOSITE+3*i); } PLAYTIMER[i] = timer_nowtime(); } break; } }
/** * Process the next main command. */ static void engine_handle_cmd(struct android_app* app, int32_t cmd) { struct engine* engine = (struct engine*)app->userData; switch (cmd) { case APP_CMD_SAVE_STATE: // The system has asked us to save our current state. Do so. engine->app->savedState = malloc(sizeof(struct saved_state)); *((struct saved_state*)engine->app->savedState) = engine->state; engine->app->savedStateSize = sizeof(struct saved_state); break; case APP_CMD_INIT_WINDOW: // The window is being shown, get it ready. if (engine->app->window != NULL) { engine_init_display(engine); setupGraphics(engine->width, engine->height); InitSensor(); engine_draw_frame(engine); } break; case APP_CMD_TERM_WINDOW: // The window is being hidden or closed, clean it up. engine_term_display(engine); break; case APP_CMD_GAINED_FOCUS: //_EnableSensor(); StartSensor(); engine->animating = 1; break; case APP_CMD_LOST_FOCUS: //_DisableSensor(); // Also stop animating. engine->animating = 0; PauseSensor(); //engine_draw_frame(engine); break; } }
XnStatus XnSensor::InitImpl(const XnDeviceConfig *pDeviceConfig) { XnStatus nRetVal = XN_STATUS_OK; xnLogVerbose(XN_MASK_DEVICE_SENSOR, "Initializing device sensor..."); // Frame Sync XnCallbackHandle hCallbackDummy; nRetVal = m_FrameSync.OnChangeEvent().Register(FrameSyncPropertyChangedCallback, this, hCallbackDummy); XN_IS_STATUS_OK(nRetVal); nRetVal = GetFirmware()->GetParams()->m_Stream0Mode.OnChangeEvent().Register(FrameSyncPropertyChangedCallback, this, hCallbackDummy); XN_IS_STATUS_OK(nRetVal); nRetVal = GetFirmware()->GetParams()->m_Stream1Mode.OnChangeEvent().Register(FrameSyncPropertyChangedCallback, this, hCallbackDummy); XN_IS_STATUS_OK(nRetVal); // other stuff m_FrameSyncDump = xnDumpFileOpen(XN_DUMP_FRAME_SYNC, "FrameSync.csv"); xnDumpFileWriteString(m_FrameSyncDump, "HostTime(us),DepthNewData,DepthTimestamp(ms),ImageNewData,ImageTimestamp(ms),Diff(ms),Action\n"); nRetVal = XnDeviceBase::InitImpl(pDeviceConfig); XN_IS_STATUS_OK(nRetVal); // now that everything is configured, open the sensor nRetVal = InitSensor(pDeviceConfig); if (nRetVal != XN_STATUS_OK) { Destroy(); return (nRetVal); } xnLogInfo(XN_MASK_DEVICE_SENSOR, "Device sensor initialized"); return (XN_STATUS_OK); }
Sensors::Sensors() : Subsystem("Sensors") { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS tote1 = RobotMap::sensorstote1; tote2 = RobotMap::sensorstote2; tote3 = RobotMap::sensorstote3; tote4 = RobotMap::sensorstote4; tote5 = RobotMap::sensorstote5; bin = RobotMap::sensorscontainerSensor; m_index = 0; m_first = true; InitSensor(0, SR_CM(15), ADC_MAX, tote1, &(m_sensor_vals.tote1)); InitSensor(1, SR_CM(15), ADC_MAX, tote2, &(m_sensor_vals.tote2)); InitSensor(2, SR_CM(15), ADC_MAX, tote3, &(m_sensor_vals.tote3)); InitSensor(3, SR_CM(15), ADC_MAX, tote4, &(m_sensor_vals.tote4)); InitSensor(4, SR_CM(15), ADC_MAX, tote5, &(m_sensor_vals.tote5)); InitSensor(5, LR_CM(38), LR_CM(20), bin, &(m_sensor_vals.bin)); //sPort->SetTimeout(0); //packet = pdata.packet; // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS }
int __devinit videoin_init(void) { int ret = 0; int i; unsigned int u32PacketBuf0, u32PacketBuf1, u32PacketBuf2; unsigned int u32PlanarBuf0, u32PlanarBuf1; UINT32 u32PhyAddr; videoin_priv_t *priv = (videoin_priv_t *)&videoin_priv; g_u32FbPhyAddr = inp32(REG_LCM_FSADDR); DBG_PRINTF("%s\n",__FUNCTION__); /* initialize locks */ init_MUTEX(&videoin_priv.lock); priv->jdev.owner = THIS_MODULE; priv->jdev.type = VID_TYPE_CAPTURE | VID_TYPE_SCALES; priv->jdev.hardware = VID_HARDWARE_W55FA93; priv->jdev.release = videoin_release; priv->jdev.fops = &videoin_fops; priv->jdev.priv = &videoin_priv; priv->preview_width = LCDWIDTH; //CONFIG_VIDEOIN_PREVIEW_RESOLUTION_X; priv->preview_height = LCDHEIGHT; //CONFIG_VIDEOIN_PREVIEW_RESOLUTION_Y; priv->videoin_buffersize = CONFIG_VIDEOIN_PREVIEW_BUFFER_SIZE; priv->videoin_buffer = kmalloc(sizeof(__u8*) * CONFIG_VIDEOIN_BUFFER_COUNT, GFP_KERNEL); #ifdef CONFIG_VIDEOIN_VPOST_OVERLAY_BUFFER DBG_PRINTF("\nUsing Overlay\n"); /*For 1 * Packet pipe*/ u32PhysPacketBuf = w55fa93_FB_BG_PHY_ADDR; u32VirtPacketBuf = phys_to_virt(u32PhysPacketBuf); /*For Planar pipe*/ priv->vaddr = videoin_dmamalloc(VIDEOIN_ENCODE_BUFFER_SIZE); if (!priv->vaddr) return -ENOMEM; #else DBG_PRINTF("\nUsing FSC\n"); //Allocate 3 buffer for preview u32PacketBuf0 = videoin_dmamalloc_phy(0, LCDWIDTH*LCDHEIGHT*LCDBPP/8); //Packet buffer 0 if (!u32PacketBuf0) { printk("VideoIn allocated buffer fail\n"); return -ENOMEM; } u32PacketBuf1 = videoin_dmamalloc_phy(1, LCDWIDTH*LCDHEIGHT*LCDBPP/8); //Packet buffer 1 if (!u32PacketBuf1) { printk("VideoIn allocated buffer fail\n"); return -ENOMEM; } u32PacketBuf2 = videoin_dmamalloc_phy(2, LCDWIDTH*LCDHEIGHT*LCDBPP/8); //Packet buffer 2 if (!u32PacketBuf2) { printk("VideoIn allocated buffer fail\n"); return -ENOMEM; } //Allocate 2 buffer for JEPG encode u32PlanarBuf0 = videoin_dmamalloc_phy(3, VIDEOIN_ENCODE_BUFFER_SIZE); //Planar buffer 0 w55fa93_JPG_Y0_ADDR = u32PlanarBuf0; if (!u32PlanarBuf0) { printk("VideoIn allocated buffer fail\n"); return -ENOMEM; } u32PlanarBuf1 = videoin_dmamalloc_phy(4, VIDEOIN_ENCODE_BUFFER_SIZE); //Planar buffer 1 w55fa93_JPG_Y1_ADDR = u32PlanarBuf1; if (!u32PlanarBuf1) { printk("VideoIn allocated buffer fail\n"); return -ENOMEM; } #endif for(i = 0; i < CONFIG_VIDEOIN_BUFFER_COUNT; i++) { priv->videoin_buffer[i] = priv->paddr + i * CONFIG_VIDEOIN_PREVIEW_BUFFER_SIZE; DBG_PRINTF("bufer[%d]:%x\n", i, priv->videoin_buffer[i]); } if (video_register_device(&priv->jdev, VFL_TYPE_GRABBER, videoin_nr) == -1) { printk("%s: video_register_device failed\n", __FUNCTION__); dma_free_writecombine(NULL/*dev*/, VIDEOIN_ENCODE_BUFFER_SIZE, priv->vaddr, priv->paddr); kfree(priv->videoin_buffer);//2010-07-27 return -EPIPE; } if (!request_mem_region((unsigned long)W55FA93_VA_VIDEOIN, W55FA93_SZ_VIDEOIN, "w55fa93-videoin")) { printk("%s: request_mem_region failed\n", __FUNCTION__); video_unregister_device(&videoin_priv.jdev); dma_free_writecombine(NULL/*dev*/, VIDEOIN_ENCODE_BUFFER_SIZE, priv->vaddr, priv->paddr); kfree(priv->videoin_buffer); //2010-07-27 return -EBUSY; } ret = request_irq(IRQ_CAP, irq_handler, SA_INTERRUPT, "w55fa93-videoin", priv); if (ret) { printk("cannot get irq %d - err %d\n", IRQ_CAP, ret); ret = -EBUSY; goto release_mem; } DrvVideoIn_Init(TRUE, // BOOL bIsEnableSnrClock, 0, // E_DRVVIDEOIN_SNR_SRC eSnrSrc, 24000, // UINT32 u32SensorFreq, eDrvVideoIn_2nd_SNR_CCIR601); // E_DRVVIDEOIN_DEV_TYPE eDevType if (InitSensor(u32Sensor, priv, u32PacketBuf0, u32PacketBuf1, u32PacketBuf2) == 0) { printk("Init Sensor fail\n"); ret = -EBUSY; goto release_mem; } return ret; release_mem: video_unregister_device(&priv->jdev); release_mem_region((unsigned long)W55FA93_VA_VIDEOIN, W55FA93_SZ_VIDEOIN); free_irq(IRQ_CAP,priv); videoin_free_dmamalloc_phy(); kfree(priv->videoin_buffer); //2010-07-27 return ret; }