internal void
player_camera_step(Entity *P, Camera *camera)
{
i32 distance = 24;
i32 dx;
// if (P->vx) {
dx = (entity_center_x(P) + P->dx * distance) - camera->x;
// } else {
// dx = P->x - camera->x;
// }
if (abs(dx) > 16) {
if (P->vx) {
camera->vx = signof(P->vx) * (abs(P->vx)) + (dx / 7);
} else {
camera->vx = signof(P->vx);
}
} else {
if (P->vx) {
camera->vx = P->vx + (dx / 5);
} else {
camera->vx = dx / 5;
}
}
camera->x += camera->vx;
camera->y = P->collider->y + 32;
}
static void
fix_box(int x, int y)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_box(fix_x, fix_y, cur_x, cur_y);
/* erase last lengths if appres.showlengths is true */
erase_box_lengths();
adjust_box_pos(x, y, from_x, from_y, &x, &y);
new_l = copy_line(cur_l);
if (new_l->type == T_PICTURE) {
if (signof(fix_x - from_x) != signof(fix_x - x))
new_l->pic->flipped = 1 - new_l->pic->flipped;
if (signof(fix_y - from_y) != signof(fix_y - y))
new_l->pic->flipped = 1 - new_l->pic->flipped;
}
assign_newboxpoint(new_l, fix_x, fix_y, x, y);
change_line(cur_l, new_l);
/* redraw anything under the old line */
redisplay_line(cur_l);
/* and the new line */
redisplay_line(new_l);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
void
adjust_box_pos(int curs_x, int curs_y, int orig_x, int orig_y, int *ret_x, int *ret_y)
{
int xx, sgn_csr2fix_x, yy, sgn_csr2fix_y;
double mag_csr2fix_x, mag_csr2fix_y;
switch (constrained) {
case MOVE_ARB:
*ret_x = curs_x;
*ret_y = curs_y;
break;
case BOX_HSTRETCH:
*ret_x = curs_x;
*ret_y = orig_y;
break;
case BOX_VSTRETCH:
*ret_x = orig_x;
*ret_y = curs_y;
break;
default:
/* calculate where scaled and stretched box corners would be */
xx = curs_x - fix_x;
sgn_csr2fix_x = signof(xx);
mag_csr2fix_x = (double) abs(xx);
yy = curs_y - fix_y;
sgn_csr2fix_y = signof(yy);
mag_csr2fix_y = (double) abs(yy);
if (mag_csr2fix_x * sina > mag_csr2fix_y * cosa) { /* above diagonal */
*ret_x = curs_x;
if (constrained == BOX_SCALE) {
if (cosa == 0.0) {
*ret_y = fix_y + sgn_csr2fix_y * (int) (mag_csr2fix_x);
} else {
*ret_y = fix_y + sgn_csr2fix_y * (int) (mag_csr2fix_x * sina / cosa);
}
} else {
*ret_y = fix_y + sgn_csr2fix_y * abs(fix_y - orig_y);
}
} else {
*ret_y = curs_y;
if (constrained == BOX_SCALE) {
if (sina == 0.0) {
*ret_x = fix_x + sgn_csr2fix_x * (int) (mag_csr2fix_y);
} else {
*ret_x = fix_x + sgn_csr2fix_x * (int) (mag_csr2fix_y * cosa / sina);
}
} else {
*ret_x = fix_x + sgn_csr2fix_x * abs(fix_x - orig_x);
}
}
} /* switch */
}
static void
devload(char *path)
{
int i;
Dyndev *l;
Dev *dev;
char devname[32];
l = dlload(path, _exporttab, dyntabsize(_exporttab));
if(waserror()){
dlfree(l);
nexterror();
}
snprint(devname, sizeof(devname), "%sdevtab", "XXX"); /* TO DO */
dev = dynimport(l->o, devname, signof(*dev));
if(dev == nil)
error("no devtab");
if(devno(dev->dc, 1) >= 0)
error("device loaded");
for(i = 0; devtab[i] != nil; i++)
;
if(i >= ndevs || devtab[i+1] != nil)
error("device table full");
l->dev = devtab[i] = dev;
dev->init();
l->next = loaded;
loaded = l;
poperror();
}
static int compare_by_istride(const iodim *a, const iodim *b)
{
INT sai = X(iabs)(a->is), sbi = X(iabs)(b->is);
/* in descending order of istride */
return signof(sbi - sai);
}
/* total order among iodim's */
int X(dimcmp)(const iodim *a, const iodim *b)
{
INT sai = X(iabs)(a->is), sbi = X(iabs)(b->is);
INT sao = X(iabs)(a->os), sbo = X(iabs)(b->os);
INT sam = X(imin)(sai, sao), sbm = X(imin)(sbi, sbo);
/* in descending order of min{istride, ostride} */
if (sam != sbm)
return signof(sbm - sam);
/* in case of a tie, in descending order of istride */
if (sbi != sai)
return signof(sbi - sai);
/* in case of a tie, in descending order of ostride */
if (sbo != sao)
return signof(sbo - sao);
/* in case of a tie, in ascending order of n */
return signof(a->n - b->n);
}
internal void
player_camera_step(Entity *P, Camera *camera)
{
s32 dx = (P->x + P->dx * 16) - camera->x;
s32 sign = signof(dx);
if (abs(dx) > 16) {
dx = sign * 16;
}
camera->vx = sign * (speeds[abs(dx) / 2] * 2);
camera->x += camera->vx;
camera->y = P->y + 32;
}
static void
callfn(Module *m, char *fn)
{
void *v, (*f)(void);
if(m->ref != 1)
return;
v = addr(m->name, fn, m->dlm, signof(*f));
if(v != nil) {
f = v;
(*f)();
}
}
static void check_comparisons(void)
{
static const char *tab[] = {
"",
"a",
"ab",
"abc",
"abcåäö",
"ABCÅÄÖ",
};
static const int n = sizeof(tab) / sizeof(tab[0]);
int i, j;
int sign_str, sign_oct;
Octstr *os1, *os2;
for (i = 0; i < n; ++i) {
os1 = octstr_create(tab[i]);
for (j = 0; j < n; ++j) {
os2 = octstr_create(tab[j]);
sign_str = signof(strcmp(tab[i], tab[j]));
sign_oct = signof(octstr_compare(os1, os2));
if (sign_str != sign_oct)
panic(0, "strcmp (%d) and octstr_compare (%d) differ for "
"`%s' and `%s'", sign_str, sign_oct, tab[i], tab[j]);
sign_str = signof(strcasecmp(tab[i], tab[j]));
sign_oct = signof(octstr_case_compare(os1, os2));
if (sign_str != sign_oct)
panic(0, "strcasecmp (%d) and octstr_case_compare (%d) "
"differ for `%s' and `%s'", sign_str, sign_oct,
tab[i], tab[j]);
octstr_destroy(os2);
}
octstr_destroy(os1);
}
}
internal void
player_step(Entity *P)
{
i32 dx = 0;
Collision C;
world_test_move(&GAME->world, P->collider, 0, 1, CollisionTestMask_TileMap, &C, collision_stop);
P->flags = C.B ? (P->flags | EntityFlag_Grounded) : (P->flags & ~EntityFlag_Grounded);
if (button_down(Button_Right)) dx = 1;
if (button_down(Button_Left)) dx = -1;
if (P->flags & EntityFlag_Grounded) {
// printf("Grounded");
P->vy = 0;
if (button_ended_down(Button_X)) {
P->vy = -3;
}
}
else
{
P->vy += 0.4;
}
// printf("vy %f\n", P->vy);
i32 fx = 0;
if (signof(P->vx) != dx)
{
if (dx == 0) {
player_set_state(P, EntityState_Brake);
} else {
entity_flip(P, (dx < 0));
player_set_state(P, EntityState_Walk);
P->dx = dx;
}
}
fx = dx * 2;
if (fx > P->vx) {
P->vx++;
} else if (fx < P->vx) {
P->vx--;
}
if (dx == 0 && P->vx == 0 && P->state == EntityState_Brake) {
player_set_state(P, EntityState_Idle);
}
}
void get_slope(int dx, int dy, int *sxp, int *syp, int arrow)
{
double angle;
int i, s, max;
double d, d1;
struct angle_table *st;
if (dx == 0) {
*sxp = 0;
*syp = signof(dy);
return;
}
angle = atan((double) abs(dy) / (double) abs(dx)) * rad2deg;
if (arrow) {
st = arrow_angles;
max = 13;
} else {
st = line_angles;
max = 25;
}
s = 0;
d = 9.9e9;
for (i = 0; i < max; i++) {
d1 = fabs(angle - st[i].angle);
if (d1 < d) {
s = i;
d = d1;
}
}
*sxp = st[s].x;
if (dx < 0)
*sxp = -*sxp;
*syp = st[s].y;
if (dy < 0)
*syp = -*syp;
}
void getvoxelsonray(ray3f ray, int maxdepth, float *samples)
{
// Implementation is based on:
// "A Fast Voxel Traversal Algorithm for Ray Tracing"
// John Amanatides, Andrew Woo
// http://www.cse.yorku.ca/~amana/research/grid.pdf
// http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
// NOTES:
// * This code assumes that the ray's origition and direction are in 'cell coordinates', which means
// that one unit equals one cell in all directions.
// * When the ray doesn't start within the voxel grid, calculate the first origition at which the
// ray could enter the grid. If it never enters the grid, there is nothing more to do here.
// * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
// * The vector3f structure is a simple structure having three integer fields (X, Y and Z).
// The cell in which the ray starts.
int rc=0;
vector3i first;
// Rounds the origition's x, y and z down to the nearest integer values.
if ((rc=getfirstvoxel(ray, &first)) == -1) {
//fprintf(stderr, "ray misses volume\n");
return;
};
int x = first.x;
int y = first.y;
int z = first.z;
// Determine which way we go.
vector3i step;
step.x = signof(ray.dir.x);
step.y = signof(ray.dir.y);
step.z = signof(ray.dir.z);
// Calculate cell boundaries. When the step (i.e. direction sign) is origitive,
// the next boundary is AFTER our current origition, meaning that we have to add 1.
// Otherwise, it is BEFORE our current origition, in which case we add nothing.
vector3f cellboundary;
cellboundary.x = x + (step.x > 0 ? 1 : 0);
cellboundary.y = y + (step.y > 0 ? 1 : 0);
cellboundary.z = z + (step.z > 0 ? 1 : 0);
// NOTE: For the following calculations, the result will be HUGE_VAL (+infinity)
// when ray.dir.x, y or z equals zero, which is OK. However, when the left-hand
// value of the division also equals zero, the result is Single.NaN, which is not OK.
// Determine how far we can travel along the ray before we hit a voxel boundary.
vector3f tmax;
tmax.x = (cellboundary.x - ray.orig.x) / ray.dir.x; // Boundary is a plane on the YZ axis.
tmax.y = (cellboundary.y - ray.orig.y) / ray.dir.y; // Boundary is a plane on the XZ axis.
tmax.z = (cellboundary.z - ray.orig.z) / ray.dir.z; // Boundary is a plane on the XY axis.
if (isnan(tmax.x)) tmax.x = HUGE_VAL;
if (isnan(tmax.y)) tmax.y = HUGE_VAL;
if (isnan(tmax.z)) tmax.z = HUGE_VAL;
// Determine how far we must travel along the ray before we have crossed a gridcell.
vector3f deltat ;
deltat.x = step.x / ray.dir.x; // Crossing the width of a cell.
deltat.y = step.y / ray.dir.y; // Crossing the height of a cell.
deltat.z = step.z / ray.dir.z; // Crossing the depth of a cell.
if (isnan(deltat.x)) deltat.x = HUGE_VAL;
if (isnan(deltat.y)) deltat.y = HUGE_VAL;
if (isnan(deltat.z)) deltat.z = HUGE_VAL;
// For each step, determine which distance to the next voxel boundary is lowest (i.e.
// which voxel boundary is nearest) and walk that way.
for (int i = 0; i < maxdepth; i++)
{
// compute index and save it.
samples[i] = maptoffset3d(x,y,z, volume.localdims.w, volume.localdims.h, volume.localdims.d);
// Do the next step.
if (tmax.x < tmax.y && tmax.x < tmax.z)
{
// tmax.x is the lowest, an YZ cell boundary plane is nearest.
x += step.x;
tmax.x += deltat.x;
}
else if (tmax.y < tmax.z)
{
// tmax.y is the lowest, an XZ cell boundary plane is nearest.
y += step.y;
tmax.y += deltat.y;
}
else
{
// tmax.z is the lowest, an XY cell boundary plane is nearest.
z += step.z;
tmax.z += deltat.z;
}
}
}
/* The code below is based on loop_unroll processOpnd function. However it
* gathers some additional OSR-specific information which is obsolele for
* loop_unroll.
*
* TODO: create flexible mechanism for gathering info on the operands which
* would help to avoid code duplication from the one hand, and be customizable
* from another hand
*/
OSROpndInfo OSRInductionDetector::processOpnd(LoopTree* tree,
LoopNode* loopHead,
InstStack& defStack,
const Opnd* opnd,
iv_detection_flag flag) {
if (Log::isEnabled()) {
Log::out() << "Processing opnd: ";
opnd->print(Log::out());
Log::out() << "\n";
}
OSROpndInfo result;
Inst* defInst = opnd->getInst();
if (std::find(defStack.begin(), defStack.end(), defInst) !=
defStack.end()) {
result.setType(OSROpndInfo::COUNTER);
result.setIncrement(0);
result.setOpnd((Opnd*) opnd);
return result;
}
Opcode opcode = defInst->getOpcode();
if (opcode == Op_LdConstant) {
result.setType(OSROpndInfo::LD_CONST);
result.setConst(defInst->asConstInst()->getValue().i4);
result.setOpnd((Opnd*) opnd);
result.setHeader((Opnd*) opnd);
result.setHeaderFound();
return result;
}
if (!inExactLoop(tree, (Opnd*) opnd, loopHead)) {
result.setOpnd((Opnd*) opnd);
result.setHeader((Opnd*) opnd);
result.setHeaderFound();
return result;
}
defStack.push_back(defInst);
if (opcode == Op_Phi) {
OSROpndInfo info1 =
processOpnd(tree, loopHead, defStack, defInst->getSrc(0));
if (defInst->getNumSrcOperands() > 1) {
OSROpndInfo info2 =
processOpnd(tree, loopHead, defStack, defInst->getSrc(1));
if ( ((info1.isCounter() && !info1.isPhiSplit())
&& (info2.isDOL() || info2.isLDConst()))
|| ((info2.isCounter() && !info2.isPhiSplit())
&& (info1.isDOL() || info1.isLDConst())) ) {
result.setType(OSROpndInfo::COUNTER);
result.setIncrement(info1.isCounter()? info1.
getIncrement() : info2.getIncrement());
result.markPhiSplit();
result.setHeader((Opnd*) opnd);
result.setHeaderFound();
} else if ((flag == CHOOSE_MAX_IN_BRANCH) && info1.isCounter()
&& info2.isCounter()
&& signof(info1.getIncrement()) ==
signof(info2.getIncrement())) {
result.setType(OSROpndInfo::COUNTER);
result.setIncrement(std::abs(info1.getIncrement()) >
std::abs(info2.getIncrement())? info1.
getIncrement() : info2.getIncrement());
result.markPhiSplit();
result.setHeader((Opnd*) opnd);
result.setHeaderFound();
} else {
result.setType(OSROpndInfo::UNDEF);
}
}
} else if (opcode == Op_Add || opcode == Op_Sub) {
Opnd* opnd1 = defInst->getSrc(0);
Opnd* opnd2 = defInst->getSrc(1);
OSROpndInfo info1 = processOpnd(tree, loopHead, defStack, opnd1);
OSROpndInfo info2 = processOpnd(tree, loopHead, defStack, opnd2);
if ((info1.isLDConst() || info1.isDOL())
&& (info2.isLDConst() || info2.isDOL())) {
if (info1.isLDConst() && info2.isLDConst()
&& info1.getConst() == info2.getConst()) {
result.setType(OSROpndInfo::LD_CONST);
result.setConst(info1.getConst());
writeHeaderToResult(result, tree, info1, info2);
}
} else if ((info1.isCounter() && info2.isLDConst())
|| (info2.isCounter() && info1.isLDConst())) {
U_32 increment = info1.isCounter() ?
info1.getIncrement() : info2.getIncrement();
U_32 diff = info1.isLDConst()? info1.getConst() : info2.getConst();
bool monotonousFlag = increment == 0 || diff == 0
|| (opcode == Op_Add && signof(diff) == signof(increment))
|| (opcode == Op_Sub && signof(diff) != signof(increment));
if (monotonousFlag) {
result.setType(OSROpndInfo::COUNTER);
if ((info1.isCounter() && info1.isPhiSplit()) ||
(info2.isCounter() && info2.isPhiSplit())) {
result.markPhiSplit();
writeHeaderToResult(result, tree, info1, info2);
}
if (opcode == Op_Add) {
result.setIncrement(increment + diff);
writeHeaderToResult(result, tree, info1, info2);
} else {
result.setIncrement(increment - diff);
writeHeaderToResult(result, tree, info1, info2);
}
} else {
result.setType(OSROpndInfo::UNDEF);
}
} else {
result.setType(OSROpndInfo::UNDEF);
}
} else if (opcode == Op_StVar || opcode == Op_LdVar) {
Opnd* newOpnd = defInst->getSrc(0);
result = processOpnd(tree, loopHead, defStack, newOpnd);
} else if (opcode == Op_TauArrayLen) {
Opnd* arrayOpnd = defInst->getSrc(0);
result = processOpnd(tree, loopHead, defStack, arrayOpnd);
} else {
result.setType(OSROpndInfo::UNDEF);
}
defStack.pop_back();
result.setOpnd((Opnd*) opnd);
return result;
}
static OpndLoopInfo processOpnd(LoopNode* loopHead, LoopTree* lt, InstStack& defStack, Opnd* opnd) {
OpndLoopInfo result;
Inst* defInst = opnd->getInst();
if (Log::isEnabled()) {
log_ident(defStack.size()); defInst->print(Log::out()); Log::out()<<"]"<<std::endl;
}
if (std::find(defStack.begin(), defStack.end(), defInst)!=defStack.end()) {
result.setType(OpndLoopInfo::COUNTER);
result.setIncrement(0);
if (Log::isEnabled()) {
log_ident(defStack.size());
Log::out()<<"Found duplicate in def stack -> stopping recursion. ";result.print(Log::out()); Log::out()<<std::endl;
}
return result;
}
Node* defNode = defInst->getNode();
Opcode opcode = defInst->getOpcode();
if (opcode == Op_LdConstant) {
result.setType(OpndLoopInfo::LD_CONST);
result.setConst(defInst->asConstInst()->getValue().i4);
if (Log::isEnabled()) {
log_ident(defStack.size());
Log::out()<<"assigning to const -> stopping recursion. ";result.print(Log::out());Log::out()<<std::endl;
}
return result;
}
if (!loopHead->inLoop(defNode)) {
if (Log::isEnabled()) {
log_ident(defStack.size());
Log::out()<<"Inst out of the loop -> stopping recursion. ";result.print(Log::out()); Log::out()<<std::endl;
}
return result;
}
defStack.push_back(defInst);
if (opcode == Op_Phi) {
OpndLoopInfo info1 = processOpnd(loopHead, lt, defStack, defInst->getSrc(0));
OpndLoopInfo info2 = processOpnd(loopHead, lt, defStack, defInst->getSrc(1));
if (Log::isEnabled()) {
log_ident(defStack.size());
Log::out()<<"PHI(";info1.print(Log::out());Log::out()<<",";info2.print(Log::out());Log::out()<<")"<<std::endl;
}
if ( ((info1.isCounter() && !info1.isPhiSplit()) && (info2.isDOL() || info2.isLDConst()))
|| ((info2.isCounter() && !info2.isPhiSplit()) && (info1.isDOL() || info1.isLDConst())) )
{
result.setType(OpndLoopInfo::COUNTER);
result.setIncrement(info1.isCounter() ? info1.getIncrement() : info2.getIncrement());
result.markPhiSplit();
} else {
result.setType(OpndLoopInfo::UNDEF);
}
} else if (opcode == Op_Add || opcode == Op_Sub) { //todo: LADD
Opnd *op1 = defInst->getSrc(0);
Opnd *op2 = defInst->getSrc(1);
OpndLoopInfo info1 = processOpnd(loopHead, lt, defStack, op1);
OpndLoopInfo info2 = processOpnd(loopHead, lt, defStack, op2);
if ((info1.isLDConst() || info1.isDOL()) && (info2.isLDConst() || info2.isDOL())) {
if (info1.isLDConst() && info2.isLDConst() && info1.getConst() == info2.getConst()) {
result.setType(OpndLoopInfo::LD_CONST);
result.setConst(info1.getConst());
} else {
//result is DOL (default type)
}
} else if ((info1.isCounter() && info2.isLDConst()) || (info2.isCounter() && info1.isLDConst())) {
int increment = info1.isCounter()? info1.getIncrement(): info2.getIncrement();
int diff = info1.isLDConst()? info1.getConst(): info2.getConst();
//we use SSA form to analyze how opnd changes in loop and we do not analyze actual control flow,
// so we can unroll loops with monotonically changing 'counters' only.
//Example: when 'counter' changes not monotonically and we can't unroll:
//idx=0; loop {idx+=100; if(idx>=100) break; idx-=99;} ->'increment'=1 but not monotonicaly.
bool monotonousFlag = increment == 0 || diff == 0
|| (opcode == Op_Add && signof(diff) == signof(increment))
|| (opcode == Op_Sub && signof(diff) != signof(increment));
if (monotonousFlag) {
result.setType(OpndLoopInfo::COUNTER);
if ((info1.isCounter() && info1.isPhiSplit()) || (info2.isCounter() && info2.isPhiSplit())) {
result.markPhiSplit();
}
//TO IMPROVE: for loops like: for (; length-1>=0;length--){...}
//we have 2 SUBs by -1 => "-2", but real counter is changed by "-1".
//Loop unroll will use "-2". It's ok, because this value is used in a guard inst
//and ABS(increment_in_unroll) >= ABS(real_increment). This work only for monotonous loops.
//To make increment_in_unroll == real_increment we must track modifications (SUB,ADD) that affects vars only.
if (opcode == Op_Add) {
result.setIncrement(increment + diff);
} else {
result.setIncrement(increment - diff);
}
} else {
result.setType(OpndLoopInfo::UNDEF);
}
} else {
result.setType(OpndLoopInfo::UNDEF);
}
} else if (opcode == Op_StVar || opcode == Op_LdVar) {
Opnd* newOpnd = defInst->getSrc(0);
result = processOpnd(loopHead, lt, defStack, newOpnd);
} else if (opcode == Op_TauArrayLen) {
Opnd* arrayOpnd = defInst->getSrc(0);
result = processOpnd(loopHead, lt, defStack, arrayOpnd);
} else { //unsupported op
result.setType(OpndLoopInfo::UNDEF);
if (Log::isEnabled()) {
log_ident(defStack.size()); Log::out()<<"unknown op -> stopping recursion. ";
}
}
defStack.pop_back();
if (Log::isEnabled()) {
log_ident(defStack.size());
result.print(Log::out());Log::out()<<std::endl;
}
return result;
}
int main () {
unsigned char bytes[256];
int i, j, k, n;
int *result;
/* Table 2. Double-precision floating-point arithmetic. */
deq (__aeabi_dadd (dzero, done), done);
deq (__aeabi_dadd (done, done), dtwo);
deq (__aeabi_ddiv (dminus_four, dminus_two), dtwo);
deq (__aeabi_ddiv (dminus_two, dtwo), dminus_one);
deq (__aeabi_dmul (dtwo, dtwo), dfour);
deq (__aeabi_dmul (dminus_one, dminus_two), dtwo);
deq (__aeabi_dneg (dminus_one), done);
deq (__aeabi_dneg (dfour), dminus_four);
deq (__aeabi_drsub (done, dzero), dminus_one);
deq (__aeabi_drsub (dtwo, dminus_two), dminus_four);
deq (__aeabi_dsub (dzero, done), dminus_one);
deq (__aeabi_dsub (dminus_two, dtwo), dminus_four);
/* Table 3. Double-precision floating-point comparisons. */
ieq (__aeabi_dcmpeq (done, done), 1);
ieq (__aeabi_dcmpeq (done, dzero), 0);
ieq (__aeabi_dcmpeq (dNaN, dzero), 0);
ieq (__aeabi_dcmpeq (dNaN, dNaN), 0);
ieq (__aeabi_dcmplt (dzero, done), 1);
ieq (__aeabi_dcmplt (done, dzero), 0);
ieq (__aeabi_dcmplt (dzero, dzero), 0);
ieq (__aeabi_dcmplt (dzero, dNaN), 0);
ieq (__aeabi_dcmplt (dNaN, dNaN), 0);
ieq (__aeabi_dcmple (dzero, done), 1);
ieq (__aeabi_dcmple (done, dzero), 0);
ieq (__aeabi_dcmple (dzero, dzero), 1);
ieq (__aeabi_dcmple (dzero, dNaN), 0);
ieq (__aeabi_dcmple (dNaN, dNaN), 0);
ieq (__aeabi_dcmpge (dzero, done), 0);
ieq (__aeabi_dcmpge (done, dzero), 1);
ieq (__aeabi_dcmpge (dzero, dzero), 1);
ieq (__aeabi_dcmpge (dzero, dNaN), 0);
ieq (__aeabi_dcmpge (dNaN, dNaN), 0);
ieq (__aeabi_dcmpgt (dzero, done), 0);
ieq (__aeabi_dcmpgt (done, dzero), 1);
ieq (__aeabi_dcmplt (dzero, dzero), 0);
ieq (__aeabi_dcmpgt (dzero, dNaN), 0);
ieq (__aeabi_dcmpgt (dNaN, dNaN), 0);
ieq (__aeabi_dcmpun (done, done), 0);
ieq (__aeabi_dcmpun (done, dzero), 0);
ieq (__aeabi_dcmpun (dNaN, dzero), 1);
ieq (__aeabi_dcmpun (dNaN, dNaN), 1);
/* Table 4. Single-precision floating-point arithmetic. */
feq (__aeabi_fadd (fzero, fone), fone);
feq (__aeabi_fadd (fone, fone), ftwo);
feq (__aeabi_fdiv (fminus_four, fminus_two), ftwo);
feq (__aeabi_fdiv (fminus_two, ftwo), fminus_one);
feq (__aeabi_fmul (ftwo, ftwo), ffour);
feq (__aeabi_fmul (fminus_one, fminus_two), ftwo);
feq (__aeabi_fneg (fminus_one), fone);
feq (__aeabi_fneg (ffour), fminus_four);
feq (__aeabi_frsub (fone, fzero), fminus_one);
feq (__aeabi_frsub (ftwo, fminus_two), fminus_four);
feq (__aeabi_fsub (fzero, fone), fminus_one);
feq (__aeabi_fsub (fminus_two, ftwo), fminus_four);
/* Table 5. Single-precision floating-point comparisons. */
ieq (__aeabi_fcmpeq (fone, fone), 1);
ieq (__aeabi_fcmpeq (fone, fzero), 0);
ieq (__aeabi_fcmpeq (fNaN, fzero), 0);
ieq (__aeabi_fcmpeq (fNaN, fNaN), 0);
ieq (__aeabi_fcmplt (fzero, fone), 1);
ieq (__aeabi_fcmplt (fone, fzero), 0);
ieq (__aeabi_fcmplt (fzero, fzero), 0);
ieq (__aeabi_fcmplt (fzero, fNaN), 0);
ieq (__aeabi_fcmplt (fNaN, fNaN), 0);
ieq (__aeabi_fcmple (fzero, fone), 1);
ieq (__aeabi_fcmple (fone, fzero), 0);
ieq (__aeabi_fcmple (fzero, fzero), 1);
ieq (__aeabi_fcmple (fzero, fNaN), 0);
ieq (__aeabi_fcmple (fNaN, fNaN), 0);
ieq (__aeabi_fcmpge (fzero, fone), 0);
ieq (__aeabi_fcmpge (fone, fzero), 1);
ieq (__aeabi_fcmpge (fzero, fzero), 1);
ieq (__aeabi_fcmpge (fzero, fNaN), 0);
ieq (__aeabi_fcmpge (fNaN, fNaN), 0);
ieq (__aeabi_fcmpgt (fzero, fone), 0);
ieq (__aeabi_fcmpgt (fone, fzero), 1);
ieq (__aeabi_fcmplt (fzero, fzero), 0);
ieq (__aeabi_fcmpgt (fzero, fNaN), 0);
ieq (__aeabi_fcmpgt (fNaN, fNaN), 0);
ieq (__aeabi_fcmpun (fone, fone), 0);
ieq (__aeabi_fcmpun (fone, fzero), 0);
ieq (__aeabi_fcmpun (fNaN, fzero), 1);
ieq (__aeabi_fcmpun (fNaN, fNaN), 1);
/* Table 6. Floating-point to integer conversions. */
ieq (__aeabi_d2iz (dminus_one), -1);
ueq (__aeabi_d2uiz (done), 1);
leq (__aeabi_d2lz (dminus_two), -2LL);
uleq (__aeabi_d2ulz (dfour), 4LL);
ieq (__aeabi_f2iz (fminus_one), -1);
ueq (__aeabi_f2uiz (fone), 1);
leq (__aeabi_f2lz (fminus_two), -2LL);
uleq (__aeabi_f2ulz (ffour), 4LL);
/* Table 7. Conversions between floating types. */
feq (__aeabi_d2f (dtwo), ftwo);
deq (__aeabi_f2d (fminus_four), dminus_four);
/* Table 8. Integer to floating-point conversions. */
deq (__aeabi_i2d (-1), dminus_one);
deq (__aeabi_ui2d (2), dtwo);
deq (__aeabi_l2d (-1), dminus_one);
deq (__aeabi_ul2d (2ULL), dtwo);
feq (__aeabi_i2f (-1), fminus_one);
feq (__aeabi_ui2f (2), ftwo);
feq (__aeabi_l2f (-1), fminus_one);
feq (__aeabi_ul2f (2ULL), ftwo);
/* Table 9. Long long functions. */
leq (__aeabi_lmul (4LL, -1LL), -4LL);
leq (__aeabi_llsl (2LL, 1), 4LL);
leq (__aeabi_llsr (-1LL, 63), 1);
leq (__aeabi_lasr (-1LL, 63), -1);
result = lcmp_results;
for (i = 0; i < NUM_CMP_VALUES; i++)
for (j = 0; j < NUM_CMP_VALUES; j++)
for (k = 0; k < NUM_CMP_VALUES; k++)
for (n = 0; n < NUM_CMP_VALUES; n++)
{
ieq (signof (__aeabi_lcmp
(((long long)cmp_val[i] << 32) | cmp_val[k],
((long long)cmp_val[j] << 32) | cmp_val[n])),
*result);
result++;
}
result = ulcmp_results;
for (i = 0; i < NUM_CMP_VALUES; i++)
for (j = 0; j < NUM_CMP_VALUES; j++)
for (k = 0; k < NUM_CMP_VALUES; k++)
for (n = 0; n < NUM_CMP_VALUES; n++)
{
ieq (signof (__aeabi_ulcmp
(((long long)cmp_val[i] << 32) | cmp_val[k],
((long long)cmp_val[j] << 32) | cmp_val[n])),
*result);
result++;
}
ieq (__aeabi_idiv (-550, 11), -50);
ueq (__aeabi_uidiv (4000000000U, 1000000U), 4000U);
for (i = 0; i < 256; i++)
bytes[i] = i;
#ifdef __ARMEB__
ieq (__aeabi_uread4 (bytes + 1), 0x01020304U);
leq (__aeabi_uread8 (bytes + 3), 0x030405060708090aLL);
ieq (__aeabi_uwrite4 (0x66778899U, bytes + 5), 0x66778899U);
leq (__aeabi_uwrite8 (0x2030405060708090LL, bytes + 15),
0x2030405060708090LL);
#else
ieq (__aeabi_uread4 (bytes + 1), 0x04030201U);
leq (__aeabi_uread8 (bytes + 3), 0x0a09080706050403LL);
ieq (__aeabi_uwrite4 (0x99887766U, bytes + 5), 0x99887766U);
leq (__aeabi_uwrite8 (0x9080706050403020LL, bytes + 15),
0x9080706050403020LL);
#endif
for (i = 0; i < 4; i++)
ieq (bytes[5 + i], (6 + i) * 0x11);
for (i = 0; i < 8; i++)
ieq (bytes[15 + i], (2 + i) * 0x10);
exit (0);
}
//Receive encoder ticks and send 'odom' and 'tf'
void robotDataCallback(std::string * data){
if (confirm_communication){
//ROS_INFO("Robot -- Communication OK! Received: \"%s\"", data->c_str());
ROS_INFO("Traxbot is Streaming Data.");
confirm_communication = false;
}
int first_at = data->find_first_of("@", 0);
int second_at = data->find_first_of("@", first_at+1);
int first_comma = data->find_first_of(",", 0);
int second_comma = data->find_first_of(",", first_comma+1);
//protection against broken msgs from the buffer (e.g., '@6,425@6,4250,6430e')
if ( second_at > -1 || second_comma == -1){
ROS_WARN("%s ::: ENCODER MSG IGNORED", data->c_str());
return;
}
int left_encoder_count, right_encoder_count;
sscanf(data->c_str(), "@6,%d,%de", &right_encoder_count, &left_encoder_count); //encoder msg parsing
//protection against broken msgs from the buffer (e.g., '@6,425@6,4250,6430e')
if ( abs(right_encoder_count) > MAX_ENCODER_COUNT || abs(left_encoder_count) > MAX_ENCODER_COUNT){
ROS_WARN("Encoders > MAX_ENCODER_COUNT");
return;
}
double last_x = odometry_x_;
double last_y = odometry_y_;
double last_yaw = odometry_yaw_;
// It is not necessary to reset the encoders:
int right_enc_dif = 0, left_enc_dif = 0;
if ( right_encoder_prev >= (0.75*MAX_ENCODER_COUNT) && signof (right_encoder_count) == 0 && signof(right_encoder_prev) == 1 ){
right_enc_dif = (MAX_ENCODER_COUNT-right_encoder_prev) + (MAX_ENCODER_COUNT+right_encoder_count);
}else if (right_encoder_prev <= (-0.75*MAX_ENCODER_COUNT) && signof (right_encoder_count) == 1 && signof(right_encoder_prev) == 0){
right_enc_dif = -(MAX_ENCODER_COUNT+right_encoder_prev) + (right_encoder_count-MAX_ENCODER_COUNT);
}else{
right_enc_dif = right_encoder_count - right_encoder_prev;
}
if ( left_encoder_prev >= (0.75*MAX_ENCODER_COUNT) && signof (left_encoder_count) == 0 && signof(left_encoder_prev) == 1){
left_enc_dif = (MAX_ENCODER_COUNT-left_encoder_prev) + (MAX_ENCODER_COUNT+left_encoder_count);
}else if (left_encoder_prev <= (-0.75*MAX_ENCODER_COUNT) && signof (left_encoder_count) == 1 && signof(left_encoder_prev) == 0){
left_enc_dif = -(MAX_ENCODER_COUNT+left_encoder_prev) + (left_encoder_count-MAX_ENCODER_COUNT);
}else{
left_enc_dif = left_encoder_count - left_encoder_prev;
}
//calulate Odometry:
double dist = (right_enc_dif*PULSES_TO_M + left_enc_dif*PULSES_TO_M) / 2.0;
double ang = (right_enc_dif*PULSES_TO_M - left_enc_dif*PULSES_TO_M) / AXLE_LENGTH;
bool publish_info = true;
if (right_encoder_prev == right_encoder_count && left_encoder_prev == left_encoder_count){
publish_info = false;
}
// update previous encoder counts:
left_encoder_prev = left_encoder_count;
right_encoder_prev = right_encoder_count;
// Update odometry
odometry_yaw_ = NORMALIZE(odometry_yaw_ + ang); // rad
odometry_x_ = odometry_x_ + dist*cos(odometry_yaw_); // m
odometry_y_ = odometry_y_ + dist*sin(odometry_yaw_); // m
last_time = current_time;
current_time = ros::Time::now();
// Calculate the robot speed
double dt = (current_time - last_time).toSec();
double vel_x = (odometry_x_ - last_x)/dt;
double vel_y = (odometry_y_ - last_y)/dt;
double vel_yaw = (odometry_yaw_ - last_yaw)/dt;
//Publish at least at most at 75Hz
/*if (current_time.toSec() - 0.013 >= last_time_pub){
publish_info = true;
}*/
//if(publish_info){
last_time_pub = current_time.toSec();
// Since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometry_yaw_);
// First, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = odometry_x_;
odom_trans.transform.translation.y = odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
// Send the transform
odom_broadcaster_ptr->sendTransform(odom_trans); // odom->base_link transform
// Next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
// Set the position
odom.pose.pose.position.x = odometry_x_;
odom.pose.pose.position.y = odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
// Set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
// Publish the message
odom_pub_ptr->publish(odom); // odometry publisher
//}
}
int compare_double(const void *_a, const void *_b)
{
const double *a = (const double*)_a;
const double *b = (const double*)_b;
return signof(*a - *b);
}
Asserv::Asserv(IncrementalEncoder& _left, IncrementalEncoder& _right,
Timer& tim, HBridgeST &mot1, HBridgeST &mot2) :
tim(tim),
eLeft(_left), eRight(_right),
motorl(mot1), motorr(mot2),
infos(left, right, eLeft, eRight, 1, 1),
position(infos, 1000, 1000),
targetAngle(0), targetDist(0),
c_propDist(0x20000), c_propAngle(0x800),
c_intDist(0), c_intAngle(0x12),
c_velDist(0x0), c_velAngle(0),
c_accelDist(0x0), c_accelAngle(0),
maxEngine(0x3ff), minEngine(0x80),
maxForwardAccel(0x80), maxBackwardAccel(0x80),
maxRotationAccel(0x10000),
waiting(false),
date(0), dateStart(0) {
tim
.setPrescaler(42)
.setAutoReload(1000)
.setOneShot(false)
.setUIE(true)
.setURS(true);
Irq(tim.irqNr())
.setPriority(15)
.enable();
throttle = 100;
tim
.setTopCB([&tim, this](int timer_id) {
(void)timer_id;
++date;
infos.compute(targetDist, targetAngle);
position.update();
int d_d = 0, d_a = 0;
//Distance
d_d += c_accelDist * infos.getAccelDist();
d_d += c_velDist * infos.getVelocityDist();
d_d += c_propDist * infos.getDeltaDist();
d_d += c_intDist * infos.getIntegralDist();
//Angle
d_a += c_accelAngle * infos.getAccelDist();
d_a += c_velAngle * infos.getVelocityAngle();
d_a += c_propAngle * infos.getDeltaAngle();
d_a += c_intAngle * infos.getIntegralAngle();
#define abs(x) ((x) > 0 ? (x) : -(x))
#define signof(x, y) ((x) > 0 ? (y) : -(y))
#if 1
//Check if we're near the destination (dist)
int maxAccel = infos.getVelocityDist() > 0 ? maxBackwardAccel : maxForwardAccel;
int x0 = (1000*infos.getVelocityDist()*infos.getVelocityDist())/(16*maxAccel);
x0 = abs(x0);
if(abs(infos.getDeltaDist()) < x0) {
//Brrrrrrrrrrrrrrrrrrrrrrrrrraaaaaaaaaaakkkkkeeeeeeeee
d_d = 0;
}
#endif
#if 1
//Check if we're near the destination (angle)
int t0 = (1000*infos.getVelocityAngle()*infos.getVelocityAngle())/(16*maxRotationAccel);
t0 = abs(t0);
if(abs(infos.getDeltaAngle()) < t0) {
//Brrrrrrrrrrrrrrrrrrrrrrrrrraaaaaaaaaaakkkkkeeeeeeeee
d_a = 0;
}
#endif
int dl = d_d + d_a;
int dr = d_d - d_a;
dl/=0x4000;
dr/=0x4000;
if(abs(dl) > maxEngine || abs(dr) > maxEngine) {
if(abs(dl) > abs(dr)) {
dr = signof(dr, abs(dr)-abs(dl)+maxEngine);
dl = signof(dl, maxEngine);
} else {
dl = signof(dl, abs(dl)-abs(dr)+maxEngine);
dr = signof(dr, maxEngine);
}
}
int tot = abs(dl)+abs(dr);
if(tot < minEngine) {
dl = 0;
dr = 0;
}
//Check we're not blocked
if(infos.getVelocityDist() == 0 && infos.getVelocityAngle() == 0) {
beenZero++;
} else
beenZero = 0;
//150ms
if(beenZero > 150) {
if(dateStart) {
log << "At " << date << ", we finished command from " << dateStart << endl;
int res = date - dateStart;
log << "That makes time of " << res/1000 << "s" << res%1000 << "ms" << endl;
dateStart = 0;
waiting = false;
}
dl = 0;
dr = 0;
}
//ABS/ESP
if(infos.getAccelDist() > maxForwardAccel || infos.getAccelDist() < -maxBackwardAccel) {
if(throttle > 0) {
throttle -= 10;
}
log << "Throttle at " << throttle << endl;
log << " Accel = " << infos.getAccelDist() << endl;
} else {
if(throttle < 100) {
log << "Throttle at " << throttle << endl;
throttle += 10;
}
}
int ref = 0;
#if 1
if( (infos.getAccelDist() * infos.getVelocityDist()) > 0) {
//We are accelerating
//Throttling means reducing power
ref = 0;
} else if( (infos.getAccelDist() * infos.getVelocityDist()) < 0) {
//We are braking
//Throttling means increasing power
ref = signof(infos.getVelocityDist(), 4096);
}
#endif
dl = ref * (100-throttle) + throttle * dl;
dl /= 100;
dr = ref * (100-throttle) + throttle * dr;
dr /= 100;
motorl.setSpeed(dl);
motorr.setSpeed(dr);
})
.enable();
}
