// LegoCam NQC code
// 
// Accepts simple numeric messages and moves the camera in the
// appropriate direction.
// 
// Copyright (c) 2004 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

// The Message settings.

#define SWITCH           1
#define PAGE             2
#define RESET            3
#define UP               11
#define UPRIGHT          12
#define RIGHT            13
#define DOWNRIGHT        14
#define DOWN             15
#define DOWNLEFT         16
#define LEFT             17
#define UPLEFT           18
#define NEUTRAL          21
#define FULLRIGHT        22
#define HALFRIGHT        23
#define NEUTRALPAN       24
#define HALFLEFT         25
#define FULLLEFT         26
#define NEUTRALTILT      27
#define HOLD             28

// 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
#define MOTOR_SWITCH     OUT_B

// 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);
  }
}

// Task 0: Main Program

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);
  ClearTimer(0);
  ClearCounter(0);
  ClearCounter(1);
  ClearMessage();
  int msg = 0;
  repeat (2) { PlaySound (1); Wait(100); }
  while (true)
  {
    until (Message() != 0 || Timer(0) == TIME_RESET);
    msg = Message();
    if (msg != 0)
    {
      ClearMessage();
      ClearTimer(0);
      IncCounter(1);
      switch (msg)
      {
        case SWITCH:
          // Toggle the switch
          if (Counter(0) > 0) { ClearCounter(0); Off(MOTOR_SWITCH); }
          else { IncCounter(0); On(MOTOR_SWITCH); }
          break;
        case PAGE:
          // Play a sound
          repeat (3) { PlaySound(5); Wait(100); };
          break;
        case RESET:
          // Reset the RCX
          ClearCounter(0);
          ClearCounter(1);
          pan(NEUTRAL_PAN); tilt(NEUTRAL_TILT); Off(MOTOR_SWITCH);
          break;
        case UP:
          // Move the camera up
          tilt(SENSOR_TILT - VALUE_TILT);
          break;
        case UPRIGHT:
          // Move the camera up and right
          pan(SENSOR_PAN + VALUE_PAN); tilt(SENSOR_TILT - VALUE_TILT);
          break;
	      case RIGHT:
      	  // Move the camera right
          pan(SENSOR_PAN + VALUE_PAN);
          break;
        case DOWNRIGHT:
          // Move the camera down and right
          pan(SENSOR_PAN + VALUE_PAN); tilt(SENSOR_TILT + VALUE_TILT);
          break;
        case DOWN:
          // Move the camera down
          tilt(SENSOR_TILT + VALUE_TILT);
          break;
        case DOWNLEFT:
          // Move the camera down and left
          pan(SENSOR_PAN - VALUE_PAN); tilt(SENSOR_TILT + VALUE_TILT);
          break;
        case LEFT:
          // Move the camera left
          pan(SENSOR_PAN - VALUE_PAN);
          break;
        case UPLEFT:
          // Move the camera up and left
          pan(SENSOR_PAN - VALUE_PAN); tilt(SENSOR_TILT - VALUE_TILT);
          break;
        case NEUTRAL:
	        // Move the camera to neutral
          pan(NEUTRAL_PAN); tilt(NEUTRAL_TILT);
          break;
        case FULLRIGHT:
          // Move camera to full right
          pan(LIMIT_PAN_RIGHT);
          break;
        case HALFRIGHT:
          // Move camera to half right
          pan(LIMIT_PAN_RIGHT / 2);
          break;
        case NEUTRALPAN:
          // Move camera to neutral pan
          pan(NEUTRAL_PAN);
          break;
        case HALFLEFT:
          // Move camera to half left
          pan(LIMIT_PAN_LEFT / 2);
          break;
        case FULLLEFT:
          // Move camera to full left
          pan(LIMIT_PAN_LEFT);
          break;
        case NEUTRALTILT:
          // Move camera to neutral tilt
          tilt(NEUTRAL_TILT);
          break;
        case HOLD:
          // Hold camera in current position
          ClearCounter(1);
          break;
      }
    }
    else
    {
      if (Counter(1) > 0)
      {
        ClearCounter(0);
        ClearCounter(1);
        pan(NEUTRAL_PAN); tilt(NEUTRAL_TILT); Off(MOTOR_SWITCH);
      }
    }
  }
}