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;
}
