g work in progress

#pragma config(Sensor, S1,     gyroscope,      sensorEV3_Gyro, modeEV3Gyro_RateAndAngle)
#pragma config(Sensor, S2,     colorSensor,    sensorEV3_Color, modeEV3Color_Color)
#pragma config(Sensor, S3,     frontSensor,    sensorEV3_Ultrasonic)
#pragma config(Sensor, S4,     leftSensor,     sensorEV3_Ultrasonic)
#pragma config(Motor,  motorA,          leftMotor,     tmotorEV3_Large, PIDControl, driveLeft, encoder)
#pragma config(Motor,  motorB,          rightMotor,    tmotorEV3_Large, PIDControl, driveRight, encoder)
#pragma config(Motor,  motorC,          clawLifter,    tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorD,          claw,          tmotorEV3_Medium, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const int moveforwardVelocity=20;

const int claw_Velocity=100;
const int claw_PositionOpen=3*360;
const int claw_PositionClose=-3*360;

const int clawLifter_Velocity=-50;
const int clawLifter_PositionUp=360;
const int clawLifter_PositionDown=360;

const int Object_distance = 5;

const int Blue = 5;
const int Red = 3;



void moveForward(int v){
    motor[motorA]=v;
    motor[motorB]=v;
    wait1Msec(1);
}

void movement (int V1, int V2 ){
    //setMotorSpeed(motorA, V1);
    //setMotorSpeed(motorB, V2);
    motor[motorA]=V1;
    motor[motorB]=V2;
    wait1Msec(1);
}
bool objectDetectedLeft(int a)
{
    if(getUSDistance(leftSensor)<=a)
    {
        return true;
    }
    else return false;
}
bool objectDetectedFront(int a){


    if(getUSDistance(frontSensor)<=a){
        return true ;
    }
    else return false;
}




void openClaw (){
    moveMotorTarget(claw,claw_PositionOpen,claw_Velocity);
    sleep(500);
}

void closeClaw(){
    moveMotorTarget(claw,claw_PositionClose,-claw_Velocity);
    sleep(500);
}

void liftClaw(){
    moveMotorTarget(clawLifter,clawLifter_PositionUp+10,clawLifter_Velocity);
    sleep(500);
}

void fallClaw(){
    moveMotorTarget(clawLifter,clawLifter_PositionDown,-clawLifter_Velocity);
    sleep(500);
}


void inizialize(){
    liftClaw();
    openClaw();
}


void liftObject(){
    movement(0,0);
    fallClaw();
    delay(1000);
    closeClaw();
    delay(1000);
    liftClaw();

}

void releaseObject(){
    movement(0,0);
    fallClaw();
    openClaw();
    liftClaw();
}



void turn( char a)
{
    resetGyro(gyroscope);
    switch (a)
    {
    case 'L':
        resetGyro(gyroscope);
        while (getGyroDegrees(gyroscope)<99)
        {
            motor[motorA]=10;
            motor[motorB]=-10;
            wait1Msec(1);
        }
        break;
    case 'R' :
        resetGyro(gyroscope);
        while (getGyroDegrees(gyroscope)>-99 )
        {
            motor[motorA]=-10;
            motor[motorB]=10;
            wait1Msec(1);
        }
        break;

    }
}

void dropObject()
{
    movement(-10,-10);
    sleep(1000);
    movement(0,0);
    turn('R');
    movement(0,0);
    while(!objectDetectedFront(3.5))
    {
        movement(10,10);
    }
    movement(0,0);
    openClaw();
}
void routine(int n)// Funzione cerca oggetto è afferra; Warka
{
    while(!objectDetectedLeft(20)) //Mentre il LeftSensor non rileva oggetti va avanti
    {
        movement(n*moveforwardVelocity,n*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(objectDetectedFront(Object_distance))
        {
            movement(0,0);
            liftObject();
            break;
        }
    }
}
void Program(){

    inizialize();
    routine(1);
    movement(0,0);
    dropObject();
    delay(500);
    closeClaw();
    openClaw();
    delay(500);
    routine(-1)
}

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

}