/*JSON{
"type" : "constructor",
"class" : "Timer",
"name" : "Timer",
"generate" : "jswrap_Timer_constructor",
"params" : [ ["timerName", "JsVar", "Timer Name"],
["group", "int", "Timer group"],
["index", "int", "Timer index"],
["isrIndex", "int", "isr (0 = Espruino, 1 = test)"] ],
"return" : ["JsVar","A Timer Object"]
}
Creates a Timer Object
*/
JsVar *jswrap_Timer_constructor(JsVar *timerName,int group, int index, int isrIndex){
int idx; char name[20];
JsVar *timer = jspNewObject(0, "Timer");
if(!timer) return 0;
name[jsvGetString(timerName, name, sizeof(name))] = '\0';
idx = timer_Init(name,group,index,isrIndex);
jsvObjectSetChildAndUnLock(timer, "index", jsvNewFromInteger(idx));
return timer;
}
void timer_Set(int tempo)
{
timer_Init(); /* Initialisation du timer */
T1_MR0 = tempo-1; /* Spécification de la valeur de reset de TC où tempo est une multiple de 0,1s
* Dans notre cas, pour une interruption périodique de 0.1, on mettra tempo = 1
*/
T1_TCR = 0x00000001; /* Démarrage du compteur (voir pg. 136 du manuel) */
}
//---
void xmain(int argc, char *argv[])
{
vc_initBuiltins();
vc_initLibrary();
InitGarlick();
Handle::init();
strcpy(mapname,"");
LoadConfig();
if (argc == 2)
{
if (strlen(argv[1]) > 254)
err("Mapname argument too long!");
strcpy(mapname, argv[1]);
}
InitVideo();
mouse_Init();
InitKeyboard();
joy_Init();
InitScriptEngine();
gameWindow->setTitle(APPNAME);
if (sound) snd_Init(soundengine);
win_movie_init();
ResetSprites();
timer_Init(gamerate);
LUA *lua;
se = lua = new LUA();
#ifdef ALLOW_SCRIPT_COMPILATION
DisplayCompileImage();
lua->compileSystem();
CompileMaps("lua", lua);
#endif
se->ExecAutoexec();
while (true && strlen(mapname))
Engine_Start(mapname);
err("");
}
int main(void)
{
unsigned int XXL,YXL,ZXL;
//unsigned int ADC1,ADC2,ADC3;
// int16_t ZGY;
unsigned int XGY,YGY,ZGY;
// unsigned char XYZ_buffer[100];
unsigned int timer=0;
unsigned int timer_old=0;
unsigned int diff_counter=0;
unsigned int m_IMU_count=0;
double m_XXL_sum=0;
double m_YXL_sum=0;
double m_ZXL_sum=0;
double m_XGY_sum=0;
double m_YGY_sum=0;
double m_ZGY_sum=0;
clk_init();
port_init();
USART_INIT();
timer_Init();
int_init();
// sei();
// spi_init();
/* Init SS pin as output with wired AND and pull-up. */
//120813 portC를 D로 바꿈 SPI인거 같아서
PORTD.DIRSET = PIN0_bm;
PORTD.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc;
PORTD.DIRSET = PIN1_bm;
PORTD.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTD.OUTSET = PIN0_bm;
PORTD.OUTSET = PIN1_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTD;
/* Initialize SPI master on port D. */
SPI_MasterInit(&spiMasterD,
&SPID,
&PORTD,
false,
SPI_MODE_3_gc,
SPI_INTLVL_OFF_gc,
false,
SPI_PRESCALER_DIV16_gc);
/* Use USARTD0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USARTD0_data, &USARTD0, USART_DREINTLVL_HI_gc);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USARTD0_data.usart, USART_RXCINTLVL_HI_gc);
/* Enable PMIC interrupt level low. */
PMIC.CTRL |= PMIC_HILVLEX_bm;
/* Enable global interrupts.*/
sei();
/* Initialize ACC & Gyro */
// Init_L3G4200DH();
Init_LIS3DH();
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData_gyro_init,
GYRO_DATA,
NUM_BYTES,
&PORTD,
PIN1_bm);
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN1_bm);
// USART_Rx_Enable(&USARTD0);
// USART_Tx_Enable(&USARTD0);
double z_deg=0;
unsigned int oldT=0;
unsigned int newT=0;
unsigned int dT=0;
int i;
double x1m=0;
double x1p=0;
double P1p=1.0;
double x2m[4]={0,0,0,0};
double x2p[4]={0,0,0,0};
double P2p[4][4]={{0.1,0,0,0},
{0,0.1,0,0},
{0,0,0.1,0},
{0,0,0,0.1}};
double enc_time_cnt=0;
for(i=0;i<DELAY_COMP_G;i++)
{
m_XGY_buf[i]=0.0;
m_YGY_buf[i]=0.0;
m_ZGY_buf[i]=0.0;
m_XXL_buf[i]=0.0;
m_YXL_buf[i]=0.0;
m_ZXL_buf[i]=0.0;
m_enc_buf[i]=0.0;
m_enc_timebuf[i]=0.0;
m_T_buf[i]=0.0;
}
//char XYZ_buffer_debug[20];
//sprintf((char*)XYZ_buffer_debug,"Hello W");
//uartSendTX((unsigned char*)XYZ_buffer_debug);
while(1)
{
//if(1)
if(samplingFlag)
{
// adc_start_conversion(&ADCA, ADC_CH0);
samplingFlag=0;
timer=TCC0.CNT;
diff_counter=timer-timer_old;
/* Create data packet (SS to slave by PC0). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
ACC_DATA,
NUM_BYTES,
&PORTD,
PIN0_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(5);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
_delay_us(5);
SPI_MasterSSHigh(ssPort, PIN0_bm);
/* Create data packet (SS to slave by PC1). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData_gyro,
GYRO_DATA,
NUM_BYTES,
&PORTD,
PIN1_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
_delay_us(5);
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
_delay_us(5);
SPI_MasterSSHigh(ssPort, PIN1_bm);
timer_old=timer;
T=(double)diff_counter/2000000.0*32.0;
ACC_DATA[2]=ACC_DATA[2]+0x80;
ACC_DATA[4]=ACC_DATA[4]+0x80;
ACC_DATA[6]=ACC_DATA[6]+0x80;
YXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1];
XXL= (unsigned int)ACC_DATA[4]*256+ACC_DATA[3];
ZXL= (unsigned int)ACC_DATA[6]*256+ACC_DATA[5];
GYRO_DATA[2]=GYRO_DATA[2]+0x80;
GYRO_DATA[4]=GYRO_DATA[4]+0x80;
GYRO_DATA[6]=GYRO_DATA[6]+0x80;
XGY= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3];
YGY= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1];
ZGY= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5];
if(m_IMU_count<Tsample)
{
m_IMU_count++;
}
else if(m_IMU_count<Tsample+Bsample)
{
m_XXL_sum+=XXL;
m_YXL_sum+=YXL;
m_ZXL_sum+=ZXL;
//m_XGY_sum+=XGY;
//m_YGY_sum+=YGY;
m_ZGY_sum+=ZGY;
m_IMU_count++;
}
else if(m_IMU_count==Tsample+Bsample)
{
//SetTimer(25,1,NULL);
m_biasXXL=(double)m_XXL_sum/(double)Bsample;
m_biasYXL=(double)m_YXL_sum/(double)Bsample;
m_biasZXL=(double)m_ZXL_sum/(double)Bsample-GRAVITY_COUNT;
//m_biasXGY=(double)m_XGY_sum/(double)Bsample;
//m_biasYGY=(double)m_YGY_sum/(double)Bsample;
m_biasZGY=(double)m_ZGY_sum/(double)Bsample;
gravityVect[0]=0;
gravityVect[1]=0;
gravityVect[2]=SCALE_ZXL*GRAVITY_COUNT;
m_IMU_count++;
}
else
{
//encoder_interface(0);
//encoder_interface(1);
//host_interface();
//unsigned int a=TCC0.CNT;
//position_estimator(XXL,YXL,ZXL,XGY,YGY,ZGY);
//unsigned int b=TCC0.CNT;
//TJX=(double)(b-a)/2000000.0*32.0;
//FF_controller();
newT=TCC0.CNT;
dT=newT-oldT;
////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
int i,j,k;
m_XXL=-SCALE_XXL*((double)XXL-(double)m_biasXXL);
m_YXL=-SCALE_YXL*((double)YXL-(double)m_biasYXL);
m_ZXL=SCALE_ZXL*((double)ZXL-(double)m_biasZXL);
m_XGY=-SCALE_XGY*((double)XGY-(double)m_biasXGY);//-0.001212142/0.00015711
m_YGY=SCALE_YGY*((double)YGY-(double)m_biasYGY);//+
//if(ZGY<3000) m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY+65536.0);
//else m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);//+
m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);
sprintf(XYZ_buffer,"%u %u %u %u %u %u \n",XXL, YXL, ZXL, XGY, YGY, ZGY);
uartC0SendTX((unsigned char*)XYZ_buffer);
}
}
}
while(true) {
nop();
}
}
void HEXIWEAR_startup( task_param_t param )
{
uint8_t
status = 0;
/** output GPIO config */
GPIO_DRV_Init( NULL, OLED_cfg );
GPIO_DRV_Init( NULL, FLASH_cfg );
GPIO_DRV_Init( NULL, PWR_cfg );
GPIO_DRV_Init( NULL, VIBRO_cfg );
GPIO_DRV_Init( NULL, RGB_cfg );
GPIO_DRV_Init( NULL, KW40_GPIO_cfg );
GPIO_DRV_Init( NULL, REL_GPIO_cfg );
/** input GPIO config */
GPIO_DRV_Init( BAT_CHG_cfg, NULL );
GPIO_DRV_Init( TAP_cfg, NULL );
// lajkatest
// GPIO_DRV_Init( GPIO_TEST_CFG, NULL );
#if defined( HEXIWEAR_DEBUG )
GPIO_DRV_Init( NULL, DEBUG_cfg );
#endif
power_ResetKW40();
timer_Init( HEXIWEAR_TIMER_SENSOR );
status |= RTC_Init();
status |= FLASH_Init( &flashModule, &flashSettings );
// intern flash initialization
INTFLASH_Init();
// enable power save by default
// power_EnablePowerSave();
power_DisablePowerSave();
// set to deep sleep by default
// power_SetSleepMode( POWER_SLEEP_TYPE_DEEP );
// RGB off
FLASH_SetOFF();
// visual indication for testing HR sensor
// MAXIM_Test();
/** create basic tasks */
status |= Notification_Init();
status |= HostInterface_Init();
status |= sensor_Init();
status |= power_Init();
status |= GuiDriver_Init();
haptic_MutexCreate();
sensor_InitAcc();
/** set GPIO interrupt for the tap function */
PORT_HAL_SetPinIntMode( PORTC, 1, kPortIntFallingEdge );
/** set charging battery interrupt */
PORT_HAL_SetPinIntMode( PORTC, 12, kPortIntEitherEdge );
NVIC_SetPriority( PORTC_IRQn, HEXIWEAR_CHG_IRQ_PRIO );
INT_SYS_EnableIRQ( PORTC_IRQn );
// status |= Run_USB_Task();
if ( HEXIWEAR_STATUS_SUCCESS != status )
{
catch( CATCH_INIT );
}
/** check for settings in flash at startup */
gui_sensorTag_CheckAtStartup();
haptic_CheckAtStartup();
CLOCK_SYS_Init( g_clockManConfigsArr, FSL_CLOCK_MANAGER_CONFIG_CNT, g_clockManCallbacksArr, FSL_CLOCK_MANAGER_CALLBACK_CNT );
POWER_SYS_Init( powerConfigsArr, 2U, powerStaticCallbacksConfigsArr , 2U );
// make battery readings regular
sensor_SetPacketTargets( PACKET_BAT, sensor_GetPacketTargets( PACKET_BAT) | PACKET_PUSH_POWER_MGMT, true );
volatile uint32_t
foo = CLOCK_SYS_GetSystemClockFreq();
while (1)
{
OSA_TaskDestroy( NULL );
}
}
void HEXIWEAR_startup( task_param_t param )
{
uint8_t
status = 0;
/** output GPIO configuration */
GPIO_DRV_Init( NULL, OLED_cfg );
GPIO_DRV_Init( NULL, FLASH_cfg );
GPIO_DRV_Init( NULL, PWR_cfg );
GPIO_DRV_Init( NULL, VIBRO_cfg );
GPIO_DRV_Init( NULL, RGB_cfg );
GPIO_DRV_Init( NULL, KW40_GPIO_cfg );
/** input GPIO configuration */
GPIO_DRV_Init( BAT_CHG_cfg, NULL );
GPIO_DRV_Init( TAP_cfg, NULL );
#if defined( HEXIWEAR_DEBUG )
GPIO_DRV_Init( NULL, DEBUG_cfg );
#endif
power_ResetKW40();
timer_Init( HEXIWEAR_TIMER_SENSOR );
status |= RTC_Init();
status |= FLASH_Init( &flashModule, &flashSettings );
// intern flash initialization
INTFLASH_Init();
// RGB off
FLASH_SetOFF();
/** create basic tasks */
status |= Notification_Init();
status |= HostInterface_Init();
status |= sensor_Init();
status |= GuiDriver_Init();
haptic_MutexCreate();
sensor_InitAcc();
/** set GPIO interrupt for the tap function */
PORT_HAL_SetPinIntMode( PORTC, 1, kPortIntFallingEdge );
/** set charging battery interrupt */
PORT_HAL_SetPinIntMode( PORTC, 12, kPortIntEitherEdge );
NVIC_SetPriority( PORTC_IRQn, HEXIWEAR_CHG_IRQ_PRIO );
INT_SYS_EnableIRQ( PORTC_IRQn );
if ( HEXIWEAR_STATUS_SUCCESS != status )
{
catch( CATCH_INIT );
}
/** check for settings in flash at startup */
gui_sensorTag_CheckAtStartup();
haptic_CheckAtStartup();
CLOCK_SYS_Init( g_clockManConfigsArr, FSL_CLOCK_MANAGER_CONFIG_CNT, g_clockManCallbacksArr, FSL_CLOCK_MANAGER_CALLBACK_CNT );
POWER_SYS_Init( powerConfigsArr, 2U, powerStaticCallbacksConfigsArr , 2U );
// turn on regular battery readings
sensor_SetPacketTargets( PACKET_BAT, sensor_GetPacketTargets( PACKET_BAT) | PACKET_PUSH_POWER_MGMT, true );
while (1)
{
power_Init();
OSA_TaskDestroy( NULL );
}
}
