Mindboards

Code: Select all

 /*

Learning brick sorter
=====================

Author: Mario Ferrari
Date:   January 2003

*/

#define BINS 3
#define RANGES 3
#define SWITCH_TIME 20
#define LITE_THRSHLD 98
#define OP_DELAY 75
#define RANGE_WIDTH 2
#define KB_YES 1
#define KB_NO -1
#define KB_UNKNOWN 0

int range;            // index of the selected range (0 based)
int top_range;        // highest used range index
int bin;              // index of the selected bin (0 based)
int pos;              // current arm position: -1=pick up station, 0=bin0, 1=bin1...
int min[RANGES];      // lower limits of the ranges
int max[RANGES];      // upper limits of the ranges
int kb[RANGES*BINS];  // the knowledge base

task main()
{

  SystemSetup();

  while (true)
  {
    // wait for button pressed
    while (SENSOR_1 < LITE_THRSHLD && SENSOR_2 == 0);
    PlaySound(SOUND_CLICK);
    
    // analyze knowledge base (set the range & bin variable)
    Analyze();
    
    // take the brick
    LowerArm();
    Wait(OP_DELAY);
    CloseHand();
    Wait(OP_DELAY);
    LiftArm();
    
    // go to bin and drop it
    Move(bin);
    OpenHand();
    
    // wait for user input
    while (SENSOR_1 <LITE_THRSHLD && SENSOR_2 == 0);
    
    // update knowledge base
    if (SENSOR_2 == 1)  // wrong
    {
      PlaySound(SOUND_LOW_BEEP);
      Wait(OP_DELAY);
      UpdateKnowledgeBase(false);
    }
    else  // right
    {
      PlaySound(SOUND_DOUBLE_BEEP);
      Wait(OP_DELAY);
      UpdateKnowledgeBase(true);
    };
    // go back to pick up station
    Move(-1);
  }
}

void SystemSetup()
{
  int i;

  SetSensor(SENSOR_1, SENSOR_LIGHT);
  SetSensor(SENSOR_2, SENSOR_TOUCH);
  SetSensor(SENSOR_3, SENSOR_TOUCH);

  pos = -1;        // initial position is pick-up station
  top_range = -1;  // means no range is yet defined
  
  // clear the knowledge base
  for (i=0;i<BINS*RANGES;i++) kb[i]=KB_UNKNOWN;
  SetUserDisplay(range*100+bin,2);
}

void Analyze()
{
  int light_value;

  // wait for coupled touch sensor released
  while (SENSOR_1>LITE_THRSHLD);
  light_value = SENSOR_1;

  // search if the light value is contained in one of the already defined ranges
  for(range=0;range<=top_range;range++) {
    if (light_value>=min[range] && light_value<=max[range]) break;
  }
  
  // if not, define a new range
  if (range>top_range) {
    top_range++;
    // if all ranges have been used, beep for error and terminate program
    if (top_range>=RANGES) {
      PlaySound(SOUND_DOWN);
      StopAllTasks();
    }
    range=top_range;
    min[range]=light_value-RANGE_WIDTH;
    max[range]=light_value+RANGE_WIDTH;
  }

  // look for a positive value in the knowledge base array
  for (bin=0;bin<BINS;bin++)
    if (kb[range*BINS+bin]==KB_YES) return;

  // otherwise, look for an "unknown" value in the knowledge base array
  for (bin=0;bin<BINS;bin++)
    if (kb[range*BINS+bin]==KB_UNKNOWN) return;

  // this point shouldn't ever be reached, unless the user provided some
  // inconsistent replies. If this happens, the program makes a random choice.
  bin=Random(BINS-1);

}

void UpdateKnowledgeBase(int ok)
{
  int r;

  // the user said OK
  if (ok)
  {
    // this bin is the right one for the given range...
    kb[range*3+bin]=KB_YES;
    // ...and the wrong one for the other ranges
    for(r=0;r<RANGES;r++)
      if (r!=range)
        kb[r*BINS+bin]=KB_NO;
  }
  
  // the user said NO
  else
    // the bin is the wrong one
    kb[range*BINS+bin]=KB_NO;
}

void Move(int dest)
{
  int dir;
  
  // if arm is already at proper position, exit
  if (dest == pos) return;
  
  // set direction according to the difference between
  // the current position and the required destination
  if (dest > pos)
  {
    dir = 1;
    OnRev(OUT_A);
  }
  else
  {
    dir = -1;
    OnFwd(OUT_A);
  }
  
  // move until destination is reached
  while (dest != pos)
  {
    while (SENSOR_3 == 1);  // wait for sensor open
    while (SENSOR_3 == 0);  // wait for sensor closed again
    pos += dir;
  }
  Off(OUT_A);
}

void OpenHand()
{
  OnFwd(OUT_C);
  Wait(SWITCH_TIME);
  Off(OUT_C);
}

void CloseHand()
{
  OnRev(OUT_C);
  Wait(SWITCH_TIME);
  Off(OUT_C);
}

void LiftArm()
{
  OnFwd(OUT_B);
  Wait(SWITCH_TIME);
  Off(OUT_B);
}

void LowerArm()
{
  OnRev(OUT_B);
  Wait(SWITCH_TIME);
  Off(OUT_B);
}