void AVLtree::balance(avlNode *p){
if(!p)
return;
else if(height(p->lchild)-height(p->rchild)==2)
{
// condition one : left-left (outside)
if(height(p->lchild->lchild)-height(p->lchild->rchild)==1)
rotateR(p);
// condition two : left-right (inside)
else{
rotateL(p->lchild);
rotateR(p);
}
}
else if(height(p->lchild)-height(p->rchild)==-2)
{
// condition four : right-right (outside)
if(height(p->lchild->lchild)-height(p->lchild->rchild)==-1)
rotateL(p);
// condition three : right-left (inside)
else{
rotateR(p->rchild);
rotateL(p);
}
}
else
p->height=std::max(height(p->lchild), height(p->rchild))+1;
}
////////////////////////////////////////
//autonomous
task autonomous()
{
slaveMotor(armL2, armL3);
slaveMotor(armL1, armL2);
slaveMotor(armR1, armR3);
slaveMotor(armR2, armR1);
resetIME();
grabAuto(127, 1250);//clutch preload
resetIME();
drive(127, 1000); //drive forward
resetIME();
rotateL(127, 1000);//turn 180 degrees
resetIME();
drive(-127, -850);//reverse to wall
resetIME();
armAuto(127, 250);//lift arm
resetIME();
grabAuto(-127, 1500);//release preload
resetIME();
armAuto(-127, -800);//arm back down
resetIME();
rotateR(127, 400);//rotate towards cube
resetIME();
drive(127,500);//drive towards cube
resetIME();
grabAuto(127, 1250);//clutch cube
resetIME();
rotateL(127, 400);//rotate towards fence
resetIME();
drive(-127, -850);//backup
resetIME();
armAuto(127,250);//raise arm
resetIME();
grabAuto(-127, 1250);//dump cube
resetIME();
armAuto(-127, -800);//lower arm
resetIME();
}
/**
* Creates a tetrimino according to its type
* */
Tetrimino* createTetrimino(short type) {
Tetrimino* tetrimino = malloc(sizeof(Tetrimino));
tetrimino->type = type;
int i, j;
for(i=0; i<TETRI_SIZE; i++)
for(j=0; j<TETRI_SIZE; j++)
tetrimino->array[i][j] = 0;
switch(type) {
case TETRI_O:
rotateO(tetrimino, ROT_0);
break;
case TETRI_J:
rotateJ(tetrimino, ROT_0);
break;
case TETRI_L:
rotateL(tetrimino, ROT_0);
break;
case TETRI_I:
rotateI(tetrimino, ROT_0);
break;
case TETRI_Z:
rotateZ(tetrimino, ROT_0);
break;
case TETRI_S:
rotateS(tetrimino, ROT_0);
break;
case TETRI_T:
rotateT(tetrimino, ROT_0);
break;
default:
break;
}
return tetrimino;
}
/**
* Rotates a Tetrimino
* */
void rotate(Tetrimino* tet, short rotation) {
short i, j ;
for(i=0; i<TETRI_SIZE; i++)
for(j=0; j<TETRI_SIZE; j++)
tet->array[i][j] = 0;
switch(tet->type) {
case TETRI_O:
rotateO(tet, rotation);
break;
case TETRI_J:
rotateJ(tet, rotation);
break;
case TETRI_L:
rotateL(tet, rotation);
break;
case TETRI_I:
rotateI(tet, rotation);
break;
case TETRI_Z:
rotateZ(tet, rotation);
break;
case TETRI_S:
rotateS(tet, rotation);
break;
case TETRI_T:
rotateT(tet, rotation);
break;
default:
break;
}
}
task usercontrol()
{
slaveMotor(shooterLB, shooterL);
slaveMotor(shooterRB, shooterR);
while(true)
{
//Warning Killer
if (0 == 1)
{
warningKiller();
rotateL(0);
rotateR(0);
}
//All the ints
int intakeForward = vexRT[Btn6U];
int intakeBackwards = vexRT[Btn5U];
int deadBand = 10;
//All the floats
float xL = vexRT[Ch4];
float yL = vexRT[Ch3];
float xR = vexRT[Ch1];
float yR = vexRT[Ch2];
//Intake & Shooter
if (vexRT[Btn7U])
{
motor[shooterL] = 100;//These numbers are abritrary. i.e. 100 = fast, 10 = slow, 0 = no power
motor[shooterR] = 100;
}
else
{
motor[shooterL] = 0;
motor[shooterR] = 0;
}
setIntake(intakeForward*75, intakeBackwards*75);
//Drive Code
motor[leftFront] = xR + yR;
motor[rightRear] = xR - yR;
motor[leftRear] = xL + yL;
motor[rightFront] = xL - yL ;
//Deadband
if(xL > deadBand || xL < -deadBand)
{
xL = xL;
}
else
{
xL = 0;
}
if(yL > deadBand || yL < -deadBand)
{
yL = yL;
}
else
{
yL = 0;
}
if(xR > deadBand || xR < -deadBand)
{
xR = xR;
}
else
{
xR = 0;
}
if(yR > deadBand || yR < -deadBand)
{
yR = yR;
}
else
{
yR = 0;
}
}
}
