/* robot.nqc
 *
 * This code runs the robot part of a remote-controlled robot.
 * The robot is a simple diff-drive with the motors plugged into
 * ports A and C. It uses two rotation sensors to make sure it's
 * going in a straight line if the remote tells it to do so. The
 * rotation sensors are in ports 1 and 3. It also has an
 * ultrasonic rangefinder mounted in front, so that it can intelligently
 * stop before running into objects.
 *
 * The remote controller code is in "remote.nqc".
 *
 * All code copyright 2003 Rachel Gockley (rgockley@andrew.cmu.edu).
 * Feel free to use it as you wish, but please let me know if you do!
 */

#include "remotecomm.nqh"

#define ENC_TIMER 1
#define PACKET_TIMER 2

#define LEFT_ENC  SENSOR_3
#define RIGHT_ENC SENSOR_1
#define SONAR     SENSOR_2

#define LEFT_OUT  OUT_C
#define RIGHT_OUT OUT_A

#define LEFT_OUT_NUM  2
#define RIGHT_OUT_NUM 0

/* the forward and reverse directions as used in SetOutput */
#define LEFT_FWD  OUT_FWD
#define LEFT_REV  OUT_REV
#define RIGHT_FWD OUT_FWD
#define RIGHT_REV OUT_REV

/* 1 if positive encoder counts imply forward, -1 if backward */
#define LEFT_DIR  1
#define RIGHT_DIR 1

/* CLICK represents how far a wheel moves each tick of the encoder.
 * To calculate it, consider:
 *  - for every full revolution of a wheel, the wheel travels by its
 *    circumference, and the attached gear advances 40 ticks.
 *  - the 40-tooth gear is connected to an 8-tooth, so for every full
 *    revolution of the larger, the smaller turns 5 full revolutions.
 *  - the 8-tooth gear shares an axle with a 24-tooth gear, which
 *    spins at the same rate
 *  - the 24-tooth gear is connected to an 8-tooth gear, which spins
 *    3 times each time the larger gear spins once
 *  - the 8-tooth gear is attached directly to the encoder, which
 *    registers 16 ticks per revolution.
 * Thus, in one full turn of the wheel, the encoder registers 
 *   5*3*16 = 240
 * ticks. The circumference of the wheel is pi*diameter, so
 *   3.14159 * 8 cm = 25.13 cm
 * And so in one tick of the encoder, the robot travels:
 *   25.13 cm / 240 ticks = 0.105 cm per tick
 * Since we can't do floating point on the RCX, we'll multiply that by
 * 1000, so CLICK is really in thousandths of cm per encoder tick.
 */
#define CLICK 105

#define BOT_RADIUS 63      /* tenths of cm */

#define MAX_VEL 700        /* tenths of cm per second (i.e. 70 cm/s) */


/********************/
/* global variables */
/********************/

int left_mult;
int right_mult;

int left_out, right_out;
int left_dir, right_dir;

void getDiff(int &diff) {
  diff = left_mult * LEFT_ENC * LEFT_DIR * left_dir 
    - right_mult * RIGHT_ENC * RIGHT_DIR * right_dir;
}

int adjustby;
task drive() {
  int l_power, r_power;
  int target;
  int diff, olddiff;

  target = 4;

  l_power = 0;
  r_power = 0;

  ClearSensor(LEFT_ENC);
  ClearSensor(RIGHT_ENC);

  // just to get things started...
  l_power = 1;
  r_power = 1;

  SetPower (LEFT_OUT, l_power);
  SetPower (RIGHT_OUT, r_power);

  olddiff = 0;
  adjustby = 1;

  while (1) {
    /* if the encoders are too different, adjust the power by a greater amount */
    getDiff(diff);

    /* if the difference has changed sign, then we're adjusting the power
     * by too much.
     */
    if (diff * olddiff < 0)
      adjustby -= 1;
    /* if the difference has increased, adjust faster! */
    else if (abs(diff) > abs(olddiff))
      adjustby += 2;
    /* if the difference is very small, slow down the adjustment rate */
    else if (abs(diff) <= 1)
      adjustby -= 1;
    /* if the difference hasn't changed, increase the adjustment rate */
    else if ((diff == olddiff) && (diff != 0))
      adjustby += 1;

    if (adjustby < 1)
      adjustby = 1;
    else if (adjustby > 5)
      adjustby = 5;

    /* try to match the left and right encoder readings. */
    if (diff > 0) {
      /* first, see if either power level should be adjusted
       * based on the desired speed. if not, then either increase
       * the right power or decrease the left power.
       */
      if (l_power > target)
	l_power -= adjustby;
      else if (r_power < target)
	r_power += adjustby;
      else if (l_power > 1)
	l_power -= adjustby;
      else
	r_power += adjustby;
    } else if (diff < 0) {
      /* do the same thing as above, but swap right and left. */
      if (r_power > target)
	r_power -= adjustby;
      else if (l_power < target)
	l_power += adjustby;
      else if (r_power > 1)
	r_power -= adjustby;
      else
	l_power += adjustby;
    }

    /* make sure we don't set power levels above 7 or below 0 */
    if (l_power > 7)
      l_power = 7;
    else if (l_power < 0)
      l_power = 0;
    if (r_power > 7)
      r_power = 7;
    else if (r_power < 0)
      r_power = 0;

    /* don't physically stop the wheel if the target velocity is
     * non-zero. this means that the robot simply can't go at some
     * very slow speeds -- if it tried, it would be a jerky, awful
     * mess.
     */
    if (l_power == 0)
      l_power = 1;
    if (r_power == 0)
      r_power = 1;

    /* check sonar readings! */
    if ((SONAR <= 5) && (left_dir == 1) && (right_dir == 1)) {
      /* too close to an obstacle, and trying to move forward. stop
       * the motors!
       * Note that the power will be turned back on by the parse_packet
       * function.
       */
      SetOutput (LEFT_OUT, OUT_OFF);
      SetOutput (RIGHT_OUT, OUT_OFF);
      l_power = 0;
      r_power = 0;
    }

    /* set the motor power */
    SetPower (LEFT_OUT, l_power);
    SetPower (RIGHT_OUT, r_power);

    olddiff = diff;

    //    Wait(1);
  } // end while

  SetOutput (LEFT_OUT, OUT_OFF);
  SetOutput (RIGHT_OUT, OUT_OFF);
}

void parse_packet() {
  int on = packet_buffer[0];
  int dir = packet_buffer[1];
  int turn = packet_buffer[2];

  // on/off
  switch (packet_buffer[0]) {
  case GO:
    left_out = OUT_ON;
    right_out = OUT_ON;
    break;
  case STOP:
    left_out = OUT_OFF;
    right_out = OUT_OFF;
    break;
  }

  switch (packet_buffer[2]) {
    // comments assume dir == FWD
  case 11: // left turn-in-place
    left_mult = -1;
    right_mult = 1;
    left_dir = (dir == FWD)? -1 : 1;
    right_dir = (dir == FWD)? 1 : -1;
    break;
  case 10: // hard left turn
    left_mult = -2;
    right_mult = 1;
    left_dir = (dir == FWD)? -1 : 1;
    right_dir = (dir == FWD)? 1 : -1;
    break;
  case 9: // medium left turn
    left_mult = 1;
    right_mult = 0;
    left_out = OUT_OFF;
    left_dir = 1;
    right_dir = (dir == FWD)? 1 : -1;
    break;
  case 8: // soft left turn
    left_mult = 2;
    right_mult = 1;
    left_dir = (dir == FWD)? 1 : -1;
    right_dir = (dir == FWD)? 1 : -1;
    break;
  case 7: // straight
  case 6:
  case 5:
    left_mult = 1;
    right_mult = 1;
    left_dir = (dir == FWD)? 1 : -1;
    right_dir = (dir == FWD)? 1 : -1;
    break;
  case 4: // soft right turn
    left_mult = 1;
    right_mult = 2;
    left_dir = (dir == FWD)? 1 : -1;
    right_dir = (dir == FWD)? 1 : -1;
    break;
  case 3: // medium right turn
    left_mult = 0;
    right_mult = 1;
    left_dir = (dir == FWD)? 1 : -1;
    right_dir = 1; 
    right_out = OUT_OFF;
    break;
  case 2: // hard right turn
    left_mult = 1;
    right_mult = -2;
    left_dir = (dir == FWD)? 1 : -1;
    right_dir = (dir == FWD)? -1 : 1;
    break;
  case 1: // right turn-in-place
    left_mult = 1;
    right_mult = -1;
    left_dir = (dir == FWD)? 1 : -1;
    right_dir = (dir == FWD)? -1 : 1;
    break;
  }

  ClearSensor (LEFT_ENC);
  ClearSensor (RIGHT_ENC);

  if (left_dir == 1)
    SetDirection (LEFT_OUT, LEFT_FWD);
  else
    SetDirection (LEFT_OUT, LEFT_REV);

  if (right_dir == 1)
    SetDirection (RIGHT_OUT, RIGHT_FWD);
  else
    SetDirection (RIGHT_OUT, RIGHT_REV);

  if (left_out == OUT_ON)
    SetOutput (LEFT_OUT, OUT_ON);
  else
    SetOutput (LEFT_OUT, OUT_OFF);

  if (right_out == OUT_ON)
    SetOutput (RIGHT_OUT, OUT_ON);
  else
    SetOutput (RIGHT_OUT, OUT_OFF);
}

void wait_for_packet() {
  int in_timeout = 0;
  ClearTimer (PACKET_TIMER);

  do {
    ReadPacket();
    if (error != 0 && error != NO_RSP) {
      // got an error - send NCK so the remote knows!
      Wait(5);
      //      SendNCK();
      PlaySound(SOUND_CLICK);
    }

    // if we haven't gotten a message for over two seconds,
    // then timeout - stop the motors!
    if (FastTimer(PACKET_TIMER) > 200 && !in_timeout) {
      SetOutput (LEFT_OUT, OUT_OFF);
      SetOutput (RIGHT_OUT, OUT_OFF);
      PlaySound (SOUND_DOWN);
      in_timeout = 1;
    }

  } while (!(error == 0 && packetlen == MAX_LENGTH));

  Wait(5);
  //  SendACK();
}

task main() {
  SetSensor(LEFT_ENC, SENSOR_ROTATION);
  SetSensor(RIGHT_ENC, SENSOR_ROTATION);
  SetSensor(SONAR, SENSOR_LIGHT);
  SetTxPower(TX_POWER_HI);

  SetUserDisplay(packet_buffer[2], 0);

  ClearSensor(LEFT_ENC);
  ClearSensor(RIGHT_ENC);

  start drive;

  while (1) {
    wait_for_packet();
    parse_packet();
  }
}