programma 15.22

void closeClaw(){
    moveMotorTarget(claw,claw_PositionClose,-claw_Velocity);
    sleep(500);
}
void liftClaw(){
    moveMotorTarget(clawLifter,clawLifter_PositionUp,clawLifter_Velocity);
    sleep(500);
}
void fallClaw(){
    moveMotorTarget(clawLifter,clawLifter_PositionDown,-clawLifter_Velocity);
    sleep(500);
}
void inizialize(){ //alza la pinza e la apre
    liftClaw();
    openClaw();
}
void liftObject(){
    movement(0,0);
    fallClaw();
    closeClaw();
    //liftClaw();

}
void releaseObject(){
    movement(0,0);
    fallClaw();
    openClaw();
    liftClaw();
}
void turn( char a){
    resetGyro(gyroscope);
    switch (a)
    {
    case 'L':
        resetGyro(gyroscope);
        while (getGyroDegrees(gyroscope)<89)
        {
            motor[motorA]=10;
            motor[motorB]=-10;
            wait1Msec(1);
        }
        break;
    case 'R' :
        resetGyro(gyroscope);
        while (getGyroDegrees(gyroscope)>-89 )
        {
            motor[motorA]=-10;
            motor[motorB]=10;
            wait1Msec(1);
        }
        break;

    }
}
void dropObject(){
    movement(-10,-10);
    sleep(500);
    turn('R');
    while(!objectDetected(Object_distance))
    {
        moveForward(10);
    }
    movement(0,0);
    openClaw();
}
void routine()// Funzione cerca oggetto è afferra
{
	while(!objectDetectedLeft(20)) //Mentre il LeftSensor non rileva oggetti va avanti
    {
        movement(moveforwardVelocity,moveforwardVelocity);
    }
	movement(0,0);//spegnimento motori
    turn('L');//gira a sinistra di 90°
	movement(0,0);//spegnimento motori
    while(!objectDetectedFront(Object_distance))//Mentre la distanza è maggiore di Object_distance va avanti
    {
        movement(10,10);
        if(objectDetected(Object_distance))
        {
			liftObject();
        }
    }
}
void Program(){

    inizialize();
	routine();



    //dropObject();

}

task main(){
    Program();
//closeClaw();

}