Exemplo n.º 1
0
task main(int argc, char** argv)
{
    EV3Init();
    SetSensorTouch(S1);
    SetSensorColour(S2);
    SetSensorColour(S3);
    SetSensorUS(S4);
    //SetSensorNXTUS(S4);
    //SetSensorNXTLight(S4);
    //SetColourSensorMode(S2, REFLECTED);
    while(1)
    {

        TextNumOut(0, LCD_LINE1, "Tch : ", SENSOR_1, 15);
        TextNumOut(0, LCD_LINE2, "LCol: ", SENSOR_2, 15);
        TextNumOut(0, LCD_LINE3, "RCol: ", SENSOR_3, 15);
        TextNumOut(0, LCD_LINE4, "US  : ", SENSOR_4, 15);
        //usleep(10000);
    }
    EV3Exit();
    return (EXIT_SUCCESS);
}
Exemplo n.º 2
0
task taskControl()
{
  Follows(main);
  int ack;
  char pfData[8];
  SetSensorTouch(IN_4);

  while(true) {

if(Sensor(IN_4) == 1)
{
StopAllTasks(); // stop the program
}
  
  
ack = bluetooth_receiveInt();


if (ack==1){
pfData[IR_LEFT]=-25;
pfData[IR_RIGHT]=25;
}

if (ack==2){
pfData[IR_LEFT]=25;
pfData[IR_RIGHT]=-25;
}

if (ack==3){
pfData[IR_LEFT]=0;
pfData[IR_RIGHT]=0;
}

if (ack==4){
pfData[IR_LEFT]=50;
pfData[IR_RIGHT]=50;
}

if (ack==5){
pfData[IR_LEFT]=-50;
pfData[IR_RIGHT]=-50;
}

if (ack==6){
pfData[IR_LEFT]=25;
pfData[IR_RIGHT]=50;
}

if (ack==7){
pfData[IR_LEFT]=50;
pfData[IR_RIGHT]=25;
}

if (ack==8){
pfData[IR_LEFT]=-25;
pfData[IR_RIGHT]=-50;
}

if (ack==9){
pfData[IR_LEFT]=-50;
pfData[IR_RIGHT]=-25;
}

motorControlDrive = (pfData[IR_LEFT] + pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
motorControlSteer = (pfData[IR_LEFT] - pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;

Wait(CONTROL_WAIT);

 }
}
Exemplo n.º 3
0
task main()
{
    EV3Init();
    SetSensorTouch(S1);
    SetSensorColour(S2);
    SetSensorColour(S3);
    SetSensorUS(S4);
    int LBLACK;
    int RBLACK;
    int LGREEN;
    int RGREEN;
    int LYELLOW;
    int RYELLOW;
    int LBG;
    int RBG;
    int LGY;
    int RGY;
    
    //Read and Display Values
    //Black
    while(EnterButtonState() == NOTPRESSED);
    LBLACK = SENSOR_2;
    RBLACK = SENSOR_3;
    TextNumOut(0, LCD_LINE1, "LBlck: ", LBLACK, 15);
    TextNumOut(0, LCD_LINE2, "RBlck: ", RBLACK, 15);
    Wait(500); //Threshold
    
    //Green
    while(EnterButtonState() == NOTPRESSED);
    LGREEN = SENSOR_2;
    RGREEN = SENSOR_3;
    TextNumOut(0, LCD_LINE1, "LGrn: ", LGREEN, 15);
    TextNumOut(0, LCD_LINE2, "RGrn: ", RGREEN, 15);
    Wait(500);
    
    //Yellow
    while(EnterButtonState() == NOTPRESSED);
    LYELLOW = SENSOR_2;
    RYELLOW = SENSOR_3;
    TextNumOut(0, LCD_LINE1, "LYlw: ", LYELLOW, 15);
    TextNumOut(0, LCD_LINE2, "RYlw: ", RYELLOW, 15);
    Wait(500);
    
    //Calc. calibration values
    while(EnterButtonState() == NOTPRESSED);
    LBG = (LBLACK + LGREEN) / 2;
    RBG = (RBLACK + RGREEN) / 2;
    LGY = (LGREEN + LYELLOW) / 2;
    RGY = (RGREEN + RYELLOW) / 2;
    TextNumOut(0, LCD_LINE1, "LBG: ", LBG, 15);
    TextNumOut(0, LCD_LINE2, "RBG: ", RBG, 15);
    TextNumOut(0, LCD_LINE3, "LGY: ", LGY, 15);
    TextNumOut(0, LCD_LINE4, "RGY: ", RGY, 15);
    Wait(500);
    
    //LBG = 10;
    //RBG = 10;
    //LGY = 100;
    //RGY = 100;
    
    //Line Follower
    while(EnterButtonState() == NOTPRESSED);
    while(SENSOR_2 < LGY || SENSOR_3 < RGY)
    while(true)
    {
        TextNumOut(0, LCD_LINE1, "Tch : ", SENSOR_1, 15);
        TextNumOut(0, LCD_LINE2, "LCol: ", SENSOR_2, 15);
        TextNumOut(0, LCD_LINE3, "RCol: ", SENSOR_3, 15);
        TextNumOut(0, LCD_LINE4, "US  : ", SENSOR_4, 15);
        if(SENSOR_2 > LGY)
        {
            OnFwd(OUT_A, -50);
            OnFwd(OUT_D, 90);
        }
        else if(SENSOR_3 > RGY)
        {
            OnFwd(OUT_A, 90);
            OnFwd(OUT_D, -50);
        }
        else if(SENSOR_2 < LBG)
        {
            OnFwd(OUT_A, -50);
            OnFwd(OUT_D, 90);
        }
        else if (SENSOR_3 < RBG)
        {
            OnFwd(OUT_A, 90);
            OnFwd(OUT_D, -50);
        }
        else OnFwd(OUT_AD, 80);
        Wait(1);
    }
    
    //Vind het blikje
    OnFwd(OUT_D, 100);
    OnFwd(OUT_A, -100);
    Wait(300);
    
    OnFwd(OUT_A, 80);
    OnFwd(OUT_C, -80);
    while(SENSOR_4 > 25);
    Wait(75);
    OnFwd(OUT_AC, 100);
    Wait(2000);
    Off(OUT_AC);
    
    EV3Exit();
    return (EXIT_SUCCESS);
}