void main() {
configure();
setPID();
sendText("Starting...");
//while(1)
// checkSonar('L');
/*********************** Starting Point ***************************/
LEVEL1_STATE = NOT_REACHED;
sendText("Starting Block ");
LEVEL = 0;
while(isAllWhite())
moveStraightSlow();
LEVEL = 1;
sendText("Level 1 Starting ");
LEVEL1_STATE = REACHED;
stop();
/*********************** Level 1 Starting ***************************/
moveForward(205,200);
delay_ms(100);
while(!isAllBlack()){
//lineFollowNormalWorked();
if( Scout==1 &&(Sensor3==1 || Sensor4==1 || Sensor5==1 || Sensor6==1 || Sensor7==1) && Sensor1==0 && Sensor2==0 && Sensor8==0 && Sensor9==0)
lineFollowPID();
else
lineFollowNormalWorked();
}
stop();
sendText(" End of Level 1. Level 2 Starting ");
LEVEL1_STATE = COMPLETED;
LEVEL2_STATE = REACHED;
LEVEL = 2;
/*********************** Level 2 Starting ***************************/
while(!isAllWhite()){
goThroughObstaclesJay();
}
stop();
sendText(" End of Level 2. Level 3 Starting ");
LEVEL2_STATE = COMPLETED;
LEVEL3_STATE = REACHED;
LEVEL = 3;
/*********************** Level 3 Starting ***************************/
level3Aligning();
while(!isAllBlack()){
stop();
}
/*********************** Complete ***************************/
}
void Group::masterStartProcesses()
{
try {
std::unique_lock<std::mutex> map_lock(_process_map_mutex);
for (auto& process_entry : _process_by_name) {
auto process = process_entry.second;
pid_t pid = process->spawn(_process_argc, _process_argv);
process->setPID(pid);
_process_by_pid[pid] = process;
int rfd = -1, wfd = -1;
process->getMessageBusFDs(/*rfd_ptr=*/&rfd, /*wfd_ptr=*/&wfd);
if (rfd == -1 || wfd == -1) {
throw PGL_BUG("Got invalid message bus fds after process spawn");
}
_process_by_fd[rfd] = process;
_process_by_fd[wfd] = process;
}
}
catch(const std::exception& ex) {
masterStopProcesses();
}
}
MotorController::MotorController(ros::NodeHandle* nodehandle) : nh_(*nodehandle)
{
// initialize private node handle
ros::NodeHandle pNh("~");
// initialize subscriber to /motors topic
cmd_sub_ = nh_.subscribe("/motors", 1, &MotorController::cmdCallback, this);
// initialize publishers to publish mbed stats
enc_pub_ = nh_.advertise<igvc_msgs::velocity_pair>("/encoders", 1000);
enabled_pub_ = nh_.advertise<std_msgs::Bool>("/robot_enabled", 1);
battery_pub_ = nh_.advertise<std_msgs::Float64>("/battery", 1);
// get server ip address and port number from the launch file
igvc::getParam(pNh, std::string("ip_addr"), ip_addr_);
igvc::getParam(pNh, std::string("port"), tcpport_);
ROS_INFO_STREAM("Connecting to server:"
<< "\n\tIP: " << ip_addr_ << "\n\tPort: " << std::to_string(tcpport_));
sock_ = igvc::make_unique<EthernetSocket>(ip_addr_, tcpport_);
ROS_INFO_STREAM("Using Boost " << (*sock_).getBoostVersion());
ROS_INFO_STREAM("Successfully Connected to TCP Host:"
<< "\n\tIP: " << (*sock_).getIP() << "\n\tPort: " << (*sock_).getPort());
igvc::getParam(pNh, std::string("battery_alpha"), battery_alpha_);
igvc::getParam(pNh, std::string("min_battery_voltage"), min_battery_voltage_);
battery_avg_ = min_battery_voltage_;
// PID variables
igvc::getParam(pNh, std::string("p_l"), p_l_);
igvc::getParam(pNh, std::string("p_r"), p_r_);
igvc::getParam(pNh, std::string("d_l"), d_l_);
igvc::getParam(pNh, std::string("d_r"), d_r_);
igvc::getParam(pNh, std::string("i_r"), i_r_);
igvc::getParam(pNh, std::string("i_l"), i_l_);
igvc::getParam(pNh, "kv_l", kv_l_);
igvc::getParam(pNh, "kv_r", kv_r_);
igvc::param(pNh, "log_period", log_period_, 5.0);
// communication frequency
igvc::getParam(pNh, std::string("frequency"), frequency_);
ros::Rate rate(frequency_);
setPID(); // Set PID Values on mbed
// send motor commands
while (ros::ok())
{
sendRequest();
recieveResponse();
ros::spinOnce();
rate.sleep();
}
}
void FanControl::start() {
// Setting Menu
int input = 1;
// Motor Test ///////////////////////
cout << "[Test Motors - Speed at 1200]\n";
while(1) {
setMotorSpeedAll(1200);
sleep(1);
setMotorSpeedAll(1);
cout << "\n(1).Again?\n(ANY).Next Step \n";
cin >> input;
cout << endl;
if ( input != 1 ) break;
}
input = 0;
// Pid Setting ///////////////////////
cout << "\n\t[Do you want to set PID values?]\n(Enter 1, if Yes.\nEnter any to use previous values\n";
cin >> input;
if ( input == 1)
setPID();
else {
cout << "Restoring Setting ...\n";
try {
restoreConfig();
} catch (int e) {
if ( e == 1) {
cout << "\nPrevious Configuration Does not exist!\n";
} else{
cout << "\nWarning: /config/config.txt is Broken\n";
}
}
}
system("clear");
cout << "\n\t Current Setting is ... \n";
displaySetting();
cout << "\n\t[Do you want to set output speed Limit?]\n";
cout << "Enter 1, if Yes.nEnter any, if No. \n";
cin >> input;
if ( input == 1) {
// char buffer[10];
int max, min;
cout << "Max speed = ";
cin >> max;
//pidOutLimitMax = atof(buffer);
cout << "\nMin speed = ";
cin >> min;
// pidOutLimitMin = atof(buffer);
//setOutputLimitsX(pidOutLimitMin, pidOutLimitMax);
//setOutputLimitsY(pidOutLimitMin, pidOutLimitMax);
setMaxMinSpeed(max, min);
cout<<"\n[New setting applied]\n";
saveConfig();
}
Process::Process(int pid, int pageTableSize) {
setPID(pid);
pageTable = new ResidentFrame[pageTableSize];
memoryAccessCount=0;
pageFaultCount=0;
totalPageFaults=0;
numberOfIntervals=0;
residentSetSum=0;
intervalStart=0;
}
void main() {
configure();
UART1_Write_Text(ConnectionEstablished);
setPID();
//testPIC();
debugText = " Starting.. ";
UART1_Write_Text(debugText);
delay_ms(1000);
debugText = "Test Sensors ";
UART1_Write_Text(debugText);
for(count=0; count<1; count++){
receiveCommand = UART1_Read();
sendSensorStatus();
delay_ms(1000);
} //while( receiveCommand != 'A' && receiveCommand != 'a' );
debugText = "Start Line follow.. ";
UART1_Write_Text(debugText);
//lineFollowNormal();
//lineFollow();
lineFollowPID();
testPIC();
while(1){
debugText = " Rotate Clockwise ";
UART1_Write_Text(debugText);
rotateClockwise(255);
delay_ms(220);
stop();
delay_ms(2000);
debugText = " Rotate Anti Clockwise ";
UART1_Write_Text(debugText);
rotateAntiClockwise(255);
delay_ms(220);
stop();
delay_ms(2000);
}
}
int proctable_add_process(struct proc *proc_created, struct proc *proc_parent) {
KASSERT(procTableLock != NULL);
KASSERT(proc_created != NULL);
// Grow the proctable as more processes come in.
// Subtract 1 for zero-indexing.
if (procCount == pidLimit - 1) {
if (pidLimit < MAX_PID) {
pidLimit = pidLimit * 2;
procarray_setsize(procTable, pidLimit);
}
else {
return -1;
}
}
// Assign a PID to the new process
for (int i = MIN_PID; i < pidLimit; i++) {
if (procarray_get(procTable, i) == NULL) {
setPID(proc_created, i);
procarray_set(procTable, i, proc_created);
break;
}
}
// Check to see if a PID was available
if (getPID(proc_created) == PROC_NO_PID) {
return -1;
}
DEBUG(DB_EXEC, "New process in table: %d\n", getPID(proc_created));
// Increase the count of processes in the procTable
procCount++;
// Assign the process' parent PID
if (proc_parent == NULL) {
setPPID(proc_created, PROC_NO_PID);
}
else {
setPPID(proc_created, getPID(proc_parent));
}
// Finally, set the state of the proces to running
setState(proc_created, PROC_RUNNING);
return 0;
}
/**
* Starts a new process for debugging.
*
* @param path The path to the executable of the process.
* @param tids The thread IDs of the threads that belong to the target process.
*
* @return A NaviError code that describes whether the operation was successful or not.
*/
NaviError LinuxSystem::startProcess(
const NATIVE_STRING path,
const std::vector<const NATIVE_STRING>& commands) {
pid_t pid = fork();
if (pid == -1) {
msglog->log(LOG_ALWAYS, "Error: Couldn't fork process");
return NaviErrors::COULDNT_OPEN_TARGET_PROCESS;
} else if (pid == 0) {
ptrace(PTRACE_TRACEME, 0, 0, 0);
char** child_arguments = new char*[1 + commands.size() + 1];
child_arguments[0] = new char[strlen(path) + 1];
strcpy(child_arguments[0], path);
for (unsigned int i = 0; i < commands.size(); i++) {
child_arguments[i + 1] = new char[strlen(commands[i]) + 1];
strcpy(child_arguments[i + 1], commands[i]);
}
child_arguments[1 + commands.size()] = 0;
// Child process
if (execvp(path, child_arguments) == -1) {
msglog->log(LOG_ALWAYS, "Error: Could not start the child process '%s'",
path);
exit(0);
}
return NaviErrors::SUCCESS;
} else {
int status;
if (waitpid(pid, &status, __WALL) == -1) {
msglog->log(LOG_ALWAYS, "Error: Wait for target process failed '%s'",
path);
exit(0);
}
if (WIFSTOPPED(status)) {
if (WSTOPSIG(status) == SIGTRAP) {
msglog->log(LOG_VERBOSE, "Initial STOP signal received");
} else {
msglog->log(LOG_ALWAYS, "Error: Received unexpected STOP signal");
exit(0);
}
} else {
msglog->log(LOG_ALWAYS, "Error: Did not receive initial STOP signal");
exit(0);
}
if (ptrace(
PTRACE_SETOPTIONS,
pid,
0,
PTRACE_O_TRACECLONE | PTRACE_O_TRACEFORK | PTRACE_O_TRACEVFORK
| PTRACE_O_TRACEVFORKDONE) == -1) {
msglog->log(LOG_ALWAYS, "Error: Could not set ptrace options");
exit(0);
}
msglog->log(LOG_VERBOSE, "PID of the child process is %d", pid);
setPID(pid);
Thread ts(pid, SUSPENDED);
tids.push_back(ts);
setActiveThread(pid);
lastMapFileSize = getFileSize(
"/proc/" + zylib::zycon::toString(pid) + "/maps");
fillModules(pid, modules);
this->modules = modules;
std::map<std::string, Module>::const_iterator cit = this->modules.find(
getTargetApplicationPath().string());
if (cit != this->modules.end()) {
Module processModule = cit->second;
processStart(processModule, ts);
} else {
msglog->log(LOG_ALWAYS,
"Error: Unable to determine main process module for '%s'",
getTargetApplicationPath().string().c_str());
exit(0);
}
return NaviErrors::SUCCESS;
}
}
void eDVBServiceController::scanPMT( PMT *pmt )
{
Decoder::parms.pmtpid=pmt->pid;
usedCASystems.clear();
PMTEntry *audio=0, *ac3_audio=0, *video=0, *teletext=0;
int audiopid=-1, videopid=-1, ac3pid=-1, tpid=-1;
int sac3default=eAudio::getInstance()->getAC3default();
if ( Decoder::parms.pcrpid != pmt->PCR_PID && !service.path.size() )
Decoder::parms.pcrpid = pmt->PCR_PID;
// get last selected audio / video pid from pid cache
eService *sp=eServiceInterface::getInstance()->addRef(service);
if (sp)
{
if (sp->dvb)
{
#ifndef DISABLE_FILE
FillPIDsFromFile(sp);
#endif
videopid=sp->dvb->get(eServiceDVB::cVPID);
audiopid=sp->dvb->get(eServiceDVB::cAPID);
ac3pid=sp->dvb->get(eServiceDVB::cAC3PID);
sp->dvb->set(eServiceDVB::cPCRPID, Decoder::parms.pcrpid);
}
}
int isca=checkCA(calist, pmt->program_info, pmt->program_number);
audioStreams.clear();
videoStreams.clear();
subtitleStreams.clear();
int content_pid=-1;
ePtrList<PMTEntry>::iterator TTXIt=pmt->streams.end();
for (ePtrList<PMTEntry>::iterator i(pmt->streams); i != pmt->streams.end(); ++i)
{
PMTEntry *pe=*i;
bool isAudio=false;
int tmp=0;
switch (pe->stream_type)
{
case 1: // ISO/IEC 11172 Video
case 2: // ITU-T Rec. H.262 | ISO/IEC 13818-2 Video or ISO/IEC 11172-2 constrained parameter video stream
if ( (!video) || (pe->elementary_PID == videopid) )
video=pe;
isca+=checkCA(calist, pe->ES_info, pmt->program_number);
videoStreams.push_back(pe);
break;
case 3: // ISO/IEC 11172 Audio
case 4: // ISO/IEC 13818-3 Audio
isAudio=true;
case 6:
{
isca+=checkCA(calist, pe->ES_info, pmt->program_number);
audioStream stream(pe);
for (ePtrList<Descriptor>::iterator ii(pe->ES_info); ii != pe->ES_info.end(); ++ii)
{
switch( ii->Tag() )
{
case DESCR_AC3:
stream.isAC3=1;
break;
case DESCR_REGISTRATION:
if (!memcmp(((RegistrationDescriptor*)*ii)->format_identifier, "DTS", 3))
stream.isDTS=1;
break;
case DESCR_TELETEXT:
if ( (!teletext) || (pe->elementary_PID == tpid) )
{
TTXIt = i;
teletext=pe;
}
break;
case DESCR_SUBTITLING:
subtitleStreams.push_back(pe);
break;
case DESCR_ISO639_LANGUAGE:
stream.text=getISO639Description(((ISO639LanguageDescriptor*)*ii)->language_code);
break;
case DESCR_STREAM_ID:
stream.component_tag=((StreamIdentifierDescriptor*)*ii)->component_tag;
break;
case DESCR_LESRADIOS:
{
LesRadiosDescriptor *d = (LesRadiosDescriptor*)*ii;
if ( (stream.component_tag >= 0 || d->id) && d->name )
stream.text.sprintf("%d.) %s", d->id, d->name.c_str());
else
isAudio=false;
break;
}
default:
break;
}
}
if (stream.isAC3)
{
stream.text+=" (AC3)";
isAudio=true;
if ( (!ac3_audio) || (pe->elementary_PID == ac3pid) )
ac3_audio=pe;
}
else if (stream.isDTS)
{
stream.text+=" (DTS)";
isAudio=true;
if ( (!ac3_audio) || (pe->elementary_PID == ac3pid) )
ac3_audio=pe;
}
else if (isAudio && ( (!audio) || (pe->elementary_PID == audiopid) ) )
audio=pe;
if (!stream.text)
stream.text.sprintf("PID %04x", pe->elementary_PID);
if (isAudio)
audioStreams.push_back(stream);
}
case 5: // private section
{
if ( content_pid != -1)
continue;
for (ePtrList<Descriptor>::iterator ii(pe->ES_info); ii != pe->ES_info.end(); ++ii)
{
switch( ii->Tag() )
{
case DESCR_PRIV_DATA_SPEC:
if ( ((PrivateDataSpecifierDescriptor*)*ii)->private_data_specifier == 190 )
tmp |= 1;
break;
case 0x90:
{
UnknownDescriptor *descr = (UnknownDescriptor*) *ii;
if ( descr->length() == 4 && !descr->data[0] && !descr->data[1] && descr->data[2] == 0xFF && descr->data[3] == 0xFF )
tmp |= 2;
}
default:
break;
}
}
if ( tmp == 3 )
content_pid = pe->elementary_PID;
}
default:
break;
}
}
if ( content_pid != -1 )
/*emit*/ dvb.gotContentPid(content_pid);
// get audio priority channel
audio = priorityAudio(audio);
ePtrList<PMTEntry>::iterator tmp = pmt->streams.end();
if (TTXIt != tmp)
{
// move teletext to the end of the list
--tmp;
if ( tmp != TTXIt )
std::iter_swap(tmp, TTXIt);
}
int needAC3Workaround=0;
switch (eSystemInfo::getInstance()->getHwType())
{
case eSystemInfo::dbox2Nokia:
case eSystemInfo::dbox2Philips:
case eSystemInfo::dbox2Sagem:
needAC3Workaround=1;
default:
break;
}
if ( !needAC3Workaround && ac3_audio && ( sac3default || (ac3pid != -1) || (!audio) ) )
{
audiopid = ac3pid;
audio = ac3_audio;
}
if ( video )
{
if ( video->elementary_PID != videopid )
setPID(video);
}
else if (sp && sp->dvb) // handle no more existing vpid
{
Decoder::parms.vpid = -1;
sp->dvb->set(eServiceDVB::cVPID, -1);
}
if ( audio )
{
if ( audiopid != audio->elementary_PID )
setPID(audio);
}
else if (sp && sp->dvb) // handle no more existing apid
{
Decoder::parms.apid = -1;
sp->dvb->set(eServiceDVB::cAPID, -1);
sp->dvb->set(eServiceDVB::cAC3PID, -1);
}
if ( teletext )
{
if ( tpid != teletext->elementary_PID )
setPID(teletext);
}
else if (sp && sp->dvb) // handle no more existing tpid
{
Decoder::parms.tpid = -1;
sp->dvb->set(eServiceDVB::cTPID, -1);
}
/*emit*/ dvb.scrambled(isca);
int hideerror=0;
eConfig::getInstance()->getKey("/elitedvb/extra/hideerror", hideerror);
if ( hideerror || (dvb.recorder && service.path) )
;
else if (isca && !service.path && !calist )
{
eDebug("NO CASYS");
service_state=ENOCASYS;
}
if ((Decoder::parms.vpid==-1) && (Decoder::parms.apid==-1))
service_state=ENOSTREAM;
// for (ePtrList<CA>::iterator i(calist); i != calist.end(); ++i)
// eDebug("CA %04x ECMPID %04x", i->casysid, i->ecmpid);
setDecoder();
// AC3 DBOX2 Workaround... buggy drivers...
if ( needAC3Workaround && ac3_audio && ( sac3default || (ac3pid != -1) || (!audio) ) )
{
setPID(ac3_audio);
setDecoder();
}
if (sp)
eServiceInterface::getInstance()->removeRef(service);
}
void eDVBServiceController::EITready(int error)
{
eDebug("EITready (%d)", error);
bool haveAC3=false, changed=false;
if (!error)
{
EIT *eit=dvb.getEIT();
for (ePtrList<EITEvent>::iterator e(eit->events); e != eit->events.end(); ++e)
{
// eDebug("running_status = %d", e->running_status );
if ((e->running_status>=2) || (!e->running_status) ) // currently running service
{
for (ePtrList<Descriptor>::iterator d(e->descriptor); d != e->descriptor.end(); ++d)
{
if (d->Tag()==DESCR_COMPONENT)
{
for (std::list<audioStream>::iterator it(audioStreams.begin())
;it != audioStreams.end(); ++it )
{
if (((ComponentDescriptor*)*d)->component_tag == it->component_tag )
{
changed=true;
eString tmp = ((ComponentDescriptor*)*d)->text;
if (tmp)
{
it->text=tmp;
if ( it->isAC3 )
it->text+=" (AC3)";
else if ( it->isDTS )
it->text+=" (DTS)";
}
}
if ( !haveAC3 && (it->isAC3 || it->isDTS) )
haveAC3=true;
}
}
}
break;
}
}
if ( service.getServiceType() == 4 ) // NVOD Service
{
delete dvb.parentEIT;
dvb.parentEIT = new EIT( eit );
dvb.parentEIT->events.setAutoDelete(true);
eit->events.setAutoDelete(false);
}
/*emit*/ dvb.gotEIT(eit, 0);
eit->unlock();
}
else
/*emit*/ dvb.gotEIT(0, error);
if ( changed && !(haveAC3 && eAudio::getInstance()->getAC3default()) )
{
PMTEntry *audio = 0;
audio = priorityAudio(audio);
if (audio)
{
setPID(audio);
setDecoder();
}
}
}