void progSkills()
{
//while (SensorValue[bumper] == 0) {}
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);
baseStop();
// Deploy
deploy();
moveLiftAuto(1,3100);
moveLift(20);
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);
baseStop();
/*
moveStraight(127);
wait1Msec(200);
moveStraight(-127);
wait1Msec(200);
*/
moveStraight(127);
wait1Msec(200);
intake(-1);
moveStraight( -127);
wait1Msec(200);
baseStop();
//intake(-1);
wait1Msec(2000);
moveStraight(50);
wait1Msec(100);
baseStop();
intake(1);
moveLift(-30);
wait1Msec(500);
intake(0);
liftStop();
moveStraight(-40);
wait1Msec(200);
baseStop();
SensorValue[leftBackS] = 0;
SensorValue[rightBackS] = 0;
while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)
{
while (SensorValue[rightLiftS] < 4000)
{
moveLift(-70);
turnOnSpot(2,90);
}
liftStop();
}
baseStop();
intake(1);
moveStraight(70);
wait1Msec(1000);
baseStop();
SensorValue[leftBackS] = 0;
SensorValue[rightBackS] = 0;
while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
{
turnOnSpot(2,90);
}
baseStop();
moveStraight(127);
wait1Msec(2000);
baseStop();
while(SensorValue[bumper] == 0) {}
intake(-1);
while(SensorValue[bumper] == 0) {}
intake(0);
// Raise Lift
// Output
while(SensorValue[bumper] == 0) {}
intake (1);
moveStraight(127);
wait10Msec(50);
turnOnSpot(1,60);
wait10Msec(30);
turnOnSpot(2,60);
wait10Msec(30);
baseStop();
SensorValue[leftBackS] = 0;
SensorValue[rightBackS] = 0;
while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
{
turnOnSpot(1,90);
}
intake(-1);
wait10Msec(100);
}
//////////////////////////////////////////////////////////////////////////////////////////////////
// place ring
//
// Place autonomous ring while sensing the IR beacon
//
/////////////////////////////////////////////////////////////////////////////////////////////////
void placering (string iSide, int iPos, int iColumn, int iHeight)
{
if (iColumn == 2)
{
moveLift (0, 0, UP);
}
else
{
if (iHeight == 1)
{
moveLift(40, 571, UP);
}
else if (iHeight == 2)
{
moveLift(40, 3095, UP);
}
else if (iHeight == 3)
{
moveLift(40, 5233, UP);
}
}
if (iPos == 1)
{
if (iColumn == 1)
{
path1 (iSide,33, 15);
}
else if (iColumn == 2)
{
move(50, 16, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 45, LEFT);
}
else
{
GyroTurn (50, 45, RIGHT);
}
move(50, 21, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 45, LEFT);
}
else
{
GyroTurn (50, 45, RIGHT);
}
move(50, 13, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 30, RIGHT);
}
else
{
GyroTurn (50, 30, LEFT);
}
move(50, 1, BACKWARD);
moveLift(40, 500, UP);
move(50, 3, FORWARD);
}
else if (iColumn == 3)
{
path2 (iSide, 20, 31);
}
}
else if (iPos == 2)
{
if (iColumn == 1)
{
path1 (iSide, 15, 12);
}
else if (iColumn == 2)
{
move(50, 33, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 90, LEFT);
}
else
{
GyroTurn (50, 90, RIGHT);
}
move(50, 19, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 30, RIGHT);
}
else
{
GyroTurn (50, 30, LEFT);
}
move(50, 2, BACKWARD);
moveLift(40, 500, UP);
move(50, 3, FORWARD);
}
else if (iColumn == 3)
{
path2 (iSide, 35, 13);
}
}
else if (iPos == 3)
{
if (iColumn == 1)
{
path1 (iSide, 12, 14);
}
else if (iColumn == 2)
{
move(50, 12, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 90, LEFT);
}
else
{
GyroTurn (50, 90, RIGHT);
}
move(50, 13, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 90, RIGHT);
}
else
{
GyroTurn (50, 90, LEFT);
}
move(50, 18, FORWARD);
if (strcmp(iSide, "left")== 0)
{
GyroTurn (50, 35, LEFT);
}
else
{
GyroTurn (50, 35, RIGHT);
}
move(50, 1, BACKWARD);
moveLift(40, 500, UP);
move(50, 4, FORWARD);
}
else if (iColumn == 3)
{
path3 (iSide, 15);
}
}
servo[pivotLeft] = 0;
servo[pivotRight] = 255;
if (iHeight == 1)
{
moveLift(40, 571, DOWN);
}
else if (iHeight == 2)
{
moveLift(40, 3095, DOWN);
}
else if (iHeight == 3)
{
moveLift(40, 5233,DOWN);
}
}
