/*
BRAS (French - "Arm")
=====================

AKA Grabber II

NB - THIS IS NQC 2.X CODE.

Ken McMullan, 14-Feb-2000
*/

#define MtrArm OUT_A
#define MtrBase OUT_B
#define MtrClaw OUT_C

#define PwrArm 6  // a bit less than full power
#define PwrBase 7 // full power
#define PwrClaw 4  // a bit more than half power

#define SenArm SENSOR_1
#define SenBase SENSOR_2
#define SenClaw SENSOR_3

#define DirUp OUT_FWD
#define DirDown OUT_REV
#define DirLeft OUT_REV
#define DirRight OUT_FWD
#define DirOpen OUT_REV
#define DirClose OUT_FWD

#define ArmTime 400 // time (1/100s) to move arm fully up or down

#define BaseRev 1152 // sensor units per full revolution of the base

int BaseDir, BaseCount, ClawStuck, ClawDir; // globals

task ClawMonitor () // tell program if claw has stopped moving
{
  int OldClaw;

  while (true)                
  {
    OldClaw=SenClaw;                   // take position reading
    Wait(20);                          // doze for a fifth of a second
    OldClaw ||=SenClaw-OldClaw;        // calculate absolute movement
    if (OldClaw <= 10) { ClawStuck = -1; }
  }
} // ClawMonitor

sub Claw ()
{
  SelectDisplay(DISPLAY_SENSOR_3);
  if (ClawDir==DirOpen)                // set claw motor direction
    { SetDirection(MtrClaw,DirOpen); }
  else
    { SetDirection(MtrClaw,DirClose); }
  if (ClawDir!=0)
  {
    On(MtrClaw);                         // run the motor
    ClawStuck=0;
    while (ClawStuck == 0) { }           // wait for claw to stop moving
  }
  Off(MtrClaw);                          // and stop
} // Claw

task SetBase ()
{
  int AbsBase;
  
  while (true)
  {
    if (BaseDir!=0)
    {
      On(MtrBase);                     // run the motor
      if (BaseDir==DirRight)           // set base motor direction
        { SetDirection(MtrBase,DirRight); }
      else
        { SetDirection(MtrBase,DirLeft); }
    }
    else { Off(MtrBase); }             // BaseDir=0, so stop
  
    AbsBase ||= SenBase;
    if (AbsBase >= BaseCount)          // stop base when in position
      { Off(MtrBase); }

  } // while true
} // SetBase

task main ()
{
  SetSensor(SenArm, SENSOR_TOUCH);     // set up sensors
  SetSensor(SenBase, SENSOR_ROTATION);
  SetSensor(SenClaw, SENSOR_ROTATION);
  SetPower(MtrArm,PwrArm);             // set up motors
  SetPower(MtrBase,PwrBase);
  SetPower(MtrClaw,PwrClaw);

  start ClawMonitor;

  // initialise arm to a known state

  SetDirection(MtrArm,DirDown);        // set arm motor direction
  SetDirection(MtrClaw,DirOpen);       // set claw motor direction
  On(MtrArm);                          // run arm motor
  On(MtrClaw);                         // run claw motor
  ClawStuck=0;
  while (SenArm == 0)                  // loop until arm sensor pressed
  {
    if (ClawStuck != 0)                // if claw stops moving
      { Off(MtrClaw); }                // switch it off
  }
  Off(MtrArm);                         // stop arm motor
  while (ClawStuck == 0) {}            // wait to ensure claw has stopped
    { Off(MtrClaw); }                  // stop it

  ClearSensor(SenClaw);                // claw sensor=0 when fully opened

  // arm is now fully down, with claw fully open. NB base direction is unknown

  start SetBase;

  ClawDir=DirClose; Claw ();

  SetDirection(MtrArm,DirUp);
  OnFor(MtrArm,ArmTime);

  BaseDir=DirRight;                    // turn tight
  BaseCount=BaseRev/4;                 // quarter turn

} // main