Exemple #1
0
static int wait_twi_complete()
{
  int status;
  int timeout = systick_get_ms() + TWI_TIMEOUT;
  do {
    status = twi_status();
    if (status <= 0) return status;
  } while (systick_get_ms() < timeout);
  return status;
}
Exemple #2
0
void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4, S32 datas32, S32 datas32_2) {


    static U8 data_log_buffer[32];

    //左辺は結局data_log_bufferの中身を表している。要するにただの代入
    //32ビット分アクセスするためにdata_log_bufferの[1]から[3]へはアクセスしてない。
    *((U32 *)(&data_log_buffer[0]))  = (U32)systick_get_ms();
    *(( S8 *)(&data_log_buffer[4]))  =  (S8)data1;
    *(( S8 *)(&data_log_buffer[5]))  =  (S8)data2;
    *((U16 *)(&data_log_buffer[6]))  = (U16)ecrobot_get_light_sensor(NXT_PORT_S3); //light sensor value
    *((S32 *)(&data_log_buffer[8]))  = (S32)nxt_motor_get_count(0);
    *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
    *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
    *((S16 *)(&data_log_buffer[20])) = (S16)adc1;
    *((S16 *)(&data_log_buffer[22])) = (S16)adc2;
    *((S16 *)(&data_log_buffer[24])) = (S16)adc3;
    *((S16 *)(&data_log_buffer[26])) = (S16)adc4;
    *((S32 *)(&data_log_buffer[28])) = (U32)datas32_2;

    ecrobot_send_bt_packet(data_log_buffer, 32);



}
//  :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet...
static void
decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors )
{
    U8 temp_port;
    
    output_packet->port = input_packet->port;
    output_packet->operation = input_packet->operation;
    
    switch (input_packet->operation) {
        case LIGHT_SENSOR:
            decode_bro_port (input_packet->port, &temp_port);
            output_packet->data = (float) ecrobot_get_light_sensor(temp_port);
            break;
            
        case TOUCH_SENSOR:
            decode_bro_port (input_packet->port, &temp_port);
            output_packet->data = (float) ecrobot_get_touch_sensor(temp_port);
            break;
            
        case SOUND_SENSOR:
            decode_bro_port (input_packet->port, &temp_port);
            output_packet->data = (float) ecrobot_get_sound_sensor(temp_port);
            break;
            
        case RADAR_SENSOR:
            decode_bro_port (input_packet->port, &temp_port);
            output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port);
            break;
            
        case TACHO_COUNT:
            decode_bro_port (input_packet->port, &temp_port);
            output_packet->data = (float) nxt_motor_get_count(temp_port);
            break;
 
       case SET_POWER:
            decode_bro_port (input_packet->port, &temp_port);
            switch (temp_port) {
                case NXT_PORT_A:
                    motors->first.speed_control_type = RAW_POWER;
                    motors->first.powers[0] = input_packet->data;
					output_packet->data = (float) nxt_motor_get_count(NXT_PORT_A);
					output_packet->timestamp = (float) systick_get_ms(); 
                break;
                
                case NXT_PORT_B:
                    motors->second.speed_control_type = RAW_POWER;
                    motors->second.powers[0] = input_packet->data;
                break;
                
                case NXT_PORT_C:
                    motors->third.speed_control_type = RAW_POWER;
                    motors->third.powers[0] = input_packet->data;
                break;
            };
            break;
        default:
            //Nothing HERE
            break;
    };
}
void sonar_sensor_measurement(void)
{
    // Variables used for distance measurement
    S32 distance_left = 0;
    S32 distance_right = 0;
    int measurements = 0;

    // Display menu
    reset_sonar_measurement_menu();

    while(true)
    {
        // Update values
        distance_left += ecrobot_get_sonar_sensor(SONAR_SENSOR_FRONT);
        distance_right += ecrobot_get_sonar_sensor(SONAR_SENSOR_REAR);

        // Calculate average of new and previous
        distance_left /= 2;
        distance_right /= 2;

        // Wait when over some measurements
        if(measurements++ % MEASUREMENT_WAIT_INTERVAL == 0)
        {
            // Update the display
            display_sonar_sensor_measurements(distance_left, distance_right);

            systick_wait_ms(MEASUREMENT_WAIT);
        }

        // Chek if the enter button is pressed for long enough time to exit
        if(ecrobot_is_ENTER_button_pressed())
        {   
            // Time when the button was pressed
            int enter_button_pressed_start_time = systick_get_ms();

            // Continusly check if enought time has past to exit
            while(ecrobot_is_ENTER_button_pressed())
            {
                if(enter_button_pressed_start_time + ENTER_BUTTON_EXIT_TIMEOUT < 
                   systick_get_ms()) 
                {   
                    return;
                }
            }
        }
    }
}
Exemple #5
0
/*---------------------------------------------------------------------
 * This is the low level mechanism for monotonically increasing
 * at a constant rate.  Substitute code here to read some
 * sort of hardware timer.
 *-------------------------------------------------------------------*/
static ETimer_time_t
ETimer_msec_time( void )
{
  ETimer_time_t t;
  //ftime( &systyme );
  //t = ( systyme.time * USEC_CONVERT ) + systyme.millitm;
  t = systick_get_ms();
  return ( t - tinit );

}
Exemple #6
0
void ts_dispatch_timers()
{
	if (!ts_timers_count || ts_timers[0].timestamp > systick_get_ms())
		return;

	TaskType taskId = ts_timers[0].task;
	EventMaskType timerId = ts_timers[0].timer;
	for (U8 i = 1; i < ts_timers_count; ++i)
		ts_timers[i - 1] = ts_timers[i];
	--ts_timers_count;

	SetEvent(taskId, timerId);
}
//bluetoothログ送信関数
void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4){
	U8 data_log_buffer[32];
	
	*((U32 *)(&data_log_buffer[0]))  = (U32)systick_get_ms();
	*(( S8 *)(&data_log_buffer[4]))  =  (S8)data1;
	*(( S8 *)(&data_log_buffer[5]))  =  (S8)data2;
	*((U16 *)(&data_log_buffer[6]))  = (U16)ecrobot_get_light_sensor(NXT_PORT_S3);
	*((S32 *)(&data_log_buffer[8]))  = (S32)nxt_motor_get_count(0);
	*((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
	*((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
	*((S16 *)(&data_log_buffer[20])) = (S16)adc1;
	*((S16 *)(&data_log_buffer[22])) = (S16)adc2;
	*((S16 *)(&data_log_buffer[24])) = (S16)adc3;
	*((S16 *)(&data_log_buffer[26])) = (S16)adc4;
	*((S32 *)(&data_log_buffer[28])) = (S32)ecrobot_get_sonar_sensor(NXT_PORT_S2);

	ecrobot_send_bt_packet(data_log_buffer, 32);
}
Exemple #8
0
/*---------------------------------------------------------------------
 * Initialize the tick mechanism and the timer instances.
 *-------------------------------------------------------------------*/
void
TIM_init( void )
{
#if ESCHER_SYS_MAX_XTUML_TIMERS > 0
  u2_t i;
  /*-----------------------------------------------------------------*/
  /* Build the collection (linked list) of timers.                   */
  /*-----------------------------------------------------------------*/
  animate = 0; inanimate = 0;
  for ( i = 0; i < ESCHER_SYS_MAX_XTUML_TIMERS; i++ ) {
    swtimers[ i ].expiration = 0;
    swtimers[ i ].recurrence = 0;
    swtimers[ i ].event = 0;
    swtimers[ i ].next = inanimate;
    inanimate = &swtimers[ i ];
  }
#endif   /* if ESCHER_SYS_MAX_XTUML_TIMERS > 0 */
  //ftime( &systyme );            /* Initialize the hardware ticker.   */
  //tinit = ( systyme.time * USEC_CONVERT ) + systyme.millitm;
  tinit = systick_get_ms();
}
Exemple #9
0
void wait(U32 delay)
{
	if (ts_timers_count >= TS_MAX_TIMERS)
		return;

	const U32 timestamp = systick_get_ms() + delay;
	U8 index = ts_bin_search(timestamp);
	if (index < ts_timers_count && ts_timers[index].timestamp <= timestamp)
		++index;

	if (ts_timers_count)
		for (U8 i = ts_timers_count - 1; i >= index; --i)
			ts_timers[i + 1] = ts_timers[i];

	ts_timers[index].timestamp = timestamp;
	ts_timers[index].task = runtsk;
	// TODO: This will not probably work if user declares own timers. Events must be taken by name or passed as parameter.
	const EventMaskType timerId = (1UL << runtsk);
	ts_timers[index].timer = timerId;
	++ts_timers_count;

	WaitEvent(timerId);
	ClearEvent(timerId);
}
Exemple #10
0
void error_detect_is_dog_alive() {
	if(systick_get_ms() - lastDogKickReceived > 200) { // > X, where X is the time in ms. that we allow the NXTs to NOT communicate.
		emergency_stop("RS485 Error","Watchdog Timeout");
	}
}
Exemple #11
0
void error_detect_dogkick_received() {
	lastDogKickReceived = systick_get_ms();
	//Check if "the dog is dead" (we havn't received anything for too long)
}
Exemple #12
0
void error_detect_initialize() {
	lastDogKickReceived = systick_get_ms() + 1000; //Gives us a second to start the other NXT
}
void LVC_doCtrl(LightValCtrl *self){
	self->turn = PLVC_calcCtrlVal(&mPIDLightValCtrl,LV_getTargLightVal(&mLightVal),LV_getLightVal(&mLightVal),systick_get_ms()*0.001);
	PIDLightValCtrlParm *parameter = LVC_getCtrlParm(&mLightValCtrl);
}
Exemple #14
0
/**
 * NOTE: The technique is not the same as that used in TinyVM.
 * The return value indicates the impact of the call on the VM
 * system. EXEC_CONTINUE normal return the system should return to the return
 * address provided by the VM. EXEC_RUN The call has modified the value of
 * VM PC and this should be used to restart execution. EXEC_RETRY The call
 * needs to be re-tried (typically for a GC failure), all global state
 * should be left intact, the PC has been set appropriately.
 *
 */
int dispatch_native(TWOBYTES signature, STACKWORD * paramBase)
{
  STACKWORD p0 = paramBase[0];
  switch (signature) {
  case wait_4_5V:
    return monitor_wait((Object *) word2ptr(p0), 0);
  case wait_4J_5V:
    return monitor_wait((Object *) word2ptr(p0), ((int)paramBase[1] > 0 ? 0x7fffffff : paramBase[2]));
  case notify_4_5V:
    return monitor_notify((Object *) word2ptr(p0), false);
  case notifyAll_4_5V:
    return monitor_notify((Object *) word2ptr(p0), true);
  case start_4_5V:
    // Create thread, allow for instruction restart
    return init_thread((Thread *) word2ptr(p0));
  case yield_4_5V:
    schedule_request(REQUEST_SWITCH_THREAD);
    break;
  case sleep_4J_5V:
    sleep_thread(((int)p0 > 0 ? 0x7fffffff : paramBase[1]));
    schedule_request(REQUEST_SWITCH_THREAD);
    break;
  case getPriority_4_5I:
    push_word(get_thread_priority((Thread *) word2ptr(p0)));
    break;
  case setPriority_4I_5V:
    {
      STACKWORD p = (STACKWORD) paramBase[1];

      if (p > MAX_PRIORITY || p < MIN_PRIORITY)
	return throw_new_exception(JAVA_LANG_ILLEGALARGUMENTEXCEPTION);
      else
	set_thread_priority((Thread *) word2ptr(p0), p);
    }
    break;
  case currentThread_4_5Ljava_3lang_3Thread_2:
    push_ref(ptr2ref(currentThread));
    break;
  case interrupt_4_5V:
    interrupt_thread((Thread *) word2ptr(p0));
    break;
  case interrupted_4_5Z:
    {
      JBYTE i = currentThread->interruptState != INTERRUPT_CLEARED;

      currentThread->interruptState = INTERRUPT_CLEARED;
      push_word(i);
    }
    break;
  case isInterrupted_4_5Z:
    push_word(((Thread *) word2ptr(p0))->interruptState
	      != INTERRUPT_CLEARED);
    break;
  case join_4_5V:
    join_thread((Thread *) word2ptr(p0), 0);
    break;
  case join_4J_5V:
    join_thread((Thread *) word2obj(p0), paramBase[2]);
    break;
  case halt_4I_5V:
    schedule_request(REQUEST_EXIT);
    break;
  case shutdown_4_5V:
    shutdown_program(false);
    break;
  case currentTimeMillis_4_5J:
    push_word(0);
    push_word(systick_get_ms());
    break;
  case readSensorValue_4I_5I:
    push_word(sp_read(p0, SP_ANA));
    break;
  case setPowerTypeById_4II_5V:
    sp_set_power(p0, paramBase[1]);
    break;
  case freeMemory_4_5J:
    push_word(0);
    push_word(getHeapFree());
    break;
  case totalMemory_4_5J:
    push_word(0);
    push_word(getHeapSize());
    break;
  case floatToRawIntBits_4F_5I:	// Fall through
  case intBitsToFloat_4I_5F:
    push_word(p0);
    break;
  case doubleToRawLongBits_4D_5J:	// Fall through
  case longBitsToDouble_4J_5D:
    push_word(p0);
    push_word(paramBase[1]);
    break;
  case drawString_4Ljava_3lang_3String_2II_5V:
    {
      String *p = (String *)word2obj(p0);
      Object *charArray;
      if (!p) return throw_new_exception(JAVA_LANG_NULLPOINTEREXCEPTION);
      charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p)));
      if (!charArray) return throw_new_exception(JAVA_LANG_NULLPOINTEREXCEPTION);
      display_goto_xy(paramBase[1], paramBase[2]);
      display_jstring(p);
    }
    break;
  case drawInt_4III_5V:
    display_goto_xy(paramBase[1], paramBase[2]);
    display_int(p0, 0);
    break;
  case drawInt_4IIII_5V:
     display_goto_xy(paramBase[2], paramBase[3]);
     display_int(p0, paramBase[1]);
    break;   
  case asyncRefresh_4_5V:
    display_update();
    break;
  case clear_4_5V:
    display_clear(0);
    break;
  case getDisplay_4_5_1B:
    push_word(display_get_array());
    break;
  case setAutoRefreshPeriod_4I_5I:
    push_word(display_set_auto_update_period(p0));
    break;
  case getRefreshCompleteTime_4_5I:
    push_word(display_get_update_complete_time());
    break;
  case bitBlt_4_1BIIII_1BIIIIIII_5V:
    {
      Object *src = word2ptr(p0);
      Object *dst = word2ptr(paramBase[5]);
      display_bitblt((byte *)(src != NULL ?jbyte_array(src):NULL), paramBase[1], paramBase[2], paramBase[3], paramBase[4], (byte *)(dst!=NULL?jbyte_array(dst):NULL), paramBase[6], paramBase[7], paramBase[8], paramBase[9], paramBase[10], paramBase[11], paramBase[12]);
      break;
    }
  case getSystemFont_4_5_1B:
    push_word(display_get_font());
    break;
  case setContrast_4I_5V:
    nxt_lcd_set_pot(p0);
    break;
  case getBatteryStatus_4_5I:
    push_word(battery_voltage());
    break;
  case getButtons_4_5I:
    push_word(buttons_get());
    break;
  case getTachoCountById_4I_5I:
    push_word(nxt_motor_get_count(p0));
    break;
  case controlMotorById_4III_5V:
    nxt_motor_set_speed(p0, paramBase[1], paramBase[2]); 
    break;
  case resetTachoCountById_4I_5V:
    nxt_motor_set_count(p0, 0);
    break;
  case i2cEnableById_4II_5V:
    if (i2c_enable(p0, paramBase[1]) == 0)
      return EXEC_RETRY;
    else
      break;
  case i2cDisableById_4I_5V:
    i2c_disable(p0);
    break;
  case i2cStatusById_4I_5I:
    push_word(i2c_status(p0));
    break;
  case i2cStartById_4II_1BIII_5I:
    {
    	Object *p = word2obj(paramBase[2]);
    	JBYTE *byteArray = p ? jbyte_array(p) + paramBase[3] : NULL;
    	push_word(i2c_start(p0,
    	                    paramBase[1],
    	                    (U8 *)byteArray,
    	                    paramBase[4],
    	                    paramBase[5]));
    }
    break; 
  case i2cCompleteById_4I_1BII_5I:
    {
    	Object *p = word2ptr(paramBase[1]);
    	JBYTE *byteArray = p ? jbyte_array(p) + paramBase[2] : NULL;
    	push_word(i2c_complete(p0,
    	                       (U8 *)byteArray,
    	                       paramBase[3]));
    }
    break; 
  case playFreq_4III_5V:
    sound_freq(p0,paramBase[1], paramBase[2]);
    break;
  case btGetBC4CmdMode_4_5I:
    push_word(bt_get_mode());
    break;
  case btSetArmCmdMode_4I_5V:
    if (p0 == 0) bt_set_arm7_cmd();
    else bt_clear_arm7_cmd(); 
    break;
  case btSetResetLow_4_5V:
    bt_set_reset_low();
    break;
  case btSetResetHigh_4_5V:
    bt_set_reset_high();
    break;
  case btWrite_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(bt_write(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case btRead_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(bt_read(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case btPending_4_5I:
    {
      push_word(bt_event_check(0xffffffff));
    }
    break;
  case btEnable_4_5V:
    if (bt_enable() == 0)
      return EXEC_RETRY;
    else
      break;
  case btDisable_4_5V:
    bt_disable();
    break;
  case usbRead_4_1BII_5I:
     {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(udp_read(byteArray,paramBase[1], paramBase[2]));
    } 
    break;
  case usbWrite_4_1BII_5I:
     {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(udp_write(byteArray,paramBase[1], paramBase[2]));                      
    }
    break; 
  case usbStatus_4_5I:
    {
      push_word(udp_event_check(0xffffffff));
    }
    break;
  case usbEnable_4I_5V:
    {
      udp_enable(p0);
    }
    break;
  case usbDisable_4_5V:
    {
      udp_disable();
    }
    break;
  case usbReset_4_5V:
    udp_reset();
    break; 
  case usbSetSerialNo_4Ljava_3lang_3String_2_5V: 
    {
      byte *p = word2ptr(p0);
      int len;
      Object *charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p)));

      len = get_array_length(charArray);
      udp_set_serialno((U8 *)jchar_array(charArray), len);
    }
    break;
  case usbSetName_4Ljava_3lang_3String_2_5V:
    {
      byte *p = word2ptr(p0);
      int len;
      Object *charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p)));

      len = get_array_length(charArray);
      udp_set_name((U8 *)jchar_array(charArray), len);
    }
    break;
  case flashWritePage_4_1BI_5I:
    {
      Object *p = word2ptr(p0);
      unsigned long *intArray = (unsigned long *) jint_array(p);
      push_word(flash_write_page(intArray,paramBase[1]));                      
    }
    break;
  case flashReadPage_4_1BI_5I:
    {
      Object *p = word2ptr(p0);
      unsigned long *intArray = (unsigned long *) jint_array(p);
      push_word(flash_read_page(intArray,paramBase[1]));                      
    }
    break;
  case flashExec_4II_5I:
    push_word(run_program((byte *)(&FLASH_BASE[(p0*FLASH_PAGE_SIZE)]), paramBase[1]));
    break;
  case playSample_4IIIII_5V:
    sound_play_sample(((unsigned char *) &FLASH_BASE[(p0*FLASH_PAGE_SIZE)]) + paramBase[1],paramBase[2],paramBase[3],paramBase[4]);
    break;
  case playQueuedSample_4_1BIIII_5I:
    push_word(sound_add_sample((U8 *)jbyte_array(word2obj(p0)) + paramBase[1],paramBase[2],paramBase[3],paramBase[4]));
    break;
  case getTime_4_5I:
    push_word(sound_get_time());
    break;
  case getDataAddress_4Ljava_3lang_3Object_2_5I:
    if (is_array(word2obj(p0)))
      push_word (ptr2word ((byte *) array_start(word2ptr(p0))));
    else
      push_word (ptr2word ((byte *) fields_start(word2ptr(p0))));
    break;
  case getObjectAddress_4Ljava_3lang_3Object_2_5I:
    push_word(p0);
    break;
  case gc_4_5V:
    // Restartable garbage collection
    return garbage_collect();
  case shutDown_4_5V:
    shutdown(); // does not return
  case boot_4_5V:
    display_clear(1);
    while (1) nxt_avr_firmware_update_mode(); // does not return 
  case arraycopy_4Ljava_3lang_3Object_2ILjava_3lang_3Object_2II_5V:
    return arraycopy(word2ptr(p0), paramBase[1], word2ptr(paramBase[2]), paramBase[3], paramBase[4]);
  case executeProgram_4I_5V:
    // Exceute program, allow for instruction re-start
    return execute_program(p0);
  case setDebug_4_5V:
    set_debug(word2ptr(p0));
    break;
  case eventOptions_4II_5I:
    {
      byte old = debugEventOptions[p0];
      debugEventOptions[p0] = (byte)paramBase[1];
      push_word(old);
    }
    break;
  case suspendThread_4Ljava_3lang_3Object_2_5V:
    suspend_thread(ref2ptr(p0));
    break;
  case resumeThread_4Ljava_3lang_3Object_2_5V:
    resume_thread(ref2ptr(p0));
    break;
  case getProgramExecutionsCount_4_5I:
    push_word(gProgramExecutions);
    break;
  case getFirmwareRevision_4_5I:
    push_word((STACKWORD) getRevision());
    break;
  case getFirmwareRawVersion_4_5I:
    push_word((STACKWORD) VERSION_NUMBER); 
    break;
  case hsEnable_4II_5V:
    {
      if (hs_enable((int)p0, (int)paramBase[1]) == 0)
        return EXEC_RETRY;
    }
    break;
  case hsDisable_4_5V:
    {
      hs_disable();
    }
    break;
  case hsWrite_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(hs_write(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case hsRead_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(hs_read(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case hsPending_4_5I:
    {
      push_word(hs_pending());
    }
    break;
  case hsSend_4BB_1BII_1C_5I:
    {
      Object *p = word2ptr(paramBase[2]);
      U8 *data = (U8 *)jbyte_array(p);
      p = word2ptr(paramBase[5]);
      U16 *crc = (U16 *)jchar_array(p);
      push_word(hs_send((U8) p0, (U8)paramBase[1], data, paramBase[3], paramBase[4], crc));
    }
    break;
  case hsRecv_4_1BI_1CI_5I:
    {
      Object *p = word2ptr(p0);
      U8 *data = (U8 *)jbyte_array(p);
      p = word2ptr(paramBase[2]);
      U16 *crc = (U16 *)jchar_array(p);
      push_word(hs_recv(data, paramBase[1], crc, paramBase[3]));
    }
    break;
    
  case getUserPages_4_5I:
    push_word(FLASH_MAX_PAGES - flash_start_page);
    break;
  case setVMOptions_4I_5V:
    gVMOptions = p0;
    break;
  case getVMOptions_4_5I:
    push_word(gVMOptions);
    break;
  case isAssignable_4II_5Z:
    push_word(is_assignable(p0, paramBase[1]));
    break;
  case cloneObject_4Ljava_3lang_3Object_2_5Ljava_3lang_3Object_2:
    {
      Object *newObj = clone((Object *)ref2obj(p0));
      if (newObj == NULL) return EXEC_RETRY;
      push_word(obj2ref(newObj));
    }
    break;
  case memPeek_4III_5I:
    push_word(mem_peek(p0, paramBase[1], paramBase[2]));
    break;
  case memCopy_4Ljava_3lang_3Object_2IIII_5V:
    mem_copy(word2ptr(p0), paramBase[1], paramBase[2], paramBase[3], paramBase[4]);
    break;
  case memGetReference_4II_5Ljava_3lang_3Object_2:
    push_word(mem_get_reference(p0, paramBase[1]));
    break;
  case setSensorPin_4III_5V:
    sp_set(p0, paramBase[1], paramBase[2]);
    break;
  case getSensorPin_4II_5I:
    push_word(sp_get(p0, paramBase[1]));
    break;
  case setSensorPinMode_4III_5V:
    sp_set_mode(p0, paramBase[1], paramBase[2]);
    break;
  case readSensorPin_4II_5I:
    push_word(sp_read(p0, paramBase[1]));
    break;
  case nanoTime_4_5J:
    {
      U64 ns = systick_get_ns();
      push_word(ns >> 32);
      push_word(ns);
    }
    break;
  case createStackTrace_4Ljava_3lang_3Thread_2Ljava_3lang_3Object_2_5_1I:
    {
      Object *trace = create_stack_trace((Thread *)ref2obj(p0), ref2obj(paramBase[1]));
      if (trace == NULL) return EXEC_RETRY;
      push_word(obj2ref(trace));
    }
    break;
  case registerEvent_4_5I:
    push_word(register_event((NXTEvent *) ref2obj(p0)));
    break;
  case unregisterEvent_4_5I:
    push_word(unregister_event((NXTEvent *) ref2obj(p0)));
    break;
  case changeEvent_4II_5I:
    push_word(change_event((NXTEvent *) ref2obj(p0), paramBase[1], paramBase[2]));
    break;
  case isInitialized_4I_5Z:
    push_word(is_initialized_idx(p0));
    break;
  case allocate_4II_5Ljava_3lang_3Object_2:
    {
      Object *allocated;
      if(paramBase[1]>0){
        allocated=new_single_array(p0,paramBase[1]);
      }else{
        allocated=new_object_for_class(p0);
      }
      if(allocated == NULL) return EXEC_RETRY;
      push_word(obj2ref(allocated));
    }
    break;
  case memPut_4IIII_5V:
    store_word_ns((byte *)(memory_base[p0] + paramBase[1]), paramBase[2],paramBase[3]);
    break;
  case notifyEvent_4ILjava_3lang_3Thread_2_5Z:
    push_word(debug_event(paramBase[1], NULL, (Thread*) ref2obj(paramBase[2]), 0, 0, 0, 0));
    break;
  case setThreadRequest_4Ljava_3lang_3Thread_2Llejos_3nxt_3debug_3SteppingRequest_2_5V:
    {
      Thread *th = (Thread*) ref2obj(p0);
      th->debugData = (REFERENCE) paramBase[1];
      // currently we only get stepping requests
      if(paramBase[1])
        th->flags |= THREAD_STEPPING;
      else
        th->flags &= ~THREAD_STEPPING;
    }
    break;
  case isStepping_4Ljava_3lang_3Thread_2_5Z:
    {
      Thread *th = (Thread*) ref2obj(p0);
      push_word(is_stepping(th));
    }
    break;
  case setBreakpointList_4_1Llejos_3nxt_3debug_3Breakpoint_2I_5V:
    breakpoint_set_list((Breakpoint**) array_start(p0), paramBase[1]);
    break;
  case enableBreakpoint_4Llejos_3nxt_3debug_3Breakpoint_2Z_5V:
    breakpoint_enable((Breakpoint*) word2ptr(p0), (boolean) paramBase[1]);
    break;
  case firmwareExceptionHandler_4Ljava_3lang_3Throwable_2II_5V:
    firmware_exception_handler((Throwable *)p0, paramBase[1], paramBase[2]);
    break;
  case exitThread_4_5V:
    currentThread->state = DEAD;
    schedule_request(REQUEST_SWITCH_THREAD);
    break;
  case updateThreadFlags_4Ljava_3lang_3Thread_2II_5I:
    ((Thread *)p0)->flags |= paramBase[1];
    ((Thread *)p0)->flags &= ~paramBase[2];
//printf("m %x %d\n", p0, ((Thread *)p0)->flags);
    push_word(((Thread *)p0)->flags);
    break;
    
  default:
    return throw_new_exception(JAVA_LANG_NOSUCHMETHODERROR);
  }
  return EXEC_CONTINUE;
}
Exemple #15
0
SINT main(void)
{
#ifdef NO_RUN_ENTER_STOP_EXIT
	 
	init_OS_flag(); /* this should be called before device init */
	nxt_device_init();
	ecrobot_initDeviceStatus(); // added 10/28/2010 to fix a bug by tchikama
	ecrobot_init_nxtstate();

	if  (execution_mode() == EXECUTED_FROM_FLASH)
	{
		/*
		 * Call buttons_get() because ecrobot_get_button_state() has button bouncer.
		 * The button bouncer requires multiple periodical calls to make it work, but
		 * in this case, only single call (no while loop), so buttons_get is called. 
		 */
		if ((buttons_get() & 0x0F) == (ENTER_PRESSED | STOP_PRESSED))
		{
			/* set flash request and shut down the NXT
	 	 	 * at the next start, NXT BIOS will be executed.
	 	 	 */
			display_clear(0);
   			display_goto_xy(0, 0);
   			display_string("PWR ON: NXT BIOS");
			display_update();
			systick_wait_ms(1000);
   		
			set_flash_request();
			display_clear(1);
			systick_wait_ms(10);
			nxt_lcd_power_down(); /* reset LCD hardware */
			systick_wait_ms(10);
			while(1)
			{
				nxt_avr_power_down();
			}
		}
	}

	/* device init should be called prior to running the application */
	ecrobot_device_initialize();
	ecrobot_setDeviceInitialized();
	
	nxt_motor_set_count(NXT_PORT_A, 0);
	nxt_motor_set_count(NXT_PORT_B, 0);
	nxt_motor_set_count(NXT_PORT_C, 0);
	cpp_constructor();
	display_clear(1);
	systick_wait_ms(10);

#ifdef NXT_JSP
	interrupts_get_and_disable();
#else
	disable_int(); /* set_OS_flag and Start OS have to be atomic */
#endif
	set_OS_flag(); /* this shoud be called before starting OS */
#ifdef NXT_JSP
	lejos_osek_run(); /* start TOPPERS JSP */
#else
	StartOS(1);    /* start TOPPERS OSEK */
#endif

	/* never reached here */


#else
	/* 
	 * Default start up sequence
	 */
	U32 st;
	U32 last_act_time = 0;
	U32 flash_req_cnt = 0;

	init_OS_flag(); /* this should be called before device init */
	nxt_device_init();
	ecrobot_initDeviceStatus(); // added 10/28/2010 to fix a bug by tchikama
	ecrobot_init_nxtstate();

	show_splash_screen();
	show_main_screen();
	display_status_bar(1); /* clear status bar */
	add_status_info(execution_mode());
	display_status_bar(0); /* update status bar */
	while(1)
	{
		/* device init should be called prior to running the application */
		ecrobot_device_initialize();
		ecrobot_setDeviceInitialized();
		/* check the buttons every 10msec */
		st = systick_get_ms();
		if (st >= last_act_time + 10) 
		{
			last_act_time = st;
			ecrobot_poll_nxtstate();
			display_status_bar(0);

			/* 
			 * executed in FLASH: setup for the application flash
			 * executed in SRAM:  no effect
			 */
			if ((ecrobot_get_button_state() == (ENTER_PRESSED | STOP_PRESSED)) &&
			    (execution_mode() == EXECUTED_FROM_FLASH))
			{
				flash_req_cnt++;
				/* keep pusing ENTER + STOP buttons more than 1000msec */
				if (flash_req_cnt >= 100)
				{
					/* set flash request and shut down the NXT
				 	 * at the next start, NXT BIOS will be executed.
				 	 */
					ecrobot_device_terminate();
					set_flash_request();
					display_clear(1);
					systick_wait_ms(10);
					nxt_lcd_power_down(); /* reset LCD hardware */
					systick_wait_ms(10);
					while(1)
					{
						nxt_avr_power_down();
					}
				}
			}
			else
			{
				flash_req_cnt = 0;
				if ((ecrobot_get_button_state() == EXIT_PRESSED) || (systick_get_ms() > SLEEP_TIME))
				{
					/* shut down the NXT */
					ecrobot_device_terminate();
					display_clear(1);
					systick_wait_ms(10);
					nxt_lcd_power_down(); /* reset LCD hardware */
					systick_wait_ms(10);
					while(1)
					{
						nxt_avr_power_down();
					}
				}
				else if (ecrobot_get_button_state() == RUN_PRESSED)
				{
					nxt_motor_set_count(NXT_PORT_A, 0);
					nxt_motor_set_count(NXT_PORT_B, 0);
					nxt_motor_set_count(NXT_PORT_C, 0);
					cpp_constructor();
					display_clear(1);
					systick_wait_ms(10);
#ifdef NXT_JSP
					interrupts_get_and_disable();
#else
					disable_int(); /* set_OS_flag and Start OS have to be atomic */
#endif
					set_OS_flag(); /* this shoud be called before starting OS */
#ifdef NXT_JSP
					lejos_osek_run(); /* start TOPPERS JSP */
#else
					StartOS(1);    /* start TOPPERS OSEK */
#endif
					/* never reached here */
				}
			}
		}
    }
#endif    

    return 0;
}
void SC_doCtrl(SpeedCtrl *self)
{
	float speed = S_getSpeed(&mSpeed,systick_get_ms());
	float forward = PSC_calcSpeedCtrlVal(self->mPIDSpeedCtrl,S_getTargSpeed(&mSpeed),S_getBfSpeed(&mSpeed),speed,systick_get_ms()*0.001);
	WheelActuator_set_forward(&wheelActuator ,forward);
}