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; }
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; } } } } }
/*--------------------------------------------------------------------- * 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 ); }
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); }
/*--------------------------------------------------------------------- * 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(); }
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); }
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"); } }
void error_detect_dogkick_received() { lastDogKickReceived = systick_get_ms(); //Check if "the dog is dead" (we havn't received anything for too long) }
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); }
/** * 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; }
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); }