/*
N-JABS (Not Just Another Brick Sorter)
======================================

NB - THIS IS RCX 1.X CODE.

Ken McMullan, 18-June-1999
*/

#define BrickSource 0
#define BrickUnknown -650 // these values are assigned to disinguish
#define BrickBlack -100   // between the bricks and also indicate the
#define BrickBlue -210    // value which the angle sensor on the sorter
#define BrickRed -320     // "arm" should show when the "hand" is
#define BrickYellow -430  // positioned over the appropriate bin
#define BrickWhite -540

#define OpenTime 150 // time (1/100s) to fully open hand
#define CloseTime 250 // time (1/100s) to fully close hand

#define MinBin -550 // farthest clockwise arm is allowed to rotate
#define MaxBin 10   // farthest anticlockwise arm is allowed to rotate

#define OutRed 0     // RGB Slider Position for Red light
#define OutGreen -23 // RGB Slider Position for Green light
#define OutBlue -46  // RGB Slider Position for Blue light

#define MtrArm OUT_A  // sorter arm motor
#define MtrHand OUT_B // sorter hand motor
#define MtrRGB OUT_C  // RGB slider position motor

#define SenArm IN_1   // sorter arm rotation sensor
#define SenLight IN_2 // light sensor
#define SenRGB IN_3   // RGB slider position sensor

#define PwrArm 6  // almost full power
#define PwrHand 4 // a bit more than half power
#define PwrRGB 3  // a bit less than half power

int Light; // colour of light required
int LightOK; // indicates that the selected colour of light is now current
int RedSam; // sampled reflected light with red filter
int GreenSam; // sampled reflected light with green filter
int BlueSam; // sampled reflected light with blue filter
int Brick; // the colour of the brick which has been presented
int Bin; // position of bin to put brick into
int ArmOK; // indicates that the arm has finished moving

task SetArm // set and maintain the position of the sorter arm as "Bin"

{

  Bin=BrickSource; // set (and maintain) initial position
  ArmOK=0; // not initally correct

  while (true)
  {

    // protection against moving the arm out of bounds
    if (Bin > MaxBin) { Light = MaxBin; }
    if (Bin < MinBin) { Bin = MinBin; }

    if ( SenArm > Bin ) // too far clockwise
    {
      Fwd(MtrArm,PwrArm); // move arm anticlockwise
      ArmOK = 0; // arm is moving
    }
    else if ( SenArm < Bin )  // too far anticlockwise
    {
      Rev(MtrArm,PwrArm); // move arm clockwise
      ArmOK = 0; // arm is moving
    }
    else // correct arm position established
    {
      OutputMode(MtrArm, OUT_OFF);
      ArmOK = -1; // arm is not moving
    }

  } // while true

} // SetArm

sub Pickup

// move the "hand" to the chute and pick up the brick therein

{

  Bin=BrickSource; // request arm to move to chute
  Sleep(25); // wait until arm starts to move (1/4 second)
  wait (ArmOK == -1); // wait until arm stops moving
  Fwd(MtrHand,PwrHand); // close hand
  Sleep(CloseTime); // wait until hand is closed (1 second)
  OutputMode(MtrHand, OUT_OFF);

} // Pickup

sub Drop

// move the "hand" to the appropriate bin and drop the brick

{

  Bin=Brick; // request arm to move to correct bin
  Sleep(25); // wait until arm starts to move (1/4 second)
  wait (ArmOK == -1); // wait until arm stops moving
  Rev(MtrHand,PwrHand); // open hand
  Sleep(OpenTime); // wait until hand is fully open (1 second)
  OutputMode(MtrHand, OUT_OFF);

} // Drop

task SetRGB // set and maintain the position of the RGB slider as "Light"

{

  Light=OutRed; // set (and maintain) initial colour
  LightOK=0; // not initally correct

  while (true)
  {

    // protection against moving the RGB slider out of bounds
    if (Light > OutRed) { Light = OutRed; }
    if (Light < OutBlue) { Light = OutBlue; }

    if ( SenRGB > Light ) // too far to the RED end of the filter
    {
      Fwd(MtrRGB,PwrRGB); // move filter towards BLUE
      LightOK = 0; // RGB filter is moving
    }
    else if ( SenRGB < Light )  // too far to the BLUE end of the filter
    {
      Rev(MtrRGB,PwrRGB); // move filter towards RED
      LightOK = 0; // RGB filter is moving
    }
    else // correct RGB filter selected
    {
      OutputMode(MtrRGB, OUT_OFF);
      LightOK = -1; // RGB filter is in correct position
    }

  } // while true

} // SetRGB

sub Check

// shine Red, then Green, then Blue light on the brick
// and use the values of the reflected light to assess
// the colour of the brick. Set "Brick" accordingly.

{

  Light = OutRed; // request Red light
  Sleep(25); // wait until filter starts to move (1/4 second)
  wait (LightOK == -1); // wait until filter stops moving
  RedSam = SenLight; // take sample under Red light

  Light = OutGreen; Sleep(25); // request Green light and wait
  wait (LightOK == -1); GreenSam = SenLight;

  Light = OutBlue; Sleep(25); // Blue light
  wait (LightOK == -1); BlueSam = SenLight;

  Light = OutRed; // reset to Red ready for next sample

// it works much better if thefollowing are in increasing order of "lightness"

  if (RedSam > 670 && RedSam < 745
    && GreenSam > 770 && GreenSam < 900
    && BlueSam > 790 && BlueSam < 945 ) { Brick = BrickBlack; }

  else if (RedSam > 650 && RedSam < 710
    && GreenSam > 740 && GreenSam < 840
    && BlueSam > 755 && BlueSam < 855 ) { Brick = BrickBlue; }

  else if (RedSam > 585 && RedSam < 625
    && GreenSam > 745 && GreenSam < 870
    && BlueSam > 750 && BlueSam < 860 ) { Brick = BrickRed; }

  else if (RedSam > 575 && RedSam < 615
    && GreenSam > 685 && GreenSam < 765
    && BlueSam > 735 && BlueSam < 845 ) { Brick = BrickYellow; }

  else if (RedSam > 575 && RedSam < 615
    && GreenSam > 655 && GreenSam < 730
    && BlueSam > 695 && BlueSam < 790 ) { Brick = BrickWhite; }

  else Brick = BrickUnknown; // default

} // Check

task main

{

  Sensor(SenArm, IN_CFG(STYPE_ANGLE,SMODE_ANGLE));
  Sensor(SenLight, IN_CFG(STYPE_LIGHT,SMODE_RAW));
  Sensor(SenRGB, IN_CFG(STYPE_ANGLE,SMODE_ANGLE));

  Rev(MtrHand,PwrHand); // make sure hand is fully open
  Sleep(OpenTime); // wait until hand is fully open (1 second)
  OutputMode(MtrHand, OUT_OFF);

  start SetRGB; wait (LightOK == -1); // initialise RGB filter
  start SetArm; wait (ArmOK == -1); // initialise arm

  SetDatalog(50); // open a new datalog

  while (true)
  {
    wait (LightOK == -1); // wait until the colour sensor isn't busy
    Check(); // figure out the brick colour
Datalog(RedSam);
Datalog(GreenSam);
Datalog(BlueSam);
Datalog(Brick); // log the brick colour
    Pickup(); // lift up the brick
    Drop(); // drop the brick in the appropriate place
  }

} // main