int TOBackupCountTicks(int ticks, long timeout_ms) { int to = 1; int count = 0; while (to && count < 2) { /*if times out tried no more than 3 times*/ if (backRightHit() || backSideRightHit()) { forwardCountTicksUntilTimeout(25, (long) 2000, 0); sleep(.25); right90(-right90ticks/2); forwardIntoWall((long) 3000); to = backwardCountTicksUntilTimeout(ticks, timeout_ms); } else if (backLeftHit() || backSideLeftHit()) { forwardCountTicksUntilTimeout(25, (long) 2000, 0); sleep(.25); left90(-left90ticks/2); forwardIntoWall((long) 3000); to = backwardCountTicksUntilTimeout(ticks, timeout_ms); } else { forwardCountTicksUntilTimeout(50, (long) 2000, 1); to = backwardCountTicksUntilTimeout(ticks, timeout_ms); } count++; } return count; } int TOTurn90(int dev) { int to = 1; int count = 0; if (side == 1) { while (to && count < 2) { printf("\ntimed out!!!"); if (backRightHit() || backSideRightHit()) { forwardCountTicksUntilTimeout(25, (long) 2000, 0); sleep(.25); to = left90(-left90ticks*2/3); } else if (backLeftHit() || backSideLeftHit()) { forwardCountTicksUntilTimeout(25, (long) 2000, 0); sleep(.25); to = left90(-left90ticks/2); } else if (frontRightHit() || frontSideRightHit()) { backwardCountTicksUntilTimeout(25, (long) 2000); sleep(.25); to = left90(-right90ticks*2/3); } else { right90(-right90ticks/2); sleep(.25); backwardCountTicksUntilTimeout(125, (long) 3000); sleep(.25); left90(-left90ticks/3); sleep(.25); forwardUntilHitTimeout((long) 2000); sleep(.25); backwardCountTicksUntilTimeout(25, (long) 3000); sleep(.25); to = left90(3); } count++; } if (count > 2) { lost(); } } else if (side == 0) { while (to && count < 2) { printf("\ntimed out!!!"); if (backRightHit() || backSideRightHit()) { forwardCountTicksUntilTimeout(25, (long) 2000, 0); sleep(.25); to = right90(-right90ticks/2); } else if (backLeftHit() || backSideLeftHit()) { forwardCountTicksUntilTimeout(25, (long) 2000, 0); sleep(.25); to = right90(-right90ticks*2/3); } else if (frontLeftHit() || frontSideLeftHit()) { backwardCountTicksUntilTimeout(25, (long) 2000); sleep(.25); to = right90(-right90ticks*2/3); } else { left90(-left90ticks/2); sleep(.25); backwardCountTicksUntilTimeout(125, (long) 3000); sleep(.25); right90(-right90ticks/3); sleep(.25); forwardUntilHitTimeout((long) 2000); sleep(.25); backwardCountTicksUntilTimeout(25, (long) 3000); sleep(.25); to = right90(3); } count++; } /*while*/ if (count > 2) { lost(); } } } int TOForwardCountTicks(int ticks, long timeout_ms, int hit) { /*hit - 1 means call forwardCountTicksUntilHitorTimeout 0 means call forwardCountTicksUntilTimeout */ int to = 1; int count = 0; while (to && count < 2) { /*if times out tried no more than 3 times*/ if (frontRightHit() || frontSideRightHit()) { backwardCountTicksUntilTimeout(50, (long) 3000); sleep(.25); left90(-left90ticks/2); if (!hit) { to = forwardCountTicksUntilTimeout(ticks, timeout_ms, 0); } else { to = forwardCountTicksUntilTimeout(ticks, timeout_ms, 1); } } else if (frontLeftHit() || frontSideLeftHit()) { backwardCountTicksUntilTimeout(50, (long) 3000); sleep(.25); right90(-right90ticks/2); if (!hit) { to = forwardCountTicksUntilTimeout(ticks, timeout_ms, 0); } else { to = forwardCountTicksUntilTimeout(ticks, timeout_ms, 1); } } else { backwardCountTicksUntilTimeout(50, (long) 3000); if (!hit) { to = forwardCountTicksUntilTimeout(ticks, timeout_ms, 0); } else { to = forwardCountTicksUntilTimeout(ticks, timeout_ms, 1); } } count++; } return count; }