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