bool BacktraceThread::Unwind(size_t num_ignore_frames, ucontext_t* ucontext) {
if (ucontext) {
// Unwind using an already existing ucontext.
return impl_->Unwind(num_ignore_frames, ucontext);
}
// Prevent multiple threads trying to set the trigger action on different
// threads at the same time.
if (pthread_mutex_lock(&g_sigaction_mutex) < 0) {
BACK_LOGW("sigaction failed: %s", strerror(errno));
return false;
}
ThreadEntry* entry = ThreadEntry::Get(Pid(), Tid());
entry->Lock();
struct sigaction act, oldact;
memset(&act, 0, sizeof(act));
act.sa_sigaction = SignalHandler;
act.sa_flags = SA_RESTART | SA_SIGINFO | SA_ONSTACK;
sigemptyset(&act.sa_mask);
if (sigaction(THREAD_SIGNAL, &act, &oldact) != 0) {
BACK_LOGW("sigaction failed %s", strerror(errno));
entry->Unlock();
ThreadEntry::Remove(entry);
pthread_mutex_unlock(&g_sigaction_mutex);
return false;
}
if (tgkill(Pid(), Tid(), THREAD_SIGNAL) != 0) {
BACK_LOGW("tgkill %d failed: %s", Tid(), strerror(errno));
sigaction(THREAD_SIGNAL, &oldact, NULL);
entry->Unlock();
ThreadEntry::Remove(entry);
pthread_mutex_unlock(&g_sigaction_mutex);
return false;
}
// Wait for the thread to get the ucontext.
entry->Wait(0);
// After the thread has received the signal, allow other unwinders to
// continue.
sigaction(THREAD_SIGNAL, &oldact, NULL);
pthread_mutex_unlock(&g_sigaction_mutex);
bool unwind_done = impl_->Unwind(num_ignore_frames, entry->GetUcontext());
// Tell the signal handler to exit and release the entry.
entry->Wake();
return unwind_done;
}
task main(){
int spdCmd;
//---------------------------INIT-----------------------------------//
Pid_Init();
//---------------------------END INIT-------------------------------//
// while(SensorValue(endPgm) == 0){
while(true){
ClearTimer(T1);
hogCPU(); //Prevent other tasks from running when this one is.
//writeDebugStreamLine("Foreground\n"); //Let me know when the foreground runs
count=count+1; // Count the number of times the foreground runs.
//--------------------------FOREGROUND------------------------------------//
spdCmd=50;
int mtrCnt=nMotorEncoder[rtMotor];
int mtrCmd=Pid(spdCmd, mtrCnt, mtrCntOld, integral);
motor[rtMotor]=motor[ltMotor]=mtrCmd;
//--------------------------END FOREGROUND--------------------------------//
timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
releaseCPU(); // Let other tasks run now.
wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
//------------Debug------------//
if (count==1){
writeDebugStreamLine("SpdCmd[%i],Kp[%i],Ki[%i],Kd[%i]",spdCmd,Kp,Ki,Kd);
writeDebugStreamLine("TimeLeft,Error,MtrCmd,Speed,Integral,Distance");
}
writeDebugStreamLine("%2i,%3i,%3i,%3i,%3i,%5i",timeLeft,error,mtrCmd,speed,integral,mtrCnt);
}// While Loop
}// Foreground Main Task
void LocalEps::Print(){
cout <<endl;
for(int i=0;i<nnr;i++){
if(Pid(i) < 0) continue;
for(int p=0;p<DIM;p++){
cout << setw(4) << setprecision(0) << fixed<< i << " " << setw(4) << setprecision(0) << p ;
cout << fixed << " ---- D = " ;
cout << setw(11) << setprecision(4) << fixed<< D_fl[i][p][XX] << ' ';
cout << setw(11) << setprecision(4) << fixed<< D_fl[i][p][YY] << ' ';
cout << setw(11) << setprecision(4) << fixed<< D_fl[i][p][ZZ] << ' ';
cout << fixed << " ---- G = " ;
cout << setw(11) << setprecision(4) << fixed<< G_fl[i][p][XX] << ' ';
cout << setw(11) << setprecision(4) << fixed<< G_fl[i][p][YY] << ' ';
cout << setw(11) << setprecision(4) << fixed<< G_fl[i][p][ZZ] << ' ';
cout << fixed << " ---- Inv(G) =" ;
cout << setw(11) << setprecision(4) << fixed<< Gm1_fl[i][p][XX] << ' ';
cout << setw(11) << setprecision(4) << fixed<< Gm1_fl[i][p][YY] << ' ';
cout << setw(11) << setprecision(4) << fixed<< Gm1_fl[i][p][ZZ] << ' ';
cout << fixed << " ---- eps =" ;
cout << setw(11) << setprecision(4) << fixed<< eps[i][p][XX] << ' ';
cout << setw(11) << setprecision(4) << fixed<< eps[i][p][YY] << ' ';
cout << setw(11) << setprecision(4) << fixed<< eps[i][p][ZZ] << ' ';
cout <<endl;
}
// eps[o][p]=(4.0*M_PI*P_avg[o][p])/E_avg[o][p]+1.0;
// for(int p=0;p<DIM;p++) std::cout << o << " " << P_avg[o][p] << " " << E_avg[o][p] << " " << eps[o][p] << std::endl;
}
};
bool BacktraceThread::Unwind(size_t num_ignore_frames) {
ThreadEntry* entry = ThreadEntry::AddThreadToUnwind(
thread_intf_, Pid(), Tid(), num_ignore_frames);
if (!entry) {
return false;
}
// Prevent multiple threads trying to set the trigger action on different
// threads at the same time.
bool retval = false;
if (pthread_mutex_lock(&g_sigaction_mutex) == 0) {
struct sigaction act, oldact;
memset(&act, 0, sizeof(act));
act.sa_sigaction = SignalHandler;
act.sa_flags = SA_RESTART | SA_SIGINFO | SA_ONSTACK;
sigemptyset(&act.sa_mask);
if (sigaction(SIGURG, &act, &oldact) == 0) {
retval = TriggerUnwindOnThread(entry);
sigaction(SIGURG, &oldact, NULL);
} else {
BACK_LOGW("sigaction failed %s", strerror(errno));
}
pthread_mutex_unlock(&g_sigaction_mutex);
} else {
BACK_LOGW("unable to acquire sigaction mutex.");
}
if (retval) {
FinishUnwind();
}
delete entry;
return retval;
}
const std::string& xorg::testing::XServer::GetVersion(void) {
if (Pid() == -1 || !d_->version.empty())
return d_->version;
std::ifstream logfile;
logfile.open(d_->options["-logfile"].c_str());
std::string prefix = "X.Org X Server ";
if (logfile.is_open()) {
std::string line;
while (getline(logfile, line)) {
size_t start = line.find(prefix);
if (start == line.npos)
continue;
line = line.substr(prefix.size());
/* RCs have the human-readable version after the version */
size_t end = line.find(" ");
if (end == line.npos)
end = line.size();
d_->version = line.substr(0, end);
break;
}
}
return d_->version;
}
task autonomous() // main task basically, but this will run if we are put itno autonomous mode
{
MVR = 77;
MVL = 77;
resetTimer(T2);
while (true)
{
change = full;
if (time1[T1]>200)
{
Speed();
resetTimer(T1);
SensorValue[LeftSpeed] = 0;
SensorValue[RightSpeed] = 0;
Pid();
}
change = full;
flyWheelRun();
if (time1[T2]>2000)
{
Staggershot();
}
}
}
bool UnwindStackPtrace::Unwind(size_t num_ignore_frames, ucontext_t* context) {
std::unique_ptr<unwindstack::Regs> regs;
if (context == nullptr) {
uint32_t machine_type;
regs.reset(unwindstack::Regs::RemoteGet(Tid(), &machine_type));
} else {
regs.reset(unwindstack::Regs::CreateFromUcontext(unwindstack::Regs::GetMachineType(), context));
}
error_ = BACKTRACE_UNWIND_NO_ERROR;
return ::Unwind(Pid(), memory_.get(), regs.get(), GetMap(), &frames_, num_ignore_frames);
}
bool BacktraceThread::TriggerUnwindOnThread(ThreadEntry* entry) {
entry->state = STATE_WAITING;
if (tgkill(Pid(), Tid(), SIGURG) != 0) {
BACK_LOGW("tgkill failed %s", strerror(errno));
return false;
}
// Allow up to ten seconds for the dump to start.
int wait_millis = 10000;
int32_t state;
while (true) {
state = android_atomic_acquire_load(&entry->state);
if (state != STATE_WAITING) {
break;
}
if (wait_millis--) {
usleep(1000);
} else {
break;
}
}
bool cancelled = false;
if (state == STATE_WAITING) {
if (android_atomic_acquire_cas(state, STATE_CANCEL, &entry->state) == 0) {
BACK_LOGW("Cancelled dump of thread %d", entry->tid);
state = STATE_CANCEL;
cancelled = true;
} else {
state = android_atomic_acquire_load(&entry->state);
}
}
// Wait for at most ten seconds for the cancel or dump to finish.
wait_millis = 10000;
while (android_atomic_acquire_load(&entry->state) != STATE_DONE) {
if (wait_millis--) {
usleep(1000);
} else {
BACK_LOGW("Didn't finish thread unwind in 60 seconds.");
break;
}
}
return !cancelled;
}
size_t
Vehicle_pickDeliver::pop_back() {
invariant();
pgassert(!empty());
auto pick_itr = m_path.rbegin();
while (pick_itr != m_path.rend() && !pick_itr->is_pickup()) {
++pick_itr;
}
pgassert(pick_itr->is_pickup());
ID deleted_pick_id = pick_itr->id();
auto delivery_id = problem->node(deleted_pick_id).Did();
m_path.erase((pick_itr + 1).base());
auto delivery_itr = m_path.rbegin();
while (delivery_itr != m_path.rend()
&& !(delivery_itr->id() ==delivery_id)) {
++delivery_itr;
}
pgassert(delivery_itr->is_delivery());
pgassert(delivery_itr->Pid() == deleted_pick_id);
m_path.erase((delivery_itr + 1).base());
/* figure out from where the evaluation is needed */
evaluate(1);
ID deleted_order_id(
problem->order_of(problem->node(deleted_pick_id)).id());
orders_in_vehicle.erase(orders_in_vehicle.find(deleted_order_id));
invariant();
return deleted_order_id;
}
size_t
Vehicle_pickDeliver::pop_front() {
invariant();
pgassert(!empty());
auto pick_itr = m_path.begin();
while (pick_itr != m_path.end() && !pick_itr->is_pickup()) {
++pick_itr;
}
pgassert(pick_itr->is_pickup());
ID deleted_pick_id = pick_itr->id();
auto delivery_id = problem->node(deleted_pick_id).Did();
m_path.erase(pick_itr);
auto delivery_itr = m_path.begin();
while (delivery_itr != m_path.end()
&& !(delivery_itr->id() == delivery_id)) {
++delivery_itr;
}
pgassert(delivery_itr->is_delivery());
pgassert(delivery_itr->Pid() == deleted_pick_id);
m_path.erase(delivery_itr);
evaluate(1);
ID deleted_order_id(
problem->order_of(problem->node(deleted_pick_id)).id());
orders_in_vehicle.erase(orders_in_vehicle.find(deleted_order_id));
invariant();
return deleted_order_id;
}
task usercontrol()
{
while (true)
{
ball();
if (time1[T1]>100)
{
Speed();
resetTimer();
SensorValue[LeftSpeed] = 0;
SensorValue[RightSpeed] = 0;
}
Drive();
Fire();
Pnumatics();
Pid();
Drive2();
Fire2();
Pnumatics2();
Pid2();
}
}
task usercontrol() // main task but for driver mode.
{
while (true)
{
if (time1[T1]>200)
{
Speed();
resetTimer(T1);
SensorValue[LeftSpeed] = 0;
SensorValue[RightSpeed] = 0;
if (time1[T4]>1000)
{
Pid();
}
}
Drive();
Intake();
flyWheelRun();
Pnumatics();
SpeedControls();
SpeedControls2();
}
}
std::string UnwindStackPtrace::GetFunctionNameRaw(uintptr_t pc, uintptr_t* offset) {
return ::GetFunctionName(Pid(), GetMap(), pc, offset);
}
xorg::testing::XServer::~XServer() {
if (Pid() > 0)
if (!Terminate(3000))
Kill(300);
}
