/*RCX 1 -- 3 touch sensors for arm's movement, 3 motors for arm's movement
forward, arm's rotation, and to flip the switch to move arm up and down. */
int smallpiece;     //to tell the RCX if it is picking up a small piece
int done;
int moved=0;

task placepiece()
  {
  
     done=0;

     if(moved>1)
     SendMessage(6);   // to tell other RCX to move arm back to normal position
     
     Wait(400);
     
     while(SENSOR_1!=1) //move to pieces
     OnFor(OUT_B,1);
     

Wait(200);

     OnFor(OUT_A,10);  //to hit switch to move arm down
     Wait(200);
     
     if(smallpiece==0)
     OnFor(OUT_C,150);  //to grab the piece
     if(smallpiece==1)
     OnFor(OUT_C,200);  //to grab the piece
     Wait(200);
     
     Rev(OUT_A);
       if(smallpiece==1)
     OnFor(OUT_A,15);  //to lift arm back up again
       if(smallpiece==0)
        OnFor(OUT_A,10);
     Toggle(OUT_A);
     Wait(200);
     Rev(OUT_B);

     Wait(200);
     
     while(SENSOR_2!=1) //move to builder
     OnFor(OUT_B,1);

     Wait(200);
     
     //****************************
     if(moved==2)
     {
     SendMessage(4); // to tell the other RCX to move the arm up all the way
     moved=2;
     }
     //****************************
     if(moved==1)
     {
     SendMessage(2); // to tell the other RCX to move the arm up one level
     moved=2;
     }
     //****************************
     
     Wait(300);
     
     OnFor(OUT_A,10);   //to move arm down on the builder
     Toggle(OUT_A);
     
     Wait(300);


     
     Rev(OUT_C);

     if(smallpiece==0)
     OnFor(OUT_C,150);   //to drop the piece
     if(smallpiece==1)
     OnFor(OUT_C,200);   //to drop the piece

     Wait(300);

     Toggle(OUT_A);
     OnFor(OUT_A,15);  //to lift arm back up
     Wait(300);
     Toggle(OUT_A);
     
     Toggle(OUT_B);
     Toggle(OUT_C);
     done=1;
     
     if(moved==0)
     moved=1;

   }                   //end task




task main()
{
  SetPower(OUT_A,OUT_FULL); //Claw
  SetPower(OUT_C,OUT_FULL); //motor that controls forward/backward movement of arm
  SetPower(OUT_B,OUT_LOW); //rotation motor
  
  SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH); //sensor to stop when arm is over peices
  SetSensorType(SENSOR_2, SENSOR_TYPE_TOUCH); //sensor to stop when arm is over builder
  SetSensorType(SENSOR_3, SENSOR_TYPE_TOUCH); //sensor to tell when arm is pushed all the way BACK
  
  SetSensorMode(SENSOR_1, SENSOR_MODE_BOOL);
  SetSensorMode(SENSOR_2, SENSOR_MODE_BOOL);
  SetSensorMode(SENSOR_3, SENSOR_MODE_BOOL);
  
  PlaySound(SOUND_FAST_UP);



start placepiece;
while(done!=1);

//-------------------------------------------------------------

     SendMessage(1);  //to tell other RCX to push the brick into place
     Wait(500);
     
//---------------------

smallpiece=1;
start placepiece;
while(done!=1);

//------------
              SendMessage(3); // to tell the other RCX to push the 2 blocks together
              Wait(300);
              SendMessage(4);  // to tell the other RCX to move the arm up one more
              Wait(200);
//------------
smallpiece=0;
start placepiece;
while(done!=1);

//---------------
                 SendMessage(5);  // to tell the other RCX to push all the blocks together
                 Wait(300);
                 SendMessage(6);  // to tell the other RCX to move arm back to normal position
                 PlaySound(SOUND_DOWN);
                 PlaySound(SOUND_UP);

                 
}