//**************************************************************************** // Core() IS THE MAIN EXECUTION LOOP FUNCTION. // THE ONE AND ONLY BLOCKING CALL IN HERE SHALL BE THE StartReading function. //**************************************************************************** void MAPSPilote::Core() { //Without specification of reading policies, let's implement the most generic case: //an asynchronous blocking read on all the inputs. int input_that_answered; MAPSIOElt* ioelt_in = StartReading(_nb_inputs, _inputs, &input_that_answered); if (ioelt_in == NULL) return; MAPSTimestamp timestamp_in = ioelt_in->Timestamp(); switch (input_that_answered) { case 0: //We received an element from port input. { int count = ioelt_in->VectorSize() / sizeof(Zone); Zone* data_in = (Zone*)ioelt_in->Data(); Zone_Received_on_input_InPort(data_in,count, ioelt_in->Timestamp()); } break; default: Error("Unknown input."); } // Start of user code Core processing // End of user code }
void MAPSJoin::ReadInputs() { MAPSInput* inputs[4] = { &Input("LaserObject"), &Input("CameraObject"),&Input("MatchedLaser"), &Input("MatchedCamera") }; int inputThatAnswered; MAPSIOElt* ioeltin = StartReading(4, inputs, &inputThatAnswered); if (ioeltin == NULL) { return; } switch (inputThatAnswered) { case 0: input_Laser_Objects = static_cast<AUTO_Objects*>(ioeltin->Data()); ArrayLaserObjects = *input_Laser_Objects; readed[0] = true; break; case 1: input_Camera_Objects = static_cast<AUTO_Objects*>(ioeltin->Data()); ArrayCameraObjects = *input_Camera_Objects; readed[1] = true; break; case 2: input_Laser_Matched = static_cast<MATCH_OBJECTS*>(ioeltin->Data()); Laser_Matched = *input_Laser_Matched; readed[2] = true; break; case 3: input_Camera_Matched = static_cast<MATCH_OBJECTS*>(ioeltin->Data()); Camera_Matched = *input_Camera_Matched; readed[3] = true; break; default: break; } }
void MAPSwifibot_inputs_robotml_to_rtmaps::Core() { MAPSIOElt* ioeltin = StartReading(Input(0)); if (ioeltin == NULL) return; Speed_Tics_Left_Right* data_in = (Speed_Tics_Left_Right*)ioeltin->Data(); MAPSIOElt* ioeltout = StartWriting(Output(0)); ioeltout->Integer32(0) = data_in->speed_tics_left.value; ioeltout->Integer32(1) = data_in->speed_tics_right.value; ioeltout->Timestamp() = ioeltin->Timestamp(); StopWriting(ioeltout); }
void MAPSPredictor::readInputs() { //We read the inputs with the reactive metod MAPSInput* inputs[2] = { &Input("LaserObject"), &Input("CameraObject") }; int inputThatAnswered; MAPSIOElt* ioeltin = StartReading(2, inputs, &inputThatAnswered); if (ioeltin == NULL) { return; } switch (inputThatAnswered) { case 0: ArrayLaserObjects = static_cast<AUTO_Objects*>(ioeltin->Data()); LaserObjects[0] = LaserObjects[1]; LaserObjects[1] = *ArrayLaserObjects; updated[0] = true; if (!ready) { completedL[0] = completedL[1]; completedL[1] = true; } if (firsttime) { timestamp = ArrayLaserObjects->timestamp; firsttime = false; } break; case 1: ArrayCameraObjects = static_cast<AUTO_Objects*>(ioeltin->Data()); CameraObjects[0] = CameraObjects[1]; CameraObjects[1] = *ArrayCameraObjects; updated[1] = true; if (!ready) { completedC[0] = completedC[1]; completedC[1] = true; } if (firsttime) { timestamp = ArrayCameraObjects->timestamp; firsttime = false; } break; default: break; } }
void cdrWrite1(u8 rt) { int i; CDVD_LOG("CD1 write: %x (%s)", rt, CmdName[rt]); cdr.Cmd = rt; cdr.OCUP = 0; #ifdef CDRCMD_DEBUG SysPrintf("CD1 write: %x (%s)", rt, CmdName[rt]); if (cdr.ParamC) { SysPrintf(" Param[%d] = {", cdr.ParamC); for (i=0;i<cdr.ParamC;i++) SysPrintf(" %x,", cdr.Param[i]); SysPrintf("}\n"); } else SysPrintf("\n"); #endif if (cdr.Ctrl & 0x1) return; switch(cdr.Cmd) { case CdlSync: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlNop: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSetloc: StopReading(); for (i=0; i<3; i++) cdr.SetSector[i] = btoi(cdr.Param[i]); cdr.SetSector[3] = 0; if ((cdr.SetSector[0] | cdr.SetSector[1] | cdr.SetSector[2]) == 0) { *(u32 *)cdr.SetSector = *(u32 *)cdr.SetSectorSeek; } cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlPlay: cdr.Play = 1; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlForward: if (cdr.CurTrack < 0xaa) cdr.CurTrack++; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlBackward: if (cdr.CurTrack > 1) cdr.CurTrack--; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlReadN: cdr.Irq = 0; StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; StartReading(1); break; case CdlStandby: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlStop: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlPause: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x40000); break; case CdlReset: case CdlInit: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlMute: cdr.Muted = 0; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlDemute: cdr.Muted = 1; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSetfilter: cdr.File = cdr.Param[0]; cdr.Channel = cdr.Param[1]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSetmode: CDVD_LOG("Setmode %x", cdr.Param[0]); cdr.Mode = cdr.Param[0]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetmode: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetlocL: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetlocP: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetTN: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetTD: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSeekL: ((u32 *)cdr.SetSectorSeek)[0] = ((u32 *)cdr.SetSector)[0]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSeekP: ((u32 *)cdr.SetSectorSeek)[0] = ((u32 *)cdr.SetSector)[0]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlTest: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlID: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlReadS: cdr.Irq = 0; StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; StartReading(2); break; case CdlReadToc: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; default: CDVD_LOG("Unknown Cmd: %x\n", cdr.Cmd); return; } if (cdr.Stat != NoIntr) iopIntcIrq( 2 ); }
void cdrWrite1(unsigned char rt) { int i; #ifdef CDR_LOG CDR_LOG("cdrWrite1() Log: CD1 write: %x (%s)\n", rt, CmdName[rt]); #endif // psxHu8(0x1801) = rt; cdr.Cmd = rt; cdr.OCUP = 0; #ifdef CDRCMD_DEBUG SysPrintf("cdrWrite1() Log: CD1 write: %x (%s)", rt, CmdName[rt]); if (cdr.ParamC) { SysPrintf(" Param[%d] = {", cdr.ParamC); for (i=0;i<cdr.ParamC;i++) SysPrintf(" %x,", cdr.Param[i]); SysPrintf("}\n"); } else SysPrintf("\n"); #endif if (cdr.Ctrl & 0x1) return; switch(cdr.Cmd) { case CdlSync: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlNop: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSetloc: StopReading(); cdr.Seeked = 0; for (i=0; i<3; i++) cdr.SetSector[i] = btoi(cdr.Param[i]); cdr.SetSector[3] = 0; /* if ((cdr.SetSector[0] | cdr.SetSector[1] | cdr.SetSector[2]) == 0) { *(u32 *)cdr.SetSector = *(u32 *)cdr.SetSectorSeek; }*/ cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlPlay: if (!cdr.SetSector[0] && !cdr.SetSector[1] && !cdr.SetSector[2]) { if (CDR_getTN(cdr.ResultTN) != -1) { if (cdr.CurTrack > cdr.ResultTN[1]) cdr.CurTrack = cdr.ResultTN[1]; if (CDR_getTD((unsigned char)(cdr.CurTrack), cdr.ResultTD) != -1) { int tmp = cdr.ResultTD[2]; cdr.ResultTD[2] = cdr.ResultTD[0]; cdr.ResultTD[0] = tmp; if (!Config.Cdda) CDR_play(cdr.ResultTD); } } } else if (!Config.Cdda) CDR_play(cdr.SetSector); cdr.Play = 1; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlForward: if (cdr.CurTrack < 0xaa) cdr.CurTrack++; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlBackward: if (cdr.CurTrack > 1) cdr.CurTrack--; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlReadN: cdr.Irq = 0; StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; StartReading(1); break; case CdlStandby: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlStop: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlPause: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x40000); break; case CdlReset: case CdlInit: StopCdda(); StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlMute: cdr.Muted = 0; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlDemute: cdr.Muted = 1; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSetfilter: cdr.File = cdr.Param[0]; cdr.Channel = cdr.Param[1]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSetmode: #ifdef CDR_LOG CDR_LOG("cdrWrite1() Log: Setmode %x\n", cdr.Param[0]); #endif cdr.Mode = cdr.Param[0]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetmode: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetlocL: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetlocP: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetTN: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlGetTD: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSeekL: // ((u32 *)cdr.SetSectorSeek)[0] = ((u32 *)cdr.SetSector)[0]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlSeekP: // ((u32 *)cdr.SetSectorSeek)[0] = ((u32 *)cdr.SetSector)[0]; cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlTest: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlID: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; case CdlReadS: cdr.Irq = 0; StopReading(); cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; StartReading(2); break; case CdlReadToc: cdr.Ctrl|= 0x80; cdr.Stat = NoIntr; AddIrqQueue(cdr.Cmd, 0x800); break; default: #ifdef CDR_LOG CDR_LOG("cdrWrite1() Log: Unknown command: %x\n", cdr.Cmd); #endif return; } if (cdr.Stat != NoIntr) { psxHu32ref(0x1070)|= SWAP32((u32)0x4); psxRegs.interrupt|= 0x80000000; } }