// WebRCX NQC code ( 3 / 3 )
// 
// Accepts simple numeric messages and twitches the camera in the
// appropriate direction.
// 
// webrcx1.nqc: Move the camera in incremental steps
// webrcx2.nqc: Move the camera to preset positions
// webrcx3.nqc: Filtered commands
//
// Copyright (c) 2001 Scott Ireland

// Timing and sensor value settings. Actual settings will vary
// depending on hardware design.

#define TIME_RESET       10 * 600  // Neutral reset timeout -- 600 = 1 minute
#define VALUE_PAN        80        // Pan rotation sensor move value 16 = 3 deg
#define LIMIT_PAN_LEFT   -480      // Pan rotation sensor left limit value
#define LIMIT_PAN_RIGHT  480       // Pan rotation sensor right limit value
#define NEUTRAL_PAN      0         // Pan rotation sensor neutral value
#define VALUE_TILT       80        // Tilt rotation sensor move value 16 = 3 deg
#define LIMIT_TILT_UP    -240      // Tilt rotation sensor up limit value
#define LIMIT_TILT_DOWN  240       // Tilt rotation sensor down limit value
#define NEUTRAL_TILT     0         // Tilt rotation sensor neutral value

// Input and Output device ports. No changes needed,
// unless you use different ports.

#define SENSOR_PAN       SENSOR_1
#define LIGHT_SENSOR     SENSOR_2
#define SENSOR_TILT      SENSOR_3
#define MOTOR_PAN        OUT_A
#define MOTOR_TILT       OUT_C

// Global variables used in the program.

int SAVE_PAN;
int SAVE_TILT;
int SAVE_LIGHT;
int VALUE_LIGHT;
int VALUE_DIR;

// Pan the camera to the desired position.

void pan(int value)
{
  // This adjusts for motor drift 
  value = ((((value * 10) / VALUE_PAN) + (sign(value) * 5)) / 10) * VALUE_PAN;
  if (OutputStatus (0) > 0x7F)
  {
    Off(MOTOR_PAN);
  }
  else if (SENSOR_PAN < value && value <= LIMIT_PAN_RIGHT)
  {
    // Move Right
    OnFwd(MOTOR_PAN);
    until (SENSOR_PAN >= value);
    Off(MOTOR_PAN);
  }
  else if (SENSOR_PAN > value && value >= LIMIT_PAN_LEFT)
  {
    // Move Left
    OnRev(MOTOR_PAN);
    until (SENSOR_PAN <= value);
    Off(MOTOR_PAN);
  }
}

// Tilt the camera to the desired position.

void tilt(int value)
{
  // This adjusts for motor drift
  value = ((((value * 10) / VALUE_TILT) + (sign(value) * 5)) / 10) * VALUE_TILT;
  if (OutputStatus (2) > 0x7F)
  {
    Off(MOTOR_TILT);
  }
  else if (SENSOR_TILT > value && value >= LIMIT_TILT_UP)
  {
    // Move Up
    OnFwd(MOTOR_TILT);
    until (SENSOR_TILT <= value);
    Off(MOTOR_TILT);
  }
  else if (SENSOR_TILT < value && value <= LIMIT_TILT_DOWN)
  {
    // Move Down
    OnRev(MOTOR_TILT);
    until (SENSOR_TILT >= value);
    Off(MOTOR_TILT);
  }
}

// Seek light scan function.

void scan()
{
  SAVE_PAN = SENSOR_PAN;
  SAVE_TILT = SENSOR_TILT;
  SAVE_LIGHT = LIGHT_SENSOR;
  VALUE_DIR = 1;
  pan(LIMIT_PAN_RIGHT);
  until (SENSOR_PAN <= LIMIT_PAN_LEFT)
  {
    check_tilt();
    pan(SENSOR_PAN - VALUE_PAN);
  }
  check_tilt();
  pan(SAVE_PAN); tilt(SAVE_TILT);
}

void check_tilt()
{
  if (VALUE_DIR == 1)
  {
    VALUE_DIR = -1;
    tilt(LIMIT_TILT_DOWN);
    until (SENSOR_TILT <= LIMIT_TILT_UP) 
    { 
      check_value(); 
      tilt(SENSOR_TILT - VALUE_TILT); 
    }
  }
  else
  {
    VALUE_DIR = 1;
    tilt(LIMIT_TILT_UP);
    until (SENSOR_TILT >= LIMIT_TILT_DOWN)
    {
      check_value();
      tilt(SENSOR_TILT + VALUE_TILT);
    }
  }
  check_value();
}

void check_value()
{
  VALUE_LIGHT = LIGHT_SENSOR;
//  PlaySound (0);
  if (VALUE_LIGHT > SAVE_LIGHT)
  {
    SAVE_PAN = SENSOR_PAN; SAVE_TILT = SENSOR_TILT; SAVE_LIGHT = VALUE_LIGHT;
  }
}

// Task 0: Initialize the RCX

task main()
{
  SetSensor(LIGHT_SENSOR, SENSOR_LIGHT);
  SetSensor(SENSOR_PAN, SENSOR_ROTATION);
  SetSensor(SENSOR_TILT, SENSOR_ROTATION);
  SetPower(MOTOR_PAN + MOTOR_TILT, OUT_FULL);
  ClearSensor(SENSOR_PAN);
  ClearSensor(SENSOR_TILT);
  SetSleepTime(0);
  repeat (2) { PlaySound (1); Wait(100); }
}

// Task 1: Wait until the reset time has passed and move to neutral

task timer()
{
  ClearTimer(0); 
  until (Timer(0) == TIME_RESET) { };
  pan(NEUTRAL_PAN); tilt(NEUTRAL_TILT);
}

// Task 2: Play a sound

task sound()
{
  stop timer; repeat (3) { PlaySound(5); Wait(100); }; start timer;
}

// Task 3: Seek light

task seek_light()
{
  stop timer; scan(); pan(SAVE_PAN); tilt(SAVE_TILT); start timer;
}