void noteProducerFinished(IRowStream * stream)
{
waitForStop();
if (stream)
stream->stop();
notifyStopped();
}
void DebuggerIPCServer::handleContinue(double time_ns) {
if (time_ns != -1) {
double current_sim_time = data_manager->getSimulationTime();
double break_time = current_sim_time + time_ns;
Breakpoint bkpt(true);
bkpt.temp = true;
bkpt.addCondition(Breakpoint::BreakpointCondition::BREAK_AT_TIME,
std::to_string(break_time));
data_manager->addBreakpoint(bkpt);
}
std::unique_lock<std::mutex> lock(bkpt_mutex);
continue_command = true;
lock.unlock();
bkpt_cv.notify_all();
waitForStop();
DebuggerMessage *reply;
if (reply_message == NULL) {
reply = new GenericAcknowledgeMessage(
PFPSimDebugger::GenericAcknowledgeMsg_Status_SUCCESS);
} else {
reply = reply_message;
}
send(reply);
delete reply;
}
bool MaemoSshRunner::runInternal()
{
createConnection();
connect(m_connection.data(), SIGNAL(remoteOutput(QByteArray)),
this, SLOT(handleRemoteOutput(QByteArray)));
initState();
if (!m_connection->start())
return false;
if (stopRequested())
return true;
waitForStop();
return !m_connection->hasError();
}
void turn90(int degrees)
{
nMotorEncoder[rightDrive] = 0;
nMotorEncoder[leftDrive] = 0;
nMotorEncoderTarget[rightDrive] = -degrees*encoderTicksPerDegree;
nMotorEncoderTarget[leftDrive] = degrees*encoderTicksPerDegree;
if (degrees < 0)
{
motor[rightDrive] = 90;
motor[leftDrive] = -90;
}
else
{
motor[rightDrive] = -90;
motor[leftDrive] = 90;
}
waitForStop();
nMotorEncoder[rightDrive] = 0;
nMotorEncoder[leftDrive] = 0;
}
void DebuggerIPCServer::handleRun(double time_ns) {
if (time_ns != -1) {
double current_sim_time = data_manager->getSimulationTime();
double break_time = current_sim_time + time_ns;
Breakpoint bkpt(true);
bkpt.temp = true;
bkpt.addCondition(Breakpoint::BreakpointCondition::BREAK_AT_TIME,
std::to_string(break_time));
data_manager->addBreakpoint(bkpt);
}
ulock->unlock(); // release mutex so that program can start
waitForStop();
DebuggerMessage *reply;
if (reply_message == NULL) {
reply = new GenericAcknowledgeMessage(
PFPSimDebugger::GenericAcknowledgeMsg_Status_SUCCESS);
} else {
reply = reply_message;
}
send(reply);
delete reply;
}
virtual void noteStrandFinished(IRowStream * stream)
{
waitForStop();
if (stream)
stream->stop();
}
int moveToLocation(int trainNum, int source, int dest, track_node *track, int doReverse, int speed, int *velocity) {
struct Path path;
struct VelocityProfile profile;
BFS(source, dest, track, &path, doReverse);
if((path.node[path.numNodes-2])->reverse == path.node[path.numNodes-1]) {
path.numNodes--;
}
int periodic = Create(2, periodicTask);
initProfile(&profile, trainNum, speed, &path, source, periodic, velocity);
int curNode = 1;
profile.location = curNode-1;
int timeout = false;
int switchNode, switchDistance;
Putc2(1, (char)speed, (char)trainNum);
setAccelerating(&profile);
while(curNode < path.numNodes) {
if((path.node[curNode-1])->reverse == path.node[curNode]) {
waitForStop(&profile);
if(curNode != 1) {
profile.displayLocation = curNode;
profile.delta = -300000;
}
Putc2(1, (char)15, (char)trainNum);
Putc2(1, (char)speed, (char)trainNum);
setAccelerating(&profile);
}
profile.reverseNode = findNextReverseNode(&path, curNode);
int offset = 0;
while(curNode+offset < path.numNodes) {
switchNode = distanceBefore(&path, (profile.velocity)[speed]*50, curNode+offset, &switchDistance);
if((path.node[curNode+offset])->type == NODE_BRANCH && switchNode == curNode-1) {
if(switchDistance >= 0) {
waitForDistance(&profile, switchDistance);
}
if(adjDirection(path.node[curNode+offset], path.node[curNode+offset+1]) == DIR_STRAIGHT) {
setSwitchState((path.node[curNode+offset])->num, 'S');
}else{
setSwitchState((path.node[curNode+offset])->num, 'C');
}
}else if(switchNode > curNode-1) {
break;
}
offset++;
}
if(curNode == path.numNodes-1) break;
int distance = adjDistance(path.node[curNode-1], path.node[curNode]);
if(curNode == 1 && (path.node[0])->reverse == path.node[1]) {
}else if((path.node[curNode])->type == NODE_SENSOR) {
int reply;
int sensorNum = (path.node[curNode])->num;
int sensorTask = Create(2, sensorWaitTask);
Send(sensorTask, (char *)&sensorNum, sizeof(int), (char *)&reply, sizeof(int));
int src = waitForDistance(&profile, distance+150000);
if(src == periodic) {
printf("timeout at sensor %s\r", (path.node[curNode])->name);
timeout = true;
}else{
float err = profile.delta/1000 - distance/1000;
printf("distance error at node %s: %dmm\r", (path.node[curNode])->name, (int)err);
}
Destroy(sensorTask);
}else{
waitForDistance(&profile, distance);
}
setLocation(&profile, curNode);
curNode++;
if(timeout) {
profile.delta = 150000;
timeout = false;
}
}
int reply;
int sensorNum = (path.node[curNode])->num;
int sensorTask = Create(2, sensorWaitTask);
Send(sensorTask, (char *)&sensorNum, sizeof(int), (char *)&reply, sizeof(int));
int distance = adjDistance(path.node[curNode-1], path.node[curNode]);
int src = waitForDistanceOrStop(&profile, distance);
if(src == periodic) {
printf("timeout at sensor %s\r", (path.node[curNode])->name);
}else{
float err = profile.delta/1000 - distance/1000;
printf("distance error at node %s: %dmm\r", (path.node[curNode])->name, (int)err);
}
Destroy(sensorTask);
setLocation(&profile, path.numNodes-1);
waitForStop(&profile);
Destroy(periodic);
int curLocation = 0;
while(curLocation < TRACK_MAX && strcmp(track[curLocation].name,
(path.node[path.numNodes-1])->name) != 0) curLocation++;
return curLocation;
}
