/*
Program : ch_lego_RemoteControl_02.nqc
Date    : 2006-06-07
Model   : Output - A Drive Motor
                   B SpyCam Up/Down Motor
                   C Turn Gear Motor
          Input  - 1 Angle Sensor for steer control
                 - 2 Hitechnic Remote Control Sensor Base
                 - 3
Control : Remote Control Transmitting Code
          10 Moving Forward / Stop when Forwarding(A)
          20 Driving Backward  / Stop when Backwarding(A)
          30 Turn Left / Stop when Leftting(C)
          40 Turn Right / Stop when Rightting(C)
          50 Up Camera / Stop when Upping(B)
          60 Down Camera / Stop when Downning(B)
          70 Turn Steer to Center position
          80 Stop all(A)(B)(C)

          BUTTON LAYOUT :
          < Go/Stop Forward      > Go/Stop Backward
          < Turn/Stop Left       > Turn/Stop Right
          < Up/Stop Camera       > Down/Stop Camera
          < Turn Steer to Center > STOP ALL Motors
*/

#define DRIVE            OUT_A
#define CAMERA           OUT_B
#define STEER            OUT_C
#define STEER_ROTATION   SENSOR_1
#define REMOTE_CMD       SENSOR_2
#define MOVE_STOP        10
#define MOVE_FWD         11
#define MOVE_REV         12
#define TURN_STOP        20
#define TURN_LEFT        21
#define TURN_RIGHT       22
#define CAM_STOP         30
#define CAM_UP           31
#define CAM_DOWN         32
#define TURN_LIMIT        3
#define REACH_LEFT_LIMIT  1
#define REACH_RIGHT_LIMIT 2
#define NOT_ALLOW_TURN    1

int move_mode, turn_mode, cam_mode;
int steer_status=0, steer_mid_val=0, diff_val=0, reach_limit=0;
int button_action=0, cmd_code=0;

void Reset_All ()
/*
1. Stop all Motors and reset all status codes
2. Must turn the steer wheel to center position manually
*/
{
  Off(DRIVE+CAMERA+STEER);
  move_mode = MOVE_STOP;
  turn_mode = TURN_STOP;
  cam_mode  = CAM_STOP;
  ClearSensor(STEER_ROTATION);
  steer_mid_val = STEER_ROTATION;
  reach_limit = 0;
  steer_status = 0;
  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(50);
}

void Reset_steer_Center ()
/*
Turn steer to center position
*/
{
  steer_status = NOT_ALLOW_TURN;
  Off(STEER);
  SetPower(STEER,5);
  ClearTimer(0);
  do
  {
    diff_val = STEER_ROTATION - steer_mid_val;
    if (diff_val > 0) {OnFwd(STEER);}
    else
    if (diff_val < 0) {OnRev(STEER);}
    else              {break;}
  }
  while (Timer(0) < 50);
  Off(STEER);
  SetPower(STEER,7);
  turn_mode = TURN_STOP;
  ClearSensor(STEER_ROTATION);
  steer_mid_val = STEER_ROTATION;
  reach_limit = 0;
  steer_status = 0;
  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(50);
}

void Stop_All ()
/*
Stop all Motors
*/
{
  Off(DRIVE+CAMERA+STEER);
  move_mode = MOVE_STOP;
  turn_mode = TURN_STOP;
  cam_mode  = CAM_STOP;
  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(50);
}

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

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

void Button_3_LEFT ()
/*
1. Turn left or stop turn when turnning left now or reach left limit
2. When tuning steer to center, disable this function(steer_status)
*/
{
  if (steer_status != NOT_ALLOW_TURN)
  {
    if (turn_mode == TURN_LEFT || reach_limit == REACH_LEFT_LIMIT)
    {
      Off(STEER);
      turn_mode = TURN_STOP;
      PlaySound(SOUND_DOWN);
    }
    else
    {
      OnFwd(STEER);
      turn_mode = TURN_LEFT;
      PlaySound(SOUND_UP);
    }
    Wait(50);
  }
}

void Button_4_RIGHT ()
/*
1. Turn right or stop turn when turnning right now or reach right limit
2. When tuning steer to center, disable this function(steer_status)
*/
{
  if (steer_status != NOT_ALLOW_TURN)
  {
    if (turn_mode == TURN_RIGHT || reach_limit == REACH_RIGHT_LIMIT)
    {
      Off(STEER);
      turn_mode = TURN_STOP;
      PlaySound(SOUND_DOWN);
    }
    else
    {
      OnRev(STEER);
      turn_mode = TURN_RIGHT;
      PlaySound(SOUND_UP);
    }
    Wait(50);
  }
}

void Button_5_UP ()
/*
Up camera or stop when up now
*/
{
  if (cam_mode == CAM_UP)
  {
    Off(CAMERA);
    cam_mode = CAM_STOP;
    PlaySound(SOUND_DOWN);
  }
  else
  {
    OnRev(CAMERA);
    cam_mode = CAM_UP;
    PlaySound(SOUND_UP);
  }
  Wait(50);
}

void Button_6_DOWN ()
/*
Down camera or stop when down now
*/
{
  if (cam_mode == CAM_DOWN)
  {
    Off(CAMERA);
    cam_mode = CAM_STOP;
    PlaySound(SOUND_DOWN);
  }
  else
  {
    OnFwd(CAMERA);
    cam_mode = CAM_DOWN;
    PlaySound(SOUND_UP);
  }
  Wait(50);
}

task check_steer_POS ()
{
  while (true)
  {
    if (turn_mode != TURN_STOP)
    {
      diff_val = STEER_ROTATION - steer_mid_val;
      if (turn_mode == TURN_LEFT && diff_val <=  -TURN_LIMIT)
      {
        Off(STEER);
        turn_mode = TURN_STOP;
        reach_limit = REACH_LEFT_LIMIT;
        PlaySound(SOUND_DOWN);
      }
      else
      if (turn_mode == TURN_RIGHT && diff_val >= TURN_LIMIT)
      {
        Off(STEER);
        turn_mode = TURN_STOP;
        reach_limit = REACH_RIGHT_LIMIT;
        PlaySound(SOUND_DOWN);
      }
      else {reach_limit = 0;}
    }
    Wait(50);
  }
}

task main ()
{
  SetSensor(STEER_ROTATION,SENSOR_ROTATION);
  Reset_All();
  start check_steer_POS;
  SetSensor(REMOTE_CMD,SENSOR_LIGHT);
  while(true)
  {
   cmd_code = REMOTE_CMD;
   if (cmd_code > 5)
   {
     button_action = ((cmd_code+5)/10);
     switch(button_action)
     {
       case 1:
         Button_1_FWD();
         break;
       case 2:
         Button_2_REV();
         break;
       case 3:
         Button_3_LEFT();
         break;
       case 4:
         Button_4_RIGHT();
         break;
       case 5:
         Button_5_UP();
         break;
       case 6:
         Button_6_DOWN();
         break;
       case 7:
         stop check_steer_POS;
         Reset_steer_Center();
         start check_steer_POS;
         break;
       case 8:
         Stop_All();
         break;
       default:
         break;
     }
   }
  }
}