void initialize_motors(engines_t * motors){
nxt_motor_set_count(NXT_PORT_A, 1000);
for(int i = 0; i < SAMP_NUM; i++){
motors->first.speeds[i] = 0;
motors->first.revolutions[i] = nxt_motor_get_count(NXT_PORT_A);
motors->first.times[i] = (long) ecrobot_get_systick_ms();
}
for(int j = 0; j < BUFFER_SIZE; j++){
motors->first.speed_ref[j] = 0;
motors->first.distance_ref[j] = 0;
}
nxt_motor_set_count(NXT_PORT_B, 1000);
for(int i = 0; i < SAMP_NUM; i++){
motors->second.speeds[i] = 0;
motors->second.revolutions[i] = nxt_motor_get_count(NXT_PORT_B);
motors->second.times[i] = (long) ecrobot_get_systick_ms();
}
for(int j = 0; j < BUFFER_SIZE; j++){
motors->first.speed_ref[j] = 0;
motors->first.distance_ref[j] = 0;
}
nxt_motor_set_count(NXT_PORT_C, 1000);
for(int i = 0; i < SAMP_NUM; i++){
motors->third.speeds[i] = 0;
motors->third.revolutions[i] = nxt_motor_get_count(NXT_PORT_C);
motors->third.times[i] = (long) ecrobot_get_systick_ms();
}
for(int j = 0; j < BUFFER_SIZE; j++){
motors->third.speed_ref[j] = 0;
motors->third.distance_ref[j] = 0;
}
}
void ecrobot_device_initialize(void)
{
ecrobot_init_nxtcolorsensor(NXT_PORT_S1, NXT_COLORSENSOR); // initialize a sensor
ecrobot_init_sonar_sensor(NXT_PORT_S4);
nxt_motor_set_count(NXT_PORT_B, 0);
}
//*****************************************************************************
// ���� : ecrobot_device_initialize
// �� : �Ȃ�
// �߂�l : �Ȃ�
// �T�v : ECROBOT�f�o�C�X�������t�b�N��
//*****************************************************************************
void ecrobot_device_initialize()
{
ecrobot_set_light_sensor_active(NXT_PORT_S3); /* ���Z���T�ԐFLED��ON */
ecrobot_init_sonar_sensor(NXT_PORT_S2); /* �����g�Z���T(I2C�ʐM)������ */
nxt_motor_set_count(NXT_PORT_A, 0); /* ���S��~�p���[�^�G���R�[�_���Z�b�g */
ecrobot_init_bt_slave(PASS_KEY); /* Bluetooth�ʐM���� */
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static U32 garage_trace(
void
)
{
U32 ret = RET_REMAIN;
F32 forward;
F32 turn;
F32 u_turn;
U16 lightness;
static U32 cnt = 0;
lightness = ecrobot_get_light_sensor(PORT_LIGHT);
garage_calc_pid(lightness, &turn);
/* 旋回量をフィルタリング */
if (turn > 100) {
turn = 100;
} else if (turn < -100) {
turn = -100;
}
/* 旋回量の絶対値をとる */
if (turn > 0) {
u_turn = turn;
} else {
u_turn = (-1) * turn;
}
cnt++;
if (cnt > 250) {
/* 旋回量から前進量を調整する */
if (u_turn < 30) {
forward = 100;
} else if (u_turn < 50) {
forward = 90;
} else {
forward = 85;
}
/* 倒立振子制御 */
garage_stand(forward, turn, GYRO_OFFSET);
/* 灰色検知 */
if (lightness > 610) {
nxt_motor_set_count(PORT_MOTOR_TAIL, 0);
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)10, 1);
gkGarageInfo.motor_r = nxt_motor_get_count(PORT_MOTOR_R);
gkGarageInfo.motor_l = nxt_motor_get_count(PORT_MOTOR_L);
ret = RET_FINISH;
} else { /* nothing */ }
} else {
forward = 50;
/* 倒立振子制御 */
garage_stand(forward, turn, GYRO_OFFSET);
}
return ret;
}
/* nxtOSEK hooks */
void ecrobot_device_initialize(void)
{
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
balance_init();
nxt_motor_set_count(PORT_MOTOR_L, 0);
nxt_motor_set_count(PORT_MOTOR_R, 0);
ecrobot_init_sonar_sensor(PORT_SONAR);
ecrobot_set_light_sensor_active(PORT_LIGHT);
/* ���O */
if (ecrobot_get_bt_status() == BT_NO_INIT) {
ecrobot_set_bt_device_name("ETROBOLT255");
} else { /* NOTHING */ }
ecrobot_init_bt_slave("LEJOS_OSEK");
lt_ini();
lookup_ini();
seesaw_ini();
garage_ini();
}
inline std_return RTE_SchussMotor_SetOutputValue_OSPort_Out(uint32_t* degree)
{
#define CORRECTION 11
#ifdef RTE_SchussMotor_SetOutputValue_OSPort_Out_ENGINE
nxt_motor_set_count(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT, 0);
nxt_motor_set_speed(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT, 35, 1);
while ( nxt_motor_get_count(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT) < (*degree - CORRECTION) )
{
;
}
nxt_motor_set_speed(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT, 0, 1);
#endif
return 0;
}
//*****************************************************************************
// 関数名 : ecrobot_device_initialize
// 引数 : なし
// 戻り値 : なし
// 概要 : ECROBOTデバイス初期化処理フック関数
//*****************************************************************************
void ecrobot_device_initialize()
{
/*************デバイス名を設定する***************/
if(ecrobot_get_bt_status()==BT_NO_INIT){
/**
* Bluetooth通信用デバイス名の変更は、Bluetooth通信接続が確立されていない場合のみ有効です。
* 通信接続確立時にはデバイス名は変更されません。(下記のAPIは何もしません)
*/
ecrobot_set_bt_device_name(DEVICE_NAME);
}
/************************************************/
ecrobot_set_light_sensor_active(NXT_PORT_S3); /* 光センサ赤色LEDをON */
ecrobot_init_sonar_sensor(NXT_PORT_S2); /* 超音波センサ(I2C通信)を初期化 */
nxt_motor_set_count(NXT_PORT_A, 0); /* 完全停止用モータエンコーダリセット */
ecrobot_init_bt_slave(PASS_KEY); /* Bluetooth通信初期化 */
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_pol_triger(
void
)
{
U8 touch;
U32 bt = 0;
char buf[128];
U32 size;
S8 pwm_l;
S8 pwm_r;
/* タッチセンサ */
touch = ecrobot_get_touch_sensor(PORT_TOUCH);
/* Bluetooth */
size = ecrobot_read_bt(buf, 0, 128);
if (size > 0) {
if (buf[0] == 's') {
ecrobot_send_bt("start", 0, 5);
bt = 1;
} else {
ecrobot_send_bt("unknown", 0, 7);
}
} else { /* nothing */ }
if (touch == 1 || bt == 1) {
/* 倒立制御 */
balance_control(
(F32)0,
(F32)0,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
GYRO_OFFSET,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
nxt_motor_set_count(PORT_MOTOR_TAIL, (int)0);
gkRobotStat.robotStat = ROBOT_STAT_STAND;
} else { /* NOTHING */ }
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
*********************************************************/
static U32 seesaw_forward(
void
)
{
U32 ret = RET_REMAIN;
U16 lightness;
F32 turn;
static int motor_r = 0;
static int motor_l = 0;
int cur_motor_r;
int cur_motor_l;
lightness = ecrobot_get_light_sensor(PORT_LIGHT);
turn = (0.2) * (530 - lightness);
seesaw_stand((F32)15, (F32)turn, GYRO_OFFSET + 2);
//seesaw_stand((F32)1, (F32)0, GYRO_OFFSET);
if (motor_r == 0) {
motor_r = nxt_motor_get_count(PORT_MOTOR_R);
motor_l = nxt_motor_get_count(PORT_MOTOR_L);
nxt_motor_set_count(PORT_MOTOR_TAIL, 0);
} else {
///* 尻尾制御 */
//tail = nxt_motor_get_count(PORT_MOTOR_TAIL);
//if (tail < SEESAW_TAIL_ANGLE_BLAKE) {
// nxt_motor_set_speed(PORT_MOTOR_TAIL, 20, 1);
//} else {
// nxt_motor_set_speed(PORT_MOTOR_TAIL, 0, 1);
//}
cur_motor_r = nxt_motor_get_count(PORT_MOTOR_R);
cur_motor_l = nxt_motor_get_count(PORT_MOTOR_L);
if (cur_motor_r - motor_r > 900 && cur_motor_l - motor_l > 900) {
ret = RET_FINISH;
} else { /* nothing */ }
}
return ret;
}
void rotate(int degree) {
nxt_motor_set_count(NXT_PORT_A, degree*5+nxt_motor_get_count(NXT_PORT_A));
nxt_motor_set_count(NXT_PORT_B, -(degree*5)-nxt_motor_get_count(NXT_PORT_B));
stop();
}
void seesaw_ride_pair( Command* cmd , ROBOT_STATUS* status )
{
signed char left;
signed char right;
f4 f4t_yp;
f4 f4t_yd;
static u2 u2_count = 0;
static u2 u2_gyro_temp=0;
static s4 s4t_distance = 0;
static s4 s4t_motor_L1 = 0;
static s4 s4t_motor_R1 = 0;
static s4 s4t_motor_L2 = 0;
static s4 s4t_motor_R2 = 0;
static s4 s4t_motor_L3 = 0;
static s4 s4t_motor_R3 = 0;
static s4 s4t_motor_L4 = 0;
static s4 s4t_motor_R4 = 0;
static s1 tail_seesaw = 0;
static u1 u1s_trace = 0;
static u1 u1s_touritu = 0;
s4t_motor_L1 = nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R1 = nxt_motor_get_count(PORT_MOTOR_R);
static u4 u4_recieve = 0;
u1 receive_data[90] = {0};
/* user logic start */
u1s_2msflipcount_see = 1 - u1s_2msflipcount_see;
display_clear(0);
display_goto_xy(0, 0); display_string("stage"); display_unsigned(u1g_seesaw_ride_pair_mode,3);
display_goto_xy(0, 1); display_string("LEFT"); display_unsigned(left,3);
display_goto_xy(0, 2); display_string("RIGHT"); display_unsigned(right,3);
display_goto_xy(0, 3); display_string("count"); display_unsigned(u1s_touritu,3);
display_update();
if ( u1s_2msflipcount_see == 1 )
{
switch(u1g_seesaw_ride_pair_mode) /* シーソーシーケンス開始 */
{
case 0 : /* 倒立開始 */
// u1s_trace = 1; /* トレースOFF */
u1s_touritu = 1; /* トレースON */
f4g_forward = 0;
balance_init(); /* balance APIの初期化 */
nxt_motor_set_count(PORT_MOTOR_L, 0); /* 左モータ・カウント値[°]を0リセット */
nxt_motor_set_count(PORT_MOTOR_R, 0); /* 右モータ・カウント値[°]を0リセット */
s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R);
//u1g_seesaw_ride_pair_mode = 1;
u2_count = 0;
tail_seesaw = tailControl( 30, status->motorangle_T);
if(status->motorangle_T > 10){
u1g_seesaw_ride_pair_mode = 1;
tail_seesaw = 0;
}
break;
// case 51:
// u1s_touritu = 0;
// break;
case 1:
if(status->lightval > GRAY_WHITE){
u1s_trace = 1;
f4g_forward = 20;
}else{
f4g_forward = 20;
chokushin2();
u1g_seesaw_ride_pair_mode = 2;
}
break;
case 2 : /* 段差検知まで */
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance < 300){
u1s_trace = 1; /*トレースの設定*/
f4g_forward = 20;
}else{
if(f4g_turn < 3 && f4g_turn > -3){
u2_count++;
}else{
u2_count = 0;
}
}
if(u2_count > 10 && f4g_turn < 1 && f4g_turn > -1){
u1g_seesaw_ride_pair_mode++;
u2_count = 0;
f4g_turn = 0;
}
break;
case 3:
u1s_trace = 0;
f4g_forward = 30;
chokushin2();
if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){
u1g_seesaw_ride_pair_mode++;
f4g_forward = 100;
s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R);
u2_count = 0;
u1s_trace = 0;
u2_gyro_temp = robot.robotSensor.GYRO.gyroOffsetValue - 5;
}
break;
case 4 : /* 検知後ある一定以上上る */
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance < 20){ //ﾍﾞｰｽ100
f4g_forward = 100;
chokushin2();
}else if(s4t_distance < 60){ //べーす３００
robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp;
f4g_forward = 40;
chokushin2();
}else{
u2_count = 0;
f4g_forward = 0;
chokushin2();
u1g_seesaw_ride_pair_mode = 5;
}
break;
case 5: /* Bluetoothが来るまで待機 */
f4g_forward = 0;
chokushin2();
u4_recieve = ecrobot_read_bt_packet( receive_data, 90 );
if ( u4_recieve > 0 )
{
ecrobot_sound_tone(100, 100, 50);
u2_count = 0;
f4g_forward = 0;
chokushin2();
u1g_seesaw_ride_pair_mode++;
}
break;
case 6: /* Bluetoothが来たから上る */
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance < 1060){
f4g_forward = 20;
chokushin2();
}else{
robot.robotSensor.GYRO.gyroOffsetValue = robot.robotSensor.GYRO.gyroOffsetValue -5;
u1g_seesaw_ride_pair_mode++;
// ecrobot_sound_tone(100, 100, 50);
}
break;
case 7 :
if(status->gyroval < status->gyro_offset - 80){
robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp;
u1g_seesaw_ride_pair_mode++;
nxt_motor_set_count(PORT_MO_TAIL, 0);
s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R);
}else{
f4g_forward = 20;
chokushin2();
}
break;
case 8 : /* Bluetoothが来たから上る */
s4t_motor_L4 =s4t_motor_L3;
s4t_motor_R4 =s4t_motor_R3;
s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R);
if((s4t_motor_R3 - s4t_motor_R4) < 0 && (s4t_motor_L3 - s4t_motor_L4)){
// u1s_touritu = 0;
// left = 0;
// right = 0;
// f4g_forward = 0;
// f4g_turn = 0;
u1g_seesaw_ride_pair_mode++;
}else{
f4g_forward = -60;
chokushin2();
}
break;
case 9 :
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance > 1060){
f4g_forward = -20;
chokushin2();
}else{
u1g_seesaw_ride_pair_mode++;
// ecrobot_sound_tone(100, 100, 50);
}
case 10 :
u1s_touritu = 0;
left = 0;
right = 0;
f4g_forward = 0;
f4g_turn = 0;
break;
}
if (u1s_trace == 1)
{
f4t_yp = (f4)(GRAY_WHITE - status->lightval );
f4t_yd = (f4t_yp - f4s_last_yp) / 0.004;
f4s_last_yp = f4t_yp;
f4g_turn = (f4)((f4)TRACE_P * f4t_yp) + (f4)((f4)TRACE_D * f4t_yd);
// f4g_turn = -f4g_turn;
/* 旋回方向決定 */
if(f4g_turn > 50){
f4g_turn = 50;
}else if(f4g_turn < -50){
f4g_turn = -50;
}else{
f4g_turn = f4g_turn;
}
f4g_turn = -f4g_turn;
}
if (u1s_touritu == 1)
{
balance_control(
(f4)f4g_forward, /* 前後進命令(+:前進, -:後進) */
(f4)-f4g_turn, /* 旋回命令(+:右旋回, -:左旋回) */
// (f4)-6, /* 旋回命令(+:右旋回, -:左旋回) */
(f4)status->gyroval, /* ジャイロセンサ値 */
(f4)status->gyro_offset, /* ジャイロセンサオフセット値 */
(f4)status->motorangle_L, /* 左モータ回転角度[deg] */
(f4)status->motorangle_R, /* 右モータ回転角度[deg] */
(f4)status->batteryval, /* バッテリ電圧[mV] */
&left, /* 左モータPWM出力値 */
&right /* 右モータPWM出力値 */
);
setMotorVal( left , right , 0);
}else{
tail_seesaw = tailControl( 40, nxt_motor_get_count(PORT_MO_TAIL));
setMotorVal( left , right , tail_seesaw );
}
}
}
/**
* 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;
}
void resetMotorCounters()
{
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,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 RN_setting()
{
static float beforestop;
switch (setting_mode){
case (RN_SETTINGMODE_START):
RN_calibrate();
break;
case (RN_RUN):
RA_linetrace_PID(30);
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_before = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
break;
case(RN_SLOW_RUN):
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_slow = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
if((distance_before - distance_slow <= -5))
{
gyro_offset -= 15;
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
setting_mode = RN_STOP_WAIT;
}
else
{
RA_speed(-50,8);
cmd_turn = RA_wheels(cmd_turn);
}
break;
case(RN_BACK_RUN):
RA_linetrace_PID(-15);
break;
case(RN_HIGH_RUN):
break;
case(RN_STOP_WAIT):
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_stop = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
beforestop = distance_stop;
wait_count += 1;
RA_linetrace(0,2);
if(wait_count == 550)
{
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
RA_hensareset();
wait_count = 0;
gyro_offset -= 15;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
}
break;
case(RN_STOP):
nxt_motor_set_speed(NXT_PORT_C, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
case(RN_LITTLE_RUN):
wait_count += 1;
mode_count += 1;
RA_linetrace(5,10);
if(wait_count >= 500)
{
wait_count = 0;
setting_mode = RN_LITTLE_LOW;
}
if(mode_count >= 5000)
{
mode_count = 0;
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
RA_hensareset();
wait_count = 0;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
}
break;
case(RN_LITTLE_LOW):
wait_count += 1;
mode_count += 1;
RA_linetrace(-3,2);
if(wait_count >= 500)
{
wait_count = 0;
setting_mode = RN_LITTLE_RUN;
}
if(mode_count >= 5000)
{
mode_count = 0;
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
RA_hensareset();
wait_count = 0;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
}
break;
default:
break;
}
}
//*****************************************************************************
// TASK : tsk0
// DESCRIPTION : 4msec periodical Task and controls NXTway-GS
// INIT_MODE
// ��
// CAL_MODE
// ��
// CONTROL_MODE
//*****************************************************************************
void tsk0(VP_INT exinf)
{
static U32 gyro_offset; /* gyro sensor offset value */
static U32 avg_cnt; /* average count to calc gyro offset */
static U32 cal_start_time; /* calibration start time */
static U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */
SINT i;
S8 cmd_forward, cmd_turn;
S8 pwm_l, pwm_r;
switch(nxtway_gs_mode){
case(INIT_MODE):
gyro_offset = 0;
avg_cnt = 0;
for (i = 0; i < BT_RCV_BUF_SIZE; i++){
bt_receive_buf[i] = 0;
}
balance_init(); /* NXTway-GS C API initialize function */
nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */
nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */
cal_start_time = ecrobot_get_systick_ms();
nxtway_gs_mode = CAL_MODE;
break;
case(CAL_MODE):
gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);
avg_cnt++;
if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {
/* 1000msec later, start balancing */
gyro_offset /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 30U); /* beep a tone */
nxtway_gs_mode = CONTROL_MODE;
}
break;
case(CONTROL_MODE):
(void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);
/*
* R/C command from NXT GamePad
* buf[0]: -100(forward max.) to 100(backward max.)
* buf[1]: -100(turn left max.) to 100(turn right max.)
*/
cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */
cmd_turn = (S8)bt_receive_buf[1];
if (obstacle_flag == 1){
/* make NXTway-GS move backward to avoid obstacle */
cmd_forward = -100;
cmd_turn = 0;
}
/* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
(F32)gyro_offset,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */
break;
default:
/* unexpected mode */
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
break;
}
ext_tsk(); /* terminates this task */
}
