g versione finale

#pragma config(Sensor, S1,     gyroscope,      sensorEV3_Gyro)
#pragma config(Sensor, S2,     colorSensor,    sensorEV3_Color, modeEV3Color_RGB_Raw)
#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=50;
const int claw_PositionOpen=3*360;
const int claw_PositionClose=-2.5*360;

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

const double Front_Object_distance = 11;
const int Left_Object_distance= 38;
const int Blue = 5;
const int Red = 3;
bool compreso= false;
bool mainore =false;
bool maggiore =false;
int tragitto=1;
int oggettipresi=0;
int distans=0;
int contatore = 0;
int temp=0;
int colore;
void moveForward(double v){
    if(v>0){
        double n= v/20.4
        moveMotorTarget(motorA,n*360, 20);
        moveMotorTarget(motorB,n*360, 20);
        sleep(n*1000);
    }
    else if(v<0){
        double n= v/20.4
        moveMotorTarget(motorA,n*360, -20);
        moveMotorTarget(motorB,n*360, -20);
        sleep(n*1000);
    }
}

void movement (int V1, int V2 ){ // Funzione potenza ai 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){
    if(getUSDistance(leftSensor)<=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);
    motor[claw]=-claw_Velocity;
    delay(1500);
    motor[claw]=0;
}

void liftClaw(){ //Alza braccio
    //moveMotorTarget(clawLifter,clawLifter_PositionUp+50,clawLifter_Velocity);
    motor[clawLifter]=clawLifter_Velocity;
    sleep(1000);
}

void fallClaw(){ //Abbassa braccio
    motor[clawLifter]=-clawLifter_Velocity;
    sleep(1300);
        motor[clawLifter]=0;
}


void turn(int c, int gradi){
    resetGyro(gyroscope);
    switch(c){
    case 1:
        while(getGyroDegrees(gyroscope)<gradi){
            motor[motorA]=-10;
            motor[motorB]=10;
            wait1Msec(1);}
        break;
    case 2:     while(getGyroDegrees(gyroscope)>-gradi){
            motor[motorA]=10;
            motor[motorB]=-10;
            wait1Msec(1);
        }
        break;}
}



void initialize(){// Funzione di inizializazione
    liftClaw();
    openClaw();
    moveForward(40);
    turn('R',90);
    moveForward(30);

}

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

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


task main(){
    //closeClaw();
//inizio procedura inizializzazione
    resetGyro(gyroscope);
    liftClaw();
    openClaw();
    movement(-30,-30);
    wait1Msec(1000);
    movement(0,0);
    wait1Msec(500);
    while(true){
        movement(20,20);
        wait1Msec(2000);
        movement(0,0);
        wait1Msec(500);
        turn(2,90);
        movement(0,0);
        sleep(500);
        movement(-50,-50);
        sleep(1500);
        movement(0,0);
        sleep(500);
        movement(50,50);
        sleep(1800+temp);
        //fine inizializzazione
        //AVANTI FIN QUANDO NON RILEVA NULLA A SINISTRA
        while(!objectDetectedLeft(Left_Object_distance)){
            movement(20,20);
            sleep(500);
            movement(0,0);
            sleep(500);
        }
        //  UNA VOLTA RILEVATO GIRA A SINISTRA
        moveForward(-6);
        sleep(1000);
        //gira finche non lo trovi
        while(!objectDetectedFront(20)){movement(-10,10);delay(100);}
        //vai avanti finche non lo becchi
        while(!objectDetectedFront(Front_Object_distance)){
            movement(10,10);
            sleep(500);
            movement(0,0);
            sleep(500);
        }
        //PRENDE L'OGGETTO
        fallClaw();
        //giusto
        //Inizio procedura di drop
        colore=0;
         colore = getColorName(colorSensor);
         delay(100);
        if(!(colore==3)){ //Se il colore non é ne rosso ne blu(non funziona manco per sbaglio)
        // vai anvanti di un po
        while (getUSDistance(frontSensor)>6.5){
                movement(10,10);
                delay(100);
                }
            movement(0,0);
            sleep(500);
            //chiudi e alza pinza
            closeClaw();
            liftclaw();
            sleep (1500);
            // indietro per allinearsi con il muro
            movement(-50,-50);
            delay(3500);
            //gira a destra di 90
            turn(2,90);
            movement(0,0);
            delay(500);
            //finchè non ce la cassa va avanti radente al muro
            while(!objectDetectedFront(5))
            {
                movement(100,100);

            }
            movement(0,0);
            //apre(droppa) e chiude la pinza
            openClaw();
            sleep(500);
            closeClaw();

        }
        //Fine funzione di drop
        //funziona piu o meno
        // va indietro fino all'inizio
        movement(-200,-200);
        sleep(6000);
        movement(0,0);
        sleep(500);
        //gira a sinista
        turn(1,90);
        //va indietro
        movement(-50,-50);
        sleep(1000);
        //apre la pinza
        openClaw();
        //non so se funziona
        temp +=400;
    }
}