/*$Id: brain.c,v 1.83 1997/02/22 12:47:52 nwv Exp $*/

/*****************************************************************************
 *                                                                           *
 *                                  BRAIN.C                                  *
 *                                                                           *
 *                     Mapping Robot CS148 Final Project                     *
 *   Murat Gorguner (mg@cs.brown.edu) & Neil W. Van Dyke (nwv@cs.brown.edu)  *
 *     CS148, Building Intelligent Robots, Brown University, April 1996      *
 *                                                                           *
 *      For use with Interactive C 3.1b3 and the MIT 6.270 2.21 board.       *
 *     Requires that the "encoders.lis" shaft encoder support be loaded.     *
 *                                                                           *
 *****************************************************************************/

/*****************************************************************************
 * NOTE: This current version of the file is RCS version 1.82 with dead code *
 *       removed and this comment added.  This is the C code for the robot-  *
 *       resident control program for Magellan.  If you are interested in    *
 *       the less important Java map viewer, the Perl SPARCstation serial    *
 *       port code, or the Neil's course paper, please contact Neil at       *
 *       "nwv@cs.brown.edu" or "nwv@acm.org".  Also, if you get any use out  *
 *       of this code, I'd appreciate hearing of it.                         *
 *****************************************************************************/

/*-------------------------------------------------- Configuration Constants */

/* Misc. */
#define CmProtrudingPastSideRearSensor      0
#define CmTraveledPerWheelEncoderCount      1.67
#define FrontWallSensorThresholdForLeftTurn 8
#define IdealWallFollowingDistance          5
#define Map_MaxSegmentCount                 500
#define RobotLengthCm                       16
#define Serial_Enabled                      True
#define WallFollowAdjustmentFactor          10
#define WheelEncoderCountsPer90DegreeTurn   6

/* Motor Calibration */
#define DriveMotors_LeftMotorPercent  90
#define DriveMotors_RightMotorPercent 100

/* Analog Ports */
#define WallSensors_FrontDetector     23
#define WallSensors_SideFrontDetector 25
#define WallSensors_SideRearDetector  27

/* Digital Ports */
#define WheelEncoding_LeftEncoder  0
#define WheelEncoding_RightEncoder 1

/* Motor Ports */
#define DriveMotors_RightMotor        0
#define DriveMotors_LeftMotor         1
#define WallSensors_FrontEmitter      2
#define WallSensors_SideEmitters1     3
#define WallSensors_SideFrontEmitter2 4
#define WallSensors_SideRearEmitter2  5

/*------------------------------------------------------------------ Generic */

/* Boolean Type */
#define Boolean int
#define True    1
#define False   0

/* Integer Absolute Value */
int
abs(int n) {
  if (n < 0) return (-(n));
  return n;
}

/* Integer Average */
int
avg(int a, int b) {
  return (a + b) / 2;
}

/* Floating-Point Min (from "menu.c") */
float
fmin(float a, float b) {
  if (a < b) return(a);
  return(b);
}

/* Integer Max */
int
max(int a, int b) {
  if (a > b) return(a);
  return(b);
}

int
sqr(int n) {
  return (n * n);
}

/*---------------------------------------------------------------------- Map */

/* Map Turn Codes */
#define Map_TurnCode_Left  -1
#define Map_TurnCode_None  0
#define Map_TurnCode_Right 1

#define Map_EnsureSegmentComplete() {		\
  if (Map_WaitingForTurnCode) {			\
    Map_AddTurnCode(Map_TurnCode_None);		\
  }						\
}

int     Map_SegmentCount;
int     Map_SegmentLength[Map_MaxSegmentCount];
int     Map_SegmentTurnCode[Map_MaxSegmentCount];
Boolean Map_WaitingForTurnCode;

/*
 * Note: No overflow checking is done on the map data arrays.
 */

void
Map_AddLength(int length) {
  Map_EnsureSegmentComplete();
  Map_SegmentLength[Map_SegmentCount] = length;
  Map_WaitingForTurnCode = True;
}

void
Map_AddLengthFromWheelEncoding() {
  Map_AddLength(WheelEncoding_MaxCounterValueInCm());
}

void
Map_AddTurnCode(int turn) {
  if (Map_WaitingForTurnCode) {
    Map_SegmentTurnCode[Map_SegmentCount] = turn;
    Map_SegmentCount++;
    Map_WaitingForTurnCode = False;
  } else {
    Map_AddSegment(0, turn);
  }
}

void
Map_AddSegment(int length, int turn) {
  Map_EnsureSegmentComplete();
  Map_SegmentLength[Map_SegmentCount] = length;
  Map_SegmentTurnCode[Map_SegmentCount] = turn;
  Map_SegmentCount++;
}

void
Map_Finalize() {
  Map_EnsureSegmentComplete();
}

void
Map_Initialize() {
  Map_SegmentCount = 0;
  Map_WaitingForTurnCode = False;
}

/*------------------------------------------------------------ WheelEncoding */

#define WheelEncoding_Start() {			\
  enable_encoder(WheelEncoding_LeftEncoder);	\
  enable_encoder(WheelEncoding_RightEncoder);	\
}

#define WheelEncoding_Stop() {			\
  disable_encoder(WheelEncoding_LeftEncoder);	\
  disable_encoder(WheelEncoding_RightEncoder);	\
}

#define WheelEncoding_CounterValue(port) read_encoder(port)

#define WheelEncoding_CounterValueInCm(port) \
(int)((float)WheelEncoding_CounterValue(port) * CmTraveledPerWheelEncoderCount)

#define WheelEncoding_AverageCounterValue()			\
((WheelEncoding_CounterValue(WheelEncoding_LeftEncoder) +	\
  WheelEncoding_CounterValue(WheelEncoding_RightEncoder))	\
 / 2)

#define WheelEncoding_MaxCounterValue()				\
  max(WheelEncoding_CounterValue(WheelEncoding_LeftEncoder),	\
      WheelEncoding_CounterValue(WheelEncoding_RightEncoder))

#define WheelEncoding_MaxCounterValueInCm()	\
(int)((float)WheelEncoding_MaxCounterValue() *	\
      CmTraveledPerWheelEncoderCount)

#define WheelEncoding_WaitUntilNCounts(n) {				   \
  while ((WheelEncoding_CounterValue(WheelEncoding_LeftEncoder)  < (n)) && \
	 (WheelEncoding_CounterValue(WheelEncoding_RightEncoder) < (n)))   \
    /*NOP*/; 								   \
}

/*-------------------------------------------------------------- DriveMotors */

#define DriveMotors_IntegerPercent(n,percent) \
((((n) * (percent)) / 100) * 80 / 100)

#define DriveMotors_SetFullForward() {		\
  DriveMotors_SetSpeeds(100, 100);		\
}

int DriveMotors_LeftMotorSpeed;
int DriveMotors_RightMotorSpeed;

void
DriveMotors_Initialize() {
  DriveMotors_LeftMotorSpeed  = 0;
  DriveMotors_RightMotorSpeed = 0;
}

void
DriveMotors_MoveForwardNCm(int cm) {
  DriveMotors_TurnOff();
  WheelEncoding_Start();
  DriveMotors_SetFullForward();
  WheelEncoding_WaitUntilNCounts((int)((float)cm /
				       CmTraveledPerWheelEncoderCount));
  WheelEncoding_Stop();
  DriveMotors_TurnOff();
}

void
DriveMotors_SetSpeeds(int left, int right) {
  if (left != DriveMotors_LeftMotorSpeed) {
    motor(DriveMotors_LeftMotor,
	  DriveMotors_IntegerPercent(left, DriveMotors_LeftMotorPercent));
    DriveMotors_LeftMotorSpeed = left;
  }
  if (right != DriveMotors_RightMotorSpeed) {
    motor(DriveMotors_RightMotor,
	  DriveMotors_IntegerPercent(right, DriveMotors_RightMotorPercent));
    DriveMotors_RightMotorSpeed = right;
  }
  Status_Update();
}

void
DriveMotors_TurnOff() {
  off(DriveMotors_LeftMotor);
  off(DriveMotors_RightMotor);
  DriveMotors_LeftMotorSpeed = 0;
  DriveMotors_RightMotorSpeed = 0;
  Status_Update();
}

void
DriveMotors_Turn90Degrees(int direction) {
  DriveMotors_TurnOff();
  WheelEncoding_Start();
  DriveMotors_SetSpeeds((50 * direction), (50 * direction * -1));
  WheelEncoding_WaitUntilNCounts(WheelEncoderCountsPer90DegreeTurn);
  DriveMotors_SetSpeeds((50 * direction), 0);
  WheelEncoding_WaitUntilNCounts(WheelEncoderCountsPer90DegreeTurn + 1);
  DriveMotors_TurnOff();
  WheelEncoding_Stop();
}

void
DriveMotors_TurnLeft90Degrees() {
  DriveMotors_Turn90Degrees(-1);
}

void
DriveMotors_TurnRight90Degrees() {
  DriveMotors_Turn90Degrees(1);
}

/*------------------------------------------------------------------- Escape */

void
Escape_Handle() {
  int leftSpeed  = DriveMotors_LeftMotorSpeed;
  int rightSpeed = DriveMotors_RightMotorSpeed;
  DriveMotors_TurnOff();

  while (escape_button()) /*NOP*/;

  UI_DisplayTwoLines("Connect to Sun,",
		     "press CHOOSE.");
  UI_WaitForChooseButton();

  Escape_TransmitMap();

  UI_DisplayTwoLines("Press CHOOSE to",
		     "resume run.");
  UI_WaitForChooseButton();

  DriveMotors_SetSpeeds(leftSpeed, rightSpeed);
}

void
Escape_TransmitMap() {
  int i;

  UI_DisplayLine("Sending...");
  Map_EnsureSegmentComplete();
  Serial_Start();
  for (i = 0; i < Map_SegmentCount; i++) {
    int length = Map_SegmentLength[i];
    int turnCode = Map_SegmentTurnCode[i] + 1;
    printf("%d: %d %d\n", i, length, turnCode);
    Serial_SendChr(1);
    Serial_SendChr(length);
    Serial_SendChr(turnCode);
    sleep(0.1);
  }
  Serial_SendChr(0);
  Serial_Stop();
  UI_DisplayLine("Send done.");
}

/*-------------------------------------------------------------------- Sound */

int
Sound_StateChangeBeep() {
  beep();
}

/*-------------------------------------------------------------- WallSensors */

#define WallSensors_NoWallReading -1

#define WallSensors_TurnOffEmitter(emitterPort) off(emitterPort)

#define WallSensors_TurnOnEmitter(emitterPort) fd(emitterPort)

int WallSensors_FrontReading;
int WallSensors_SideFrontReading;
int WallSensors_SideRearReading;

int WallSensors_FrontFirst;
int WallSensors_SideFrontFirst;
int WallSensors_SideRearFirst;

float WallSensors_Calibration;

#define WallSensors_GetCorrectedReading(sensor) {			  \
  int reading = WallSensors_Reading_ ## sensor				  \
    ((int)(((float)analog(WallSensors_ ## sensor ## Detector)) *	  \
	   WallSensors_Calibration));					  \
  if ((WallSensors_ ## sensor ## Reading == WallSensors_NoWallReading) || \
      (reading == WallSensors_NoWallReading)) {				  \
    WallSensors_ ## sensor ## Reading = reading;			  \
  } else if (WallSensors_ ## sensor ## First) {				  \
    WallSensors_ ## sensor ## First = False;				  \
    WallSensors_ ## sensor ## Reading = reading;			  \
  } else if ((reading - WallSensors_ ## sensor ## Reading) > 2) {	  \
    WallSensors_ ## sensor ## Reading = WallSensors_NoWallReading;	  \
  } else {								  \
    WallSensors_ ## sensor ## Reading = reading;			  \
  }									  \
}

void
WallSensors_GetReadings() {
  WallSensors_GetCorrectedReading(Front);
  WallSensors_GetCorrectedReading(SideFront);
  WallSensors_GetCorrectedReading(SideRear);
}

void
WallSensors_Initialize() {
  /* Turn on the emitters. */
  WallSensors_TurnOnEmitter(WallSensors_FrontEmitter);
  WallSensors_TurnOnEmitter(WallSensors_SideEmitters1);
  WallSensors_TurnOnEmitter(WallSensors_SideFrontEmitter2);
  WallSensors_TurnOnEmitter(WallSensors_SideRearEmitter2);

  WallSensors_FrontFirst     = True;
  WallSensors_SideFrontFirst = True;
  WallSensors_SideRearFirst  = True;
}

void
WallSensors_Finalize() {
  WallSensors_TurnOffEmitter(WallSensors_FrontEmitter);
  WallSensors_TurnOffEmitter(WallSensors_SideEmitters1);
  WallSensors_TurnOffEmitter(WallSensors_SideFrontEmitter2);
  WallSensors_TurnOffEmitter(WallSensors_SideRearEmitter2);
}

/*
 * Note: This is gross. Please see paper for rationale.
 */

int
WallSensors_Reading_Front(int a) {
  if (a > 247) return WallSensors_NoWallReading;
  if (a > 245) return 9;
  if (a > 242) return 8;
  if (a > 238) return 7;
  if (a > 231) return 6;
  if (a > 223) return 5;
  if (a > 215) return 4;
  if (a > 200) return 3;
  return 2;
}

int
WallSensors_Reading_SideFront(int a) {
  if (a > 245) return WallSensors_NoWallReading;
  if (a > 240) return 14;
  if (a > 236) return 13;
  if (a > 234) return 12;
  if (a > 228) return 11;
  if (a > 224) return 10;
  if (a > 217) return 9;
  if (a > 211) return 8;
  if (a > 192) return 7;
  if (a > 172) return 6;
  if (a > 150) return 5;
  if (a > 123) return 4;
  if (a > 74)  return 3;
  return 2;
}

int
WallSensors_Reading_SideRear(int a) {
  if (a > 245) return WallSensors_NoWallReading;
  if (a > 240) return 14;
  if (a > 236) return 13; 
  if (a > 234) return 12;
  if (a > 230) return 11;
  if (a > 226) return 10;
  if (a > 222) return 9;
  if (a > 213) return 8;
  if (a > 201) return 7;
  if (a > 185) return 6;
  if (a > 164) return 5;
  if (a > 127) return 4;
  if (a > 57)  return 3;
  return 2;

}

int
WallSensors_ReadingRelationToIdeal(int cm) {
  if (cm == IdealWallFollowingDistance) return 0;
  if (cm < IdealWallFollowingDistance)  return -1;
  return 1;
}

/*------------------------------------------------------------------- Serial */
  
#define Serial_SendChr(chr) {			\
  if (Serial_Enabled) {				\
    poke(0x102f, chr);				\
    while (!(peek(0x102e) & 0x80));		\
  }						\
}

#define Serial_SendNewline() {			\
  if (Serial_Enabled) {				\
    Serial_SendChr(0x0a);			\
  }						\
}

void
Serial_SendStr(char str[]) {
#if (Serial_Enabled == True)
  int len = _array_size(str);
  int i;
  for (i = 0; i < len; i++) {
    Serial_SendChr(str[i]);
  }
#endif
}

void
Serial_SendStrNl(char str[]) {
#if (Serial_Enabled == True)
  Serial_SendStr(str);
  Serial_SendNewline();
#endif
}

void
Serial_Start() {
#if (Serial_Enabled == True)
  poke(0x3c, 0);
#endif
}

void
Serial_Stop() {
#if (Serial_Enabled == True)
  poke(0x3c, 1);
#endif
}

/*----------------------------------------------------------------------- UI */

#define UI_DisplayLine(str) printf("%s\n", str)

#define UI_WaitForChooseButton() {		\
  while (!choose_button()) /*NOP*/;		\
  while (choose_button()) /*NOP*/;		\
}

Boolean
UI_SleepOrChooseButton(float seconds) {
  float delay;
  int   i;
  int   times;

  delay = 0.10;
  times = (int)(seconds / delay);
  for (i = 0; i < times; i++) {
    if (choose_button()) {
      while (choose_button()) /*NOP*/;
      return True;
    }
    sleep(delay);
  }
  return False;
}

void
UI_DisplayTwoLines(char line1[], char line2[]) {
  char    buf[17];
  Boolean end;
  int     i;

  end = False;
  for (i = 0; i < 16; i++) {
    if (end) {
      buf[i] = 32;
    } else {
      if (line1[i] == 0) {
	end = True;
	buf[i] = 32;
      } else {
	buf[i] = line1[i];
      }
    }
  }
  buf[16] = 0;
  printf("%s%s\n", buf, line2);
}

/*------------------------------------------------------------------- Status */

#define Status_Update() \
Status_UpdateWithMsg("")

void
Status_InternalError(char msg[])
{
  Status_UpdateWithMsg(msg);
  UI_WaitForChooseButton();
}

void
Status_UpdateWithMsg(char msg[]) {
  printf("%s L%d R%d F%d SF%d SR%d %s\n",
	 Brain_StateAbbrev[State],
	 DriveMotors_LeftMotorSpeed,
	 DriveMotors_RightMotorSpeed,
	 WallSensors_FrontReading,
	 WallSensors_SideFrontReading,
	 WallSensors_SideRearReading,
	 msg);
}

/*-------------------------------------------- Brain Initialize and Finalize */

void
Brain_Finalize() {
  UI_DisplayLine("Finalizing...");

  /* Turn off all motor ports immediately for safety. */
  alloff();

  WallSensors_Finalize();
}

void
Brain_Initialize() {
  UI_DisplayLine("Initializing...");

  /* Ensure all motor ports turned off. */
  alloff();

  DriveMotors_Initialize();
  Map_Initialize();
  WallSensors_Initialize();
  Status_Update();

}

/*------------------------------------------------------------ Brain Welcome */

#define WelcomeDisplay(line1,line2) {		\
  printf("%s%s\n", line1, line2);		\
  if (choose_button()) {			\
    while (choose_button()) /*NOP*/;		\
    return;					\
  }						\
}

#define WelcomeDisplayDelay(line1,line2,delay) {	\
  printf("%s%s\n", line1, line2); 			\
  if (UI_SleepOrChooseButton(delay)) return;		\
}

void
Brain_Welcome() {
  while (True) {
    /*.................("1234567890123456", */
    WelcomeDisplayDelay("    Welcome     ",
			"       to       ", 1.0);
    WelcomeDisplayDelay("    magellan    ",
			"                ", 0.1);
    WelcomeDisplayDelay("    MAGELLAN    ",
			"                ", 0.1);
    WelcomeDisplayDelay("  Brown CS148   ",
			" Final Project  ", 2.0);
    WelcomeDisplayDelay("by  M. Gorguner ",
			"and N. Van Dyke ", 2.0);
    WelcomeDisplayDelay("  Featuring...  ",
			"                ", 1.0);
    WelcomeDisplayDelay("* Mapping       ",
			"                ", 0.8);
    WelcomeDisplayDelay("* Wall Following",
			"                ", 0.8);
    WelcomeDisplayDelay("* Custom Infra- ",
			"  Red Sensors   ", 0.8);
    WelcomeDisplayDelay("* Wheel Shaft   ",
			"  Encoding      ", 0.8);
    WelcomeDisplayDelay("* Differential  ",
			"  Drive         ", 0.8);
    WelcomeDisplayDelay("* Kick-Butt     ",
			"  Software      ", 0.8);
    WelcomeDisplayDelay("* Artistic      ",
			"  Chassis       ", 0.8);
    WelcomeDisplayDelay("* Fun for the   ",
			"  Whole Family  ", 0.8);
    WelcomeDisplayDelay("", "", 0.8);
    WelcomeDisplayDelay("    A  A  A     ", "", 0.1);
    WelcomeDisplayDelay("", "", 0.8);
  }
}

/*------------------------------------------------------ Brain State Machine */

/* (My kingdom for a defmacro!) */
#define ACTIVITY             /*NOP*/
#define BODY_BEGIN           while (1) { \
                               if (escape_button()) Escape_Handle(); \
                               WallSensors_GetReadings();
#define BODY_END             }
#define ENTRY                /*NOP*/
#define ESCAPE_CHECK         
#define EXIT                 /*NOP*/
#define LAMBDA_TRANSITION(s) State = (Brain_State_ ## s)
#define STATE(s)             else if (State == ( Brain_State_ ## s))
#define STATES_BEGIN         if (False) /*NOP*/;
#define STATES_END           else { Status_InternalError("BADSTATE"); }
#define TRANSITION(s, cond)  {if (cond) {State = (Brain_State_ ## s); break;}}

/* State IDs */
#define Brain_State_Forward 0
#define Brain_State_Left    1
#define Brain_State_Right   2
#define Brain_State_Right2  3
#define Brain_State_Right3  4
#define Brain_State_Right4  5
#define Brain_State_Start   6

char Brain_StateAbbrev[7][4] = {
  "Fwd", "Lft", "Rt1", "Rt2", "Rt3", "Rt4", "Stt" };

int State;

int
Brain_WallFollowAdjustment() {
  return ((abs(IdealWallFollowingDistance - WallSensors_SideFrontReading) +
	   abs(IdealWallFollowingDistance - WallSensors_SideRearReading))
	  * WallFollowAdjustmentFactor);
}

void
Brain_RunStateMachine() {
  int state;

  UI_DisplayLine("Running...");

  /*
   * Note: Some of these states are presently unnecessary, but we want to keep
   * the distinction until everything is working, for ease of extending
   * behavior.
   */
  
  State = Brain_State_Forward;
  while (True) {
    Status_Update();
    Sound_StateChangeBeep();
    STATES_BEGIN 
      STATE(Forward) {
      ENTRY {
	WheelEncoding_Start();
	DriveMotors_SetFullForward();
      }
      BODY_BEGIN {
	TRANSITION(Right, (WallSensors_SideFrontReading == 
			   WallSensors_NoWallReading));
	TRANSITION(Left,
		   ((WallSensors_FrontReading != WallSensors_NoWallReading) &&
		    (WallSensors_FrontReading <= 
		     FrontWallSensorThresholdForLeftTurn)));
	ACTIVITY {

	  if (WallSensors_SideRearReading == WallSensors_NoWallReading) {
	    DriveMotors_SetSpeeds(50, 100);
	  } else {
	    int frontRel =
	      WallSensors_ReadingRelationToIdeal(WallSensors_SideFrontReading);
	    int rearRel =
	      WallSensors_ReadingRelationToIdeal(WallSensors_SideRearReading);

	    if ((frontRel == 1) && (rearRel == 1) &&
		(WallSensors_SideRearReading > WallSensors_SideFrontReading)) {
	      DriveMotors_SetSpeeds(100 - 
				    (10 * (WallSensors_SideRearReading - 
					   WallSensors_SideFrontReading)),
				    100);
	    } else if ((frontRel == -1) ||
		       ((frontRel == 0) && (rearRel == 1))) {
	      /* Left */
	      DriveMotors_SetSpeeds((100 - Brain_WallFollowAdjustment()), 100);
	    } else if ((frontRel == 1) ||
		       ((frontRel == 0) && (rearRel == -1))) {
	      /* Right */
	      DriveMotors_SetSpeeds(100, (100 - Brain_WallFollowAdjustment()));
	    } else {
	      /* Straight */
	      DriveMotors_SetFullForward();
	    }
	  }
	  Status_Update();
	}
      } BODY_END;
      EXIT {
	Status_Update();
	WheelEncoding_Stop();
	Map_AddLengthFromWheelEncoding();
      }
    } STATE(Left) {
      ENTRY {
	DriveMotors_TurnOff();
	WallSensors_GetReadings();
	Map_AddLength(WallSensors_FrontReading + 
		      avg(WallSensors_SideFrontReading,
			  WallSensors_SideRearReading) +
		      RobotLengthCm);
	DriveMotors_TurnLeft90Degrees();
	Map_AddTurnCode(Map_TurnCode_Left);
      }
      LAMBDA_TRANSITION(Forward);
    } STATE(Right) {
      ENTRY {
	WheelEncoding_Start();
	DriveMotors_SetFullForward();
      }
      BODY_BEGIN {
	TRANSITION(Right2, (WallSensors_SideRearReading ==
			    WallSensors_NoWallReading));
	ACTIVITY {
	  Status_Update();
	}
      } BODY_END;
      EXIT {
	Status_Update();
	WheelEncoding_Stop();
	Map_AddLengthFromWheelEncoding();
      }
    } STATE(Right2) {
      ENTRY {
	WheelEncoding_Start();
	DriveMotors_MoveForwardNCm((IdealWallFollowingDistance - 2) +
				   CmProtrudingPastSideRearSensor);
	WheelEncoding_Stop();
	DriveMotors_TurnRight90Degrees();
	Map_AddTurnCode(Map_TurnCode_Right);
      }
      LAMBDA_TRANSITION(Right3);
    } STATE(Right3) {
      ENTRY {
	DriveMotors_SetFullForward();
      }
      BODY_BEGIN {
	TRANSITION(Right4, (WallSensors_SideFrontReading !=
			    WallSensors_NoWallReading));
	ACTIVITY {
	  Status_Update();
	}
      } BODY_END;
      EXIT {
	Status_Update();
      }
    } STATE(Right4) {
      ENTRY {
	DriveMotors_SetFullForward();
      }
      BODY_BEGIN {
	TRANSITION(Forward, (WallSensors_SideRearReading !=
			     WallSensors_NoWallReading));
	ACTIVITY {
	  Status_Update();
	  if ((WallSensors_SideFrontReading == WallSensors_NoWallReading) ||
	      ((IdealWallFollowingDistance -
		WallSensors_SideFrontReading)
	       < -2)) {
	    DriveMotors_SetSpeeds(100, 50);
	  } else if ((IdealWallFollowingDistance -
		      WallSensors_SideFrontReading)
		     > 2) {
	    DriveMotors_SetSpeeds(50, 100);
	  } else {
	    DriveMotors_SetFullForward();
	  }
	}
      } BODY_END;
      EXIT {
	Status_Update();
      }
    } STATE(Start) {
      /* Note: This state is not currently used. */
      LAMBDA_TRANSITION(Forward);
    } STATES_END;
  }
}

int
Brain_Calibrate() {
  /* Note: Adapted from "select_float_value" in "menu.c". */
  float min_val = 0.5;
  float max_val = 1.8;

  float val;
  float coarseness = 255.0 / (max_val - min_val);
  float selection;

  while(!choose_button()) {
    WallSensors_Calibration =
      (((float)frob_knob()) / coarseness) + min_val;

    WallSensors_FrontFirst     = True;
    WallSensors_SideFrontFirst = True;
    WallSensors_SideRearFirst  = True;
    
    WallSensors_GetReadings();
    printf("Cal F%d SF%d SR%d (%f)\n", 
	   WallSensors_FrontReading,
	   WallSensors_SideFrontReading,
	   WallSensors_SideRearReading,
	   WallSensors_Calibration);
    sleep(0.1);
  }

  while (choose_button()) /*NOP*/;
}

/*--------------------------------------------------------------------- main */

void
main() {
  Brain_Initialize();
  Brain_Welcome();
  Brain_Calibrate();
  Brain_RunStateMachine();
  Brain_Finalize();
  UI_DisplayLine("End of Run");
}

/*EOF*/