s32 sys_event_flag_set(u32 eflag_id, u64 bitptn) { sys_event_flag.Log("sys_event_flag_set(eflag_id=%d, bitptn=0x%llx)", eflag_id, bitptn); EventFlag* ef; if (!sys_event_flag.CheckId(eflag_id, ef)) return CELL_ESRCH; u32 tid = GetCurrentPPUThread().GetId(); ef->m_mutex.lock(tid); ef->flags |= bitptn; if (u32 target = ef->check()) { // if signal, leave both mutexes locked... ef->signal.lock(target); ef->m_mutex.unlock(tid, target); } else { ef->m_mutex.unlock(tid); } return CELL_OK; }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd ){ bool ret = true; switch (local_status_) { case P0: cmd = tec_.Calclate(ni.pos); if (tec_.isLastCtlPoints() && evf.is_SetEvent(Robot::EV_LIGHT_BLACK)) { // 最後の制御点列で黒を検知したら終了。 Robot::Instance().Beep(); local_status_ = P1; ret = true; tec_pid_.Init(); timer_start (2000); } break; case P1: if (IsTimeup()) { Robot::Instance().Beep(Robot::B); local_status_ = P2; ret = true; timer_start (7000); } else { cmd = tec_pid_.Calclate(ri.light_sensor_val, 50, Technic::RIGHT); ret = true; } break; case P2: if (IsTimeup()) { ret = false; // 終了. Robot::Instance().Beep(Robot::A2); } else { cmd = tec_pid_.Calclate(ri.light_sensor_val, 100, Technic::RIGHT); ret = true; } break; default: ret = false; // 終了. break; } cmd.param3 = 0; return ret; }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd) { bool ret = true; switch (local_status_) { case MISSION_START: // 単純走行モード. // 走行計算. cmd = pid_trace_.Calclate(ri.light_sensor_val, speed_, edge_); cmd.param3 = tail_; // 終了条件:指定時間で終了. if (IsTimeup()) { Robot::Instance().Beep(Robot::C); local_status_ = DETECT_GRAY_ZONE; } break; case DETECT_GRAY_ZONE: // グレーゾーン検知モード. // 走行計算. cmd = pid_trace_.Calclate(ri.light_sensor_val, speed_, edge_); cmd.param3 = tail_; // 終了条件:グレー検知で終了. if (evf.is_SetEvent(Robot::EVENT_MARKER)) { Robot::Instance().Beep(Robot::D); local_status_ = MISSION_NEXT; } break; case MISSION_NEXT: // G1前半ミッション終了. Robot::Instance().Beep(Robot::E); ret = false; TimerStart(1000); break; } return ret; }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd ){ bool stay_mission = true; switch (local_status_){ case P_REACHING: //シーソーに近づく speed_ = 10; if( evf.is_SetEvent(Robot::EVENT_GYRO)){ evf.Set(Robot::EVENT_GYRO, false); Robot::Instance().Beep(); set_current_position(pos, ni); local_status_ = S_APPROACH_BACK; // 助走するために少し下がる tec_s_.Init(ri); tecType_ = TEC_S; speed_ = -30; } break; case S_APPROACH_BACK: if( get_distance(pos, ni) > 100*100){ set_current_position(pos, ni); Robot::Instance().Beep(); local_status_ = S_STAGING; // 一気に登る tec_s_.Init(ri); tecType_ = TEC_S; speed_ = 100; } break; case S_STAGING: // 一気にシーソーへ登る if( get_distance(pos, ni) > 180*180){ set_current_position(pos, ni); Robot::Instance().Beep(Robot::B); local_status_ = P_RECOVER_ON_SEESAW; // とりあえずシーソー上でラインに復帰 tecType_ = TEC_P; tec_p_.Init(); speed_ = 10; } break; case P_RECOVER_ON_SEESAW: // とりあえずシーソー上でラインに復帰 if( get_distance(pos, ni) > 200*200 ){ set_current_position(pos, ni); Robot::Instance().Beep(); local_status_ = S_GET_OFF; //左にそれながらシーソーを降りる tec_s_.Init(ri); tecType_ = TEC_S; speed_ = 40; } break; case S_GET_OFF: if(get_distance(pos, ni) > 250*250){ Robot::Instance().Beep(Robot::B); local_status_ = S_RECOVER_ON_GROUND; // ラインに復帰を試みる tec_s_.Init(ri); tecType_ = TEC_S; speed_ = 10; } break; case S_RECOVER_ON_GROUND: /* 終了条件 */ if(evf.is_SetEvent(Robot::EVENT_LIGHT_BLACK)){ set_current_position(pos, ni); local_status_ = P_FOR_NEXT_MISSION; Robot::Instance().Beep(Robot::G); tecType_ = TEC_P; tec_p_.Init(); speed_ = 20; TimerStart(10000); } break; case P_FOR_NEXT_MISSION: // 最後に安定するまでライントレースして完全復帰する /* ミッション終了の条件 */ if(get_distance(pos, ni) > 200*200){ Robot::Instance().Beep(Robot::E); stay_mission = false; } break; default: stay_mission = true; break; } // end of switch-case(local_status_) /* 走行計算 */ if(tecType_ == TEC_S){ cmd = tec_s_.Calclate(speed_, ri.rot_encoderL_val, ri.rot_encoderR_val ); if(local_status_ == S_GET_OFF){ cmd.param2 =- 3; // 左側へ降りる } if(local_status_ == S_RECOVER_ON_GROUND){ cmd.param2 =+ 3; // 右側のラインへ復帰する } }else if(tecType_ == TEC_P){ cmd = tec_p_.Calclate(ri.light_sensor_val, speed_, edge_dir); // if(local_status_ == S_STAGING || local_status_ == P_RECOVER_ON_SEESAW){ // cmd.param2 = (S16)((F32)cmd.param2 * 0.7); // } }else if(tecType_ == TEC_R){ cmd = tec_r_.Calclate(speed_, rotate_, dir_); }else if(tecType_ == TEC_T){ // しっぽの設定はcase内で設定するよ }else{} return stay_mission; }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd ){ bool ret = true; S32 diffR; S32 diffL; switch (local_status_) { // 流し走行中(階段衝突前) // do: PIDライントレース // exit: ジャイロ case STAIRS_PRE_IN: cmd = tec_pid_.Calclate(ri.light_sensor_val, (S16)(80), Technic::RIGHT); if(is_timeup()){ Robot::Instance().Beep(Robot::A2); local_status_ = STAIRS_IN; } break; // 流し走行中(階段衝突前) // do: PIDライントレース // exit: ジャイロ case STAIRS_IN: cmd = tec_pid_.Calclate(ri.light_sensor_val, (S16)(speed_ * 0.2), Technic::RIGHT); cmd.param3 = 0; if(evf.is_SetEvent(Robot::EV_GYRO)){ start_encoderR = ri.rot_encoderR_val; start_encoderL = ri.rot_encoderL_val; Robot::Instance().Beep(Robot::D); local_status_ = STAIRS_HIT_ONE; } break; // 片輪乗り上げ中 // do: 旋回走行 // exit: 340ms case STAIRS_HIT_ONE: cmd = tec_r_.Calclate(30, 100, Technic::LEFT); diffR = ri.rot_encoderR_val - start_encoderR; if(diffR > 80){//45:1, 50:2 Robot::Instance().Beep(Robot::E); local_status_ = STAIRS_HIT_BOTH; } break; // もう片輪乗り上げ中 // do: 旋回走行 // exit: 480ms case STAIRS_HIT_BOTH: cmd = tec_r_.Calclate(30, 150, Technic::RIGHT); diffL = ri.rot_encoderL_val - start_encoderL; if(diffL > 80){//45:1, 50:2]] Robot::Instance().Beep(Robot::F); local_status_ = STAIRS_FLATTEN; } break; // 安定化中 // do: 旋回走行 // exit: 480ms case STAIRS_FLATTEN: diffR = ri.rot_encoderR_val - start_encoderR; diffL = ri.rot_encoderL_val - start_encoderL; if(diffR - diffL > 1){ cmd = tec_r_.Calclate(5, 100, Technic::RIGHT); }else if(diffL - diffR > 1){ cmd = tec_r_.Calclate(5, 100, Technic::LEFT); }else{ cmd.param1 = 5; cmd.param2 = 0; flatten_count++; } if(flatten_count > 20){ Robot::Instance().Beep(Robot::B); local_status_ = STAIRS_FORWARD; timer_start(4000); tec_s_.Init(ri); } break; // ゆっくり前進 case STAIRS_FORWARD: cmd = tec_s_.Calclate(10, ri.rot_encoderL_val, ri.rot_encoderR_val); if(is_timeup() && evf.is_SetEvent(Robot::EV_GYRO)){ start_encoderR = ri.rot_encoderR_val; start_encoderL = ri.rot_encoderL_val; Robot::Instance().Beep(Robot::C); local_status_ = STAIRS_HIT_ONE2; } break; // 片輪乗り上げ中 // do: 旋回走行 case STAIRS_HIT_ONE2: cmd = tec_r_.Calclate(30, 100, Technic::RIGHT); diffL = ri.rot_encoderL_val - start_encoderL; if(diffL > 80){//45:1, 50:2 Robot::Instance().Beep(Robot::E); local_status_ = STAIRS_HIT_BOTH2; } break; // もう片輪乗り上げ中 // do: 旋回走行 case STAIRS_HIT_BOTH2: cmd = tec_r_.Calclate(30, 150, Technic::LEFT); diffR = ri.rot_encoderR_val - start_encoderR; if(diffR > 80){//45:1, 50:2]] Robot::Instance().Beep(Robot::F); local_status_ = STAIRS_FLATTEN2; flatten_count = 0; } break; // 安定化中 case STAIRS_FLATTEN2: diffR = ri.rot_encoderR_val - start_encoderR; diffL = ri.rot_encoderL_val - start_encoderL; if(diffR - diffL > 1){ cmd = tec_r_.Calclate(5, 100, Technic::RIGHT); }else if(diffL - diffR > 1){ cmd = tec_r_.Calclate(5, 100, Technic::LEFT); }else{ cmd.param1 = 5; cmd.param2 = 0; flatten_count++; } if(flatten_count > 20){ Robot::Instance().Beep(Robot::B); local_status_ = STAIRS_FORWARD2; tec_s_.Init(ri); timer_start(4000); } break; // ゆっくり前進 case STAIRS_FORWARD2: cmd = tec_s_.Calclate(10, ri.rot_encoderL_val, ri.rot_encoderR_val); if(is_timeup() && evf.is_SetEvent(Robot::EV_GYRO)){ Robot::Instance().Beep(Robot::A); local_status_ = STAIRS_OUT; tec_pid_.Init(); timer_start(1000); } break; // ライン復帰 case STAIRS_OUT: cmd = tec_pid_.Calclate(ri.light_sensor_val, (S16)(speed_ * 0.2), Technic::RIGHT); if(is_timeup()){ Robot::Instance().Beep(Robot::C); local_status_ = STAIRS_END; } break; default: ret = false; break; } return ret; }
s32 sys_event_flag_wait(u32 eflag_id, u64 bitptn, u32 mode, vm::ptr<u64> result, u64 timeout) { sys_event_flag.Log("sys_event_flag_wait(eflag_id=%d, bitptn=0x%llx, mode=0x%x, result_addr=0x%x, timeout=%lld)", eflag_id, bitptn, mode, result.addr(), timeout); if (result) *result = 0; switch (mode & 0xf) { case SYS_EVENT_FLAG_WAIT_AND: break; case SYS_EVENT_FLAG_WAIT_OR: break; default: return CELL_EINVAL; } switch (mode & ~0xf) { case 0: break; // ??? case SYS_EVENT_FLAG_WAIT_CLEAR: break; case SYS_EVENT_FLAG_WAIT_CLEAR_ALL: break; default: return CELL_EINVAL; } EventFlag* ef; if (!sys_event_flag.CheckId(eflag_id, ef)) return CELL_ESRCH; const u32 tid = GetCurrentPPUThread().GetId(); { ef->m_mutex.lock(tid); if (ef->m_type == SYS_SYNC_WAITER_SINGLE && ef->waiters.size() > 0) { ef->m_mutex.unlock(tid); return CELL_EPERM; } EventFlagWaiter rec; rec.bitptn = bitptn; rec.mode = mode; rec.tid = tid; ef->waiters.push_back(rec); if (ef->check() == tid) { u64 flags = ef->flags; ef->waiters.erase(ef->waiters.end() - 1); if (mode & SYS_EVENT_FLAG_WAIT_CLEAR) { ef->flags &= ~bitptn; } else if (mode & SYS_EVENT_FLAG_WAIT_CLEAR_ALL) { ef->flags = 0; } if (result) *result = flags; ef->m_mutex.unlock(tid); return CELL_OK; } ef->m_mutex.unlock(tid); } u64 counter = 0; const u64 max_counter = timeout ? (timeout / 1000) : ~0; while (true) { if (ef->signal.unlock(tid, tid) == SMR_OK) { ef->m_mutex.lock(tid); u64 flags = ef->flags; for (u32 i = 0; i < ef->waiters.size(); i++) { if (ef->waiters[i].tid == tid) { ef->waiters.erase(ef->waiters.begin() + i); if (mode & SYS_EVENT_FLAG_WAIT_CLEAR) { ef->flags &= ~bitptn; } else if (mode & SYS_EVENT_FLAG_WAIT_CLEAR_ALL) { ef->flags = 0; } if (u32 target = ef->check()) { // if signal, leave both mutexes locked... ef->signal.unlock(tid, target); ef->m_mutex.unlock(tid, target); } else { ef->signal.unlock(tid); } if (result) *result = flags; ef->m_mutex.unlock(tid); return CELL_OK; } } ef->signal.unlock(tid); ef->m_mutex.unlock(tid); return CELL_ECANCELED; } std::this_thread::sleep_for(std::chrono::milliseconds(1)); if (counter++ > max_counter) { ef->m_mutex.lock(tid); for (u32 i = 0; i < ef->waiters.size(); i++) { if (ef->waiters[i].tid == tid) { ef->waiters.erase(ef->waiters.begin() + i); break; } } ef->m_mutex.unlock(tid); return CELL_ETIMEDOUT; } if (Emu.IsStopped()) { sys_event_flag.Warning("sys_event_flag_wait(id=%d) aborted", eflag_id); return CELL_OK; } } }
void SPUThread::WriteChannel(u32 ch, const u128& r) { const u32 v = r._u32[3]; switch (ch) { case SPU_WrOutIntrMbox: { if (!group) // if RawSPU { if (Ini.HLELogging.GetValue()) LOG_NOTICE(Log::SPU, "SPU_WrOutIntrMbox: interrupt(v=0x%x)", v); while (!SPU.Out_IntrMBox.Push(v)) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); if (Emu.IsStopped()) { LOG_WARNING(Log::SPU, "%s(%s) aborted", __FUNCTION__, spu_ch_name[ch]); return; } } m_intrtag[2].stat |= 1; if (CPUThread* t = Emu.GetCPU().GetThread(m_intrtag[2].thread)) { if (t->GetType() == CPU_THREAD_PPU) { if (t->IsAlive()) { LOG_ERROR(Log::SPU, "%s(%s): interrupt thread was alive", __FUNCTION__, spu_ch_name[ch]); Emu.Pause(); return; } PPUThread& ppu = *(PPUThread*)t; ppu.GPR[3] = ppu.m_interrupt_arg; ppu.FastCall2(vm::read32(ppu.entry), vm::read32(ppu.entry + 4)); } } } else { const u8 code = v >> 24; if (code < 64) { /* ===== sys_spu_thread_send_event (used by spu_printf) ===== */ u8 spup = code & 63; u32 data; if (!SPU.Out_MBox.Pop(data)) { LOG_ERROR(Log::SPU, "sys_spu_thread_send_event(v=0x%x, spup=%d): Out_MBox is empty", v, spup); return; } if (Ini.HLELogging.GetValue()) { LOG_NOTICE(Log::SPU, "sys_spu_thread_send_event(spup=%d, data0=0x%x, data1=0x%x)", spup, v & 0x00ffffff, data); } EventPort& port = SPUPs[spup]; std::lock_guard<std::mutex> lock(port.m_mutex); if (!port.eq) { LOG_WARNING(Log::SPU, "sys_spu_thread_send_event(spup=%d, data0=0x%x, data1=0x%x): event queue not connected", spup, (v & 0x00ffffff), data); SPU.In_MBox.PushUncond(CELL_ENOTCONN); // TODO: check error passing return; } if (!port.eq->events.push(SYS_SPU_THREAD_EVENT_USER_KEY, GetCurrentCPUThread()->GetId(), ((u64)spup << 32) | (v & 0x00ffffff), data)) { SPU.In_MBox.PushUncond(CELL_EBUSY); return; } SPU.In_MBox.PushUncond(CELL_OK); return; } else if (code < 128) { /* ===== sys_spu_thread_throw_event ===== */ const u8 spup = code & 63; u32 data; if (!SPU.Out_MBox.Pop(data)) { LOG_ERROR(Log::SPU, "sys_spu_thread_throw_event(v=0x%x, spup=%d): Out_MBox is empty", v, spup); return; } //if (Ini.HLELogging.GetValue()) { LOG_WARNING(Log::SPU, "sys_spu_thread_throw_event(spup=%d, data0=0x%x, data1=0x%x)", spup, v & 0x00ffffff, data); } EventPort& port = SPUPs[spup]; std::lock_guard<std::mutex> lock(port.m_mutex); if (!port.eq) { LOG_WARNING(Log::SPU, "sys_spu_thread_throw_event(spup=%d, data0=0x%x, data1=0x%x): event queue not connected", spup, (v & 0x00ffffff), data); return; } // TODO: check passing spup value if (!port.eq->events.push(SYS_SPU_THREAD_EVENT_USER_KEY, GetCurrentCPUThread()->GetId(), ((u64)spup << 32) | (v & 0x00ffffff), data)) { LOG_WARNING(Log::SPU, "sys_spu_thread_throw_event(spup=%d, data0=0x%x, data1=0x%x) failed (queue is full)", spup, (v & 0x00ffffff), data); return; } return; } else if (code == 128) { /* ===== sys_event_flag_set_bit ===== */ u32 flag = v & 0xffffff; u32 data; if (!SPU.Out_MBox.Pop(data)) { LOG_ERROR(Log::SPU, "sys_event_flag_set_bit(v=0x%x (flag=%d)): Out_MBox is empty", v, flag); return; } if (flag > 63) { LOG_ERROR(Log::SPU, "sys_event_flag_set_bit(id=%d, v=0x%x): flag > 63", data, v, flag); return; } //if (Ini.HLELogging.GetValue()) { LOG_WARNING(Log::SPU, "sys_event_flag_set_bit(id=%d, v=0x%x (flag=%d))", data, v, flag); } EventFlag* ef; if (!Emu.GetIdManager().GetIDData(data, ef)) { LOG_ERROR(Log::SPU, "sys_event_flag_set_bit(id=%d, v=0x%x (flag=%d)): EventFlag not found", data, v, flag); SPU.In_MBox.PushUncond(CELL_ESRCH); return; } u32 tid = GetCurrentCPUThread()->GetId(); ef->m_mutex.lock(tid); ef->flags |= (u64)1 << flag; if (u32 target = ef->check()) { // if signal, leave both mutexes locked... ef->signal.lock(target); ef->m_mutex.unlock(tid, target); } else { ef->m_mutex.unlock(tid); } SPU.In_MBox.PushUncond(CELL_OK); return; } else if (code == 192) { /* ===== sys_event_flag_set_bit_impatient ===== */ u32 flag = v & 0xffffff; u32 data; if (!SPU.Out_MBox.Pop(data)) { LOG_ERROR(Log::SPU, "sys_event_flag_set_bit_impatient(v=0x%x (flag=%d)): Out_MBox is empty", v, flag); return; } if (flag > 63) { LOG_ERROR(Log::SPU, "sys_event_flag_set_bit_impatient(id=%d, v=0x%x): flag > 63", data, v, flag); return; } //if (Ini.HLELogging.GetValue()) { LOG_WARNING(Log::SPU, "sys_event_flag_set_bit_impatient(id=%d, v=0x%x (flag=%d))", data, v, flag); } EventFlag* ef; if (!Emu.GetIdManager().GetIDData(data, ef)) { LOG_WARNING(Log::SPU, "sys_event_flag_set_bit_impatient(id=%d, v=0x%x (flag=%d)): EventFlag not found", data, v, flag); return; } u32 tid = GetCurrentCPUThread()->GetId(); ef->m_mutex.lock(tid); ef->flags |= (u64)1 << flag; if (u32 target = ef->check()) { // if signal, leave both mutexes locked... ef->signal.lock(target); ef->m_mutex.unlock(tid, target); } else { ef->m_mutex.unlock(tid); } return; } else { u32 data; if (SPU.Out_MBox.Pop(data)) { LOG_ERROR(Log::SPU, "SPU_WrOutIntrMbox: unknown data (v=0x%x); Out_MBox = 0x%x", v, data); } else { LOG_ERROR(Log::SPU, "SPU_WrOutIntrMbox: unknown data (v=0x%x)", v); } SPU.In_MBox.PushUncond(CELL_EINVAL); // ??? return; } } break; } case SPU_WrOutMbox: { while (!SPU.Out_MBox.Push(v) && !Emu.IsStopped()) std::this_thread::sleep_for(std::chrono::milliseconds(1)); break; } case MFC_WrTagMask: { MFC1.QueryMask.SetValue(v); break; } case MFC_WrTagUpdate: { MFC1.TagStatus.PushUncond(MFC1.QueryMask.GetValue()); break; } case MFC_LSA: { MFC1.LSA.SetValue(v); break; } case MFC_EAH: { MFC1.EAH.SetValue(v); break; } case MFC_EAL: { MFC1.EAL.SetValue(v); break; } case MFC_Size: { MFC1.Size_Tag.SetValue((MFC1.Size_Tag.GetValue() & 0xffff) | (v << 16)); break; } case MFC_TagID: { MFC1.Size_Tag.SetValue((MFC1.Size_Tag.GetValue() & ~0xffff) | (v & 0xffff)); break; } case MFC_Cmd: { MFC1.CMDStatus.SetValue(v); EnqMfcCmd(MFC1); break; } case MFC_WrListStallAck: { if (v >= 32) { LOG_ERROR(Log::SPU, "MFC_WrListStallAck error: invalid tag(%d)", v); return; } StalledList temp = StallList[v]; if (!temp.MFCArgs) { LOG_ERROR(Log::SPU, "MFC_WrListStallAck error: empty tag(%d)", v); return; } StallList[v].MFCArgs = nullptr; ListCmd(temp.lsa, temp.ea, temp.tag, temp.size, temp.cmd, *temp.MFCArgs); break; } case SPU_WrDec: { m_dec_start = get_time(); m_dec_value = v; break; } case SPU_WrEventMask: { m_event_mask = v; if (v & ~(SPU_EVENT_IMPLEMENTED)) LOG_ERROR(Log::SPU, "SPU_WrEventMask: unsupported event masked (0x%x)"); break; } case SPU_WrEventAck: { m_events &= ~v; break; } default: { LOG_ERROR(Log::SPU, "%s error (v=0x%x): unknown/illegal channel (%d [%s]).", __FUNCTION__, v, ch, spu_ch_name[ch]); break; } } if (Emu.IsStopped()) LOG_WARNING(Log::SPU, "%s(%s) aborted", __FUNCTION__, spu_ch_name[ch]); }
int sys_event_flag_wait(u32 eflag_id, u64 bitptn, u32 mode, mem64_t result, u64 timeout) { sys_event_flag.Warning("sys_event_flag_wait(eflag_id=%d, bitptn=0x%llx, mode=0x%x, result_addr=0x%x, timeout=%lld)", eflag_id, bitptn, mode, result.GetAddr(), timeout); if (result.IsGood()) result = 0; switch (mode & 0xf) { case SYS_EVENT_FLAG_WAIT_AND: break; case SYS_EVENT_FLAG_WAIT_OR: break; default: return CELL_EINVAL; } switch (mode & ~0xf) { case 0: break; // ??? case SYS_EVENT_FLAG_WAIT_CLEAR: break; case SYS_EVENT_FLAG_WAIT_CLEAR_ALL: break; default: return CELL_EINVAL; } EventFlag* ef; if(!sys_event_flag.CheckId(eflag_id, ef)) return CELL_ESRCH; u32 tid = GetCurrentPPUThread().GetId(); { SMutexLocker lock(ef->m_mutex); if (ef->m_type == SYS_SYNC_WAITER_SINGLE && ef->waiters.GetCount() > 0) { return CELL_EPERM; } EventFlagWaiter rec; rec.bitptn = bitptn; rec.mode = mode; rec.tid = tid; ef->waiters.AddCpy(rec); if (ef->check() == tid) { u64 flags = ef->flags; ef->waiters.RemoveAt(ef->waiters.GetCount() - 1); if (mode & SYS_EVENT_FLAG_WAIT_CLEAR) { ef->flags &= ~bitptn; } else if (mode & SYS_EVENT_FLAG_WAIT_CLEAR_ALL) { ef->flags = 0; } if (result.IsGood()) { result = flags; return CELL_OK; } if (!result.GetAddr()) { return CELL_OK; } return CELL_EFAULT; } } u32 counter = 0; const u32 max_counter = timeout ? (timeout / 1000) : ~0; while (true) { if (ef->signal.GetOwner() == tid) { SMutexLocker lock(ef->m_mutex); ef->signal.unlock(tid); u64 flags = ef->flags; for (u32 i = 0; i < ef->waiters.GetCount(); i++) { if (ef->waiters[i].tid == tid) { ef->waiters.RemoveAt(i); if (mode & SYS_EVENT_FLAG_WAIT_CLEAR) { ef->flags &= ~bitptn; } else if (mode & SYS_EVENT_FLAG_WAIT_CLEAR_ALL) { ef->flags = 0; } if (result.IsGood()) { result = flags; return CELL_OK; } if (!result.GetAddr()) { return CELL_OK; } return CELL_EFAULT; } } return CELL_ECANCELED; } Sleep(1); if (counter++ > max_counter) { SMutexLocker lock(ef->m_mutex); for (u32 i = 0; i < ef->waiters.GetCount(); i++) { if (ef->waiters[i].tid == tid) { ef->waiters.RemoveAt(i); break; } } return CELL_ETIMEDOUT; } if (Emu.IsStopped()) { ConLog.Warning("sys_event_flag_wait(id=%d) aborted", eflag_id); return CELL_OK; } } }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd ){ bool ret = true; switch (local_status_) { case TIMER_RUN: // タイムアップ待ち if (is_state_move_) { is_state_move_ = false; tec_pid_.Init(); timer_start(2000); } cmd = tec_pid_.Calclate(ri.light_sensor_val, speed_, edge_); cmd.param3 = 0; if (is_timeup()) { is_state_move_ = true; local_status_ = UNTIL_MARKER; Robot::Instance().Beep(Robot::C); } break; case UNTIL_MARKER: // マーカー検知待ち if (is_state_move_) { is_state_move_ = false; tec_pid_.Init(); } cmd = tec_pid_.Calclate(ri.light_sensor_val, speed_, edge_); cmd.param3 = 0; if (evf.is_SetEvent(Robot::EV_MARKER)) { is_state_move_ = true; speed_ = 0; local_status_ = GARAGE_IN; Robot::Instance().Beep(Robot::D); } break; case GARAGE_IN: // ガレージ入庫 if (is_state_move_) { is_state_move_ = false; tec_s_.Init(ri); timer_start(1000); } speed_ = 15; cmd = tec_s_.Calclate(speed_, ri.rot_encoderL_val, ri.rot_encoderR_val); cmd.param3 = 100; // しっぽ下ろす if(is_timeup()){ is_state_move_ = true; local_status_ = STOP1; Robot::Instance().Beep(Robot::E); } break; case STOP1: // ガレージ停車(少し後進) if (is_state_move_) { is_state_move_ = false; tec_s_.Init(ri); timer_start(500); } speed_ = -15; cmd = tec_s_.Calclate(speed_, ri.rot_encoderL_val, ri.rot_encoderR_val); cmd.param3 = 100; if(is_timeup()){ is_state_move_ = true; local_status_ = STOP2; Robot::Instance().Beep(Robot::F); } break; case STOP2: // ガレージ停車(完全停止) cmd.Mode = RobotCmd::DIRECT_MODE; cmd.param1 = 0; cmd.param2 = 0; cmd.param3 = 100; break; default: ret = false; break; } return ret; }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd ){ bool ret = true; static int count = 0; static bool flag = false; static S32 lmotor,rmotor; switch(local_status_){ // スタート時はスピードを抑え目にして安定した走りだしをする case P_START: /* 走行計算 */ cmd = pid_trace_.Calclate(ri.light_sensor_val, (S16)(speed_), edge_); cmd.param3 = 0; /* 終了条件 */ if (IsTimeup()) { Robot::Instance().Beep(); local_status_ = P_STRAIGHT_BEFORE; TimerStop(); } break; // 直線走行 case P_STRAIGHT_BEFORE: /* 走行計算 */ cmd = pid_trace_.Calclate(ri.light_sensor_val, (S16)(speed_), edge_); if(Robot::Instance().sonar_distance < 30 && flag == false){ count++; }else { count = 0; } if(count > 3){ Robot::Instance().Beep(); count = 0; flag = true; TimerStart(800); } /* 終了条件*/ if(IsTimeup()){ Robot::Instance().Beep(); local_status_ = P_STOP; /* 初期化 */ flag = false; dist.x = ni.pos.X; dist.y = ni.pos.Y; dist.xy = (dist.x * dist.x) + (dist.y * dist.y); TimerStart(1000); } break; // 停止状態 case P_STOP: /* 走行計算 */ cmd = pid_trace_.Calclate(ri.light_sensor_val, (S16)(0), edge_); cmd.param3 = 90; /* 終了条件 */ if (IsTimeup()) { Robot::Instance().Beep(); local_status_ = P_LOOKUP; /* 初期化 */ dist.x = ni.pos.X; dist.y = ni.pos.Y; dist.xy = (dist.x * dist.x) + (dist.y * dist.y); TimerStart(100); Robot::Instance().SetGyroOffset(Robot::Instance().GetGyroOffset() - 20); } break; // LOOKUP case P_LOOKUP: /* 走行計算 */ cmd = pid_trace_.Calclate(ri.light_sensor_val, (S16)(0), edge_); cmd.param3 = 90; /* 終了条件 */ if (IsTimeup()) { Robot::Instance().Beep(); local_status_ = P_STOP_LOOKUP; /* 初期化 */ dist.x = ni.pos.X; dist.y = ni.pos.Y; dist.xy = (dist.x * dist.x) + (dist.y * dist.y); TimerStart(100); } break; //斜め停止 case P_STOP_LOOKUP: /* 走行計算 */ static int temptail = 90; cmd.Mode = RobotCmd::DIRECT_MODE; cmd.param1 = 0; cmd.param2 = 0; cmd.param3 = temptail; if (IsTimeup()) { temptail--; TimerStart(100); } /* 終了条件 */ if(temptail == 70){ local_status_ = P_STRAIGHT_LOOKUP; /* 初期化 */ temptail = 90; TimerStart(1000); } break; // 斜め停止2 case P_STOP_LOOKUP2: /* 走行計算 */ cmd.Mode = RobotCmd::DIRECT_MODE; cmd.param1 = 0; cmd.param2 = 0; cmd.param3 = 70; /* 終了条件 */ if (IsTimeup()) { local_status_ = P_STRAIGHT_LOOKUP; Robot::Instance().Beep(); } break; // 直線走行(lookup) case P_STRAIGHT_LOOKUP: /* 走行計算 */ cmd.Mode = RobotCmd::DIRECT_MODE; cmd.param1 = 37; cmd.param2 = 30; cmd.param3 = 68; /* 終了条件 */ if(((ni.pos.X - dist.x) * (ni.pos.X - dist.x) +( ni.pos.Y - dist.y) * ( ni.pos.Y - dist.y)) > DIST_LOOKUP){ Robot::Instance().Beep(); local_status_ = P_BACK; /* 初期化 */ Robot::Instance().GetMotorEncoder(lmotor,rmotor); TimerStart(2000); } break; // BACK case P_BACK: /* 走行計算 */ cmd.Mode = RobotCmd::DIRECT_MODE; cmd.param1 = -10; cmd.param2 = -10; cmd.param3 = 105; /* 終了条件 */ S32 lmotor2,rmotor2; Robot::Instance().GetMotorEncoder(lmotor2,rmotor2); if( (lmotor - lmotor2 > LOOKUPENCODER) ||( rmotor - rmotor2 > LOOKUPENCODER)){ Robot::Instance().Beep(); local_status_ = P_STRAIGHT; /* 初期化 */ Robot::Instance().SetGyroOffset(Robot::Instance().GetGyroOffset() + 20); Robot::Instance().ResetBalanceAPI(); Navigator::Instance().ResetEncoder(); TimerStart(5000); speed_ = 20; pid_trace_.Init(); } break; // STAND UP case P_STANDUP: /* 走行計算 */ cmd.Mode = RobotCmd::DIRECT_MODE; cmd.param1 = 0; cmd.param2 = 0; cmd.param3 = 108; /* 終了条件 */ if (IsTimeup()) { local_status_ = P_STRAIGHT; /* 初期化 */ Robot::Instance().SetGyroOffset(Robot::Instance().GetGyroOffset() + 20); Robot::Instance().ResetBalanceAPI(); Navigator::Instance().ResetEncoder(); speed_ = 10; tec_r_.Init(); } break; case P_STRAIGHT: /* 走行計算 */ cmd = tec_r_.Calclate(speed_, -5, edge_); cmd.param3 = 0; /* 終了条件 */ if( evf.is_SetEvent(Robot::EVENT_LIGHT_BLACK) ){ local_status_ = P_STRAIGHT2; ret = false; /* 初期化 */ speed_ = 10; TimerStart(3000); pid_trace_.Init(); } break; case P_STRAIGHT2: /* 走行計算 */ cmd = pid_trace_.Calclate(ri.light_sensor_val, (S16)(speed_), edge_); cmd.param3 = 0; /* 終了条件 */ if (IsTimeup()) { ret = false; /* 初期化 */ speed_ = 50; TimerStop(); } break; default: ret = false; } return ret; }