void setRobotPhysLimit( int distance, char direction, int height, int roller, int tray, int platform, int timer )
{
ClearTimer( T1 );
resetEncoders();
//artificiallyresetGyro();
while(time1[T1] < timer && !SensorValue[TowerLimitL] && !SensorValue[TowerLimitR] )
{
if( direction == 'S' )
setDrive( distance );
else if( direction == 'T' )
spinDrive( distance );
else
wheelDrive( distance, direction );
//setArm( height );
if( height == 0 )
moveArm( -10, -10 );
else
{
if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 1 )
moveArm( 7, 7 );
else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 )
moveArm( 7, 20 );
else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 )
moveArm( 20, 7 );
else
setArm( height );
}
moveRollers( roller, roller );
movePiston( tray, platform );
}
stopMotors();
}
//move lower intake piston
void movePPunchR(int state){
movePiston(pPunchR, state);
}
//move upper intake piston
void movePPunchL(int state){
movePiston(pPunchL, state);
}
