/*
RackPin II
==========

NB - THIS IS NQC 1.X CODE.
  
Ken McMullan, 19-May-1999

Control software for the Mark II Rack and pinion buggy.

*/

#define AutoDisplay true // auto display result of last command

#define Gpwr 9 // 9:1 motor:wheel ratio

#define WheelCir 256 // PI * 81.6mm = Circumference (mm)

#define StrMax 22 // max steering sensor value in either direction

#define StrTimeout 35 // steering timeout (3.5 seconds) if it takes longer, something's up
#define StrTimer 0 // steering timeout timer index

#define MtrPwr OUT_A // driving motor
#define MtrStr OUT_C // steering motor

#define SenPwr IN_1 // speed / distance sensor
#define SenStr IN_3 // steering position sensor

#define PowerOut 7 // max
#define SteerOut 7 // max

int Rot; // number of wheel rotations for this trip
int Trip; // distance (mm) travelled since last odometer reset
int Distance; // distance (cm) to be travelled. 0=keep going
int Moving; // flag indicating that buggy is currently moving
int Direction; // steering degree. 0=straight, +ve=Left, -ve=right
int Steering; // flag indicating that steering is in progress

sub Success
{
  PlayNote(262,14); Sleep(16); // C 4
  PlayNote(349,14); Sleep(16); // F 4
  PlayNote(440,14); Sleep(16); // A 5
  PlayNote(523,14); Sleep(32); // F 5
  PlayNote(440,12); Sleep(14); // A 5
  PlayNote(523,64); Sleep(16); // F 5
}

task Steer // maintain the direction of the steering wheels to meet "Direction"

// "Direction" is the index of the steering wheels, where 0=stright,
// +ve=Left and -ve=right. Once "Steer" is running, Setting Direction
// with aim the buggy. "Steering" is true unless there is no steering
// in progress
// This task starts by ensuring that the steering sensor has the value
// zero when the wheels are straight. sensor values run from -25 for
// full left to +25 for full right, but the program should stay within
// +-20 for safety. The max value for this limit is defined above.

{

  Steering=-1; // currently steering
  ClearTimer (StrTimer); // reset steering timeout timer
  Rev(MtrStr,PowerOut); // steer right
  wait (Timer(StrTimer) > StrTimeout) // steering jammed
  Direction=SenStr; // store current position
  Fwd(MtrStr,PowerOut); // steer left
  wait (SenStr >= Direction + 27); // until straight
  ClearSensor(SenStr); // zero steering sensor

  Direction=0; // set [maintain!] initial direction
  Steering=0; // not currently steering

  while (true)
  {

    if ( Steering == 0 ) // steering not currently busy
    {
      ClearTimer (StrTimer); // reset steering timeout timer
    }
    else if (Timer(StrTimer) > StrTimeout) // steering jammed
    {
      OutputMode(MtrStr + MtrPwr, OUT_OFF); // all stop
      PlaySound(4); // Buhhh!
      StopAllTasks();
    }

    // protection against oversteer demand
    if (Direction > StrMax) { Direction = StrMax; }
    if (Direction < 0-StrMax) { Direction = 0-StrMax; }

    if ( SenStr > Direction ) // too far left
    { 
      Rev(MtrStr,PowerOut); // steer right
      Steering = -1; // steering busy 
      if (AutoDisplay) { Display(3); } // steering sensor
    }
    else if ( SenStr < Direction ) // too far right
    {
      Fwd(MtrStr,PowerOut); // steer left
      Steering = -1; // steering busy 
      if (AutoDisplay) { Display(3); } // steering sensor
    }
    else // right on course
    {
      OutputMode(MtrStr, OUT_OFF);
      Steering = 0; // steering not busy
    }

  } // while true

} // Steer

task Odometer // keep track of distance travelled

// This task keeps the trip distance in mm, "Trip" updated.
// Stopping, then restarting this task resets "Trip".
// Executing sub Stop() also resets trip.
// Also ensures that the buggy never goes further than "Distance"
// which is to be the distance travelled in mm. 0=infinite.

{

  ClearSensor(SenPwr); // reset odometer on powerup

  while ( true )

  {
    Rot=SenPwr / 16 / Gpwr; // complete wheel rotations
    if (Rot < 0) { Rot = 0 - Rot; } // always positive
    Trip=Rot * WheelCir; // mm

    if (Distance != 0) // distance specified
    {
      if (Trip > Distance) // go the distance
      {
        Stop();
        ClearSensor(SenPwr); // reset odometer on trip completion
      }
    } // if Distance <> 0

  } // while true

} // Odometer

sub Forward

// Before calling "Forward", the programmer should set "Distance"
// to be the distance travelled in mm. 0=infinite.
// "Direction" should also be set - see "Steer"

{

  OutputDir(MtrPwr, OUT_REV); OutputMode(MtrPwr, OUT_ON);
  Moving = 1; // moving forward
  if (AutoDisplay) { Display(1); } // distance sensor

} // Forward

sub Reverse

// Before calling "Reverse", the programmer should set "Distance"
// to be the distance travelled in mm. 0=infinite.
// "Direction" should also be set - see "Steer"

{

  OutputDir(MtrPwr, OUT_FWD); OutputMode(MtrPwr, OUT_ON);
  Moving = -1; // moving backward
  if (AutoDisplay) { Display(1); } // distance sensor

} // Reverse

sub Stop

// Stops main motor and indicates via "Moving" that the buggy
// has stopped.

{

  OutputMode(MtrPwr, OUT_OFF);
  Moving = 0; // not currrently moving

} // Stop

task main

{

  Sensor(SenPwr,IN_ANGLE);
  Sensor(SenStr,IN_ANGLE);
  OutputPower(MtrStr, SteerOut);
  OutputPower(MtrPwr, PowerOut);

  start Steer; wait (Steering == 0); // steering initialisation
  start Odometer;

  Distance=250; // mm
  Direction=0; // straight
  Forward();
  wait (Moving == 0);
  Distance=750; // mm
  Direction = -20; // full right
  wait (Steering == -1); // wait until steering starts
  wait (Steering == 0); // wait until steering stops
  Forward();
  wait (Moving == 0);
  Distance=500; // mm
  Direction = 0; // straight
  wait (Steering == -1); // wait until steering starts
  wait (Steering == 0); // wait until steering stops
  Direction = 20; // full left
  Reverse();
  wait (Moving == 0);
  Distance=750; // mm
  Direction = 0; // straight
  wait (Steering == -1); // wait until steering starts
  wait (Steering == 0); // wait until steering stops
  Direction = -20; // full right
  Forward();
  wait (Moving == 0);
  Distance=250; // mm
  Direction=0; // straight
  Forward();
  wait (Moving == 0);

  Success();

} // main