//Program for new version of assembly line

#define ROT SENSOR_1
#define LIGHT SENSOR_2
#define READY SENSOR_3
#define BELT OUT_A
#define PLATE OUT_B
#define PRESS OUT_C

void move(int pos)
{
  if(ROT < pos)
  {
    OnFwd(BELT);
    until(ROT==pos);
    Off(BELT);
  }
  if(ROT > pos)
  {
    OnRev(BELT);
    until(ROT==pos);
    Off(BELT);
  }
}

void down()
{
  OnFwd(PLATE);
  Wait(200);
  Off(PLATE);
}

void up()
{
  OnRev(PLATE);
  Wait(250);
  Off(PLATE);
}

void compress()
{
  OnFwd(PRESS);
  until(LIGHT <= 30);
  OnRev(PRESS);
  Wait(300);
  Off(PRESS);
}

void assemble()
{
  until(READY == 1);
  move(0);
  move(32);
  down();
  move(65);
  up();
  compress();
  move(80);
  until(READY == 1);
  move(0);
}

void test()
{
  until(READY == 1)
    {
    down();
    up();
    }
  move(65);
  until(READY == 1);  
  move(0);
}


task main() 
{
  //Rotation sensor for conveyor belt on 1, light sensor from press on 2 and touch sensor for "Ready?" on 3
  SetSensor(ROT,SENSOR_ROTATION);
  SetSensor(LIGHT,SENSOR_LIGHT);
  SetSensor(READY,SENSOR_TOUCH);
  assemble();
}