【山寨】ev3齿轮分拣机 robot C程序。

humanxp

#pragma config(Sensor, S1,     roundtouch,     sensorEV3_Touch)
#pragma config(Sensor, S2,     catchtouch,     sensorEV3_Touch)
#pragma config(Sensor, S3,     begtouch,       sensorEV3_Color, modeEV3Color_Color)
#pragma config(Motor,  motorA,           ,             tmotorEV3_Large, openLoop)
#pragma config(Motor,  motorB,          catchmotor,    tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          roundmotor,    tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorD,           ,             tmotorEV3_Large, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void reset()
{
        setMotorBrakeMode(roundmotor,motorBrake);
        setMotorBrakeMode(catchmotor,motorBrake);
        
        setMotorSpeed(catchmotor,20);//reset catch
        repeatUntil(getTouchValue(catchtouch)==1)
        {
        }
        moveMotorTarget(catchmotor,120,-20);
        waitUntilMotorStop(catchmotor);
        
        setMotorSpeed(roundmotor,30);//reset round
        repeatUntil(getTouchValue(roundtouch)==1)
        {
        }

        sleep(50);
        moveMotorTarget(roundmotor,300,-30);
        sleep(2000);


}
task main()
{
        int fl=0,flag=300;//fl motor encode   flag  Measuring the size of the product
        
        reset();
        
        resetMotorEncoder(catchmotor);//reset catch encoder
        while(1)
{

        repeatUntil(getColorName(begtouch)==5);//color begain.
        {
        }
        displayCenteredBigTextLine(6,"clor=%d",getColorName(begtouch));
        sleep(500);
        moveMotorTarget(roundmotor,flag,-30);//round
        waitUntilMotorStop(roundmotor);
        
        setMotorSpeed(catchmotor,20);//catch.
        repeatUntil(getTouchValue(catchtouch)==1)
        {

        }
        setMotorSpeed(catchmotor,0);
        displayCenteredBigTextLine(2,"%d",getMotorEncoder(motorB));
        
        fl=getMotorEncoder(catchmotor);//Measuring the size of the product
        if(fl<89)         flag=600;
        if(fl>90)         flag=500;
        if(fl>96)  flag=400;
        if(fl>100) flag=300;
        if(fl>105) flag=200;
        if(fl>110) flag=100;
        if(fl>116)
        {
                displayCenteredBigTextLine(4,"nother"        );
                flag=100;
                }else
                {
                        displayCenteredBigTextLine(4,"       "        );
                        };
        displayCenteredBigTextLine(8,"%d",flag);               
        moveMotorTarget(roundmotor,flag,30);//put everything in order  


        waitUntilMotorStop(roundmotor);
        moveMotorTarget(catchmotor,120,-20);
        waitUntilMotorStop(roundmotor);
        sleep(2000);
}
}