/*
Program : ch_lego_RemoteControl_navi.nqc
Date    : 2006-09-25
Model   : RCX 1.0 - Navigation control
Ports   : Output - A Drive Motor
                            B Steer Motor
                            C Camera turntable Motor
          Input  - 1 Hitechnic Remote Control Sensor Base
                     - 2 Angle Sensor for steer control(B)
                     - 3 Angle Sensor for Camera turntable control(C)
Control : Remote Control Transmitting Code Raw value Range(2 RCXs ; Single RCX)
          1. 991(976~1006) ; 958(928~988)
             Move Forward & turn Camera turntable to front /
             Stop when Forwarding(A)
          2. 957(942~975)  ; 895(865~927)
             Move Backward & turn Cam turntable to back /
             Stop when Backwarding(A)
          3. 919(900~941)  ; 833(803~864)
             Turn Steer to Left / Turn Steer to center(B)(Navigation mode only)
          4. 878(863~899)  ; 769(739~802)
             Turn Steer to Right / Turn Steer to center(B)(Navigation mode only)
          5. 832(817~862)  ; 706(676~738)
             Turn Camera turntable left 1/4 circle(C) /
             Turn more 1/4 circles, but when in back side, no more turn
             (Navigation mode only)
          6. 785(770~816)  ; 646(616~675)
             Turn Camera turntable right 1/4 circle(C) /
             Turn more 1/4 circles, but when in back side, no more turn
             (Navigation mode only)
          7. 733(718~769)  ; 582(552~615)
             Reset all:Stop Drive, Turn Steer to center, Turn Camera turntable
                       to front; Switch to Navigation Mode
          8. 675(660~717)  ; 518(488~551)
             Navigation / Mission mode switch; ** Do same as [70 Reset all]
*/

#define DRIVE              OUT_A
#define STEER              OUT_B
#define CAMERA             OUT_C
#define REMOTE_CMD         SENSOR_1
#define STEER_ROTATION     SENSOR_2
#define CAMERA_ROTATION    SENSOR_3
/* Motor direction Macro */
#define DRIVE_FWD          OnFwd(OUT_A)
#define DRIVE_BACK         OnRev(OUT_A)
#define STEER_RIGHT        OnRev(OUT_B)
#define STEER_LEFT         OnFwd(OUT_B)
#define CAM_RIGHT          OnRev(OUT_C)
#define CAM_LEFT           OnFwd(OUT_C)
/* <2 RCX> Remote command base sensor Raw value definition */
#define Allow_Range        (cmd_code <= 999 && cmd_code >= 646)
#define Button_1           (cmd_code >= 975)
#define Button_2           (cmd_code >= 940)
#define Button_3           (cmd_code >= 900)
#define Button_4           (cmd_code >= 858)
#define Button_5           (cmd_code >= 809)
#define Button_6           (cmd_code >= 762)
#define Button_7           (cmd_code >= 707)
#define Button_8           (cmd_code >= 646)
/* RCX running mode switch */
#define NAVIGATION_MODE    10
#define MISSION_MODE       11
/* Drive Motor status control */
#define MOVE_STOP          12
#define MOVE_FWD           13
#define MOVE_REV           14
/* Steer Motor status control */
#define TURN_STOP          20
#define TURN_LEFT          21
#define TURN_RIGHT         22
/* Camera turntable position value */
#define CAM_IN_FRONT        0
#define CAM_IN_RIGHT        4
#define CAM_IN_RBACK        8
#define CAM_IN_LEFT        -4
#define CAM_IN_LBACK       -8
/* Steer position value */
#define STEER_IN_CENTER     0
#define STEER_IN_RIGHT      6
#define STEER_IN_LEFT      -6
/* Disable Steer/Camera turntable indicator */
#define NOT_ALLOW_S_TURN   30
/* Compensate the Rotation sensor accuracy */
#define COMPENSATION_TIME  50

int rcx_mode, move_mode, turn_mode;
int steer_status=0, steer_pos, steer_diff_val=0;
int cam_pos;
int cmd_code=0;

void Init_Rtn ()
/*
1. Stop all Motors and reset all status codes
2. Must turn the steer wheel & camera turntable to center position manually
*/
{
  rcx_mode = NAVIGATION_MODE;
  steer_status = NOT_ALLOW_S_TURN;
  Off(DRIVE+CAMERA+STEER);
  move_mode = MOVE_STOP;
  
  SetSensor(STEER_ROTATION,SENSOR_ROTATION);
  ClearSensor(STEER_ROTATION);
  steer_pos = STEER_IN_CENTER;
  turn_mode = TURN_STOP;

  SetSensor(CAMERA_ROTATION,SENSOR_ROTATION);
  ClearSensor(CAMERA_ROTATION);
  cam_pos = CAM_IN_FRONT;

  steer_status = 0;

  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(50);
}

void Turn_steer_Center ()
/*
Turn steer to center position
*/
{
  steer_status = NOT_ALLOW_S_TURN;
  Off(STEER);
  while (true)
  {
    steer_diff_val = STEER_ROTATION - STEER_IN_CENTER;
    if (steer_diff_val > 0) {STEER_LEFT;}
    else
    if (steer_diff_val < 0) {STEER_RIGHT;}
    else                    {Off(STEER); break;}
  }
  steer_pos = STEER_IN_CENTER;
  steer_status = 0;
  turn_mode = TURN_STOP;
  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(50);
}

void Turn_camera_FRONT ()
/*
Turn camera turntable to front side
*/
{
  int cam_diff_val = 0;
  
  Off(CAMERA);
  while (true)
  {
    cam_diff_val = CAMERA_ROTATION - CAM_IN_FRONT;
    if (cam_diff_val == 0)
    {
      Off(CAMERA);
      break;
    }
    else
    if (cam_diff_val > 0) {CAM_LEFT;}
    else
    if (cam_diff_val < 0) {CAM_RIGHT;}
  }
  cam_pos = CAMERA_ROTATION;
}

void Turn_camera_BACK ()
/*
Trun camera turntable to back side
*/
{
  int cam_diff_val = 0, cam_turn_target = 0;

  Off(CAMERA);
  if (cam_pos >= CAM_IN_FRONT) {cam_turn_target = CAM_IN_RBACK;}
  else                         {cam_turn_target = CAM_IN_LBACK;}
  while (true)
  {
    cam_diff_val = CAMERA_ROTATION - cam_turn_target;
    if (cam_diff_val == 0)
    {
      Off(CAMERA);
      break;
    }
    else
    if (cam_diff_val < 0) {CAM_RIGHT;}
    else
    if (cam_diff_val > 0) {CAM_LEFT;}
  }
  cam_pos = CAMERA_ROTATION;
}

void Turn_camera_RIGHT ()
/*
Turn camera right turntable 1/4 circles, but cannot over right back
*/
{
  int cam_diff_val = 0, cam_turn_target = 0;
  
  Off(CAMERA);
  if (cam_pos >= CAM_IN_RIGHT) {cam_turn_target = CAM_IN_RBACK;}
  else                         {cam_turn_target = 4 * ((cam_pos / 4) + 1);}
  while (true)
  {
    cam_diff_val = cam_turn_target - CAMERA_ROTATION;
    if (cam_diff_val == 0)
    {
      Off(CAMERA);
      break;
    }
    else
    if (cam_diff_val > 0) {CAM_RIGHT;}
    else
    if (cam_diff_val < 0) {CAM_LEFT;}
  }
  cam_pos = CAMERA_ROTATION;
}

void Turn_camera_LEFT ()
/*
Turn camera turntable left 1/4 circles, but cannot over left back
*/
{
  int cam_diff_val = 0, cam_turn_target = 0;
  
  Off(CAMERA);
  if (cam_pos <= CAM_IN_LEFT) {cam_turn_target = CAM_IN_LBACK;}
  else                        {cam_turn_target = 4 * (((cam_pos + 3) / 4) - 1);}
  while (true)
  {
    cam_diff_val = CAMERA_ROTATION - cam_turn_target;
    if (cam_diff_val == 0)
    {
    Off(CAMERA);
    break;
    }
    else
    if (cam_diff_val > 0) {CAM_LEFT;}
    else
    if (cam_diff_val < 0) {CAM_RIGHT;}
  }
  cam_pos = CAMERA_ROTATION;
}

void Button_1_FWD ()
/*
Move forward or stop move when forwarding now
*/
{
  Off(DRIVE);
  if (move_mode == MOVE_FWD)
  {
    move_mode = MOVE_STOP;
    PlaySound(SOUND_DOWN);
    Wait(50);
  }
  else
  {
    Turn_camera_FRONT();
    DRIVE_FWD;
    move_mode = MOVE_FWD;
  }
}

void Button_2_REV ()
/*
Move backward or stop move when backwarding now
*/
{
  Off(DRIVE);
  if (move_mode == MOVE_REV)
  {
    move_mode = MOVE_STOP;
    PlaySound(SOUND_DOWN);
    Wait(50);
  }
  else
  {
    Turn_camera_BACK();
    DRIVE_BACK;
    move_mode = MOVE_REV;
  }
}

void Button_3_LEFT ()
/*
1. Turn left or stop when reach left limit
2. If Steer in left side then turn back center
3. When Turn_steer_Center() is running, disable this function(steer_status)
*/
{
  if (steer_status != NOT_ALLOW_S_TURN)
  {
    if (steer_pos == STEER_IN_LEFT) {Turn_steer_Center();}
    else
    {
      STEER_LEFT;
      turn_mode = TURN_LEFT;
    }
  }
}

void Button_4_RIGHT ()
/*
1. Turn right or stop when reach right limit
2. If Steer in left side then turn back center
3. When Turn_steer_Center() is running, disable this function(steer_status)
*/
{
  if (steer_status != NOT_ALLOW_S_TURN)
  {
    if (steer_pos == STEER_IN_RIGHT) {Turn_steer_Center();}
    else
    {
      STEER_RIGHT;
      turn_mode = TURN_RIGHT;
    }
  }
}

task check_steer_POS ()
{
  while (true)
  {
    steer_diff_val = STEER_ROTATION - STEER_IN_CENTER;
    if ((turn_mode == TURN_RIGHT && steer_diff_val >= STEER_IN_RIGHT) ||
        (turn_mode == TURN_LEFT  && steer_diff_val <= STEER_IN_LEFT))
    {
      Off(STEER);
      turn_mode = TURN_STOP;
      PlaySound(SOUND_DOWN);
      Wait(50);
    }
    if (steer_diff_val == STEER_IN_CENTER) {steer_pos = STEER_IN_CENTER;}
    else
    if (steer_diff_val > STEER_IN_CENTER)  {steer_pos = STEER_IN_RIGHT;}
    else                                   {steer_pos = STEER_IN_LEFT;}
  }
}

task main ()
{
  Init_Rtn();
  start check_steer_POS;
  SetSensorType(REMOTE_CMD,SENSOR_TYPE_LIGHT);
  SetSensorMode(REMOTE_CMD,SENSOR_MODE_RAW);
  while(true)
  {
    cmd_code = REMOTE_CMD;
    if Allow_Range
    {
      if Button_1 {Button_1_FWD();}
      else
      if Button_2 {Button_2_REV();}
      else
      if Button_3
      {
        if (rcx_mode == NAVIGATION_MODE) {Button_3_LEFT();}
      }
      else
      if Button_4
      {
        if (rcx_mode == NAVIGATION_MODE) {Button_4_RIGHT();}
      }
      else
      if Button_5
      {
        if (rcx_mode == NAVIGATION_MODE) {Turn_camera_LEFT();}
      }
      else
      if Button_6
      {
        if (rcx_mode == NAVIGATION_MODE) {Turn_camera_RIGHT();}
      }
      else
      if Button_7
      {
        Off(DRIVE+STEER+CAMERA);
        rcx_mode = NAVIGATION_MODE;
        move_mode = MOVE_STOP;
        turn_mode = TURN_STOP;
        Turn_camera_FRONT();
        stop check_steer_POS;
        Turn_steer_Center();
        start check_steer_POS;
      }
      else //Button_8
      {
        Off(DRIVE+STEER+CAMERA);
        move_mode = MOVE_STOP;
        turn_mode = TURN_STOP;
        if (rcx_mode == NAVIGATION_MODE) {rcx_mode = MISSION_MODE;}
        else                             {rcx_mode = NAVIGATION_MODE;}
        Turn_camera_FRONT();
        stop check_steer_POS;
        Turn_steer_Center();
        start check_steer_POS;
      }
    }
  }
}