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