/*RCX 2 -- 3 touch sensors for arm's movement forward and backwards, 2 motors for builder
and to move the arm forward and backwards */

int done=0;



task main()
{
  SetPower(OUT_A,OUT_HALF); //To move arm forward and backward
  SetPower(OUT_C,OUT_FULL); //to pump air tank (if equipped)
  SetPower(OUT_B,OUT_FULL); //Builder

  SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH); //to sense when the arm is all the way BACK
  SetSensorType(SENSOR_2, SENSOR_TYPE_TOUCH); //To sense when the arm is half way
  SetSensorType(SENSOR_3, SENSOR_TYPE_TOUCH); //to sense when the arm is all the way forward

  SetSensorMode(SENSOR_1, SENSOR_MODE_BOOL);
  SetSensorMode(SENSOR_2, SENSOR_MODE_BOOL);
  SetSensorMode(SENSOR_3, SENSOR_MODE_BOOL);

  PlaySound(SOUND_FAST_UP);
  
  while(done==0)     //waits the command to stop
  {

     if(Message()==1)
     {
   until(Message()>0);     //wait for message

     OnFor(OUT_B,180);
     PlaySound(SOUND_CLICK);  //push plate into place
     Toggle(OUT_B);
     OnFor(OUT_B,180);
     Toggle(OUT_B);
     ClearMessage();
     }
     
     if(Message()==2)
     {
     OnFor(OUT_A,80);        //move arm up half way
     PlaySound(SOUND_CLICK);
     ClearMessage();
     }
     
     if(Message()==3)      //to push two pieces together
     {
     OnFor(OUT_B,150);
     Wait(100);
     Toggle(OUT_B);
     OnFor(OUT_B,150);
     PlaySound(SOUND_CLICK);
     ClearMessage();
     }
     
     if(Message()==4)
     {                       //to move arm all the way up
     while(SENSOR_3!=1)
     OnFor(OUT_A,2);
     
     PlaySound(SOUND_CLICK);
     ClearMessage();
     }
     
     if(Message()==5)
     {
     Toggle(OUT_B);       //to push all the plates together
     OnFor(OUT_B,130);
     Wait(100);
     Toggle(OUT_B);
     OnFor(OUT_B,130);
     ClearMessage();
     }
     
     if(Message()==6)       //to move arm back to normal position
     {
     Toggle(OUT_A);
     
     while(SENSOR_1!=1)
     OnFor(OUT_A,2);
     
     Toggle(OUT_A);
     ClearMessage();
     }
     

     


  }








}