/*
Program : ch_lego_RemoteControl_mission.nqc
Date    : 2006-09-09
Model   : RCX 2.0 - Mission control
Ports   : Output - A 
                            B Grabber Motor
                            C Camera UpDown Motor
              Input  - 1 Hitechnic Remote Control Sensor Base
                         - 2 Light for Mission mode indicater
                         - 3
  Control : Remote Control Transmitting Code Raw value Range(2 RCXs )
*/

#define DRIVE              OUT_A
#define GRABBER            OUT_B
#define CAMERA             OUT_C
#define REMOTE_CMD         SENSOR_1
#define MISSION_LIGHT      SENSOR_2
/* Motor direction Macro */
#define GRAB_HOLD          OnRev(OUT_B)
#define GRAB_REL           OnFwd(OUT_B)
#define CAM_UP             OnFwd(OUT_C)
#define CAM_DOWN           OnRev(OUT_C)
#define TURN_LIGHT_ON      OnFwd(OUT_A)
/* <2 RCX> Remote command base sensor Raw value definition */
#define Allow_Range        (cmd_code >= 643 && cmd_code <= 936)
#define Button_3           (cmd_code >= 894)
#define Button_4           (cmd_code >= 854)
#define Button_5           (cmd_code >= 805)
#define Button_6           (cmd_code >= 761)
#define Button_7           (cmd_code >= 704)
#define Button_8           (cmd_code >= 643)
/* RCX running mode switch */
#define NAVIGATION_MODE    10
#define MISSION_MODE       20
/* Grabber Motor status control */
#define GRABBER_STOP       40
#define GRABBER_HOLD       41
#define GRABBER_REL        42
/* Camera Up/Down turn status control*/
#define CAMERA_STOP        50
#define CAMERA_UP          51
#define CAMERA_DOWN        52

int rcx_mode, grabber_mode, camera_mode;
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;
  Off(DRIVE+GRABBER+CAMERA);
  grabber_mode = GRABBER_STOP;
  camera_mode = CAMERA_STOP;
  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(50);
}

void Button_3_GRAB_HOLD ()
/* Let Grabber hold or stop when holdding now  */
{
  Off(GRABBER);
  if (grabber_mode == GRABBER_HOLD)
  {
    grabber_mode = GRABBER_STOP;
    PlaySound(SOUND_DOWN);
    Wait(50);
  }
  else
  {
    GRAB_HOLD;
    grabber_mode = GRABBER_HOLD;
  }
}

void Button_4_GRAB_REL ()
/* Release Grabber or stop when releasing now */
{
  Off(GRABBER);
  if (grabber_mode == GRABBER_REL)
  {
    grabber_mode = GRABBER_STOP;
    PlaySound(SOUND_DOWN);
    Wait(50);
  }
  else
  {
    GRAB_REL;
    grabber_mode = GRABBER_REL;
  }
}

void Button_5_CAM_DOWN ()
/* Turn camera down or stop when working now */
{
  Off(CAMERA);
  if (camera_mode == CAMERA_DOWN)
  {
    camera_mode = CAMERA_STOP;
    PlaySound(SOUND_DOWN);
    Wait(50);
  }
  else
  {
    CAM_DOWN;
    camera_mode = CAMERA_DOWN;
  }
}

void Button_6_CAM_UP ()
/* Turn camera up or stop when working now  */
{
  Off(CAMERA);
  if (camera_mode == CAMERA_UP)
  {
    camera_mode = CAMERA_STOP;
    PlaySound(SOUND_DOWN);
    Wait(50);
  }
  else
  {
    CAM_UP;
    camera_mode = CAMERA_UP;
  }
}

task main ()
{
  Init_Rtn();
  SetSensorType(REMOTE_CMD,SENSOR_TYPE_LIGHT);
  SetSensorMode(REMOTE_CMD,SENSOR_MODE_RAW);

  while(true)
  {
    cmd_code = REMOTE_CMD;
    if Allow_Range
    {
      if Button_3
      {
        if (rcx_mode == MISSION_MODE) {Button_3_GRAB_HOLD();}
      }
      else
      if Button_4
      {
        if (rcx_mode == MISSION_MODE) {Button_4_GRAB_REL();}
      }
      else
      if Button_5
      {
        if (rcx_mode == MISSION_MODE) {Button_5_CAM_DOWN();}
      }
      else
      if Button_6
      {
        if (rcx_mode == MISSION_MODE) {Button_6_CAM_UP();}
      }
      else
      if Button_7
      {
        Off(DRIVE+GRABBER+CAMERA);
        grabber_mode = GRABBER_STOP;
        camera_mode = CAMERA_STOP;
        rcx_mode = NAVIGATION_MODE;
      }
      else //Button_8
      {
        Off(DRIVE+GRABBER+CAMERA);
        grabber_mode = GRABBER_STOP;
        camera_mode = CAMERA_STOP;
        if (rcx_mode == MISSION_MODE) {rcx_mode = NAVIGATION_MODE;}
        else
        {
          rcx_mode = MISSION_MODE;
          TURN_LIGHT_ON;
        }
      }
    }
  }
}