//backs up the robot back onto the junction
void backUp()
{
delay(500);
setBoth(-80);
while( (getSensor(4) < 630) || (getSensor(5) < 630) )
{
delay(50);
}
setBoth(0);
}
void leftTurn()
{
//set the motors to turn the robot
setMotorSpeed(-140,0);
setMotorSpeed(140,1);
//we will stay in this loop whilst both sensors cannot see a line
while((getSensor(4) > 630) || (getSensor(5) > 630))
{
delay(5);
}
delay(100);
//at this point the robot is facing the slightly wrong direction
//so we turn for a bit more
while((getSensor(4) < 630) && (getSensor(5) < 630))
{
delay(5);
}
//the robot has compleated a 90 degrees turn
//stop the motors
setBoth(0);
}
void manualRightTurn()
{
setMotorspeed(-150,0);
setMotorSpeed(150,1);
delay(150);
setBoth(0);
}
void Hole::setHoleSize(QString newSize, bool force) {
//DebugDialog::debug(QString("old holesize %1").arg(viewID()) + holeSize(), sceneBoundingRect());
//foreach (QGraphicsItem * childItem, childItems()) {
// DebugDialog::debug(QString(" child"), childItem->sceneBoundingRect());
//}
if (PaletteItem::setHoleSize(newSize, force, m_holeSettings)) {
setBoth(m_holeSettings.holeDiameter, m_holeSettings.ringThickness);
modelPart()->setLocalProp("hole size", newSize);
if (m_partLabel) m_partLabel->displayTextsIf();
}
//DebugDialog::debug(QString("new holesize %1 ").arg(viewID()) + holeSize(), sceneBoundingRect());
//foreach (QGraphicsItem * childItem, childItems()) {
// DebugDialog::debug(QString(" child"), childItem->sceneBoundingRect());
//}
}
void rightTurn()
{
setMotorSpeed(190,0);
setMotorSpeed(-145,1);
while((getSensor(4) > 630) || (getSensor(5) > 630))
{
delay(5);
}
delay(100);
while((getSensor(4) < 630) && (getSensor(5) < 630))
{
delay(5);
}
setBoth(0);
}
/* Deletes from |tree| and returns an item matching |item|.
Returns a null pointer if no matching item found. */
struct AVLNode *
AVL_Remove(struct AVLNode **tree, struct AVLNode *p)
{
struct AVLNode *q; /* Parent of |p|. */
int dir; /* Side of |q| on which |p| is linked. */
//q = p->Parent;
q = getParent(p);
if (q == NULL) {
q = (struct AVLNode *)tree;
dir = 0;
} else {
dir = q->Link[0] != p;
}
if (p->Link[1] == NULL) {
q->Link[dir] = p->Link[0];
if (q->Link[dir] != NULL)
//q->Link[dir]->Parent = p->Parent;
setParent(q->Link[dir], getParent(p));
} else {
struct AVLNode *r = p->Link[1];
if (r->Link[0] == NULL) {
r->Link[0] = p->Link[0];
q->Link[dir] = r;
//r->Parent = p->Parent;
if (r->Link[0] != NULL)
//r->Link[0]->Parent = r;
setParent(r->Link[0], r);
//r->Balance = p->Balance;
r->Private = p->Private;
q = r;
dir = 1;
} else {
struct AVLNode *s = r->Link[0];
while (s->Link[0] != NULL)
s = s->Link[0];
//r = s->Parent;
r = getParent(s);
r->Link[0] = s->Link[1];
s->Link[0] = p->Link[0];
s->Link[1] = p->Link[1];
q->Link[dir] = s;
if (s->Link[0] != NULL)
//s->Link[0]->Parent = s;
setParent(s->Link[0], s);
//s->Link[1]->Parent = s;
setParent(s->Link[1], s);
//s->Parent = p->Parent;
if (r->Link[0] != NULL)
//r->Link[0]->Parent = r;
setParent(r->Link[0], r);
//s->Balance = p->Balance;
s->Private = p->Private;
q = r;
dir = 0;
}
}
//tree->AVL_alloc->libavl_free (tree->AVL_alloc, p);
while (q != (struct AVLNode *)tree) {
struct AVLNode *y = q;
//if (y->Parent != NULL)
// q = y->Parent;
if (getParent(y) != NULL)
q = getParent(y);
else
q = (struct AVLNode *)tree;
if (dir == 0) {
dir = q->Link[0] != y;
//y->Balance++;
incBalance(y);
//if (y->Balance == +1)
if (getBalance(y) == +1)
break;
//else if (y->Balance == +2) {
else if (getBalance(y) == +2) {
struct AVLNode *x = y->Link[1];
//if (x->Balance == -1) {
if (getBalance(x) == -1) {
struct AVLNode *w;
//assert (x->Balance == -1);
w = x->Link[0];
x->Link[0] = w->Link[1];
w->Link[1] = x;
y->Link[1] = w->Link[0];
w->Link[0] = y;
//if (w->Balance == +1)
// x->Balance = 0, y->Balance = -1;
//else if (w->Balance == 0)
// x->Balance = y->Balance = 0;
//else /* |w->Balance == -1| */
// x->Balance = +1, y->Balance = 0;
int b = getBalance(w);
intptr_t yp = y->Private;
if (b == +1) {
setBoth(x, w, 0);
setBoth(y, w, -1);
} else if (b == 0) {
setBoth(x, w, 0);
setBoth(y, w, 0);
} else {
setBoth(x, w, +1);
setBoth(y, w, 0);
}
//w->Balance = 0;
//w->Parent = y->Parent;
setBoth(w, yp, 0);
//x->Parent = y->Parent = w;
if (x->Link[0] != NULL)
//x->Link[0]->Parent = x;
setParent(x->Link[0], x);
if (y->Link[1] != NULL)
//y->Link[1]->Parent = y;
setParent(y->Link[1], y);
q->Link[dir] = w;
} else {
y->Link[1] = x->Link[0];
x->Link[0] = y;
intptr_t yp = y->Private;
//x->Parent = y->Parent;
//y->Parent = x;
if (y->Link[1] != NULL)
//y->Link[1]->Parent = y;
setParent(y->Link[1], y);
q->Link[dir] = x;
//if (x->Balance == 0) {
// x->Balance = -1;
// y->Balance = +1;
// break;
//} else {
// x->Balance = y->Balance = 0;
// y = x;
//}
if (getBalance(x) == 0) {
setBoth(x, yp, -1);
setBoth(y, x, +1);
break;
} else {
setBoth(x, yp, 0);
setBoth(y, x, 0);
y = x;
}
}
}
} else {
dir = q->Link[0] != y;
//y->Balance--;
decBalance(y);
//if (y->Balance == -1)
if (getBalance(y) == -1)
break;
//else if (y->Balance == -2) {
else if (getBalance(y) == -2) {
struct AVLNode *x = y->Link[0];
//if (x->Balance == +1) {
if (getBalance(x) == +1) {
struct AVLNode *w;
//assert (x->Balance == +1);
w = x->Link[1];
x->Link[1] = w->Link[0];
w->Link[0] = x;
y->Link[0] = w->Link[1];
w->Link[1] = y;
//if (w->Balance == -1)
// x->Balance = 0, y->Balance = +1;
//else if (w->Balance == 0)
// x->Balance = y->Balance = 0;
//else /* |w->Balance == +1| */
// x->Balance = -1, y->Balance = 0;
int b = getBalance(w);
intptr_t yp = y->Private;
if (b == -1) {
setBoth(x, w, 0);
setBoth(y, w, +1);
} else if (b == 0) {
setBoth(x, w, 0);
setBoth(y, w, 0);
} else {
setBoth(x, w, -1);
setBoth(y, w, 0);
}
//w->Balance = 0;
//w->Parent = y->Parent;
setBoth(w, yp, 0);
//x->Parent = y->Parent = w;
if (x->Link[1] != NULL)
//x->Link[1]->Parent = x;
setParent(x->Link[1], x);
if (y->Link[0] != NULL)
//y->Link[0]->Parent = y;
setParent(y->Link[0], y);
q->Link[dir] = w;
} else {
y->Link[0] = x->Link[1];
x->Link[1] = y;
intptr_t yp = y->Private;
//x->Parent = y->Parent;
//y->Parent = x;
if (y->Link[0] != NULL)
//y->Link[0]->Parent = y;
setParent(y->Link[0], y);
q->Link[dir] = x;
//if (x->Balance == 0) {
// x->Balance = +1;
// y->Balance = -1;
// break;
//} else {
// x->Balance = y->Balance = 0;
// y = x;
//}
if (getBalance(x) == 0) {
setBoth(x, yp, +1);
setBoth(y, x, -1);
break;
} else {
setBoth(x, yp, 0);
setBoth(y, x, 0);
y = x;
}
}
}
}
}
return p;
}
/* Inserts |item| into |tree| and returns a pointer to |item|'s address.
If a duplicate item is found in the tree,
returns a pointer to the duplicate without inserting |item|.
Returns |NULL| in case of success. */
struct AVLNode *
AVL_Insert(struct AVLNode **tree, struct AVLNode *item, AVLNODECOMP cmpnode)
{
struct AVLNode *y; /* Top node to update balance factor, and parent. */
struct AVLNode *p, *q; /* Iterator, and parent. */
struct AVLNode *n; /* Newly inserted node. */
struct AVLNode *w; /* New root of rebalanced subtree. */
int dir; /* Direction to descend. */
//assert (tree != NULL && item != NULL);
y = *tree;
for (q = NULL, p = *tree; p != NULL; q = p, p = p->Link[dir]) {
int cmp = cmpnode(item, p);
if (cmp == 0)
return p;
dir = cmp > 0;
//if (p->Balance != 0)
if (getBalance(p) != 0)
y = p;
}
n = item;
n->Link[0] = n->Link[1] = NULL;
//n->Parent = q;
//n->Balance = 0;
setBoth(n, q, 0);
if (q != NULL)
q->Link[dir] = n;
else {
*tree = n;
return NULL;
}
for (p = n; p != y; p = q) {
//q = p->Parent;
q = getParent(p);
dir = q->Link[0] != p;
if (dir == 0)
//q->Balance--;
decBalance(q);
else
//q->Balance++;
incBalance(q);
}
//if (y->Balance == -2) {
if (getBalance(y) == -2) {
struct AVLNode *x = y->Link[0];
//if (x->Balance == -1) {
if (getBalance(x) == -1) {
w = x;
y->Link[0] = x->Link[1];
x->Link[1] = y;
//x->Balance = y->Balance = 0;
//x->Parent = y->Parent;
//y->Parent = x;
setBoth(x, getParent(y), 0);
setBoth(y, x, 0);
if (y->Link[0] != NULL)
//y->Link[0]->Parent = y;
setParent(y->Link[0], y);
} else {
//assert (x->Balance == +1);
w = x->Link[1];
x->Link[1] = w->Link[0];
w->Link[0] = x;
y->Link[0] = w->Link[1];
w->Link[1] = y;
//if (w->Balance == -1)
// x->Balance = 0, y->Balance = +1;
//else if (w->Balance == 0)
// x->Balance = y->Balance = 0;
//else /* |w->Balance == +1| */
// x->Balance = -1, y->Balance = 0;
int b = getBalance(w);
intptr_t yp = y->Private;
if (b == -1) {
setBoth(x, w, 0);
setBoth(y, w, 1);
} else if (b == 0) {
setBoth(x, w, 0);
setBoth(y, w, 0);
} else {
setBoth(x, w, -1);
setBoth(y, w, 0);
}
//w->Balance = 0;
//w->Parent = y->Parent;
setBoth(w, yp, 0);
//x->Parent = y->Parent = w;
if (x->Link[1] != NULL)
//x->Link[1]->Parent = x;
setParent(x->Link[1], x);
if (y->Link[0] != NULL)
//y->Link[0]->Parent = y;
setParent(y->Link[0], y);
}
//} else if (y->Balance == +2) {
} else if (getBalance(y) == +2) {
struct AVLNode *x = y->Link[1];
//if (x->Balance == +1) {
if (getBalance(x) == +1) {
w = x;
y->Link[1] = x->Link[0];
x->Link[0] = y;
//x->Balance = y->Balance = 0;
//x->Parent = y->Parent;
//y->Parent = x;
setBoth(x, getParent(y), 0);
setBoth(y, x, 0);
if (y->Link[1] != NULL)
//y->Link[1]->Parent = y;
setParent(y->Link[1], y);
} else {
//assert (x->Balance == -1);
w = x->Link[0];
x->Link[0] = w->Link[1];
w->Link[1] = x;
y->Link[1] = w->Link[0];
w->Link[0] = y;
//if (w->Balance == +1)
// x->Balance = 0, y->Balance = -1;
//else if (w->Balance == 0)
// x->Balance = y->Balance = 0;
//else /* |w->Balance == -1| */
// x->Balance = +1, y->Balance = 0;
int b = getBalance(w);
intptr_t yp = y->Private;
if (b == 1) {
setBoth(x, w, 0);
setBoth(y, w, -1);
} else if (b == 0) {
setBoth(x, w, 0);
setBoth(y, w, 0);
} else {
setBoth(x, w, 1);
setBoth(y, w, 0);
}
//w->Balance = 0;
//w->Parent = y->Parent;
setBoth(w, yp, 0);
//x->Parent = y->Parent = w;
if (x->Link[0] != NULL)
//x->Link[0]->Parent = x;
setParent(x->Link[0], x);
if (y->Link[1] != NULL)
//y->Link[1]->Parent = y;
setParent(y->Link[1], y);
}
} else
return NULL;
//if (w->Parent != NULL)
//w->Parent->Link[y != w->Parent->Link[0]] = w;
p = getParent(w);
if (p != NULL)
p->Link[y != p->Link[0]] = w;
else
*tree = w;
return NULL;
}
void moveOffJunction()
{
setBoth(150);
delay(150);
setBoth(0);
}
