// dual pick & place robot, pick&placer unit

//#include "fabriekheader.nqc"

// out A  = pickplace 1 (z-axes)
// out B  = release motor
// out C  = pick & placer 2 (z-axes)
// inp 1  = switch + rotation sensor p&p 1
// inp 2  = hoho-switch
// inp 3  = switch + rotation sensor p&p 2

    int a;
    int b;
    int c;
    int mes;
    int pos;
    int turn;
    int toggle;
    int pick[11];
    int place[11];
    int steen;


    sub init()
    {
    toggle=0;
    pick[1]=26;   place[1]=3;  //inv schuin voorkant rechts
    pick[2]=26;   place[2]=1;  //inv schuin voorkant links
    pick[3]=21;   place[3]=2;  //wielhouder
    pick[4]=36;   place[4]=2;  //balkje 1x4
    pick[5]=29;   place[5]=4;  //inv schuin links voor
    pick[6]=32;   place[6]=0;  //inv schuin rechts voor
    // reset brickrelease
    OnRev(OUT_B);
    Wait(200);
    Off(OUT_B);
    return;
    }
    
    sub null_z1()
    {
    //zoek nulpunt
    ClearSensor(SENSOR_1);
    SetSensor(SENSOR_1,SENSOR_TOUCH);
    SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
    ClearSensor(SENSOR_1);
    a=1;
    OnFwd(OUT_A);
    while (SENSOR_1>200);
    Off(OUT_A);
    a=0;
    return;
    }

    sub paksteen1()
    {
    SetSensor(SENSOR_1,SENSOR_ROTATION);
    SetSensorMode(SENSOR_1,SENSOR_MODE_ROTATION);
    ClearSensor(SENSOR_1);
    //zak naar goede hoogte
    a=-1;
    OnRev(OUT_A);
    pos=275;
    while (SENSOR_1<pos) ;
    Off(OUT_B);
    a=0;
    return;
    }
    
    sub plaatssteen1()
    {
    SetSensor(SENSOR_1,SENSOR_ROTATION);
    SetSensorMode(SENSOR_1,SENSOR_MODE_ROTATION);
    ClearSensor(SENSOR_1);
    //zak naar goede hoogte
    a=-1;
    OnRev(OUT_A);
    pos=290;
    while (SENSOR_1<pos) ;
    Off(OUT_A);
    a=0;
    //release
    if (toggle==0) {b=1;OnFwd(OUT_B);toggle=1;}
    else {b=-1;OnRev(OUT_B);toggle=0;};
    a=1;
    OnFwd(OUT_A);
    Wait(120);
    Off(OUT_A);
    a=0;
    Off(OUT_B);
    b=0;
//  PlayTone(500,120);
    return;
    } //end plaatssteen1

    sub null_z2()
    {
    //zoek nulpunt
    ClearSensor(SENSOR_3);
    SetSensor(SENSOR_3,SENSOR_TOUCH);
    SetSensorMode(SENSOR_3,SENSOR_MODE_RAW);
    ClearSensor(SENSOR_3);
    c=1;
    OnFwd(OUT_C);
    while (SENSOR_3>200);
    Off(OUT_C);
    c=0;
    return;
    }

    sub paksteen2()
    {
    SetSensor(SENSOR_3,SENSOR_ROTATION);
    SetSensorMode(SENSOR_3,SENSOR_MODE_ROTATION);
    ClearSensor(SENSOR_3);
    //zak naar goede hoogte
    c=-1;
    OnRev(OUT_C);
    pos=275;
    while (SENSOR_3<pos) ;
    Off(OUT_C);
    c=0;
    return;
    } //end paksteen2

    sub plaatssteen2()
    {
    SetSensor(SENSOR_3,SENSOR_ROTATION);
    SetSensorMode(SENSOR_3,SENSOR_MODE_ROTATION);
    ClearSensor(SENSOR_3);
    //zak naar goede hoogte
    c=-1;
    OnRev(OUT_C);
    pos=290;
    while (SENSOR_3<pos) ;
    Off(OUT_C);
    c=0;
    //release
    if (toggle==0) {b=1;OnFwd(OUT_B);toggle=1;}
    else {b=-1;OnRev(OUT_B);toggle=0;};
    c=1;
    OnFwd(OUT_C);
    Wait(120);
    Off(OUT_C);
    c=0;
    Off(OUT_B);
    b=0;
//  PlayTone(500,120);
    return;
    } //end plaats3

//*********************************************************

  task main()
  {
/*
//testloop
    init();
    null_z();
    plaatssteen();
    Wait(500);
    null_z();
    plaatssteen();
    Wait(5000);
*/

    init();
    null_z1();
    null_z2();
    PlayTone(462,20);
    PlayTone(262,50);

    while (true)
    {

    ClearMessage();
    while (Message()<200);      //Wacht op control
    mes=Message();
    PlaySound(SOUND_LOW_BEEP);
    Wait(100);

    if (mes==220)  {paksteen1();SendMessage(238);};
    if (mes==223) {paksteen2();SendMessage(241);};
    if (mes==226) {plaatssteen1();SendMessage(238);};
    if (mes==229) {plaatssteen2();SendMessage(241);};
    if (mes==232) {null_z1();SendMessage(244);};
    if (mes==235) {null_z2();SendMessage(244);};
    }
    
    } //main