/**********************************************************************
 * #define is used to declare program constants.  This is useful for
 * storing which sensor is in which port, and empericly determined
 * values like the number of ticks that make a right turn.
 **********************************************************************/

/**********************************************************************
 * Allocate motor ports.  Note these are the 2nd and 3rd motor ports
 * because I had trouble getting my connector into port 0.  On my
 * robot both of these motors are connected to the same drive train,
 * so they need to be powered the same at a given time.  We do not
 * highly advocate using two motors on one drive train.  Note that
 * multiple motors is no replacement to a well tuned gear train, and
 * it really doesn't gain you too much.
 **********************************************************************/

#define MOTOR_1       1
#define MOTOR_2       2

/**********************************************************************
 * Allocate servo ports.  I just have one servo which steers both of
 * the bakc wheels on my robot.
 **********************************************************************/

#define SERVO_PORT    5

/**********************************************************************
 * Allocate sensor ports.  I just have two sensors.  A distance sensor
 * which I use for wall following, and a shaft encoder which indicates
 * how far the robot has traveled.  This is implemented with a break
 * beam package istalled on the first axel off the motors.  The pulley
 * rotates at 1/3 the speed of the motor, so I get 2 ticks per motor
 * revolution.  This is a lot of accuracy, and the hardware shaft
 * encoders (on ports 7 and 8) can go that fast.
 **********************************************************************/

#define DIST_PORT     4
#define ENCODER_PORT  7

/**********************************************************************
 * These values were found emperically, and are the extreams and center
 * position for the servo.  Note that it is important to find the correct
 * values for the maximum left and right positions for your servo.  For
 * most servos 0 and 4000 are actually further than the servo really can
 * go.  It is bad for the servo to run it past its physical limit.
 **********************************************************************/

#define SERVO_LEFT     50
#define SERVO_RIGHT  3990
#define SERVO_CENTER 1943

/**********************************************************************
 * Here are my global variables.
 **********************************************************************/

/* This is used to determine how long it will take to move the * servo
to a new position. */

int lastServoPos = -1;

/* This is used to make accurate turns.  Note that using time to
calibrate a turn is fundametally flawed.  The reason it is in this
code is to show that it is bad. */

int ticksPer90 = 280;
int milisPer90 = 900;

/* This is the desired reading from the distance sensor for wall
following.  Note that for best results, you distance sensor should be
mounted at least 3" inside your robot. */

int ideal = 80;

/**********************************************************************
 * Turns the servo to the indicated position.  Delays while the servo
 * is turning
 **********************************************************************/

void move_servo(int pos) {
  servo(SERVO_PORT, pos);
  sleep((float)(abs(lastServoPos - pos)) / 7500.0);
  lastServoPos = pos;
}

/**********************************************************************
 * Enables the servos and encoders etc.
 **********************************************************************/

void setup() {
  ao();
  enable_servos();
  enable_encoder(ENCODER_PORT);
  move_servo(SERVO_CENTER);
}

/**********************************************************************
 * Drives both motors at the indicated speed.  Note that the handyboard
 * only has 7 speeds, so 0, 16, 33, 50, 66, 83, and 100 are really
 * the only speeds that mean much.
 **********************************************************************/

void drive(int speed) {
  motor(MOTOR_1, speed);
  motor(MOTOR_2, speed);
}

/**********************************************************************
 * Drive forward at full speed for the number of ticks indicated by
 * dist.
 **********************************************************************/

void forward(int dist) {
  move_servo(SERVO_CENTER);
  reset_encoder(ENCODER_PORT);
  drive(100);
  while (read_encoder(ENCODER_PORT) < dist) {
    sleep(0.02);
  }
  drive(0);
}

/**********************************************************************
 * Turns the servo for veering right.
 **********************************************************************/

void veer_right() {
  move_servo(((SERVO_CENTER * 9)+(SERVO_RIGHT * 1)) / 10);
}

/**********************************************************************
 * Turns the servo for veering left.
 **********************************************************************/

void veer_left() {
  move_servo(((SERVO_CENTER * 9)+(SERVO_LEFT * 1)) / 10);
}

/**********************************************************************
 * Turns the servo for turning medium right.
 **********************************************************************/

void hard_right() {
  move_servo(((SERVO_CENTER * 5)+(SERVO_RIGHT * 2)) / 7);
}

/**********************************************************************
 * Turns the servo for turning medium left.
 **********************************************************************/

void hard_left() {
  move_servo(((SERVO_CENTER * 5)+(SERVO_LEFT * 2)) / 7);
}

/**********************************************************************
 * Turn left the specified number of degrees.  Assumes the robot is
 * stopped before this is called.  This routine uses the shaft encoder
 * to figure out how far to turn.
 **********************************************************************/

void left(int angle) {
  int dest = (angle * ticksPer90)/90;
  move_servo(SERVO_LEFT);
  reset_encoder(ENCODER_PORT);
  drive(100);
  
  while (read_encoder(ENCODER_PORT) < dest) {
    sleep(0.02);
  }
  drive(0);
}

/**********************************************************************
 * Turn right the specified number of degrees.  Assumes the robot is
 * stopped before this is called.  This routine uses the shaft encoder
 * to figure out how far to turn.
 **********************************************************************/

void right(int angle) {
  int dest = ((angle/5) * ticksPer90)/18;
  move_servo(SERVO_RIGHT);
  reset_encoder(ENCODER_PORT);
  drive(100);
  
  while (read_encoder(ENCODER_PORT) < dest) {
    sleep(0.02);
  }
  drive(0);
}

/**********************************************************************
 * Turn right the specified number of degrees.  Assumes the robot is
 * stopped before this is called.  This routine times the turn.
 **********************************************************************/

void timed_right(int angle) {
  float secs = (((float)(angle))/90.0 * ((float)(milisPer90))) / 1000.0;
  move_servo(SERVO_RIGHT);
  drive(100);
  sleep(secs);
  drive(0);
}

/**********************************************************************
 * Turn left the specified number of degrees.  Assumes the robot is
 * stopped before this is called.  This routine times the turn.
 **********************************************************************/

void timed_left(int angle) {
  float secs = (((float)(angle))/90.0 * ((float)(milisPer90))) / 1000.0;
  move_servo(SERVO_LEFT);
  drive(100);
  sleep(secs);
  drive(0);
}

/**********************************************************************
 * Prompt the user to adjust the number of ticks per 90 degrees.
 **********************************************************************/

void set_ticks() {
  printf("Set Ticks Per 90\n");
  while (start_button()) {}

  while(!start_button()) {
    printf("Ticks Per 90 =  %d\n", ticksPer90-128+knob());
    sleep(0.1);
  }

  ticksPer90 = ticksPer90 - 128 + knob();
}

/**********************************************************************
 * Prompt the user to adjust the number of miliseconds per 90 degrees.
 **********************************************************************/

void set_milis() {
  printf("Set Milis Per 90\n");
  while (start_button()) {}

  while(!start_button()) {
    printf("Milis Per 90 =  %d\n", milisPer90-128+knob());
    sleep(0.1);
  }

  milisPer90 = milisPer90 - 128 + knob();
}

/**********************************************************************
 * Follows the wall for the specified distance.  Distance from wall
 * is specified by the global variable "ideal".
 **********************************************************************/

void wall_follow(int distance) {
  move_servo(SERVO_CENTER);
  reset_encoder(ENCODER_PORT);
  drive(75);
  while (read_encoder(ENCODER_PORT) < distance) {
    if (analog(DIST_PORT) < (ideal - 40)) {
      hard_left();
    } else if (analog(DIST_PORT) > (ideal + 40)) {
      hard_right();
    } else if (analog(DIST_PORT) < (ideal - 20)) {
      veer_left();
    } else if (analog(DIST_PORT) > (ideal + 20)) {
      veer_right();
    }
    sleep(0.1);
  }
  drive(0);
}
  

/**********************************************************************
 * This is our main behavior which implements a simple menu system
 * based on the wheel position.
 **********************************************************************/

#define NUM_FUNCS 7

void go() {
  int func = -1;
  int knobSample;
  while (!stop_button()) {
    knobSample = knob();
    func = knobSample / (255 / NUM_FUNCS);
    if (func >= NUM_FUNCS) func = NUM_FUNCS - 1;
    
    if (func == 0) printf("Set dist to Wall = %d\n", analog(4));
    if (func == 1) printf("Set Ticks Per 90\n");
    if (func == 2) printf("Set Milis Per 90\n");
    if (func == 3) printf("Timed Left 90\n");
    if (func == 4) printf("Encoder Left 90\n");
    if (func == 5) printf("Forward 500 Ticks\n");
    if (func == 6) printf("Follow Wall\n");
    
    if (start_button()) {
      if (func == 0) ideal = analog(4);
      if (func == 1) set_ticks();
      if (func == 2) set_milis();
      if (func == 3) timed_left(90);
      if (func == 4) left(90);
      if (func == 5) forward(500);
      if (func == 6) wall_follow(1000);
      while(start_button()) {}
    }
    sleep(0.1);
  }
}

/**********************************************************************
 * This is the main script.  It's pretty self explanitory.
 **********************************************************************/

void main() {
  
  setup();
  
  go();

}