G new version 0.1

#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               !!*//
//Il nero lo scarta, il rosso lo mette nella chest a DX e il blu a SX.

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 = 7.3;

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

int tragitto=1;
int oggettipresi=0;
int distans=0;
int contatore = 0;

void moveForward(double v){
    if(v>0){
    double n= v/20.4
    moveMotorTarget(motorA,n*360, 50);
    moveMotorTarget(motorB,n*360, 50);
    sleep(n*1000);
}
else if(v<0){
        double n= v/20.4
    moveMotorTarget(motorA,n*360, -50);
    moveMotorTarget(motorB,n*360, -50);
    sleep(n*1000);

}
}

void movement (int V1, int V2 ){ // Funzione potenza hai motori;
    motor[motorA]=V1;
    motor[motorB]=V2;
    wait1Msec(1);
}

//Funzione che rileva la presenza di oggetti sulla sinistra confrontando la distanza con parametro passato alla funzione
bool objectDetectedLeft(double a){
    distans=getUSDistance(leftSensor);
    if(distans<=a)
    {
        return true;
    }
    else return false;
}

//Funzione che rileva la presenza di oggetti  di fronte confrontando la distanza con parametro passato alla funzione
bool objectDetectedFront(double a){



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




void openClaw (){ //Funzione apri pinza
    moveMotorTarget(claw,claw_PositionOpen,claw_Velocity);
    sleep(500);
}

void closeClaw(){ //Funzione chiudi pinza
    moveMotorTarget(claw,claw_PositionClose,-claw_Velocity);
    sleep(500);
}

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

void fallClaw(){ //Abbassa braccio
    moveMotorTarget(clawLifter,clawLifter_PositionDown,-clawLifter_Velocity);
    sleep(500);
}
void turn(char a,double b)//Funzione gira di 90°
{
    resetGyro(gyroscope);
    switch (a)
    {

        case 'L':
        while(getGyroDegrees(gyroscope)<=b)
            {
                movement(-10,10);
                sleep(1);
                contatore +=1;
        }
        break;
case 'R':
        while(getGyroDegrees(gyroscope)>=-b)
            {
                movement(10,-10);
                sleep(1);
                contatore +=1;
        }
        break;
    break;

}
}



void initialize(){// Funzione di inizializazione
    liftClaw();
    openClaw();


}

void liftObject(){ // Funzione prendi oggetto
    movement(0,0);
    fallClaw();
    delay(1000);
    closeClaw();
    delay(1000);

}

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





void dropObject_right()//Porta l'ogetto alla cassa
{
    if(getColorName(colorSensor)!= colorBlue && getColorName(colorSensor) != colorRed){
        liftclaw();
        moveForward(-10);
        delay(1500);
        turn('R',75);
        while(getUSDistance(frontSensor)>20){
            movement(50,50);
        }
        movement(0,0);
        openclaw();
        delay(500);

        tragitto=-1;
    }
    else {
        liftclaw();
        movement(-30,-30);
        delay(1500);
        turn('L',84);
        while(getUSDistance(frontSensor)>20){
            movement(50,50);
        }
        movement(0,0);
        openclaw();
        delay(500);

        turn('R',84);
        tragitto=1;

    }
}



void routine(int n)// Funzione cerca oggetto è afferra; Warka
{
    while(!objectDetectedLeft(40))//Mentre il LeftSensor non rileva oggetti va avanti
    {
        movement(n*moveforwardVelocity,n*moveforwardVelocity);
    }
    movement(0,0);//spegnimento motori
    turn('L',84);//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(){


    routine(tragitto);
    movement(0,0);
    dropObject_right();
    while(true){
    if(getGyroDegrees(gyroscope)>0)
    {
        while(getGyroDegrees(gyroscope)>0)
        {
            turn('R',2);
            sleep(1000);
        }
    }
    else if(getGyroDegrees(gyroscope)<0)
    {
        while(getGyroDegrees(gyroscope)>0)
        {
            turn('L',2);
            sleep(1000);
        }
    }
    movement (-100,-100);
    sleep(2000);
}
}

task main(){
    //closeClaw();

initialize();
moveForward(45);
Program();


}