static void poseCommanderTask(void* param) {
uint32_t lastWakeTime;
//Wait for the system to be fully started to start pose controller loop
systemWaitStart();
lastWakeTime = xTaskGetTickCount ();
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(POSECOMMANDERFREQUENCY)); // 100Hz
// read actual pose value
actualPoseGetPose(&actualPose);
// get desired pose value
if(desiredPoses.front != 0) {
desiredPose = *(Pose*)(desiredPoses.front->item);
}
// update data for logging
updateErrors(&desiredPose, &actualPose);
// test for goal reached and shift
if(reached(&desiredPose, &actualPose)) {
if(desiredPoses.front != 0) {
if(desiredPoses.front->next != 0) {
linkedlistItem front = *linkedListPopFront(&desiredPoses);
deleteLinkedlistItem(&front);
}
}
}
}
}
int main(void) {
for (int i=0; i<3; i++) {
set_output(DDRB, led[i]);
}
uint8_t tick = 0;
while(1) {
for (int i=0; i<3; i++) {
if (tick < current[i]) {
output_high(PORTB, led[i]);
} else {
output_low(PORTB, led[i]);
}
}
tick = (tick+1)%256;
if (tick == 0) {
if (reached()) {
retarget();
} else {
approach();
}
}
}
return 0;
}
node dijkstra::predecessor_node(const node& n) const
{
assert(preds_set);
if ((n == s) || (!reached(n)))
{
return node();
}
return pred[n].opposite(n);
}
dijkstra::shortest_path_edge_iterator dijkstra::shortest_path_edges_end(
const node& dest)
{
assert(preds_set);
if ((shortest_path_edge_list[dest].empty()) &&
(dest != s) &&
(reached(dest)))
{
fill_edge_list(dest);
}
return shortest_path_edge_list[dest].end();
}
void dijkstra::fill_node_list(const node& dest)
{
if ((dest == s) || (!reached(dest)))
{
return;
}
node cur_node = dest;
while (cur_node != node())
{
shortest_path_node_list[dest].push_front(cur_node);
cur_node = predecessor_node(cur_node);
}
}
int ballDetection::moveToBall() {
cout << "rufe Movetoball auf"<<endl;
//cout << "KoordX: "<< KoordX<<endl;
//int headingCount = 0;
double obj = findObj();
// kleiner wählen damit auf ball in größerer Entfernung gefunden wird
while(obj >= 0.5) {
p3dxptr->comInt(ArCommands::VEL, 0);
//ballimBild = true;
//cout << "#Drehungen: "<< headingCount << endl;
ArPose currentPose = p3dxptr->getPose();
cout<<"Koorigiere Heading "<<currentPose.getTh()<<endl;
if(KoordX >= 0 && KoordX <= 399) {
//p3dx.comInt(ArCommands::HEAD, i);
cout<<"Setze Heading -10 "<<endl;
// headingCount++;
p3dxptr->comInt(ArCommands::HEAD, (currentPose.getTh()+5));
ArUtil::sleep(500);
}
if(KoordX > 399 && KoordX <= 799) {
//p3dx.comInt(ArCommands::HEAD, i);
cout<<"Setze Heading +10 "<<endl;
// headingCount++;
p3dxptr->comInt(ArCommands::HEAD, currentPose.getTh()-5);
ArUtil::sleep(500);
}
if(rotateToBall() == 1) {
cout << "ball in der Mitte" <<endl;
p3dxptr->comInt(ArCommands::VEL, 75);
reached();
cout << "Rückgabe Wert 1" << endl;
return 1;
}else {
p3dxptr->comInt(ArCommands::VEL, 75);
p3dxptr->comInt(ArCommands::RVEL, 0);
cout << "Rückgabe Wert 0" << endl;
return 0;
}
}
if(obj < 0.5) {
return 0;
}
}
bool test_traversable(vector<int> steps) {
vector<bool> reached(steps.size(), false);
reached[0] = true;
int i = 0;
while (i < steps.size() && reached[i] == true) {
for (int j=i+1; j<i+1+steps[i] && j < steps.size(); ++j) {
reached[j] = true;
cout << "from " << i << ": " << j << " is reachable\n";
}
i++;
cout << i << " " << reached << endl;
}
return reached[steps.size()-1];
}
bool checkRegisters(IRTrace* trace, const IRFactory& factory,
const RegAllocInfo& regs) {
assert(checkCfg(trace, factory));
auto blocks = rpoSortCfg(trace, factory);
StateVector<Block, RegState> states(&factory, RegState());
StateVector<Block, bool> reached(&factory, false);
for (auto* block : blocks) {
RegState state = states[block];
for (IRInstruction& inst : *block) {
for (SSATmp* src : inst.srcs()) {
auto const &info = regs[src];
if (!info.spilled() &&
(info.reg(0) == Transl::rVmSp ||
info.reg(0) == Transl::rVmFp)) {
// hack - ignore rbx and rbp
continue;
}
for (unsigned i = 0, n = info.numAllocatedRegs(); i < n; ++i) {
assert(state.tmp(info, i) == src);
}
}
for (SSATmp& dst : inst.dsts()) {
auto const &info = regs[dst];
for (unsigned i = 0, n = info.numAllocatedRegs(); i < n; ++i) {
state.tmp(info, i) = &dst;
}
}
}
// State contains register/spill info at current block end.
auto updateEdge = [&](Block* succ) {
if (!reached[succ]) {
states[succ] = state;
} else {
states[succ].merge(state);
}
};
if (auto* next = block->next()) updateEdge(next);
if (auto* taken = block->taken()) updateEdge(taken);
}
return true;
}
bool checkRegisters(const IRUnit& unit, const RegAllocInfo& regs) {
assert(checkCfg(unit));
auto blocks = rpoSortCfg(unit);
StateVector<Block, RegState> states(unit, RegState());
StateVector<Block, bool> reached(unit, false);
for (auto* block : blocks) {
RegState state = states[block];
for (IRInstruction& inst : *block) {
if (inst.op() == Jmp) continue; // handled by Shuffle
auto& inst_regs = regs[inst];
for (int i = 0, n = inst.numSrcs(); i < n; ++i) {
auto const &rs = inst_regs.src(i);
if (!rs.spilled()) {
// hack - ignore rbx and rbp
bool ignore_frame_regs;
switch (arch()) {
case Arch::X64:
ignore_frame_regs = (rs.reg(0) == X64::rVmSp ||
rs.reg(0) == X64::rVmFp);
break;
case Arch::ARM:
ignore_frame_regs = (rs.reg(0) == ARM::rVmSp ||
rs.reg(0) == ARM::rVmFp);
break;
}
if (ignore_frame_regs) continue;
}
DEBUG_ONLY auto src = inst.src(i);
assert(rs.numWords() == src->numWords() ||
(src->isConst() && rs.numWords() == 0));
DEBUG_ONLY auto allocated = rs.numAllocated();
if (allocated == 2) {
if (rs.spilled()) {
assert(rs.slot(0) != rs.slot(1));
} else {
assert(rs.reg(0) != rs.reg(1));
}
}
for (unsigned i = 0, n = rs.numAllocated(); i < n; ++i) {
assert(state.tmp(rs, i) == src);
}
}
auto update = [&](SSATmp* tmp, const PhysLoc& loc) {
for (unsigned i = 0, n = loc.numAllocated(); i < n; ++i) {
state.tmp(loc, i) = tmp;
}
};
if (inst.op() == Shuffle) {
checkShuffle(inst, regs);
for (unsigned i = 0; i < inst.numSrcs(); ++i) {
update(inst.src(i), inst.extra<Shuffle>()->dests[i]);
}
} else {
for (unsigned i = 0; i < inst.numDsts(); ++i) {
update(inst.dst(i), inst_regs.dst(i));
}
}
}
// State contains the PhysLoc->SSATmp reverse mappings at block end;
// propagate the state to succ
auto updateEdge = [&](Block* succ) {
if (!reached[succ]) {
states[succ] = state;
} else {
states[succ].merge(state);
}
};
if (auto* next = block->next()) updateEdge(next);
if (auto* taken = block->taken()) updateEdge(taken);
}
return true;
}
Graph::Graph(const int *cartan,
const std::vector<Word>& gens, //namesake
const std::vector<Word>& v_cogens,
const std::vector<Word>& e_gens,
const std::vector<Word>& f_gens,
const Vect& weights)
{
//define symmetry group relations
std::vector<Word> words = words_from_cartan(cartan);
{
const Logging::fake_ostream& os = logger.debug();
os << "relations =";
for (int w=0; w<6; ++w) {
Word& word = words[w];
os << "\n ";
for (unsigned i=0; i<word.size(); ++i) {
os << word[i];
}
}
os |0;
}
//check vertex stabilizer generators
{
const Logging::fake_ostream& os = logger.debug();
os << "v_cogens =";
for (unsigned w=0; w<v_cogens.size(); ++w) {
const Word& jenn = v_cogens[w]; //namesake
os << "\n ";
for (unsigned t=0; t<jenn.size(); ++t) {
int j = jenn[t];
os << j;
Assert (0<=j and j<4,
"generator out of range: letter w["
<< w << "][" << t << "] = " << j );
}
}
os |0;
}
//check edge generators
{
const Logging::fake_ostream& os = logger.debug();
os << "e_gens =";
for (unsigned w=0; w<e_gens.size(); ++w) {
const Word& edge = e_gens[w];
os << "\n ";
for (unsigned t=0; t<edge.size(); ++t) {
int j = edge[t];
os << j;
Assert (0<=j and j<4,
"generator out of range: letter w["
<< w << "][" << t << "] = " << j );
}
}
os |0;
}
//check face generators
{
const Logging::fake_ostream& os = logger.debug();
os << "f_gens =";
for (unsigned w=0; w<f_gens.size(); ++w) {
const Word& face = f_gens[w];
os << "\n ";
for (unsigned t=0; t<face.size(); ++t) {
int j = face[t];
os << j;
Assert (0<=j and j<4,
"generator out of range: letter w["
<< w << "][" << t << "] = " << j );
}
}
os |0;
}
//build symmetry group
Group group(words);
logger.debug() << "group.ord = " << group.ord |0;
//build subgroup
std::vector<int> subgroup; subgroup.push_back(0);
std::set<int> in_subgroup; in_subgroup.insert(0);
for (unsigned g=0; g<subgroup.size(); ++g) {
int g0 = subgroup[g];
for (unsigned j=0; j<gens.size(); ++j) {
int g1 = group.left(g0,gens[j]);
if (in_subgroup.find(g1) != in_subgroup.end()) continue;
subgroup.push_back(g1);
in_subgroup.insert(g1);
}
}
logger.debug() << "subgroup.ord = " << subgroup.size() |0;
//build cosets and count ord
std::map<int,int> coset; //maps group elements to cosets
ord = 0; //used as coset number
for (unsigned g=0; g<subgroup.size(); ++g) {
int g0 = subgroup[g];
if (coset.find(g0) != coset.end()) continue;
int c0 = ord++;
coset[g0] = c0;
std::vector<int> members(1, g0);
std::vector<int> others(0);
for (unsigned i=0; i<members.size(); ++i) {
int g1 = members[i];
for (unsigned w=0; w<v_cogens.size(); ++w) {
int g2 = group.left(g1, v_cogens[w]);
if (coset.find(g2) != coset.end()) continue;
coset[g2] = c0;
members.push_back(g2);
}
}
}
logger.info() << "cosets table built: " << " ord = " << ord |0;
//build edge lists
std::vector<std::set<int> > neigh(ord);
for (unsigned g=0; g<subgroup.size(); ++g) {
int g0 = subgroup[g];
int c0 = coset[g0];
for (unsigned w=0; w<e_gens.size(); ++w) {
int g1 = group.left(g0, e_gens[w]);
Assert (in_subgroup.find(g1) != in_subgroup.end(),
"edge leaves subgroup");
int c1 = coset[g1];
if (c0 != c1) neigh[c0].insert(c1);
}
}
// make symmetric
for (int c0=0; c0<ord; ++c0) {
const std::set<int>& n = neigh[c0];
for (std::set<int>::iterator c1=n.begin(); c1!=n.end(); ++c1) {
neigh[*c1].insert(c0);
}
}
// build edge table
adj.resize(ord);
for (int c=0; c<ord; ++c) {
adj[c].insert(adj[c].begin(), neigh[c].begin(), neigh[c].end());
}
neigh.clear();
deg = adj[0].size();
logger.info() << "edge table built: deg = " << deg |0;
//define faces
for (unsigned g=0; g<f_gens.size(); ++g) {
const Word& face = f_gens[g];
logger.debug() << "defining faces on " << face |0;
Logging::IndentBlock block;
//define basic face in group
Ring basic(1,0);
// g = 0;
int g0 = 0;
for (unsigned c=0; true; ++c) {
g0 = group.left(g0, face[c%face.size()]);
if (c >= face.size() and g0 == 0) break;
if (in_subgroup.find(g0) != in_subgroup.end() and g0 != basic.back()) {
basic.push_back(g0);
}
}
for (unsigned c=0; c<basic.size(); ++c) {
logger.debug() << " corner: " << basic[c] |0;
}
logger.debug() << "sides/face (free) = " << basic.size() |0;
//build orbit of basic face
std::vector<Ring> faces_g; faces_g.push_back(basic);
FaceRecognizer recognized; recognized(basic);
for (unsigned i=0; i<faces_g.size(); ++i) {
const Ring f = faces_g[i];
for (unsigned j=0; j<gens.size(); ++j) {
//right action of group on faces
Ring f_j(f.size());
for (unsigned c=0; c<f.size(); ++c) {
f_j[c] = group.right(f[c],gens[j]);
}
//add face
if (not recognized(f_j)) {
faces_g.push_back(f_j);
//logger.debug() << "new face: " << f_j |0;
} else {
//logger.debug() << "old face: " << f_j|0;
}
}
}
//hom face down to quotient graph
recognized.clear();
for (unsigned f=0; f<faces_g.size(); ++f) {
const Ring face_g = faces_g[f];
Ring face;
face.push_back(coset[face_g[0]]);
for (unsigned i=1; i<face_g.size(); ++i) {
int c = coset[face_g[i]];
if (c != face.back() and c != face[0]) {
face.push_back(c);
}
}
if (face.size() < 3) continue;
if (not recognized(face)) {
faces.push_back(face);
}
}
}
ord_f = faces.size();
logger.info() << "faces defined: order = " << ord_f |0;
//define vertex coset
std::vector<Word> vertex_coset;
for (unsigned g=0; g<subgroup.size(); ++g) {
int g0 = subgroup[g];
if (coset[g0]==0) vertex_coset.push_back(group.parse(g0));
}
//build geometry
std::vector<Mat> gen_reps(gens.size());
points.resize(ord);
build_geom(cartan, vertex_coset, gens, v_cogens, weights,
gen_reps, points[0]);
std::vector<int> pointed(ord,0);
pointed[0] = true;
logger.debug() << "geometry built" |0;
//build point sets
std::vector<int> reached(1,0);
std::set<int> is_reached;
is_reached.insert(0);
for (unsigned g=0; g<subgroup.size(); ++g) {
int g0 = reached[g];
for (unsigned j=0; j<gens.size(); ++j) {
int g1 = group.right(g0,gens[j]);
if (is_reached.find(g1) == is_reached.end()) {
if (not pointed[coset[g1]]) {
vect_mult(gen_reps[j], points[coset[g0]],
points[coset[g1]]);
pointed[coset[g1]] = true;
}
reached.push_back(g1);
is_reached.insert(g1);
}
}
}
logger.debug() << "point set built." |0;
//build face normals
normals.resize(ord_f);
for (int f=0; f<ord_f; ++f) {
Ring& face = faces[f];
Vect &a = points[face[0]];
Vect &b = points[face[1]];
Vect &c = points[face[2]];
Vect &n = normals[f];
cross4(a,b,c, n);
normalize(n);
/*
Assert1(fabs(inner(a,n)) < 1e-6,
"bad normal: <n,a> = " << fabs(inner(a,n)));
Assert1(fabs(inner(b,n)) < 1e-6,
"bad normal: <n,b> = " << fabs(inner(b,n)));
Assert1(fabs(inner(c,n)) < 1e-6,
"bad normal: <n,b> = " << fabs(inner(c,n)));
*/
}
logger.debug() << "face normals built." |0;
}
bool
MidpointRule::integrate(real_type toTEnd)
{
real_type rtol = 1e-12;
real_type atol = 1e-10;
Vector dy(mState.size());
while (!reached(toTEnd)) {
real_type t = getTime();
real_type h = maxStepsize(toTEnd);
// Often used values.
real_type h2 = 0.5*h;
// We assume that the problem is nonstiff.
// This is a sensible assumption since this symmetric integrator is not
// stiffly accurate anyway.
// If it is not stiff, we solve the nonlinear equation
// with fixpoint iteration.
// The GNI papers suggest this anyway.
// An initial guess for the fixpoint iteration.
// Note that the polynomial coefficients p1 and p2 are set to zero at the
// first step past a reset.
if (mCollocationPolynomialValid) {
dy = old_dy;
} else {
dy.clear();
}
// Solve the implicit equation
bool converging;
bool converged = false;
unsigned maxit = 10;
real_type prev_err = Limits<real_type>::max();
do {
// Compute new approximations to the state at the inner stages
Vector y = mState + 0.5*dy;
// Compute new approximations to the state derivatives
evalFunction(t+h2, y, mDeriv);
// Check if the increment is small enough ...
real_type err = scaledDiff(y, mState + h2*mDeriv, atol, rtol);
converged = err < 1;
// Check if we do converge in any way.
converging = err < prev_err;
prev_err = err;
// Use the new approximation
dy = h*mDeriv;
++mStats.numIter;
} while (!converged && 0 < --maxit && converging);
if (converged) {
// Update the solution
mState += dy;
// Compute the collocation polynomial.
// Is used for a predictor of the next fixpoint iterate start guess.
old_dy = dy;
mCollocationPolynomialValid = true;
++mStats.numSteps;
} else {
// If we cannot solve the nonlinear equation, do an explicit euler step
mCollocationPolynomialValid = false;
Log(TimeStep, Warning) << "MidpointRule did not converge" << std::endl;
evalFunction(t, mState, mDeriv);
mState += h*mDeriv;
++mStats.numFailed;
++mStats.numSteps;
}
// Increment the simulation time ...
mTime += h;
}
return true;
}
void waypoints_follower::run()
{
const double MAX_TWIST_LINEAR = 1.0;
const double MAX_TWIST_ANGULAR = 0.5;
const double TURNING_RADIUS=0.4;
if (!active) return;
// ROS_INFO("active");
if (!localized)
{
ROS_WARN("waiting for localization");
return;
}
if (reached(next_target) || start_new_target) //Change of target!
{
start_new_target = false;
if (targets.size()==0)
{
deactivate(deactivate_reason::NO_MORE_TARGETS);
return;
}
std::unique_lock<std::mutex>(targets_mtx);
next_target = targets.front();
targets.pop_front();
geometry_msgs::Pose2D current_pose;
current_pose.x=x;
current_pose.y=y;
xtarget = next_target.x;
ytarget = next_target.y;
ROS_INFO_STREAM("starting new target "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000);
straight=true;
turning=false;
}
/* More complex implementation, for the future, it chooses speed and angular radius
if (distance(next_target)<TURNING_RADIUS && !turning && targets.size()>0 && !start_new_target) //Will not use circle if this is the last target or if we are starting now
{
//TODO start turning along a circle
//Find next heading, circle radius, circle length, circle angle
double current_heading=theta;
double heading=atan2(ytarget-targets.front().y,xtarget-targets.front().x);
double current_speed=twist.linear.x;
double next_speed=distance(next_target,targets.front())/(targets.front().z-next_target.z);
double delta_heading=heading-current_heading;
bool rotate_left;
if((0<delta_heading && delta_heading<M_PI) || (-2*M_PI<delta_heading && delta_heading<-M_PI ))
rotate_left=true;
else rotate_left=false;
bool ok=false;
double desired_speed=(current_speed+next_speed)/2.0;
double radius=max_turning_radius/10.0;
double alpha=2*radius/wheel_distance;
while(!ok)
{
double right_wheel_speed=-desired_speed*(1/alpha+1);
if (right_wheel_speed>max_speed) //Too fast, slow down
{
right_wheel_speed=max_speed;
}
double left_wheel_speed=2*desired_speed-right_wheel_speed;
if (left_wheel_speed>max_speed) //Also too fast, change speed and turning radius
{
desired_speed=0.9*desired_speed;
radius=radius+max_turning_radius*0.1;
}
else
{
ok=true;
}
}
turning = true;
twist.linear.x=desired_speed;
twist.angular.z=(right_wheel_speed-left_wheel_speed)/2.0;
}
*/
if (distance(next_target)<TURNING_RADIUS && distance(next_target)>reached_threshold && !turning && targets.size()>0 && !start_new_target) //Will not use circle if this is the last target or if we are starting now
{
turning=true;
straight=false;
desired_heading=atan2(targets.front().y-ytarget,targets.front().x-xtarget);
double current_speed=twist.linear.x;
double next_speed=distance(next_target,targets.front())/(targets.front().z-next_target.z);
if(desired_speed>MAX_TWIST_LINEAR) desired_speed=MAX_TWIST_LINEAR;
desired_speed=(current_speed+next_speed)/2.0;
twist.linear.x=desired_speed;
ROS_INFO_STREAM("starting turning from near "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000<< "to next target");
}
double length = distance(next_target);
double theta_err;
if (turning)
{
//either keep publishing same rotation and speed or feedback on the circle information
twist.linear.x=desired_speed*(1+ki1*(TURNING_RADIUS-length));
twist.angular.z=-ki2*sin(desired_heading-theta);
theta_err=desired_heading-theta;
if (fabs(desired_heading-theta)<0.2 )//|| distance())
{
//We kind of reached the desired heading, we should switch to the next target and stop turning
std::unique_lock<std::mutex>(targets_mtx);
next_target = targets.front();
targets.pop_front();
geometry_msgs::Pose2D current_pose;
current_pose.x=x;
current_pose.y=y;
xtarget = next_target.x;
ytarget = next_target.y;
straight=true;
ROS_INFO_STREAM("starting new target "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000);
ROS_INFO("starting straight");
turning=false;
}
}
if (straight)
{
double delta = next_target.z - ros::Time::now().toSec();
if (delta<-0)
{
twist.linear.x = 0;
ROS_WARN_STREAM("Target time is in the past, stopping");
deactivate(deactivate_reason::GLOBAL_TARGET_NOT_REACHED);
}
else
{
twist.linear.x=length/delta*1.1;
theta_err=atan2(ytarget-y,xtarget-x)-theta;
if (fabs(fabs(theta_err)-M_PI)<0.1) theta_err=theta_err+0.1;
twist.angular.z=-kp2*sin(theta_err);
}
}
twist.linear.x = std::min(twist.linear.x,MAX_TWIST_LINEAR);
comand_pub.publish(twist);
ROS_DEBUG_STREAM("controller run xt: "<<xtarget<<" yt: "<<ytarget<<" x: "<<x<<" y: "<<y<<" t "<<next_target.z);
ROS_DEBUG_STREAM("controller run theta:"<<theta<<" error "<<theta_err<<" v: "<<twist.linear.x<<" w: "<<twist.angular.z);
ros::spinOnce();
}
